├── .gitattributes ├── .gitignore ├── .travis.yml ├── .vscode ├── extensions.json └── settings.json ├── README.md ├── STM32-Flight-Computer.code-workspace ├── TODO.md ├── include ├── README ├── constants.h ├── helpers.h ├── mean_sensor_filter.h └── state.h ├── lib ├── Arduino_LSM9DS1_ID6589 │ ├── .library.json │ ├── CHANGELOG │ ├── README.adoc │ ├── examples │ │ ├── SimpleAccelerometer │ │ │ └── SimpleAccelerometer.ino │ │ ├── SimpleGyroscope │ │ │ └── SimpleGyroscope.ino │ │ └── SimpleMagnetometer │ │ │ └── SimpleMagnetometer.ino │ ├── keywords.txt │ ├── library.properties │ └── src │ │ ├── Arduino_LSM9DS1.h │ │ ├── LSM9DS1.cpp │ │ └── LSM9DS1.h ├── IMU │ ├── IMU.cpp │ ├── IMU.h │ ├── LSM9DS0.h │ └── LSM9DS1.h └── README ├── platformio.ini ├── src └── main.cpp └── test └── README /.gitattributes: -------------------------------------------------------------------------------- 1 | # Auto detect text files and perform LF normalization 2 | * text=auto 3 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Prerequisites 2 | *.d 3 | 4 | # Compiled Object files 5 | *.slo 6 | *.lo 7 | *.o 8 | *.obj 9 | 10 | # Precompiled Headers 11 | *.gch 12 | *.pch 13 | 14 | # Compiled Dynamic libraries 15 | *.so 16 | *.dylib 17 | *.dll 18 | 19 | # Fortran module files 20 | *.mod 21 | *.smod 22 | 23 | # Compiled Static libraries 24 | *.lai 25 | *.la 26 | *.a 27 | *.lib 28 | 29 | # Executables 30 | *.exe 31 | *.out 32 | *.app 33 | 34 | # PlatformIO additions 35 | .pio 36 | .vscode/.browse.c_cpp.db* 37 | .vscode/c_cpp_properties.json 38 | .vscode/launch.json 39 | .vscode/ipch 40 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | # Continuous Integration (CI) is the practice, in software 2 | # engineering, of merging all developer working copies with a shared mainline 3 | # several times a day < https://docs.platformio.org/page/ci/index.html > 4 | # 5 | # Documentation: 6 | # 7 | # * Travis CI Embedded Builds with PlatformIO 8 | # < https://docs.travis-ci.com/user/integration/platformio/ > 9 | # 10 | # * PlatformIO integration with Travis CI 11 | # < https://docs.platformio.org/page/ci/travis.html > 12 | # 13 | # * User Guide for `platformio ci` command 14 | # < https://docs.platformio.org/page/userguide/cmd_ci.html > 15 | # 16 | # 17 | # Please choose one of the following templates (proposed below) and uncomment 18 | # it (remove "# " before each line) or use own configuration according to the 19 | # Travis CI documentation (see above). 20 | # 21 | 22 | 23 | # 24 | # Template #1: General project. Test it using existing `platformio.ini`. 25 | # 26 | 27 | language: python 28 | python: 29 | - "2.7" 30 | 31 | sudo: false 32 | cache: 33 | directories: 34 | - "~/.platformio" 35 | 36 | env: 37 | - PLATFORMIO_CI_SRC = src/main.cpp 38 | 39 | install: 40 | - pip install -U platformio 41 | 42 | # Libraries from PlatformIO Library Registry: 43 | - platformio lib -g install 31 # Adafruit Unified Sensor 44 | - platformio lib -g install 528 # Adafruit BMP280 Library 45 | - platformio lib -g install 868 # SD 46 | 47 | script: 48 | - platformio run 49 | 50 | 51 | # 52 | # Template #2: The project is intended to be used as a library with examples. 53 | # 54 | 55 | # language: python 56 | # python: 57 | # - "2.7" 58 | # 59 | # sudo: false 60 | # cache: 61 | # directories: 62 | # - "~/.platformio" 63 | # 64 | # env: 65 | # - PLATFORMIO_CI_SRC=path/to/test/file.c 66 | # - PLATFORMIO_CI_SRC=examples/file.ino 67 | # - PLATFORMIO_CI_SRC=path/to/test/directory 68 | # 69 | # install: 70 | # - pip install -U platformio 71 | # - platformio update 72 | # 73 | # script: 74 | # - platformio ci --lib="." --board=ID_1 --board=ID_2 --board=ID_N 75 | -------------------------------------------------------------------------------- /.vscode/extensions.json: -------------------------------------------------------------------------------- 1 | { 2 | // See http://go.microsoft.com/fwlink/?LinkId=827846 3 | // for the documentation about the extensions.json format 4 | "recommendations": [ 5 | "christian-kohler.path-intellisense", 6 | "davidanson.vscode-markdownlint", 7 | "eamodio.gitlens", 8 | "geddski.macros", 9 | "hookyqr.beautify", 10 | "ms-vscode-remote.vscode-remote-extensionpack", 11 | "ms-vscode.cpptools", 12 | "ms-vsliveshare.vsliveshare-pack", 13 | "platformio.platformio-ide", 14 | "streetsidesoftware.code-spell-checker", 15 | "vscode-icons-team.vscode-icons", 16 | "vscodevim.vim", 17 | "zixuanwang.linkerscript" 18 | ] 19 | } 20 | -------------------------------------------------------------------------------- /.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "cSpell.words": [ 3 | "ESCs", 4 | "FTDI", 5 | "Ozzymaker", 6 | "STSTM", 7 | "UART", 8 | "adafruit", 9 | "ebug", 10 | "erial", 11 | "gitattributes", 12 | "gitignore", 13 | "ldscripts", 14 | "mvhs", 15 | "platformio", 16 | "println", 17 | "stackoverflow", 18 | "strikethrough", 19 | "struct" 20 | ], 21 | "terminal.integrated.scrollback": 10000 22 | } -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # STM32-Flight-Computer 2 | 3 | ![Travis-CI Build Status](https://travis-ci.com/MVHS-Rocket-Group/STM32-Flight-Computer.svg?branch=master) 4 | 5 | Travis CI Dashboard: [travis-ci.com/MVHS-Rocket-Group/STM32-Flight-Computer](https://travis-ci.com/MVHS-Rocket-Group/STM32-Flight-Computer) 6 | 7 | ## Repository Structure 8 | 9 | - `.pio/*`: PlatformIO config files and build directory. (*Generated upon execution of first build*) 10 | - `.vscode/*`: VSCode config files. 11 | - `include/*`: C/C++ header files. 12 | - `constants.h`: Provides constant definitions. 13 | - `helpers.h`: Provides development infrastructure for project. 14 | - `mean_sensor_filter.h`: Lightweight sensor noise filter taking a time-average for all inputs. 15 | - `state.h`: State `struct` providing convenient storage for all values in the state vector. 16 | - `lib/*`: C/C++ libraries. 17 | - `Arduino_LSM9DS1_ID6589/*`: Slightly modified version of [this library from PlatformIO registry](https://platformio.org/lib/show/6589/Arduino_LSM9DS1) tweaked to use the correct addresses for the `LSM9DS1` chip as configured by Ozzymaker. 18 | - `src/*`: C/C++ source code files. 19 | - `main.cpp`: Primary entry point for program. 20 | - `.gitattributes`, `.gitignore`: Git SCM config files. 21 | - `travis.yml`: Travis CI (*continuous integration*) automated build checker config. 22 | - `platformio.ini`: PlatformIO build config. 23 | - `STM32-Flight-Computer.code-workspace`: VSCode workspace file. 24 | - `TODO.md`: TODO list for project. 25 | 26 | ## Description 27 | 28 | STM32 (*Blue Pill development board, `STM32F103C8T6` MCU part*) flight computer and FDR for high-power SRM rockets. This program is developed in conjunction with the [PlatformIO](https://platformio.org) IDE system with integration into VSCode and Atom editors, a substantial upgrade from the oxymoron Arduino IDE. 29 | 30 | Program upload and line-by-line debugging support is provided by the wonderful [ST-Link V2](https://smile.amazon.com/Aideepen-ST-Link-Programming-Emulator-Downloader/dp/B01J7N3RE6) connected to the STM32's SWD (***S**erial **W**ire **D**ebug*) port and a serial text terminal by an [FTDI breakout board](https://smile.amazon.com/HiLetgo-FT232RL-Converter-Adapter-Breakout/dp/B00IJXZQ7C) connected to `UART1` on the STM32. 31 | 32 | ### Relevant Peripherals 33 | 34 | - [Ozzymaker BerryIMU](http://ozzmaker.com/product/berryimu-accelerometer-gyroscope-magnetometer-barometricaltitude-sensor) via I2C 35 | - [SD Card breakout board](https://smile.amazon.com/SenMod-Adapter-Reader-Module-Arduino/dp/B01JYNEX56) via SPI 36 | - [3v3 to 5v signal converter](https://smile.amazon.com/Logic-Converter-Bi-Directional-Module-Arduino/dp/B014MC1OAG) 37 | 38 | ## Principal control flow 39 | 40 | - [Arduino framework](https://docs.platformio.org/en/latest/frameworks/arduino.html) for [STM32 targets](https://docs.platformio.org/en/latest/platforms/ststm32.html) in [PlatformIO](https://platformio.org) 41 | - Control-loop paradigm with State and Goal messages logged. 42 | - Gist of Features: 43 | - Flight controller (*PWM Output*) 44 | - Flight state data logger 45 | - [IMU board: Ozzymaker BerryIMU](http://ozzmaker.com/product/berryimu-accelerometer-gyroscope-magnetometer-barometricaltitude-sensor) 46 | - [LSM9DS1 PIO Library](https://platformio.org/lib/show/6589/Arduino_LSM9DS1) 47 | - [BMP280 PIO Library](http://platformio.org/lib/show/528/Adafruit%20BMP280%20Library) 48 | - [SD PIO Library](http://platformio.org/lib/show/868/SD) 49 | - Important flight events: e.g. Launch detection, arming of different systems, control loop decisions, deployments (detected via accelerometer edges?) 50 | - Camera recorder via “pressing” the record button? 51 | - Landing buzzer control? 52 | - Arming switch toggle (*software interrupt*) 53 | 54 | ## Background on PWM control for Servo Motors and ESCs 55 | 56 | RC PWM has a "window" period of 20ms (milliseconds), with a pulse ranging in width from 1ms to 2ms, where 1ms is ~0% command and 2ms is ~100% command. Duty cycle, a percentage, is a ratio of on-time to off-time. 57 | 58 | ![ESC PWM Diagram](https://upload.wikimedia.org/wikipedia/commons/b/b7/Sinais_controle_servomotor.JPG) 59 | 60 | Therefore: 61 | 62 | - 0% throttle command --> 5% duty cycle 63 | - 100% throttle command --> 10% duty cycle 64 | 65 | [Documentation on PWM usage in Arduino](https://electronicshobbyists.com/arduino-pwm-tutorial) 66 | 67 | ## Rocket IMU Axes 68 | 69 | From perspective of a cockpit at the nose cone: (***TODO: Verify if these are still correct!***) 70 | 71 | ![Originally defined like a fighter plane due to how early spacecraft were flight cockpits plopped on the top of rocket boosters.](https://qph.fs.quoracdn.net/main-qimg-67b906f1ec6e62819e16134e76b8830f-c) 72 | 73 | | Vehicle Axis: | Axis Description: | IMU Measurement Axis: | 74 | |--------------:|-------------------|:----------------------| 75 | | X | *roll - vertical axis through center of rocket* | +X (*acc*), +X (*gyro*) | 76 | | Y | *pitch - horizontal axis* | -Y (*acc*), +Y (*gyro*) | 77 | | Z | *yaw - horizontal axis* | -Z (*acc*), +Z (*gyro*) | 78 | 79 | ## Helpful Resources 80 | 81 | - ESC 82 | - [ESC Specs](https://hobbyking.com/en_us/turnigy-monster-2000-200a-4-12s-brushless-esc.html) 83 | - [ESC manual](https://cdn-global-hk.hobbyking.com/media/file/969150300X462171X21.pdf) 84 | - [ESC programming card](https://hobbyking.com/en_us/turnigy-monster-2000-esc-programming-card.html) 85 | 86 | ## PlatformIO STM32 linker scripts 87 | 88 | For some reason, the default `STSTM32` linker scripts for the generic `STM32F103C8`, the chip on the Blue Pill, assume that the the MCU has 64K of program flash memory, which is just not the case, as most boards around have 128K of flash. This shouldn't be an issue, but for the extra headroom, this is a good mod. [Original post on PlatformIO Community forum.](https://community.platformio.org/t/stm32f1-blue-pill-stuck-in-dfu-mode-after-upload/6853/19?u=ifconfig) 89 | 90 | | File | Original | Post-mod | 91 | |------|----------|----------| 92 | | `[USER_DIR]\.platformio\platforms\ststm32\boards\genericSTM32F103C8.json` | `"maximum_size": 65536,` | `"maximum_size": 131072,` | 93 | | `[USER_DIR]\.platformio\platforms\ststm32\ldscripts\stm32f103x8.ld` | `FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 64K` | `FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 128K` | 94 | -------------------------------------------------------------------------------- /STM32-Flight-Computer.code-workspace: -------------------------------------------------------------------------------- 1 | { 2 | "folders": [ 3 | { 4 | "path": "." 5 | } 6 | ], 7 | "settings": { 8 | "files.associations": { 9 | "array": "cpp", 10 | "atomic": "cpp", 11 | "*.tcc": "cpp", 12 | "cctype": "cpp", 13 | "clocale": "cpp", 14 | "cmath": "cpp", 15 | "cstdarg": "cpp", 16 | "cstddef": "cpp", 17 | "cstdint": "cpp", 18 | "cstdio": "cpp", 19 | "cstdlib": "cpp", 20 | "cwchar": "cpp", 21 | "cwctype": "cpp", 22 | "deque": "cpp", 23 | "unordered_map": "cpp", 24 | "vector": "cpp", 25 | "exception": "cpp", 26 | "algorithm": "cpp", 27 | "functional": "cpp", 28 | "iterator": "cpp", 29 | "memory": "cpp", 30 | "memory_resource": "cpp", 31 | "optional": "cpp", 32 | "string": "cpp", 33 | "string_view": "cpp", 34 | "system_error": "cpp", 35 | "tuple": "cpp", 36 | "type_traits": "cpp", 37 | "utility": "cpp", 38 | "fstream": "cpp", 39 | "initializer_list": "cpp", 40 | "iosfwd": "cpp", 41 | "istream": "cpp", 42 | "limits": "cpp", 43 | "new": "cpp", 44 | "ostream": "cpp", 45 | "sstream": "cpp", 46 | "stdexcept": "cpp", 47 | "streambuf": "cpp", 48 | "cinttypes": "cpp", 49 | "typeinfo": "cpp" 50 | }, 51 | "cSpell.words": [ 52 | "ESCs", 53 | "FTDI", 54 | "Ozzymaker", 55 | "STSTM", 56 | "UART", 57 | "adafruit", 58 | "ebug", 59 | "erial", 60 | "gitattributes", 61 | "gitignore", 62 | "ldscripts", 63 | "platformio", 64 | "println", 65 | "strikethrough", 66 | "struct" 67 | ] 68 | } 69 | } -------------------------------------------------------------------------------- /TODO.md: -------------------------------------------------------------------------------- 1 | # TODO 2 | 3 | ~~Strikethrough~~ means an item is complete. 4 | 5 | ## Incomplete 6 | 7 | - Clear `// TODO:` annotations in `main.cpp` after discussion 8 | - Implement `FlightState`-based control loop in `void loop()` with `switch()` block 9 | - ~~Add goal items to the state vector~~ 10 | - ~~Duty cycle goal for fan pods is only != MIN_COMMAND when in `POWERED_ASSENT`and `BALLISTIC_TRAJECTORY` states~~ 11 | - Place timeout on spinning motors after `POWERED_ASSENT` activated 12 | - Save timestamp for each state transition into a global variable 13 | - ~~Transition to `CALIBRATING_ESC`: when arming switch triggered (LOW --> HIGH)~~ 14 | - ~~Transition to `ARMED`: when 3-second ESC calibration is complete~~ 15 | - ~~Transition to `POWERED_ASSENT`: when IMU sees >2g's of vertical acceleration~~ 16 | - ~~Transition to `BALLISTIC_TRAJECTORY`: when IMU sees <2g's of vertical acceleration~~ 17 | - ~~Transition to `CHUTE_DEPLOYED`: when IMU sees violent jerk of deployment mech~~ 18 | - ~~Transition to `LANDED`: when IMU sees violent jerk of landing~~ 19 | - Implement event tracking, add as detected to state vector 20 | - Decide what events are worth noting down 21 | 22 | ### Aspirations 23 | 24 | - Camera recorder via “pressing” the record button? 25 | - Landing buzzer control? 26 | 27 | ## Completed 28 | 29 | - ~~Flight controller (*PWM Output*)~~ 30 | - ~~Flight state data logger~~ 31 | - ~~Implement `CALIBRATING` and `DISARMED` states in `FlightState` enum type~~ 32 | - ~~Rename `ON_PAD` state to `ARMED`~~ 33 | - ~~Verify that pin assignments in `constants.h` match reality~~ 34 | - ~~Configure TravisCI~~ 35 | - ~~Implement RGB status LED control~~ 36 | - ~~Add debug color for error code to EXTernal LED (*activated in `setup()` if I/O not initialized properly*)~~ 37 | - ~~EXPeriment codes~~ 38 | - ~~RED when >1g~~ 39 | - ~~GREEN when within +/- .1g~~ 40 | - ~~BLUE when negative <0g~~ 41 | - ~~EXTernal codes~~ 42 | - ~~RED when EXC calibration complete~~ 43 | - ~~GREEN when fans turned on~~ 44 | - ~~RED when fans off~~ 45 | - ~~Create constants for pin assignments and colors in `constants.h`~~ 46 | - ~~Implement helper function to write color to LED already~~ 47 | -------------------------------------------------------------------------------- /include/README: -------------------------------------------------------------------------------- 1 | 2 | This directory is intended for project header files. 3 | 4 | A header file is a file containing C declarations and macro definitions 5 | to be shared between several project source files. You request the use of a 6 | header file in your project source file (C, C++, etc) located in `src` folder 7 | by including it, with the C preprocessing directive `#include'. 8 | 9 | ```src/main.c 10 | 11 | #include "header.h" 12 | 13 | int main (void) 14 | { 15 | ... 16 | } 17 | ``` 18 | 19 | Including a header file produces the same results as copying the header file 20 | into each source file that needs it. Such copying would be time-consuming 21 | and error-prone. With a header file, the related declarations appear 22 | in only one place. If they need to be changed, they can be changed in one 23 | place, and programs that include the header file will automatically use the 24 | new version when next recompiled. The header file eliminates the labor of 25 | finding and changing all the copies as well as the risk that a failure to 26 | find one copy will result in inconsistencies within a program. 27 | 28 | In C, the usual convention is to give header files names that end with `.h'. 29 | It is most portable to use only letters, digits, dashes, and underscores in 30 | header file names, and at most one dot. 31 | 32 | Read more about using header files in official GCC documentation: 33 | 34 | * Include Syntax 35 | * Include Operation 36 | * Once-Only Headers 37 | * Computed Includes 38 | 39 | https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html 40 | -------------------------------------------------------------------------------- /include/constants.h: -------------------------------------------------------------------------------- 1 | #ifndef CONSTANTS_H 2 | #define CONSTANTS_H 3 | 4 | #define SD_CS_PIN PB0 5 | #define SERIAL_TERM_BAUD 115200 6 | #define DEBUG_MODE true 7 | 8 | #define ARM_SWITCH_PIN PA0 9 | #define PWM_POD1_PIN PB3 10 | #define PWM_POD2_PIN PB4 11 | 12 | // EXPeriment package status LED. 13 | #define EXP_LED_R_PIN PB14 14 | #define EXP_LED_G_PIN PB13 15 | #define EXP_LED_B_PIN PB12 16 | 17 | // EXTernal status LED. 18 | #define EXT_LED_R_PIN PA1 19 | #define EXT_LED_G_PIN PA2 20 | #define EXT_LED_B_PIN PA3 21 | 22 | #define PWM_WRITE_RES 12 // Change from default 8-bits to 12-bits 23 | 24 | #define PWM_WRITE_RES 12 // Change from default 8-bits to 12-bits 25 | #define PWM_FREQ 1 / 0.02 // frequency (Hz) = 1 / period (sec) 26 | #define PWM_MAX_DUTY pow(2, PWM_WRITE_RES) * 2 / 20 // PWM value when firewalling the throttle. 27 | #define PWM_MIN_DUTY pow(2, PWM_WRITE_RES) * 1 / 20 // PWM value when at idle throttle.define PWM_POD2_PIN PB4 28 | 29 | // RGB colors for the status LED. 30 | #define RGB_RED 255, 0, 0 31 | #define RGB_GREEN 0, 255, 0 32 | #define RGB_BLUE 0, 0, 255 33 | #define RGB_YELLOW 255, 255, 0 34 | 35 | #endif -------------------------------------------------------------------------------- /include/helpers.h: -------------------------------------------------------------------------------- 1 | #ifndef HELPERS_H 2 | #define HELPERS_H 3 | #include // Universal Arduino header 4 | #include // I2C comms support for IMU, Barometer 5 | #include // Constants 6 | #include // State class 7 | #include // en.cppreference.com/w/cpp/container/array 8 | 9 | // Scans for devices at all available addresses on the I2C bus and prints 10 | // results. Common I2C address associations: 11 | // https://cdn-learn.adafruit.com/downloads/pdf/i2c-addresses.pdf 12 | void detect_I2C_devices() { 13 | int count = 0; 14 | Wire.begin(); 15 | Serial.println("Searching for I2C Devices..."); 16 | 17 | for (byte i = 8; i < 120; i++) { 18 | Wire.beginTransmission(i); 19 | if (Wire.endTransmission() == 0) { 20 | Serial.print("\tFound: "); 21 | Serial.print(i, DEC); 22 | Serial.print(" aka. 0x"); 23 | Serial.println(i, HEX); 24 | count++; 25 | delay(1); 26 | } 27 | } 28 | 29 | Serial.println("Found " + (String)count + " device(s)."); 30 | } 31 | 32 | // Log the debug message only if the DEBUG flag is set 33 | inline void logMsg(const String& msg) { 34 | if (DEBUG_MODE) Serial.println("DEBUG: " + msg); 35 | } 36 | inline void logErr(const String& msg) { Serial.println("Error: " + msg); } 37 | 38 | class CodeTimer { 39 | public: 40 | CodeTimer(const String& name) : _name(name) { 41 | Serial.println("Begin execution of \"" + _name + "\"."); 42 | _begin = micros(); 43 | } 44 | 45 | ~CodeTimer() { 46 | unsigned long duration = micros() - _begin; 47 | Serial.println("\"" + _name + "\" execution took " + duration / 1000.0 + 48 | "ms."); 49 | } 50 | 51 | private: 52 | unsigned long _begin; 53 | String _name; 54 | }; 55 | 56 | // Set EXPeriment package status LED. 57 | void set_exp_led_color(uint8_t r, uint8_t g, uint8_t b) { 58 | analogWrite(EXP_LED_R_PIN, map(r, 0, 255, 0, PWM_RESOLUTION)); 59 | analogWrite(EXP_LED_G_PIN, map(g, 0, 255, 0, PWM_RESOLUTION)); 60 | analogWrite(EXP_LED_B_PIN, map(b, 0, 255, 0, PWM_RESOLUTION)); 61 | } 62 | 63 | // Set EXTernal status LED. 64 | void set_ext_led_color(uint8_t r, uint8_t g, uint8_t b) { 65 | analogWrite(EXT_LED_R_PIN, map(r, 0, 255, 0, PWM_RESOLUTION)); 66 | analogWrite(EXT_LED_G_PIN, map(g, 0, 255, 0, PWM_RESOLUTION)); 67 | analogWrite(EXT_LED_B_PIN, map(b, 0, 255, 0, PWM_RESOLUTION)); 68 | } 69 | 70 | // Returns whether or not value is within +/- range of assertion. Range should 71 | // be positive. 72 | inline bool within(double value, double assertion, double range) { 73 | return std::abs(value - assertion) < range; 74 | } 75 | 76 | // Define overloads to the /=, *=, and += operators for std::array to help with 77 | // vector arithmentic. 78 | // https://stackoverflow.com/a/3595874/3339274 79 | 80 | template 81 | std::array& operator/=(std::array& dividend, 82 | const T& divisor) { 83 | for (unsigned int i = 0; i < dividend.size(); i++) dividend[i] /= divisor; 84 | return dividend; 85 | } 86 | 87 | template 88 | std::array& operator*=(std::array& arr, const T& scalar) { 89 | for (unsigned int i = 0; i < arr.size(); i++) arr[i] *= scalar; 90 | return arr; 91 | } 92 | template 93 | std::array& operator*=(const T& scalar, std::array& arr) { 94 | for (unsigned int i = 0; i < arr.size(); i++) arr[i] *= scalar; 95 | return arr; 96 | } 97 | 98 | template 99 | std::array& operator+=(std::array& arr1, 100 | std::array& arr2) { 101 | for (unsigned int i = 0; i < arr1.size(); i++) arr1[i] += arr2[i]; 102 | return arr1; 103 | } 104 | 105 | // Define overload to the + operator between types and String objects. 106 | 107 | // https://stackoverflow.com/a/34106613/3339274 108 | template 109 | String& operator+(String& str, T& item) { 110 | return str + (String)item; 111 | } 112 | template 113 | String& operator+(T& item, String& str) { 114 | return (String)item + str; 115 | } 116 | 117 | #endif 118 | -------------------------------------------------------------------------------- /include/mean_sensor_filter.h: -------------------------------------------------------------------------------- 1 | #ifndef MEAN_SENSOR_FILTER_H 2 | #define MEAN_SENSOR_FILTER_H 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | class MeanSensorFilter { 10 | public: 11 | MeanSensorFilter(int max_history_size) : _max_history_size(max_history_size) { 12 | // Initialize the vectors off with the max history size so no reallocation 13 | // should be necessary. 14 | // https://stackoverflow.com/a/11457629/3339274 15 | _acc_history.reserve(_max_history_size); 16 | _gyro_history.reserve(_max_history_size); 17 | _mag_history.reserve(_max_history_size); 18 | _press_history.reserve(_max_history_size); 19 | _temp_history.reserve(_max_history_size); 20 | } 21 | 22 | void add_data(State* state) { 23 | // Maintain history size, remove first item if the buffer gets too long. 24 | if(_acc_history.size() >= _max_history_size) { 25 | // https://stackoverflow.com/a/875109/3339274 26 | _acc_history.erase(_acc_history.begin()); 27 | _gyro_history.erase(_gyro_history.begin()); 28 | _mag_history.erase(_mag_history.begin()); 29 | _press_history.erase(_press_history.begin()); 30 | _temp_history.erase(_temp_history.begin()); 31 | } 32 | 33 | // Add new data to history. 34 | _acc_history.push_back(state->_acc_raw); 35 | _gyro_history.push_back(state->_gyro_raw); 36 | _mag_history.push_back(state->_mag_raw); 37 | _press_history.push_back(state->_press_raw); 38 | _temp_history.push_back(state->_temp_raw); 39 | } 40 | 41 | void calculate_filter(State* state) { 42 | // Create temporary objects to store running tallies and eventually averages. 43 | std::array acc_temp = {0.0, 0.0, 0.0}; 44 | std::array gyro_temp = {0.0, 0.0, 0.0}; 45 | std::array mag_temp = {0.0, 0.0, 0.0}; 46 | double press_temp = 0.0; 47 | double temp_temp = 0.0; 48 | int current_buffer_size = _acc_history.size(); 49 | 50 | // Fill temporary objects with a sum of all data points in the buffer. 51 | for(uint16_t i = 0; i < current_buffer_size; i++) { 52 | acc_temp += _acc_history[i]; 53 | gyro_temp += _gyro_history[i]; 54 | mag_temp += _mag_history[i]; 55 | 56 | press_temp += _press_history[i]; 57 | temp_temp += _temp_history[i]; 58 | } 59 | 60 | // Convert totals to averages... 61 | acc_temp /= (double)current_buffer_size; 62 | gyro_temp /= (double)current_buffer_size; 63 | mag_temp /= (double)current_buffer_size; 64 | 65 | press_temp = press_temp / current_buffer_size; 66 | temp_temp = temp_temp / current_buffer_size; 67 | 68 | // Store averages into the state object. 69 | state->_acc_f = acc_temp; 70 | state->_gyro_f = gyro_temp; 71 | state->_mag_f = mag_temp; 72 | state->_press_f = press_temp; 73 | state->_temp_f = temp_temp; 74 | } 75 | 76 | private: 77 | unsigned int _max_history_size; 78 | 79 | std::vector> _acc_history; 80 | std::vector> _gyro_history; 81 | std::vector> _mag_history; 82 | 83 | std::vector _press_history; 84 | std::vector _temp_history; 85 | }; 86 | 87 | #endif -------------------------------------------------------------------------------- /include/state.h: -------------------------------------------------------------------------------- 1 | #ifndef STATE_H 2 | #define STATE_H 3 | #include // Universal Arduino header 4 | #include // Constants 5 | #include // Helper objects 6 | #include // en.cppreference.com/w/cpp/container/array 7 | #include // Math operations 8 | 9 | enum FlightState { 10 | DISARMED, 11 | CALIBRATING_ESC, 12 | ARMED, 13 | POWERED_ASSENT, 14 | BALLISTIC_TRAJECTORY, 15 | CHUTE_DEPLOYED, 16 | LANDED 17 | }; 18 | // https://stackoverflow.com/a/5094430/3339274 19 | inline String FlightState_text(FlightState state) { 20 | switch (state) { 21 | case DISARMED: 22 | return "DISARMED"; 23 | case CALIBRATING_ESC: 24 | return "CALIBRATING_ESC"; 25 | case ARMED: 26 | return "ARMED"; 27 | case POWERED_ASSENT: 28 | return "POWERED_ASSENT"; 29 | case BALLISTIC_TRAJECTORY: 30 | return "BALLISTIC_TRAJECTORY"; 31 | case CHUTE_DEPLOYED: 32 | return "CHUTE_DEPLOYED"; 33 | case LANDED: 34 | return "LANDED"; 35 | default: 36 | return "[Unknown FlightState type]"; 37 | } 38 | } 39 | 40 | struct State { 41 | public: 42 | double _time; // Time since boot (seconds) 43 | FlightState _current_flight_state; // Flight state in current timestep 44 | FlightState _next_flight_state; // Calculated flight state 45 | String _events_list; // String list of event labels. 46 | 47 | // Raw sensor readings 48 | std::array _acc_raw; // Acceleration (m/s^2) 49 | std::array _gyro_raw; // Angular velocity (deg/s) 50 | std::array _mag_raw; // Magnetometer values (uT) 51 | double _press_raw; // Atmospheric pressure (hPa) 52 | double _temp_raw; // Ambient temperature (C) 53 | 54 | // Filtered sensor readings (rolling average) 55 | std::array _acc_f; // Acceleration (m/s^2) 56 | std::array _gyro_f; // Angular velocity (deg/s) 57 | std::array _mag_f; // Magnetometer values (uT) 58 | double _press_f; // Atmospheric pressure (hPa) 59 | double _temp_f; // Ambient temperature (C) 60 | 61 | // Goals/outputs 62 | std::array _external_led; // R, G, B state sent to LED 63 | std::array _experiment_led; // R, G, B state sent to LED 64 | int _esc_pod_1_pwm; // PWM value sent to ESC pod 1 65 | int _esc_pod_2_pwm; // PWM value sent to ESC pod 2 66 | 67 | // Quick and dirty contstructor, doesn't set anything but time. 68 | State() { _time = millis() / 1000.0; } 69 | 70 | // Takes in sensor readings, leaving goals/outputs to be manually set later. 71 | State(std::array& acc_raw, std::array& gyro_raw, 72 | std::array& mag_raw, double press_raw, double temp_raw, 73 | std::array& acc_f, std::array& gyro_f, 74 | std::array& mag_f, double press_f, double temp_f) 75 | : _acc_raw(acc_raw), 76 | _gyro_raw(gyro_raw), 77 | _mag_raw(mag_raw), 78 | _press_raw(press_raw), 79 | _temp_raw(temp_raw), 80 | _acc_f(acc_f), 81 | _gyro_f(gyro_f), 82 | _mag_f(mag_f), 83 | _press_f(press_f), 84 | _temp_f(temp_f) { 85 | _time = millis() / 1000.0; 86 | } 87 | 88 | // https://stackoverflow.com/a/117458/3339274 89 | void add_event(const String& event) { _events_list += event; } 90 | 91 | static String header_line() { 92 | // https://stackoverflow.com/a/3859167/3339274 93 | return "time, flight_state, acc_x_raw, acc_y_raw, acc_z_raw, gyro_x_raw, " 94 | "gyro_y_raw, gyro_z_raw, mag_x_raw, mag_y_raw, mag_z_raw, " 95 | "press_raw, " 96 | "temp_raw, acc_x_f, acc_y_f, acc_z_f, gyro_x_f, gyro_y_f, gyro_z_f, " 97 | "mag_x_f, mag_y_f, mag_z_f, press_f, temp_f, external_led, " 98 | "experiment_led, esc_pod_1_pwm, esc_pod_2_pwm, events"; 99 | } 100 | 101 | double acc_magnitude(bool filtered) const { 102 | if (filtered) 103 | return sqrt(pow(_acc_f[0], 2) + pow(_acc_f[1], 2) + pow(_acc_f[2], 2)); 104 | else 105 | return sqrt(pow(_acc_raw[0], 2) + pow(_acc_raw[1], 2) + 106 | pow(_acc_raw[2], 2)); 107 | } 108 | 109 | double gyro_magnitude(bool filtered) const { 110 | if (filtered) 111 | return sqrt(pow(_gyro_f[0], 2) + pow(_gyro_f[1], 2) + pow(_gyro_f[2], 2)); 112 | else 113 | return sqrt(pow(_gyro_raw[0], 2) + pow(_gyro_raw[1], 2) + 114 | pow(_gyro_raw[2], 2)); 115 | } 116 | 117 | double mag_magnitude(bool filtered) const { 118 | if (filtered) 119 | return sqrt(pow(_mag_f[0], 2) + pow(_mag_f[1], 2) + pow(_mag_f[2], 2)); 120 | else 121 | return sqrt(pow(_mag_raw[0], 2) + pow(_mag_raw[1], 2) + 122 | pow(_mag_raw[2], 2)); 123 | } 124 | 125 | String format_log_line() const { 126 | // First cast to String type is required to start off the expression with 127 | // a String type to concatonate onto. This is because it is illegal in C++ 128 | // to define an addition operator involving two non-"user-defined" types, 129 | // e.g. double and const char[3] like in the case below. 130 | return (String)_time + ", " + FlightState_text(_current_flight_state) + 131 | ", " + 132 | 133 | _acc_raw[0] + ", " + _acc_raw[1] + ", " + _acc_raw[2] + ", " + 134 | _gyro_raw[0] + ", " + _gyro_raw[1] + ", " + _gyro_raw[2] + ", " + 135 | _mag_raw[0] + ", " + _mag_raw[1] + ", " + _mag_raw[2] + ", " + 136 | _press_raw + ", " + _temp_raw + ", " + 137 | 138 | _acc_f[0] + ", " + _acc_f[1] + ", " + _acc_f[2] + ", " + _gyro_f[0] + 139 | ", " + _gyro_f[1] + ", " + _gyro_f[2] + ", " + _mag_f[0] + ", " + 140 | _mag_f[1] + ", " + _mag_f[2] + ", " + _press_f + ", " + _temp_f + 141 | ", " + 142 | 143 | format_arr(_external_led) + ", " + format_arr(_experiment_led) + 144 | ", " + _esc_pod_1_pwm + ", " + _esc_pod_2_pwm + ", " + 145 | 146 | (_events_list == "" ? "[None]" : _events_list); 147 | } 148 | 149 | private: 150 | // Simple space-separated std::array string formatter. 151 | template 152 | static String format_arr(const std::array& arr) { 153 | String output = "\""; 154 | 155 | for (unsigned int i = 0; i < arr.size() - 1; i++) 156 | output += (String)arr[i] + " "; 157 | 158 | output += (String)arr[arr.size() - 1] + "\""; 159 | 160 | return output; 161 | } 162 | }; 163 | 164 | #endif -------------------------------------------------------------------------------- /lib/Arduino_LSM9DS1_ID6589/.library.json: -------------------------------------------------------------------------------- 1 | { 2 | "name": "Arduino_LSM9DS1", 3 | "version": "1.0.0", 4 | "keywords": [ 5 | "sensors" 6 | ], 7 | "description": "Allows you to read the accelerometer, magnetometer and gyroscope values from the LSM9DS1 IMU on your Arduino Nano 33 BLE Sense.", 8 | "frameworks": [ 9 | "arduino" 10 | ], 11 | "platforms": [ 12 | "atmelavr", 13 | "atmelsam", 14 | "espressif32", 15 | "espressif8266", 16 | "infineonxmc", 17 | "intel_arc32", 18 | "kendryte210", 19 | "microchippic32", 20 | "nordicnrf51", 21 | "nordicnrf52", 22 | "ststm32", 23 | "ststm8", 24 | "teensy", 25 | "timsp430" 26 | ], 27 | "authors": [ 28 | { 29 | "email": "info@arduino.cc", 30 | "url": null, 31 | "maintainer": true, 32 | "name": "Arduino" 33 | } 34 | ], 35 | "repository": { 36 | "type": "git", 37 | "url": "https://github.com/arduino-libraries/Arduino_LSM9DS1" 38 | }, 39 | "homepage": null, 40 | "export": { 41 | "include": null, 42 | "exclude": [ 43 | "extras", 44 | "docs", 45 | "tests", 46 | "test", 47 | "*.doxyfile", 48 | "*.pdf" 49 | ] 50 | }, 51 | "id": 6589 52 | } -------------------------------------------------------------------------------- /lib/Arduino_LSM9DS1_ID6589/CHANGELOG: -------------------------------------------------------------------------------- 1 | Arduino_LSM9DS1 ?.?.? - ????.??.?? 2 | 3 | Arduino_LSM9DS1 1.0.0 - 2019.07.31 4 | 5 | * Initial release 6 | -------------------------------------------------------------------------------- /lib/Arduino_LSM9DS1_ID6589/README.adoc: -------------------------------------------------------------------------------- 1 | = LSM9DS1 Library for Arduino = 2 | 3 | Allows you to read the accelerometer, magnetometer and gyroscope values from the LSM9DS1 IMU on your Arduino Nano 33 BLE Sense. 4 | 5 | == License == 6 | 7 | Copyright (c) 2019 Arduino SA. All rights reserved. 8 | 9 | This library is free software; you can redistribute it and/or 10 | modify it under the terms of the GNU Lesser General Public 11 | License as published by the Free Software Foundation; either 12 | version 2.1 of the License, or (at your option) any later version. 13 | 14 | This library is distributed in the hope that it will be useful, 15 | but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 17 | Lesser General Public License for more details. 18 | 19 | You should have received a copy of the GNU Lesser General Public 20 | License along with this library; if not, write to the Free Software 21 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 22 | -------------------------------------------------------------------------------- /lib/Arduino_LSM9DS1_ID6589/examples/SimpleAccelerometer/SimpleAccelerometer.ino: -------------------------------------------------------------------------------- 1 | /* 2 | Arduino LSM9DS1 - Simple Accelerometer 3 | 4 | This example reads the acceleration values from the LSM9DS1 5 | sensor and continuously prints them to the Serial Monitor 6 | or Serial Plotter. 7 | 8 | The circuit: 9 | - Arduino Nano 33 BLE Sense 10 | 11 | created 10 Jul 2019 12 | by Riccardo Rizzo 13 | 14 | This example code is in the public domain. 15 | */ 16 | 17 | #include 18 | 19 | void setup() { 20 | Serial.begin(9600); 21 | while (!Serial); 22 | Serial.println("Started"); 23 | 24 | if (!IMU.begin()) { 25 | Serial.println("Failed to initialize IMU!"); 26 | while (1); 27 | } 28 | 29 | Serial.print("Accelerometer sample rate = "); 30 | Serial.print(IMU.accelerationSampleRate()); 31 | Serial.println(" Hz"); 32 | Serial.println(); 33 | Serial.println("Acceleration in G's"); 34 | Serial.println("X\tY\tZ"); 35 | } 36 | 37 | void loop() { 38 | float x, y, z; 39 | 40 | if (IMU.accelerationAvailable()) { 41 | IMU.readAcceleration(x, y, z); 42 | 43 | Serial.print(x); 44 | Serial.print('\t'); 45 | Serial.print(y); 46 | Serial.print('\t'); 47 | Serial.println(z); 48 | } 49 | } 50 | -------------------------------------------------------------------------------- /lib/Arduino_LSM9DS1_ID6589/examples/SimpleGyroscope/SimpleGyroscope.ino: -------------------------------------------------------------------------------- 1 | /* 2 | Arduino LSM9DS1 - Simple Gyroscope 3 | 4 | This example reads the gyroscope values from the LSM9DS1 5 | sensor and continuously prints them to the Serial Monitor 6 | or Serial Plotter. 7 | 8 | The circuit: 9 | - Arduino Nano 33 BLE Sense 10 | 11 | created 10 Jul 2019 12 | by Riccardo Rizzo 13 | 14 | This example code is in the public domain. 15 | */ 16 | 17 | #include 18 | 19 | void setup() { 20 | Serial.begin(9600); 21 | while (!Serial); 22 | Serial.println("Started"); 23 | 24 | if (!IMU.begin()) { 25 | Serial.println("Failed to initialize IMU!"); 26 | while (1); 27 | } 28 | Serial.print("Gyroscope sample rate = "); 29 | Serial.print(IMU.gyroscopeSampleRate()); 30 | Serial.println(" Hz"); 31 | Serial.println(); 32 | Serial.println("Gyroscope in degrees/second"); 33 | Serial.println("X\tY\tZ"); 34 | } 35 | 36 | void loop() { 37 | float x, y, z; 38 | 39 | if (IMU.gyroscopeAvailable()) { 40 | IMU.readGyroscope(x, y, z); 41 | 42 | Serial.print(x); 43 | Serial.print('\t'); 44 | Serial.print(y); 45 | Serial.print('\t'); 46 | Serial.println(z); 47 | } 48 | } 49 | -------------------------------------------------------------------------------- /lib/Arduino_LSM9DS1_ID6589/examples/SimpleMagnetometer/SimpleMagnetometer.ino: -------------------------------------------------------------------------------- 1 | /* 2 | Arduino LSM9DS1 - Simple Magnetometer 3 | 4 | This example reads the magnetic field values from the LSM9DS1 5 | sensor and continuously prints them to the Serial Monitor 6 | or Serial Plotter. 7 | 8 | The circuit: 9 | - Arduino Nano 33 BLE Sense 10 | 11 | created 10 Jul 2019 12 | by Riccardo Rizzo 13 | 14 | This example code is in the public domain. 15 | */ 16 | 17 | #include 18 | 19 | void setup() { 20 | Serial.begin(9600); 21 | while (!Serial); 22 | Serial.println("Started"); 23 | 24 | if (!IMU.begin()) { 25 | Serial.println("Failed to initialize IMU!"); 26 | while (1); 27 | } 28 | Serial.print("Magnetic field sample rate = "); 29 | Serial.print(IMU.magneticFieldSampleRate()); 30 | Serial.println(" uT"); 31 | Serial.println(); 32 | Serial.println("Magnetic Field in uT"); 33 | Serial.println("X\tY\tZ"); 34 | } 35 | 36 | void loop() { 37 | float x, y, z; 38 | 39 | if (IMU.magneticFieldAvailable()) { 40 | IMU.readMagneticField(x, y, z); 41 | 42 | Serial.print(x); 43 | Serial.print('\t'); 44 | Serial.print(y); 45 | Serial.print('\t'); 46 | Serial.println(z); 47 | } 48 | } 49 | -------------------------------------------------------------------------------- /lib/Arduino_LSM9DS1_ID6589/keywords.txt: -------------------------------------------------------------------------------- 1 | ####################################### 2 | # Syntax Coloring Map For Arduino_LSM9DS1 3 | ####################################### 4 | # Class 5 | ####################################### 6 | 7 | Arduino_LSM9DS1 KEYWORD1 8 | LSM9DS1 KEYWORD1 9 | IMU KEYWORD1 10 | 11 | ####################################### 12 | # Methods and Functions 13 | ####################################### 14 | 15 | begin KEYWORD2 16 | end KEYWORD2 17 | 18 | readAcceleration KEYWORD2 19 | readGyroscope KEYWORD2 20 | readMagneticField KEYWORD2 21 | gyroscopeAvailable KEYWORD2 22 | accelerationAvailable KEYWORD2 23 | magneticFieldAvailable KEYWORD2 24 | accelerationSampleRate KEYWORD2 25 | gyroscopeSampleRate KEYWORD2 26 | magneticFieldSampleRate KEYWORD2 27 | 28 | ####################################### 29 | # Constants 30 | ####################################### 31 | -------------------------------------------------------------------------------- /lib/Arduino_LSM9DS1_ID6589/library.properties: -------------------------------------------------------------------------------- 1 | name=Arduino_LSM9DS1 2 | version=1.0.0 3 | author=Arduino 4 | maintainer=Arduino 5 | sentence=Allows you to read the accelerometer, magnetometer and gyroscope values from the LSM9DS1 IMU on your Arduino Nano 33 BLE Sense. 6 | paragraph= 7 | category=Sensors 8 | url=https://github.com/arduino-libraries/Arduino_LSM9DS1 9 | architectures=* 10 | includes=Arduino_LSM9DS1.h 11 | -------------------------------------------------------------------------------- /lib/Arduino_LSM9DS1_ID6589/src/Arduino_LSM9DS1.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of the Arduino_LSM9DS1 library. 3 | Copyright (c) 2019 Arduino SA. All rights reserved. 4 | 5 | This library is free software; you can redistribute it and/or 6 | modify it under the terms of the GNU Lesser General Public 7 | License as published by the Free Software Foundation; either 8 | version 2.1 of the License, or (at your option) any later version. 9 | 10 | This library is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 13 | Lesser General Public License for more details. 14 | 15 | You should have received a copy of the GNU Lesser General Public 16 | License along with this library; if not, write to the Free Software 17 | Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 18 | */ 19 | 20 | #ifndef _LSM9DS1_H_ 21 | #define _LSM9DS1_H_ 22 | 23 | #include "LSM9DS1.h" 24 | 25 | #endif 26 | -------------------------------------------------------------------------------- /lib/Arduino_LSM9DS1_ID6589/src/LSM9DS1.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of the Arduino_LSM9DS1 library. 3 | Copyright (c) 2019 Arduino SA. All rights reserved. 4 | 5 | This library is free software; you can redistribute it and/or 6 | modify it under the terms of the GNU Lesser General Public 7 | License as published by the Free Software Foundation; either 8 | version 2.1 of the License, or (at your option) any later version. 9 | 10 | This library is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 13 | Lesser General Public License for more details. 14 | 15 | You should have received a copy of the GNU Lesser General Public 16 | License along with this library; if not, write to the Free Software 17 | Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 18 | */ 19 | 20 | #include "LSM9DS1.h" 21 | 22 | // #define LSM9DS1_ADDRESS 0x6b 23 | #define LSM9DS1_ADDRESS 0x6A 24 | 25 | #define LSM9DS1_WHO_AM_I 0x0f 26 | #define LSM9DS1_CTRL_REG1_G 0x10 27 | #define LSM9DS1_STATUS_REG 0x17 28 | #define LSM9DS1_OUT_X_G 0x18 29 | #define LSM9DS1_CTRL_REG6_XL 0x20 30 | #define LSM9DS1_CTRL_REG8 0x22 31 | #define LSM9DS1_OUT_X_XL 0x28 32 | 33 | // magnetometer 34 | // #define LSM9DS1_ADDRESS_M 0x1e 35 | #define LSM9DS1_ADDRESS_M 0x1C 36 | 37 | #define LSM9DS1_CTRL_REG1_M 0x20 38 | #define LSM9DS1_CTRL_REG2_M 0x21 39 | #define LSM9DS1_CTRL_REG3_M 0x22 40 | #define LSM9DS1_STATUS_REG_M 0x27 41 | #define LSM9DS1_OUT_X_L_M 0x28 42 | 43 | LSM9DS1Class::LSM9DS1Class(TwoWire& wire) : 44 | _wire(&wire) 45 | { 46 | } 47 | 48 | LSM9DS1Class::~LSM9DS1Class() 49 | { 50 | } 51 | 52 | int LSM9DS1Class::begin() 53 | { 54 | _wire->begin(); 55 | 56 | // reset 57 | writeRegister(LSM9DS1_ADDRESS, LSM9DS1_CTRL_REG8, 0x05); 58 | writeRegister(LSM9DS1_ADDRESS_M, LSM9DS1_CTRL_REG2_M, 0x0c); 59 | 60 | delay(10); 61 | 62 | if (readRegister(LSM9DS1_ADDRESS, LSM9DS1_WHO_AM_I) != 0x68) { 63 | end(); 64 | 65 | return 0; 66 | } 67 | 68 | if (readRegister(LSM9DS1_ADDRESS_M, LSM9DS1_WHO_AM_I) != 0x3d) { 69 | end(); 70 | 71 | return 0; 72 | } 73 | 74 | writeRegister(LSM9DS1_ADDRESS, LSM9DS1_CTRL_REG1_G, 0x78); // 119 Hz, 2000 dps, 16 Hz BW 75 | writeRegister(LSM9DS1_ADDRESS, LSM9DS1_CTRL_REG6_XL, 0x70); // 119 Hz, 4G 76 | 77 | writeRegister(LSM9DS1_ADDRESS_M, LSM9DS1_CTRL_REG1_M, 0xb4); // Temperature compensation enable, medium performance, 20 Hz 78 | writeRegister(LSM9DS1_ADDRESS_M, LSM9DS1_CTRL_REG2_M, 0x00); // 4 Gauss 79 | writeRegister(LSM9DS1_ADDRESS_M, LSM9DS1_CTRL_REG3_M, 0x00); // Continuous conversion mode 80 | 81 | return 1; 82 | } 83 | 84 | void LSM9DS1Class::end() 85 | { 86 | writeRegister(LSM9DS1_ADDRESS_M, LSM9DS1_CTRL_REG3_M, 0x03); 87 | writeRegister(LSM9DS1_ADDRESS, LSM9DS1_CTRL_REG1_G, 0x00); 88 | writeRegister(LSM9DS1_ADDRESS, LSM9DS1_CTRL_REG6_XL, 0x00); 89 | 90 | _wire->end(); 91 | } 92 | 93 | int LSM9DS1Class::readAcceleration(double& x, double& y, double& z) 94 | { 95 | int16_t data[3]; 96 | 97 | if (!readRegisters(LSM9DS1_ADDRESS, LSM9DS1_OUT_X_XL, (uint8_t*)data, sizeof(data))) { 98 | x = NAN; 99 | y = NAN; 100 | z = NAN; 101 | 102 | return 0; 103 | } 104 | 105 | x = data[0] * 4.0 / 32768.0; 106 | y = data[1] * 4.0 / 32768.0; 107 | z = data[2] * 4.0 / 32768.0; 108 | 109 | return 1; 110 | } 111 | 112 | int LSM9DS1Class::accelerationAvailable() 113 | { 114 | if (readRegister(LSM9DS1_ADDRESS, LSM9DS1_STATUS_REG) & 0x01) { 115 | return 1; 116 | } 117 | 118 | return 0; 119 | } 120 | 121 | float LSM9DS1Class::accelerationSampleRate() 122 | { 123 | return 119.0F; 124 | } 125 | 126 | int LSM9DS1Class::readGyroscope(double& x, double& y, double& z) 127 | { 128 | int16_t data[3]; 129 | 130 | if (!readRegisters(LSM9DS1_ADDRESS, LSM9DS1_OUT_X_G, (uint8_t*)data, sizeof(data))) { 131 | x = NAN; 132 | y = NAN; 133 | z = NAN; 134 | 135 | return 0; 136 | } 137 | 138 | x = data[0] * 2000.0 / 32768.0; 139 | y = data[1] * 2000.0 / 32768.0; 140 | z = data[2] * 2000.0 / 32768.0; 141 | 142 | return 1; 143 | } 144 | 145 | int LSM9DS1Class::gyroscopeAvailable() 146 | { 147 | if (readRegister(LSM9DS1_ADDRESS, LSM9DS1_STATUS_REG) & 0x02) { 148 | return 1; 149 | } 150 | 151 | return 0; 152 | } 153 | 154 | float LSM9DS1Class::gyroscopeSampleRate() 155 | { 156 | return 119.0F; 157 | } 158 | 159 | int LSM9DS1Class::readMagneticField(double& x, double& y, double& z) 160 | { 161 | int16_t data[3]; 162 | 163 | if (!readRegisters(LSM9DS1_ADDRESS_M, LSM9DS1_OUT_X_L_M, (uint8_t*)data, sizeof(data))) { 164 | x = NAN; 165 | y = NAN; 166 | z = NAN; 167 | 168 | return 0; 169 | } 170 | 171 | x = data[0] * 4.0 * 100.0 / 32768.0; 172 | y = data[1] * 4.0 * 100.0 / 32768.0; 173 | z = data[2] * 4.0 * 100.0 / 32768.0; 174 | 175 | return 1; 176 | } 177 | 178 | int LSM9DS1Class::magneticFieldAvailable() 179 | { 180 | if (readRegister(LSM9DS1_ADDRESS_M, LSM9DS1_STATUS_REG_M) & 0x08) { 181 | return 1; 182 | } 183 | 184 | return 0; 185 | } 186 | 187 | float LSM9DS1Class::magneticFieldSampleRate() 188 | { 189 | return 20.0; 190 | } 191 | 192 | int LSM9DS1Class::readRegister(uint8_t slaveAddress, uint8_t address) 193 | { 194 | _wire->beginTransmission(slaveAddress); 195 | _wire->write(address); 196 | if (_wire->endTransmission() != 0) { 197 | return -1; 198 | } 199 | 200 | if (_wire->requestFrom(slaveAddress, 1) != 1) { 201 | return -1; 202 | } 203 | 204 | return _wire->read(); 205 | } 206 | 207 | int LSM9DS1Class::readRegisters(uint8_t slaveAddress, uint8_t address, uint8_t* data, size_t length) 208 | { 209 | _wire->beginTransmission(slaveAddress); 210 | _wire->write(0x80 | address); 211 | if (_wire->endTransmission(false) != 0) { 212 | return -1; 213 | } 214 | 215 | if (_wire->requestFrom(slaveAddress, length) != length) { 216 | return 0; 217 | } 218 | 219 | for (size_t i = 0; i < length; i++) { 220 | *data++ = _wire->read(); 221 | } 222 | 223 | return 1; 224 | } 225 | 226 | int LSM9DS1Class::writeRegister(uint8_t slaveAddress, uint8_t address, uint8_t value) 227 | { 228 | _wire->beginTransmission(slaveAddress); 229 | _wire->write(address); 230 | _wire->write(value); 231 | if (_wire->endTransmission() != 0) { 232 | return 0; 233 | } 234 | 235 | return 1; 236 | } 237 | 238 | #ifdef ARDUINO_ARDUINO_NANO33BLE 239 | LSM9DS1Class IMU(Wire1); 240 | #else 241 | LSM9DS1Class IMU(Wire); 242 | #endif 243 | -------------------------------------------------------------------------------- /lib/Arduino_LSM9DS1_ID6589/src/LSM9DS1.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of the Arduino_LSM9DS1 library. 3 | Copyright (c) 2019 Arduino SA. All rights reserved. 4 | 5 | This library is free software; you can redistribute it and/or 6 | modify it under the terms of the GNU Lesser General Public 7 | License as published by the Free Software Foundation; either 8 | version 2.1 of the License, or (at your option) any later version. 9 | 10 | This library is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 13 | Lesser General Public License for more details. 14 | 15 | You should have received a copy of the GNU Lesser General Public 16 | License along with this library; if not, write to the Free Software 17 | Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 18 | */ 19 | 20 | #include 21 | #include 22 | 23 | class LSM9DS1Class { 24 | public: 25 | LSM9DS1Class(TwoWire& wire); 26 | virtual ~LSM9DS1Class(); 27 | 28 | int begin(); 29 | void end(); 30 | 31 | // Accelerometer 32 | virtual int readAcceleration(double& x, double& y, double& z); // Results are in G (earth gravity). 33 | virtual int accelerationAvailable(); // Number of samples in the FIFO. 34 | virtual float accelerationSampleRate(); // Sampling rate of the sensor. 35 | 36 | // Gyroscope 37 | virtual int readGyroscope(double& x, double& y, double& z); // Results are in degrees/second. 38 | virtual int gyroscopeAvailable(); // Number of samples in the FIFO. 39 | virtual float gyroscopeSampleRate(); // Sampling rate of the sensor. 40 | 41 | // Magnetometer 42 | virtual int readMagneticField(double& x, double& y, double& z); // Results are in uT (micro Tesla). 43 | virtual int magneticFieldAvailable(); // Number of samples in the FIFO. 44 | virtual float magneticFieldSampleRate(); // Sampling rate of the sensor. 45 | 46 | private: 47 | int readRegister(uint8_t slaveAddress, uint8_t address); 48 | int readRegisters(uint8_t slaveAddress, uint8_t address, uint8_t* data, size_t length); 49 | int writeRegister(uint8_t slaveAddress, uint8_t address, uint8_t value); 50 | 51 | private: 52 | TwoWire* _wire; 53 | }; 54 | 55 | extern LSM9DS1Class IMU; 56 | -------------------------------------------------------------------------------- /lib/IMU/IMU.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | This program reads the angles and heading from the accelerometer, gyroscope 3 | and compass on a BerryIMU connected to an Arduino. 4 | 5 | 6 | Both the BerryIMUv1 and BerryIMUv2 are supported. 7 | Feel free to do whatever you like with this code 8 | Distributed as-is; no warranty is given. 9 | http://ozzmaker.com/ 10 | */ 11 | 12 | #include 13 | #include 14 | #include "LSM9DS0.h" 15 | #include "LSM9DS1.h" 16 | 17 | int LSM9DS0 = 0; 18 | int LSM9DS1 = 0; 19 | 20 | void writeTo(int device, byte address, byte val) { 21 | Wire.beginTransmission(device); // start transmission to device 22 | Wire.write(address); // send register address 23 | Wire.write(val); // send value to write 24 | Wire.endTransmission(); // end transmission 25 | } 26 | void readFrom(int device, byte address, int num, byte buff[]) { 27 | Wire.beginTransmission(device); // start transmission to device 28 | Wire.write(address); // sends address to read from 29 | Wire.endTransmission(); // end transmission 30 | 31 | Wire.beginTransmission( 32 | device); // start transmission to device (initiate again) 33 | Wire.requestFrom(device, num); // request 6 bytes from device 34 | 35 | int i = 0; 36 | while (Wire.available()) // device may send less than requested (abnormal) 37 | { 38 | buff[i] = Wire.read(); // receive a byte 39 | i++; 40 | } 41 | Wire.endTransmission(); // end transmission 42 | } 43 | 44 | void detectIMU() { 45 | Wire.begin(); 46 | byte LSM9DS0_WHO_AM_I_G_response; 47 | byte LSM9DS0_WHO_AM_I_XM_response; 48 | byte LSM9DS1_WHO_M_response; 49 | byte LSM9DS1_WHO_XG_response; 50 | 51 | byte WHOresponse[2]; 52 | 53 | // TODO: Why does this need to be commented out? Does the I2C sender have a builtin timeout? 54 | // Detect if BerryIMUv1 (Which uses a LSM9DS0) is connected 55 | // readFrom(LSM9DS0_GYR_ADDRESS, LSM9DS0_WHO_AM_I_G, 1, WHOresponse); 56 | // LSM9DS0_WHO_AM_I_G_response = WHOresponse[0]; 57 | 58 | // readFrom(LSM9DS0_ACC_ADDRESS, LSM9DS0_WHO_AM_I_XM, 1, WHOresponse); 59 | // LSM9DS0_WHO_AM_I_XM_response = WHOresponse[0]; 60 | 61 | // if (LSM9DS0_WHO_AM_I_G_response == 0xD4 && 62 | // LSM9DS0_WHO_AM_I_XM_response == 0x49) { 63 | // Serial.println("\n\n LSM9DS0 found \n\n"); 64 | // LSM9DS0 = 1; 65 | // } 66 | 67 | // Detect if BerryIMUv2 (Which uses a LSM9DS1) is connected 68 | readFrom(LSM9DS1_MAG_ADDRESS, LSM9DS1_WHO_AM_I_M, 1, WHOresponse); 69 | LSM9DS1_WHO_M_response = WHOresponse[0]; 70 | 71 | readFrom(LSM9DS1_GYR_ADDRESS, LSM9DS1_WHO_AM_I_XG, 1, WHOresponse); 72 | LSM9DS1_WHO_XG_response = WHOresponse[0]; 73 | 74 | if (LSM9DS1_WHO_XG_response == 0x68 && LSM9DS1_WHO_M_response == 0x3D) { 75 | Serial.println("\n\nLSM9DS1 found \n\n"); 76 | LSM9DS1 = 1; 77 | } 78 | 79 | delay(1000); 80 | } 81 | 82 | void enableIMU() { 83 | if (LSM9DS0) { // For BerryIMUv1 84 | // Enable accelerometer 85 | writeTo( 86 | LSM9DS0_ACC_ADDRESS, LSM9DS0_CTRL_REG1_XM, 87 | 0b01100111); // z,y,x axis enabled, continuos update, 100Hz data rate 88 | writeTo(LSM9DS0_ACC_ADDRESS, LSM9DS0_CTRL_REG2_XM, 89 | 0b00100000); // +/- 16G full scale 90 | 91 | // Enable the magnetometer 92 | writeTo(LSM9DS0_MAG_ADDRESS, LSM9DS0_CTRL_REG5_XM, 93 | 0b11110000); // Temp enable, M data rate = 50Hz 94 | writeTo(LSM9DS0_MAG_ADDRESS, LSM9DS0_CTRL_REG6_XM, 95 | 0b01100000); // +/-12gauss 96 | writeTo(LSM9DS0_MAG_ADDRESS, LSM9DS0_CTRL_REG7_XM, 97 | 0b00000000); // Continuous-conversion mode 98 | 99 | // Enable Gyro 100 | writeTo(LSM9DS0_GYR_ADDRESS, LSM9DS0_CTRL_REG1_G, 101 | 0b00001111); // Normal power mode, all axes enabled 102 | writeTo(LSM9DS0_GYR_ADDRESS, LSM9DS0_CTRL_REG4_G, 103 | 0b00110000); // Continuos update, 2000 dps full scale 104 | } else if (LSM9DS1) { // For BerryIMUv2 105 | // Enable the gyroscope 106 | writeTo(LSM9DS1_GYR_ADDRESS, LSM9DS1_CTRL_REG4, 107 | 0b00111000); // z, y, x axis enabled for gyro 108 | writeTo(LSM9DS1_GYR_ADDRESS, LSM9DS1_CTRL_REG1_G, 109 | 0b10111000); // Gyro ODR = 476Hz, 2000 dps 110 | writeTo(LSM9DS1_GYR_ADDRESS, LSM9DS1_ORIENT_CFG_G, 111 | 0b10111000); // Swap orientation 112 | 113 | // Enable the accelerometer 114 | writeTo(LSM9DS1_ACC_ADDRESS, LSM9DS1_CTRL_REG5_XL, 115 | 0b00111000); // z, y, x axis enabled for accelerometer 116 | writeTo(LSM9DS1_ACC_ADDRESS, LSM9DS1_CTRL_REG6_XL, 0b00101000); // +/- 16g 117 | 118 | // Enable the magnetometer 119 | writeTo( 120 | LSM9DS1_MAG_ADDRESS, LSM9DS1_CTRL_REG1_M, 121 | 0b10011100); // Temp compensation enabled,Low power mode mode,80Hz ODR 122 | writeTo(LSM9DS1_MAG_ADDRESS, LSM9DS1_CTRL_REG2_M, 123 | 0b01000000); // +/-12gauss 124 | writeTo(LSM9DS1_MAG_ADDRESS, LSM9DS1_CTRL_REG3_M, 125 | 0b00000000); // continuos update 126 | writeTo(LSM9DS1_MAG_ADDRESS, LSM9DS1_CTRL_REG4_M, 127 | 0b00000000); // lower power mode for Z axis 128 | } 129 | } 130 | 131 | void readACC(byte buff[]) { 132 | if (LSM9DS0) // For BerryIMUv1 133 | readFrom(LSM9DS0_ACC_ADDRESS, 0x80 | LSM9DS0_OUT_X_L_A, 6, buff); 134 | 135 | else if (LSM9DS1) // For BerryIMUv2 136 | readFrom(LSM9DS1_ACC_ADDRESS, 0x80 | LSM9DS1_OUT_X_L_XL, 6, buff); 137 | } 138 | 139 | void readMAG(byte buff[]) { 140 | if (LSM9DS0) // For BerryIMUv1 141 | readFrom(LSM9DS0_MAG_ADDRESS, 0x80 | LSM9DS0_OUT_X_L_M, 6, buff); 142 | else if (LSM9DS1) // For BerryIMUv2 143 | readFrom(LSM9DS1_MAG_ADDRESS, 0x80 | LSM9DS1_OUT_X_L_M, 6, buff); 144 | } 145 | 146 | void readGYR(byte buff[]) { 147 | if (LSM9DS0) // For BerryIMUv1 148 | readFrom(LSM9DS0_GYR_ADDRESS, 0x80 | LSM9DS0_OUT_X_L_G, 6, buff); 149 | else if (LSM9DS1) // For BerryIMUv2 150 | readFrom(LSM9DS1_GYR_ADDRESS, 0x80 | LSM9DS1_OUT_X_L_G, 6, buff); 151 | } 152 | -------------------------------------------------------------------------------- /lib/IMU/IMU.h: -------------------------------------------------------------------------------- 1 | void detectIMU(); 2 | void enableIMU(); 3 | void readACC(byte buff[]); 4 | void readMAG(byte buff[]); 5 | void readGYR(byte buff[]); 6 | 7 | 8 | void writeTo(int device, byte address, byte val); 9 | void readFrom(int device, byte address, int num, byte buff[]); 10 | 11 | -------------------------------------------------------------------------------- /lib/IMU/LSM9DS0.h: -------------------------------------------------------------------------------- 1 | #define LSM9DS0_MAG_ADDRESS 0x1E 2 | #define LSM9DS0_ACC_ADDRESS 0x1E 3 | #define LSM9DS0_GYR_ADDRESS 0x6A 4 | 5 | //LSM9DS0 Gyro Registers 6 | #define LSM9DS0_WHO_AM_I_G 0x0F 7 | #define LSM9DS0_CTRL_REG1_G 0x20 8 | #define LSM9DS0_CTRL_REG2_G 0x21 9 | #define LSM9DS0_CTRL_REG3_G 0x22 10 | #define LSM9DS0_CTRL_REG4_G 0x23 11 | #define LSM9DS0_CTRL_REG5_G 0x24 12 | #define LSM9DS0_REFERENCE_G 0x25 13 | #define LSM9DS0_STATUS_REG_G 0x27 14 | #define LSM9DS0_OUT_X_L_G 0x28 15 | #define LSM9DS0_OUT_X_H_G 0x29 16 | #define LSM9DS0_OUT_Y_L_G 0x2A 17 | #define LSM9DS0_OUT_Y_H_G 0x2B 18 | #define LSM9DS0_OUT_Z_L_G 0x2C 19 | #define LSM9DS0_OUT_Z_H_G 0x2D 20 | #define LSM9DS0_FIFO_CTRL_REG_G 0x2E 21 | #define LSM9DS0_FIFO_SRC_REG_G 0x2F 22 | #define LSM9DS0_INT1_CFG_G 0x30 23 | #define LSM9DS0_INT1_SRC_G 0x31 24 | #define LSM9DS0_INT1_THS_XH_G 0x32 25 | #define LSM9DS0_INT1_THS_XL_G 0x33 26 | #define LSM9DS0_INT1_THS_YH_G 0x34 27 | #define LSM9DS0_INT1_THS_YL_G 0x35 28 | #define LSM9DS0_INT1_THS_ZH_G 0x36 29 | #define LSM9DS0_INT1_THS_ZL_G 0x37 30 | #define LSM9DS0_INT1_DURATION_G 0x38 31 | 32 | //LSM9DS0 Accel and Magneto Registers 33 | #define LSM9DS0_OUT_TEMP_L_XM 0x05 34 | #define LSM9DS0_OUT_TEMP_H_XM 0x06 35 | #define LSM9DS0_STATUS_REG_M 0x07 36 | #define LSM9DS0_OUT_X_L_M 0x08 37 | #define LSM9DS0_OUT_X_H_M 0x09 38 | #define LSM9DS0_OUT_Y_L_M 0x0A 39 | #define LSM9DS0_OUT_Y_H_M 0x0B 40 | #define LSM9DS0_OUT_Z_L_M 0x0C 41 | #define LSM9DS0_OUT_Z_H_M 0x0D 42 | #define LSM9DS0_WHO_AM_I_XM 0x0F 43 | #define LSM9DS0_INT_CTRL_REG_M 0x12 44 | #define LSM9DS0_INT_SRC_REG_M 0x13 45 | #define LSM9DS0_INT_THS_L_M 0x14 46 | #define LSM9DS0_INT_THS_H_M 0x15 47 | #define LSM9DS0_OFFSET_X_L_M 0x16 48 | #define LSM9DS0_OFFSET_X_H_M 0x17 49 | #define LSM9DS0_OFFSET_Y_L_M 0x18 50 | #define LSM9DS0_OFFSET_Y_H_M 0x19 51 | #define LSM9DS0_OFFSET_Z_L_M 0x1A 52 | #define LSM9DS0_OFFSET_Z_H_M 0x1B 53 | #define LSM9DS0_REFERENCE_X 0x1C 54 | #define LSM9DS0_REFERENCE_Y 0x1D 55 | #define LSM9DS0_REFERENCE_Z 0x1E 56 | #define LSM9DS0_CTRL_REG0_XM 0x1F 57 | #define LSM9DS0_CTRL_REG1_XM 0x20 58 | #define LSM9DS0_CTRL_REG2_XM 0x21 59 | #define LSM9DS0_CTRL_REG3_XM 0x22 60 | #define LSM9DS0_CTRL_REG4_XM 0x23 61 | #define LSM9DS0_CTRL_REG5_XM 0x24 62 | #define LSM9DS0_CTRL_REG6_XM 0x25 63 | #define LSM9DS0_CTRL_REG7_XM 0x26 64 | #define LSM9DS0_STATUS_REG_A 0x27 65 | #define LSM9DS0_OUT_X_L_A 0x28 66 | #define LSM9DS0_OUT_X_H_A 0x29 67 | #define LSM9DS0_OUT_Y_L_A 0x2A 68 | #define LSM9DS0_OUT_Y_H_A 0x2B 69 | #define LSM9DS0_OUT_Z_L_A 0x2C 70 | #define LSM9DS0_OUT_Z_H_A 0x2D 71 | #define LSM9DS0_FIFO_CTRL_REG 0x2E 72 | #define LSM9DS0_FIFO_SRC_REG 0x2F 73 | #define LSM9DS0_INT_GEN_1_REG 0x30 74 | #define LSM9DS0_INT_GEN_1_SRC 0x31 75 | #define LSM9DS0_INT_GEN_1_THS 0x32 76 | #define LSM9DS0_INT_GEN_1_DURATION 0x33 77 | #define LSM9DS0_INT_GEN_2_REG 0x34 78 | #define LSM9DS0_INT_GEN_2_SRC 0x35 79 | #define LSM9DS0_INT_GEN_2_THS 0x36 80 | #define LSM9DS0_INT_GEN_2_DURATION 0x37 81 | #define LSM9DS0_CLICK_CFG 0x38 82 | #define LSM9DS0_CLICK_SRC 0x39 83 | #define LSM9DS0_CLICK_THS 0x3A 84 | #define LSM9DS0_TIME_LIMIT 0x3B 85 | #define LSM9DS0_TIME_LATENCY 0x3C 86 | #define LSM9DS0_TIME_WINDOW 0x3D 87 | 88 | -------------------------------------------------------------------------------- /lib/IMU/LSM9DS1.h: -------------------------------------------------------------------------------- 1 | 2 | #define LSM9DS1_MAG_ADDRESS 0x1C //Would be 0x1E if SDO_M is HIGH 3 | #define LSM9DS1_ACC_ADDRESS 0x6A 4 | #define LSM9DS1_GYR_ADDRESS 0x6A //Would be 0x6B if SDO_AG is HIGH 5 | 6 | 7 | #///////////////////////////////////////// 8 | #// LSM9DS1 Accel/Gyro (XL/G) Registers // 9 | #///////////////////////////////////////// 10 | #define LSM9DS1_ACT_THS 0x04 11 | #define LSM9DS1_ACT_DUR 0x05 12 | #define LSM9DS1_INT_GEN_CFG_XL 0x06 13 | #define LSM9DS1_INT_GEN_THS_X_XL 0x07 14 | #define LSM9DS1_INT_GEN_THS_Y_XL 0x08 15 | #define LSM9DS1_INT_GEN_THS_Z_XL 0x09 16 | #define LSM9DS1_INT_GEN_DUR_XL 0x0A 17 | #define LSM9DS1_REFERENCE_G 0x0B 18 | #define LSM9DS1_INT1_CTRL 0x0C 19 | #define LSM9DS1_INT2_CTRL 0x0D 20 | #define LSM9DS1_WHO_AM_I_XG 0x0F 21 | #define LSM9DS1_CTRL_REG1_G 0x10 22 | #define LSM9DS1_CTRL_REG2_G 0x11 23 | #define LSM9DS1_CTRL_REG3_G 0x12 24 | #define LSM9DS1_ORIENT_CFG_G 0x13 25 | #define LSM9DS1_INT_GEN_SRC_G 0x14 26 | #define LSM9DS1_OUT_TEMP_L 0x15 27 | #define LSM9DS1_OUT_TEMP_H 0x16 28 | #define LSM9DS1_STATUS_REG_0 0x17 29 | #define LSM9DS1_OUT_X_L_G 0x18 30 | #define LSM9DS1_OUT_X_H_G 0x19 31 | #define LSM9DS1_OUT_Y_L_G 0x1A 32 | #define LSM9DS1_OUT_Y_H_G 0x1B 33 | #define LSM9DS1_OUT_Z_L_G 0x1C 34 | #define LSM9DS1_OUT_Z_H_G 0x1D 35 | #define LSM9DS1_CTRL_REG4 0x1E 36 | #define LSM9DS1_CTRL_REG5_XL 0x1F 37 | #define LSM9DS1_CTRL_REG6_XL 0x20 38 | #define LSM9DS1_CTRL_REG7_XL 0x21 39 | #define LSM9DS1_CTRL_REG8 0x22 40 | #define LSM9DS1_CTRL_REG9 0x23 41 | #define LSM9DS1_CTRL_REG10 0x24 42 | #define LSM9DS1_INT_GEN_SRC_XL 0x26 43 | #define LSM9DS1_STATUS_REG_1 0x27 44 | #define LSM9DS1_OUT_X_L_XL 0x28 45 | #define LSM9DS1_OUT_X_H_XL 0x29 46 | #define LSM9DS1_OUT_Y_L_XL 0x2A 47 | #define LSM9DS1_OUT_Y_H_XL 0x2B 48 | #define LSM9DS1_OUT_Z_L_XL 0x2C 49 | #define LSM9DS1_OUT_Z_H_XL 0x2D 50 | #define LSM9DS1_FIFO_CTRL 0x2E 51 | #define LSM9DS1_FIFO_SRC 0x2F 52 | #define LSM9DS1_INT_GEN_CFG_G 0x30 53 | #define LSM9DS1_INT_GEN_THS_XH_G 0x31 54 | #define LSM9DS1_INT_GEN_THS_XL_G 0x32 55 | #define LSM9DS1_INT_GEN_THS_YH_G 0x33 56 | #define LSM9DS1_INT_GEN_THS_YL_G 0x34 57 | #define LSM9DS1_INT_GEN_THS_ZH_G 0x35 58 | #define LSM9DS1_INT_GEN_THS_ZL_G 0x36 59 | #define LSM9DS1_INT_GEN_DUR_G 0x37 60 | 61 | #/////////////////////////////// 62 | #// LSM9DS1 Magneto Registers // 63 | #/////////////////////////////// 64 | #define LSM9DS1_OFFSET_X_REG_L_M 0x05 65 | #define LSM9DS1_OFFSET_X_REG_H_M 0x06 66 | #define LSM9DS1_OFFSET_Y_REG_L_M 0x07 67 | #define LSM9DS1_OFFSET_Y_REG_H_M 0x08 68 | #define LSM9DS1_OFFSET_Z_REG_L_M 0x09 69 | #define LSM9DS1_OFFSET_Z_REG_H_M 0x0A 70 | #define LSM9DS1_WHO_AM_I_M 0x0F 71 | #define LSM9DS1_CTRL_REG1_M 0x20 72 | #define LSM9DS1_CTRL_REG2_M 0x21 73 | #define LSM9DS1_CTRL_REG3_M 0x22 74 | #define LSM9DS1_CTRL_REG4_M 0x23 75 | #define LSM9DS1_CTRL_REG5_M 0x24 76 | #define LSM9DS1_STATUS_REG_M 0x27 77 | #define LSM9DS1_OUT_X_L_M 0x28 78 | #define LSM9DS1_OUT_X_H_M 0x29 79 | #define LSM9DS1_OUT_Y_L_M 0x2A 80 | #define LSM9DS1_OUT_Y_H_M 0x2B 81 | #define LSM9DS1_OUT_Z_L_M 0x2C 82 | #define LSM9DS1_OUT_Z_H_M 0x2D 83 | #define LSM9DS1_INT_CFG_M 0x30 84 | #define LSM9DS1_INT_SRC_M 0x30 85 | #define LSM9DS1_INT_THS_L_M 0x32 86 | #define LSM9DS1_INT_THS_H_M 0x33 87 | 88 | #//////////////////////////////// 89 | #// LSM9DS1 WHO_AM_I Responses // 90 | #//////////////////////////////// 91 | #define LSM9DS1_WHO_AM_I_AG_RSP 0x68 92 | #define LSM9DS1_WHO_AM_I_M_RSP 0x3D 93 | 94 | 95 | -------------------------------------------------------------------------------- /lib/README: -------------------------------------------------------------------------------- 1 | 2 | This directory is intended for project specific (private) libraries. 3 | PlatformIO will compile them to static libraries and link into executable file. 4 | 5 | The source code of each library should be placed in a an own separate directory 6 | ("lib/your_library_name/[here are source files]"). 7 | 8 | For example, see a structure of the following two libraries `Foo` and `Bar`: 9 | 10 | |--lib 11 | | | 12 | | |--Bar 13 | | | |--docs 14 | | | |--examples 15 | | | |--src 16 | | | |- Bar.c 17 | | | |- Bar.h 18 | | | |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html 19 | | | 20 | | |--Foo 21 | | | |- Foo.c 22 | | | |- Foo.h 23 | | | 24 | | |- README --> THIS FILE 25 | | 26 | |- platformio.ini 27 | |--src 28 | |- main.c 29 | 30 | and a contents of `src/main.c`: 31 | ``` 32 | #include 33 | #include 34 | 35 | int main (void) 36 | { 37 | ... 38 | } 39 | 40 | ``` 41 | 42 | PlatformIO Library Dependency Finder will find automatically dependent 43 | libraries scanning project source files. 44 | 45 | More information about PlatformIO Library Dependency Finder 46 | - https://docs.platformio.org/page/librarymanager/ldf.html 47 | -------------------------------------------------------------------------------- /platformio.ini: -------------------------------------------------------------------------------- 1 | ;PlatformIO Project Configuration File 2 | ; 3 | ; Build options: build flags, source filter 4 | ; Upload options: custom upload port, speed and extra flags 5 | ; Library options: dependencies, extra library storages 6 | ; Advanced options: extra scripting 7 | ; 8 | ; Please visit documentation for the other options and examples 9 | ; https://docs.platformio.org/page/projectconf.html 10 | 11 | [env:genericSTM32F103C8] 12 | platform = ststm32 13 | board = genericSTM32F103C8 14 | framework = arduino 15 | upload_protocol = stlink 16 | debug_tool = stlink 17 | monitor_speed = 115200 18 | ; board_build.core = maple 19 | board_build.core = stm32 20 | ; https://docs.platformio.org/en/latest/projectconf/section_env_build.html 21 | build_flags = 22 | -Wdeprecated-declarations ; Disable "deprecated-declarations" compiler 23 | ; warnings, spit out ad nosium by the "SD" library 24 | ; https://docs.platformio.org/en/latest/projectconf/section_env_library.html 25 | lib_deps = 26 | 31 ; Adafruit Unified Sensor 27 | 528 ; Adafruit BMP280 Library 28 | 868 ; SD 29 | ; 4175 ; Adafruit LSM9DS1 Library 30 | ; 6589 ; Arduino_LSM9DS1 -------------------------------------------------------------------------------- /src/main.cpp: -------------------------------------------------------------------------------- 1 | #include // For BMP280 Barometer 2 | #include // Universal Arduino header 3 | #include // For LSM9DS01 IMU 4 | #include // For SD Card 5 | #include // SPI comms support for SD 6 | #include // I2C comms support for IMU, Barometer 7 | #include // Constants 8 | #include // Helper objects 9 | #include // MeanSensorFilter class 10 | #include // State class 11 | #include // en.cppreference.com/w/cpp/container/array 12 | 13 | Adafruit_BMP280 bmp; 14 | Adafruit_Sensor *bmp_temp = bmp.getTemperatureSensor(); 15 | Adafruit_Sensor *bmp_pressure = bmp.getPressureSensor(); 16 | String log_file_name = ""; // Path on SD card to current log file. 17 | MeanSensorFilter filter(25); 18 | State previous_timestep; 19 | 20 | unsigned int esc_calibration_begin; 21 | 22 | void setup() { 23 | // Begin serial connection. 24 | Serial.begin(SERIAL_TERM_BAUD); 25 | while (!Serial) delayMicroseconds(1); 26 | 27 | { // I/O pins setup. 28 | analogWriteResolution(PWM_WRITE_RES); 29 | analogWriteFrequency(PWM_FREQ); 30 | pinMode(PWM_POD1_PIN, OUTPUT); 31 | pinMode(PWM_POD2_PIN, OUTPUT); 32 | pinMode(ARM_SWITCH_PIN, INPUT_PULLUP); 33 | pinMode(EXP_LED_R_PIN, OUTPUT); 34 | pinMode(EXP_LED_G_PIN, OUTPUT); 35 | pinMode(EXP_LED_B_PIN, OUTPUT); 36 | pinMode(EXT_LED_R_PIN, OUTPUT); 37 | pinMode(EXT_LED_G_PIN, OUTPUT); 38 | pinMode(EXT_LED_B_PIN, OUTPUT); 39 | } 40 | 41 | // Wait for 5 seconds to allow for terminal connection. 42 | for (uint8_t i = 0; i < 5; i++) { 43 | Serial.print("."); 44 | delay(500); 45 | } 46 | Serial.println(); 47 | 48 | // Lists all available I2C devices for debug purposes. 49 | detect_I2C_devices(); 50 | 51 | { // I2C sensor init. 52 | logMsg("Begin: initializing IMU..."); 53 | if (!IMU.begin()) { 54 | logErr("Failed IMU initialization!"); 55 | // Set error code on external LED. 56 | set_ext_led_color(RGB_YELLOW); 57 | while (1) delayMicroseconds(1); 58 | } else 59 | logMsg("End: initializing IMU."); 60 | 61 | logMsg("Begin: initializing BMP280..."); 62 | if (!bmp.begin()) { 63 | logErr("Failed BMP280 initialization!"); 64 | // Set error code on external LED. 65 | set_ext_led_color(RGB_YELLOW); 66 | while (1) delayMicroseconds(1); 67 | } else 68 | logMsg("End: initializing BMP280."); 69 | } 70 | 71 | // Set default settings for BMP280. (from datasheet) 72 | bmp.setSampling(Adafruit_BMP280::MODE_NORMAL, /* Operating Mode. */ 73 | Adafruit_BMP280::SAMPLING_X2, /* Temp. oversampling */ 74 | Adafruit_BMP280::SAMPLING_X16, /* Pressure oversampling */ 75 | Adafruit_BMP280::FILTER_X16, /* Filtering. */ 76 | Adafruit_BMP280::STANDBY_MS_500); /* Standby time. */ 77 | 78 | { // SPI SD card init. 79 | logMsg("Begin: SD card init..."); 80 | if (!SD.begin(SD_CS_PIN)) { 81 | logErr("SD Card failed, or not present"); 82 | // Set error code on external LED. 83 | set_ext_led_color(RGB_YELLOW); 84 | while (1) delayMicroseconds(1); 85 | } 86 | logMsg("End: SD card init."); 87 | } 88 | 89 | { // Log file init. 90 | // Find out what the latest log file is, and use the next one 91 | int num = 0; 92 | while (log_file_name == "") { 93 | if (SD.exists("log" + (String)num + ".csv")) 94 | num++; 95 | else 96 | log_file_name = "log" + (String)num + ".csv"; 97 | } 98 | logMsg("Using log file: " + (String)log_file_name); 99 | 100 | // Send header line 101 | File log_file = SD.open(log_file_name, FILE_WRITE); 102 | if (log_file) { 103 | log_file.println(State::header_line()); 104 | log_file.close(); 105 | logMsg("CSV log header line written"); 106 | } else 107 | logErr("Error opening " + (String)log_file_name); 108 | } 109 | 110 | // Initialize first loop iteration with DISARMED State. 111 | previous_timestep._current_flight_state = DISARMED; 112 | previous_timestep._next_flight_state = DISARMED; 113 | } 114 | 115 | void loop() { 116 | // CodeTimer loopTimer("void loop()"); 117 | std::array acc_raw; // Acceleration (m/s^2) 118 | std::array gyro_raw; // Angular velocity (deg/s) 119 | std::array mag_raw; // Magnetometer values (uT) 120 | double press_raw; // Atmospheric pressure (hPa) 121 | double temp_raw; // Ambient temperature (C) 122 | 123 | { // Read data from sensors. 124 | // CodeTimer dataReadTimer("sensor read"); 125 | IMU.readAcceleration(acc_raw[0], acc_raw[1], acc_raw[2]); 126 | IMU.readGyroscope(gyro_raw[0], gyro_raw[1], gyro_raw[2]); 127 | IMU.readMagneticField(mag_raw[0], mag_raw[1], mag_raw[2]); 128 | // Adjust acceleration measurements from g's to m/s^2. 129 | acc_raw *= 9.81; 130 | 131 | sensors_event_t temp_event, pressure_event; 132 | bmp_temp->getEvent(&temp_event); 133 | bmp_pressure->getEvent(&pressure_event); 134 | temp_raw = temp_event.temperature; 135 | press_raw = pressure_event.pressure; 136 | } 137 | 138 | // Save recorded data to state vector, update filter. 139 | State current_timestep(acc_raw, gyro_raw, mag_raw, press_raw, temp_raw, 140 | acc_raw, gyro_raw, mag_raw, press_raw, temp_raw); 141 | current_timestep._current_flight_state = previous_timestep._next_flight_state; 142 | { 143 | // CodeTimer recordAndFilter("filtering"); 144 | filter.add_data(¤t_timestep); 145 | filter.calculate_filter(¤t_timestep); 146 | } 147 | 148 | // Set default color for external LED in state vector. 149 | current_timestep._external_led = {0, 0, 0}; 150 | 151 | // Flight control loop. 152 | switch (current_timestep._current_flight_state) { 153 | case DISARMED: 154 | // Dummy case, only to be used if/when arming switch implemented. 155 | current_timestep._next_flight_state = CALIBRATING_ESC; 156 | current_timestep._esc_pod_1_pwm = PWM_MIN_DUTY; 157 | current_timestep._esc_pod_2_pwm = PWM_MIN_DUTY; 158 | logMsg("Begin: ESC MIN_COMMAND calibration..."); 159 | esc_calibration_begin = millis(); 160 | break; 161 | 162 | case CALIBRATING_ESC: 163 | current_timestep._next_flight_state = CALIBRATING_ESC; 164 | // ESC startup protocol: MIN_COMMAND for 3 seconds to let ESC 165 | // self-calibrate. 166 | current_timestep._esc_pod_1_pwm = PWM_MIN_DUTY; 167 | current_timestep._esc_pod_2_pwm = PWM_MIN_DUTY; 168 | 169 | set_ext_led_color(RGB_BLUE); 170 | current_timestep._external_led = {RGB_BLUE}; 171 | 172 | // After 3 seconds, the program contunues and jumps to ARMED state. 173 | if (millis() - esc_calibration_begin >= 3000) { 174 | current_timestep._next_flight_state = ARMED; 175 | logMsg("END: ESC MIN_COMMAND calibration."); 176 | } 177 | break; 178 | 179 | case ARMED: 180 | current_timestep._next_flight_state = ARMED; 181 | current_timestep._esc_pod_1_pwm = PWM_MIN_DUTY; 182 | current_timestep._esc_pod_2_pwm = PWM_MIN_DUTY; 183 | 184 | set_ext_led_color(RGB_RED); 185 | current_timestep._external_led = {RGB_RED}; 186 | 187 | // TODO: Is the IMU y-axis vertical to the rocket? 188 | // Move to POWERED_ASSENT when IMU sees >2g's of vertical acceleration. 189 | if (current_timestep._acc_f[1] > 9.81 * 2) { 190 | current_timestep._next_flight_state = POWERED_ASSENT; 191 | } 192 | break; 193 | 194 | case POWERED_ASSENT: 195 | current_timestep._next_flight_state = POWERED_ASSENT; 196 | // TODO: Do we want full power on the motors? 197 | current_timestep._esc_pod_1_pwm = PWM_MAX_DUTY; 198 | current_timestep._esc_pod_2_pwm = PWM_MAX_DUTY; 199 | 200 | set_ext_led_color(RGB_GREEN); 201 | current_timestep._external_led = {RGB_GREEN}; 202 | 203 | // TODO: Is the IMU y-axis vertical to the rocket? 204 | // Move to BALLISTIC_TRAJECTORY when IMU sees <2g's of vertical 205 | // acceleration. 206 | if (current_timestep._acc_f[1] < 9.81 * 2) { 207 | current_timestep._next_flight_state = BALLISTIC_TRAJECTORY; 208 | } 209 | break; 210 | 211 | case BALLISTIC_TRAJECTORY: 212 | current_timestep._next_flight_state = BALLISTIC_TRAJECTORY; 213 | // TODO: Do we want full power on the motors? 214 | current_timestep._esc_pod_1_pwm = PWM_MAX_DUTY; 215 | current_timestep._esc_pod_2_pwm = PWM_MAX_DUTY; 216 | 217 | set_ext_led_color(RGB_GREEN); 218 | current_timestep._external_led = {RGB_GREEN}; 219 | 220 | // TODO: Is this a good trigger point? 221 | // Move to CHUTE_DEPLOYED when IMU sees a violent jerk from chute release. 222 | if (current_timestep.acc_magnitude(false) > 9.81 * 2) { 223 | current_timestep._next_flight_state = CHUTE_DEPLOYED; 224 | } 225 | break; 226 | 227 | case CHUTE_DEPLOYED: 228 | current_timestep._next_flight_state = CHUTE_DEPLOYED; 229 | current_timestep._esc_pod_1_pwm = PWM_MIN_DUTY; 230 | current_timestep._esc_pod_2_pwm = PWM_MIN_DUTY; 231 | 232 | set_ext_led_color(RGB_RED); 233 | current_timestep._external_led = {RGB_RED}; 234 | 235 | // TODO: Is this a good trigger point? 236 | // Move to LANDED when IMU sees a violent jerk from landing. 237 | if (current_timestep.acc_magnitude(false) > 9.81 * 2) { 238 | current_timestep._next_flight_state = LANDED; 239 | } 240 | break; 241 | 242 | case LANDED: 243 | current_timestep._next_flight_state = LANDED; 244 | // Dummy case, sit here and wait for rocket to be retrieved. 245 | current_timestep._esc_pod_1_pwm = PWM_MIN_DUTY; 246 | current_timestep._esc_pod_2_pwm = PWM_MIN_DUTY; 247 | 248 | set_ext_led_color(RGB_RED); 249 | current_timestep._external_led = {RGB_RED}; 250 | break; 251 | 252 | default: 253 | logErr("Unknown FlightState type detected!"); 254 | break; 255 | } 256 | 257 | // TODO: Is the IMU y-axis vertical to the rocket? 258 | // Experiment LED controller. 259 | if (within(current_timestep._acc_f[1], 9.81, 0.10)) { 260 | set_exp_led_color(RGB_RED); 261 | current_timestep._experiment_led = {RGB_RED}; 262 | } else if (within(current_timestep._acc_f[1], 0, 0.10)) { 263 | set_exp_led_color(RGB_GREEN); 264 | current_timestep._experiment_led = {RGB_GREEN}; 265 | } else if (current_timestep._acc_f[1] < 0.0) { 266 | set_exp_led_color(RGB_BLUE); 267 | current_timestep._experiment_led = {RGB_BLUE}; 268 | } else { 269 | set_exp_led_color(RGB_YELLOW); 270 | current_timestep._experiment_led = {RGB_YELLOW}; 271 | } 272 | 273 | // Write out PWM pulses to ESCs from state vector goals. 274 | analogWrite(PWM_POD1_PIN, current_timestep._esc_pod_1_pwm); 275 | analogWrite(PWM_POD2_PIN, current_timestep._esc_pod_2_pwm); 276 | 277 | { // Write state info to SD file, log to serial. 278 | // CodeTimer logging("Logging"); 279 | String log_line = current_timestep.format_log_line(); 280 | logMsg(log_line); 281 | File log_file = SD.open(log_file_name, FILE_WRITE); 282 | if (log_file) { 283 | log_file.println(log_line); 284 | log_file.close(); 285 | } else 286 | logErr("Failed to open " + log_file_name); 287 | } 288 | 289 | previous_timestep = current_timestep; 290 | 291 | delay(10); 292 | } -------------------------------------------------------------------------------- /test/README: -------------------------------------------------------------------------------- 1 | 2 | This directory is intended for PIO Unit Testing and project tests. 3 | 4 | Unit Testing is a software testing method by which individual units of 5 | source code, sets of one or more MCU program modules together with associated 6 | control data, usage procedures, and operating procedures, are tested to 7 | determine whether they are fit for use. Unit testing finds problems early 8 | in the development cycle. 9 | 10 | More information about PIO Unit Testing: 11 | - https://docs.platformio.org/page/plus/unit-testing.html 12 | --------------------------------------------------------------------------------