Components and supplies
SD Card Adapter
AMS117 3.3V stepdown
AMS117 5V stepdown
MKR GPS shield
Nano 33 BLE Sense
Project description
Code
DataLogger repository
AHRS_gvars.h
c_cpp
Global variable for AHRS algoritm
1uint8_t OSR = ADC_4096; // set pressure amd temperature oversample rate 2uint8_t Gscale = GFS_245DPS; // gyro full scale 3uint8_t Godr = GODR_238Hz; // gyro data sample rate 4uint8_t Gbw = GBW_med; // gyro data bandwidth 5uint8_t Ascale = AFS_2G; // accel full scale 6uint8_t Aodr = AODR_238Hz; // accel data sample rate 7uint8_t Abw = ABW_50Hz; // accel data bandwidth 8uint8_t Mscale = MFS_4G; // mag full scale 9uint8_t Modr = MODR_10Hz; // mag data sample rate 10uint8_t Mmode = MMode_HighPerformance; // magnetometer operation mode 11float aRes, gRes, mRes; // scale resolutions per LSB for the sensors 12int myLed = 13; 13uint16_t Pcal[8]; // calibration constants from MS5611 PROM registers 14unsigned char nCRC; // calculated check sum to ensure PROM integrity 15//-(rk) uint32_t D1 = 0, D2 = 0; // raw MS5611 pressure and temperature data 16double dT, OFFSET, SENS, T2, OFFSET2, SENS2; // First order and second order corrections for raw S5637 temperature and pressure data 17int16_t accelCount[3], gyroCount[3], magCount[3]; // Stores the 16-bit signed accelerometer, gyro, and mag sensor output 18float gyroBias[3] = {0, 0, 0}, accelBias[3] = {0, 0, 0}, magBias[3] = {0, 0, 0}; // Bias corrections for gyro, accelerometer, and magnetometer 19int16_t tempCount; // temperature raw count output 20float temperature; // Stores the LSM9DS1gyro internal chip temperature in degrees Celsius 21double Temperature, Pressure; // stores MS5611 pressures sensor pressure and temperature 22 23// global constants for 9 DoF fusion and AHRS (Attitude and Heading Reference System) 24float GyroMeasError = PI * (40.0f / 180.0f); // gyroscope measurement error in rads/s (start at 40 deg/s) 25float GyroMeasDrift = PI * (0.0f / 180.0f); // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s) 26// There is a tradeoff in the beta parameter between accuracy and response speed. 27// In the original Madgwick study, beta of 0.041 (corresponding to GyroMeasError of 2.7 degrees/s) was found to give optimal accuracy. 28// However, with this value, the LSM9SD0 response time is about 10 seconds to a stable initial quaternion. 29// Subsequent changes also require a longish lag time to a stable output, not fast enough for a quadcopter or robot car! 30// By increasing beta (GyroMeasError) by about a factor of fifteen, the response time constant is reduced to ~2 sec 31// I haven't noticed any reduction in solution accuracy. This is essentially the I coefficient in a PID control sense; 32// the bigger the feedback coefficient, the faster the solution converges, usually at the expense of accuracy. 33// In any case, this is the free parameter in the Madgwick filtering and fusion scheme. 34float beta = sqrt(3.0f / 4.0f) * GyroMeasError; // compute beta 35float zeta = sqrt(3.0f / 4.0f) * GyroMeasDrift; // compute zeta, the other free parameter in the Madgwick scheme usually set to a small or zero value 36#define Kp 2.0f * 5.0f // these are the free parameters in the Mahony filter and fusion scheme, Kp for proportional feedback, Ki for integral 37#define Ki 0.0f 38 39uint32_t delt_t = 0, count = 0, sumCount = 0; // used to control display output rate 40float pitch, yaw, roll; 41float deltat = 0.0f, sum = 0.0f; // integration interval for both filter schemes 42uint32_t lastUpdate = 0, firstUpdate = 0; // used to calculate integration interval 43uint32_t Now = 0; // used to calculate integration interval 44 45float ax, ay, az, gx, gy, gz, mx, my, mz; // variables to hold latest sensor data values 46float q[4] = {1.0f, 0.0f, 0.0f, 0.0f}; // vector to hold quaternion 47float eInt[3] = {0.0f, 0.0f, 0.0f}; // vector to hold integral error for Mahony method 48
AHRS.h
c_cpp
Header file for the AHRS / Madgwick library
1/* LSM9DS1_MS5611_t3 Basic Example Code 2by: Kris Winer 3date: November 1, 2014 4license: Beerware - Use this code however you'd like. If you 5find it useful you can buy me a beer some time. 6Demonstrate basic LSM9DS1 functionality including parameterizing the register addresses, initializing the sensor, 7getting properly scaled accelerometer, gyroscope, and magnetometer data out. Added display functions to 8allow display to on breadboard monitor. Addition of 9 DoF sensor fusion using open source Madgwick and 9Mahony filter algorithms. Sketch runs on the 3.3 V 8 MHz Pro Mini and the Teensy 3.1. 10This sketch is intended specifically for the LSM9DS1+MS5611 Add-on shield for the Teensy 3.1. 11It uses SDA/SCL on pins 17/16, respectively, and it uses the Teensy 3.1-specific Wire library i2c_t3.h. 12The MS5611 is a simple but high resolution pressure sensor, which can be used in its high resolution 13mode but with power consumption od 20 microAmp, or in a lower resolution mode with power consumption of 14only 1 microAmp. The choice will depend on the application. 15SDA and SCL should have external pull-up resistors (to 3.3V). 164K7 resistors are on the LSM9DS1+MS5611 Teensy 3.1 add-on shield/breakout board. 17Hardware setup: 18LSM9DS1Breakout --------- Arduino 19VDD ---------------------- 3.3V 20VDDI --------------------- 3.3V 21SDA ----------------------- A4 22SCL ----------------------- A5 23GND ---------------------- GND 24Note: The LSM9DS1 is an I2C sensor and can use the Arduino Wire library. 25Because the sensor is not 5V tolerant, we are using either a 3.3 V 8 MHz Pro Mini or a 3.3 V Teensy 3.1. 26We have disabled the internal pull-ups used by the Wire library in the Wire.h/twi.c utility file. 27We are also using the 400 kHz fast I2C mode by setting the TWI_FREQ to 400000L /twi.h utility file. 28 29Modified by Ruud Kapteijn 3016-Jan-2021 31Moulded into class AHRS 32taken out NOKIA 5110 monochrome display code 33code: https://github.com/j-mcc1993/LSM9DS1/blob/master/LSM9DS1_BasicAHRS_Nano33.ino 34discussion threa: https://github.com/kriswiner/LSM9DS1/issues/14 35 */ 36 37// See MS5611-02BA03 Low Voltage Barometric Pressure Sensor Data Sheet 38#define MS5611_RESET 0x1E 39#define MS5611_CONVERT_D1 0x40 40#define MS5611_CONVERT_D2 0x50 41#define MS5611_ADC_READ 0x00 42 43// See also LSM9DS1 Register Map and Descriptions, http://www.st.com/st-web-ui/static/active/en/resource/technical/document/datasheet/DM00103319.pdf 44// Accelerometer and Gyroscope registers 45#define LSM9DS1XG_ACT_THS 0x04 46#define LSM9DS1XG_ACT_DUR 0x05 47#define LSM9DS1XG_INT_GEN_CFG_XL 0x06 48#define LSM9DS1XG_INT_GEN_THS_X_XL 0x07 49#define LSM9DS1XG_INT_GEN_THS_Y_XL 0x08 50#define LSM9DS1XG_INT_GEN_THS_Z_XL 0x09 51#define LSM9DS1XG_INT_GEN_DUR_XL 0x0A 52#define LSM9DS1XG_REFERENCE_G 0x0B 53#define LSM9DS1XG_INT1_CTRL 0x0C 54#define LSM9DS1XG_INT2_CTRL 0x0D 55#define LSM9DS1XG_WHO_AM_I 0x0F // should return 0x68 56#define LSM9DS1XG_CTRL_REG1_G 0x10 57#define LSM9DS1XG_CTRL_REG2_G 0x11 58#define LSM9DS1XG_CTRL_REG3_G 0x12 59#define LSM9DS1XG_ORIENT_CFG_G 0x13 60#define LSM9DS1XG_INT_GEN_SRC_G 0x14 61#define LSM9DS1XG_OUT_TEMP_L 0x15 62#define LSM9DS1XG_OUT_TEMP_H 0x16 63#define LSM9DS1XG_STATUS_REG 0x17 64#define LSM9DS1XG_OUT_X_L_G 0x18 65#define LSM9DS1XG_OUT_X_H_G 0x19 66#define LSM9DS1XG_OUT_Y_L_G 0x1A 67#define LSM9DS1XG_OUT_Y_H_G 0x1B 68#define LSM9DS1XG_OUT_Z_L_G 0x1C 69#define LSM9DS1XG_OUT_Z_H_G 0x1D 70#define LSM9DS1XG_CTRL_REG4 0x1E 71#define LSM9DS1XG_CTRL_REG5_XL 0x1F 72#define LSM9DS1XG_CTRL_REG6_XL 0x20 73#define LSM9DS1XG_CTRL_REG7_XL 0x21 74#define LSM9DS1XG_CTRL_REG8 0x22 75#define LSM9DS1XG_CTRL_REG9 0x23 76#define LSM9DS1XG_CTRL_REG10 0x24 77#define LSM9DS1XG_INT_GEN_SRC_XL 0x26 78//#define LSM9DS1XG_STATUS_REG 0x27 // duplicate of 0x17! 79#define LSM9DS1XG_OUT_X_L_XL 0x28 80#define LSM9DS1XG_OUT_X_H_XL 0x29 81#define LSM9DS1XG_OUT_Y_L_XL 0x2A 82#define LSM9DS1XG_OUT_Y_H_XL 0x2B 83#define LSM9DS1XG_OUT_Z_L_XL 0x2C 84#define LSM9DS1XG_OUT_Z_H_XL 0x2D 85#define LSM9DS1XG_FIFO_CTRL 0x2E 86#define LSM9DS1XG_FIFO_SRC 0x2F 87#define LSM9DS1XG_INT_GEN_CFG_G 0x30 88#define LSM9DS1XG_INT_GEN_THS_XH_G 0x31 89#define LSM9DS1XG_INT_GEN_THS_XL_G 0x32 90#define LSM9DS1XG_INT_GEN_THS_YH_G 0x33 91#define LSM9DS1XG_INT_GEN_THS_YL_G 0x34 92#define LSM9DS1XG_INT_GEN_THS_ZH_G 0x35 93#define LSM9DS1XG_INT_GEN_THS_ZL_G 0x36 94#define LSM9DS1XG_INT_GEN_DUR_G 0x37 95// 96// Magnetometer registers 97#define LSM9DS1M_OFFSET_X_REG_L_M 0x05 98#define LSM9DS1M_OFFSET_X_REG_H_M 0x06 99#define LSM9DS1M_OFFSET_Y_REG_L_M 0x07 100#define LSM9DS1M_OFFSET_Y_REG_H_M 0x08 101#define LSM9DS1M_OFFSET_Z_REG_L_M 0x09 102#define LSM9DS1M_OFFSET_Z_REG_H_M 0x0A 103#define LSM9DS1M_WHO_AM_I 0x0F // should be 0x3D 104#define LSM9DS1M_CTRL_REG1_M 0x20 105#define LSM9DS1M_CTRL_REG2_M 0x21 106#define LSM9DS1M_CTRL_REG3_M 0x22 107#define LSM9DS1M_CTRL_REG4_M 0x23 108#define LSM9DS1M_CTRL_REG5_M 0x24 109#define LSM9DS1M_STATUS_REG_M 0x27 110#define LSM9DS1M_OUT_X_L_M 0x28 111#define LSM9DS1M_OUT_X_H_M 0x29 112#define LSM9DS1M_OUT_Y_L_M 0x2A 113#define LSM9DS1M_OUT_Y_H_M 0x2B 114#define LSM9DS1M_OUT_Z_L_M 0x2C 115#define LSM9DS1M_OUT_Z_H_M 0x2D 116#define LSM9DS1M_INT_CFG_M 0x30 117#define LSM9DS1M_INT_SRC_M 0x31 118#define LSM9DS1M_INT_THS_L_M 0x32 119#define LSM9DS1M_INT_THS_H_M 0x33 120 121// Using the LSM9DS1+MS5611 Teensy 3.1 Add-On shield, ADO is set to 1 122// Seven-bit device address of accel/gyro is 110101 for ADO = 0 and 110101 for ADO = 1 123#define ADO 1 124#if ADO 125#define LSM9DS1XG_ADDRESS 0x6B // Device address when ADO = 1 126#define LSM9DS1M_ADDRESS 0x1E // Address of magnetometer 127#define MS5611_ADDRESS 0x77 // Address of altimeter 128#else 129#define LSM9DS1XG_ADDRESS 0x6A // Device address when ADO = 0 130#define LSM9DS1M_ADDRESS 0x1D // Address of magnetometer 131#define MS5611_ADDRESS 0x77 // Address of altimeter 132#endif 133 134#define SerialDebug true // set to true to get Serial output for debugging 135 136// Set initial input parameters 137enum Ascale { // set of allowable accel full scale settings 138 AFS_2G = 0, 139 AFS_16G, 140 AFS_4G, 141 AFS_8G 142}; 143 144enum Aodr { // set of allowable gyro sample rates 145 AODR_PowerDown = 0, 146 AODR_10Hz, 147 AODR_50Hz, 148 AODR_119Hz, 149 AODR_238Hz, 150 AODR_476Hz, 151 AODR_952Hz 152}; 153 154enum Abw { // set of allowable accewl bandwidths 155 ABW_408Hz = 0, 156 ABW_211Hz, 157 ABW_105Hz, 158 ABW_50Hz 159}; 160 161enum Gscale { // set of allowable gyro full scale settings 162 GFS_245DPS = 0, 163 GFS_500DPS, 164 GFS_NoOp, 165 GFS_2000DPS 166}; 167 168enum Godr { // set of allowable gyro sample rates 169 GODR_PowerDown = 0, 170 GODR_14_9Hz, 171 GODR_59_5Hz, 172 GODR_119Hz, 173 GODR_238Hz, 174 GODR_476Hz, 175 GODR_952Hz 176}; 177 178enum Gbw { // set of allowable gyro data bandwidths 179 GBW_low = 0, // 14 Hz at Godr = 238 Hz, 33 Hz at Godr = 952 Hz 180 GBW_med, // 29 Hz at Godr = 238 Hz, 40 Hz at Godr = 952 Hz 181 GBW_high, // 63 Hz at Godr = 238 Hz, 58 Hz at Godr = 952 Hz 182 GBW_highest // 78 Hz at Godr = 238 Hz, 100 Hz at Godr = 952 Hz 183}; 184 185enum Mscale { // set of allowable mag full scale settings 186 MFS_4G = 0, 187 MFS_8G, 188 MFS_12G, 189 MFS_16G 190}; 191 192enum Mmode { 193 MMode_LowPower = 0, 194 MMode_MedPerformance, 195 MMode_HighPerformance, 196 MMode_UltraHighPerformance 197}; 198 199enum Modr { // set of allowable mag sample rates 200 MODR_0_625Hz = 0, 201 MODR_1_25Hz, 202 MODR_2_5Hz, 203 MODR_5Hz, 204 MODR_10Hz, 205 MODR_20Hz, 206 MODR_80Hz 207}; 208 209#define ADC_256 0x00 // define pressure and temperature conversion rates 210#define ADC_512 0x02 211#define ADC_1024 0x04 212#define ADC_2048 0x06 213#define ADC_4096 0x08 214#define ADC_D1 0x40 215#define ADC_D2 0x50 216 217// Specify sensor full scale 218extern uint8_t OSR; // set pressure amd temperature oversample rate 219extern uint8_t Gscale; // gyro full scale 220extern uint8_t Godr; // gyro data sample rate 221extern uint8_t Gbw; // gyro data bandwidth 222extern uint8_t Ascale; // accel full scale 223extern uint8_t Aodr; // accel data sample rate 224extern uint8_t Abw; // accel data bandwidth 225extern uint8_t Mscale; // mag full scale 226extern uint8_t Modr; // mag data sample rate 227extern uint8_t Mmode; // magnetometer operation mode 228extern float aRes, gRes, mRes; // scale resolutions per LSB for the sensors 229 230// Pin definitions 231extern int myLed; 232 233extern uint16_t Pcal[8]; // calibration constants from MS5611 PROM registers 234extern unsigned char nCRC; // calculated check sum to ensure PROM integrity 235//-(rk) uint32_t D1 = 0, D2 = 0; // raw MS5611 pressure and temperature data 236extern double dT, OFFSET, SENS, T2, OFFSET2, SENS2; // First order and second order corrections for raw S5637 temperature and pressure data 237extern int16_t accelCount[3], gyroCount[3], magCount[3]; // Stores the 16-bit signed accelerometer, gyro, and mag sensor output 238extern float gyroBias[3], accelBias[3], magBias[3]; // Bias corrections for gyro, accelerometer, and magnetometer 239extern int16_t tempCount; // temperature raw count output 240extern float temperature; // Stores the LSM9DS1gyro internal chip temperature in degrees Celsius 241extern double Temperature, Pressure; // stores MS5611 pressures sensor pressure and temperature 242 243// global constants for 9 DoF fusion and AHRS (Attitude and Heading Reference System) 244extern float GyroMeasError; // gyroscope measurement error in rads/s (start at 40 deg/s) 245extern float GyroMeasDrift; // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s) 246// There is a tradeoff in the beta parameter between accuracy and response speed. 247// In the original Madgwick study, beta of 0.041 (corresponding to GyroMeasError of 2.7 degrees/s) was found to give optimal accuracy. 248// However, with this value, the LSM9SD0 response time is about 10 seconds to a stable initial quaternion. 249// Subsequent changes also require a longish lag time to a stable output, not fast enough for a quadcopter or robot car! 250// By increasing beta (GyroMeasError) by about a factor of fifteen, the response time constant is reduced to ~2 sec 251// I haven't noticed any reduction in solution accuracy. This is essentially the I coefficient in a PID control sense; 252// the bigger the feedback coefficient, the faster the solution converges, usually at the expense of accuracy. 253// In any case, this is the free parameter in the Madgwick filtering and fusion scheme. 254extern float beta; // compute beta 255extern float zeta; // compute zeta, the other free parameter in the Madgwick scheme usually set to a small or zero value 256#define Kp 2.0f * 5.0f // these are the free parameters in the Mahony filter and fusion scheme, Kp for proportional feedback, Ki for integral 257#define Ki 0.0f 258 259extern uint32_t delt_t, count, sumCount; // used to control display output rate 260extern float pitch, yaw, roll; 261extern float deltat, sum; // integration interval for both filter schemes 262extern uint32_t lastUpdate, firstUpdate; // used to calculate integration interval 263extern uint32_t Now; // used to calculate integration interval 264 265extern float ax, ay, az, gx, gy, gz, mx, my, mz; // variables to hold latest sensor data values 266extern float q[4]; // vector to hold quaternion 267extern float eInt[3]; // vector to hold integral error for Mahony method 268 269//-(rk) Prototypes of AHRS.cpp 270void getMres(); 271void getGres(); 272void getAres(); 273void readAccelData(int16_t * destination); 274void readGyroData(int16_t * destination); 275void readMagData(int16_t * destination); 276int16_t readTempData(); 277void initLSM9DS1(); 278void selftestLSM9DS1(); 279void accelgyrocalLSM9DS1(float * dest1, float * dest2); 280void magcalLSM9DS1(float * dest1); 281void writeByte(uint8_t address, uint8_t subAddress, uint8_t data); 282uint8_t readByte(uint8_t address, uint8_t subAddress); 283void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest); 284void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz); 285 286boolean AHRS_setup(); 287void AHRS_update(); 288
DataLogger program files
Files which make up the DataLogger application: DataLogger.ino: main Arduino Nano 33 BLE Sense program AHRS.h: header file for AHRS/Madgwick library AHRS.cpp: code file for AHRS/Madgwick library AHRS_gvars.h: header file with global variables for AHRS/Madgwick library Ublox_NMEA_GPS.h: header file for Ublox NMEA GPS Class Ublox_NMEA_GPS.cpp: code file for Ublox NMEA GPS Class
AHRS.cpp
c_cpp
AHRS / Madgwick library
1/* LSM9DS1_MS5611_t3 Basic Example Code 2by: Kris Winer 3date: November 1, 2014 4license: Beerware - Use this code however you'd like. If you 5find it useful you can buy me a beer some time. 6Demonstrate basic LSM9DS1 functionality including parameterizing the register addresses, initializing the sensor, 7getting properly scaled accelerometer, gyroscope, and magnetometer data out. Added display functions to 8allow display to on breadboard monitor. Addition of 9 DoF sensor fusion using open source Madgwick and 9Mahony filter algorithms. Sketch runs on the 3.3 V 8 MHz Pro Mini and the Teensy 3.1. 10This sketch is intended specifically for the LSM9DS1+MS5611 Add-on shield for the Teensy 3.1. 11It uses SDA/SCL on pins 17/16, respectively, and it uses the Teensy 3.1-specific Wire library i2c_t3.h. 12The MS5611 is a simple but high resolution pressure sensor, which can be used in its high resolution 13mode but with power consumption od 20 microAmp, or in a lower resolution mode with power consumption of 14only 1 microAmp. The choice will depend on the application. 15SDA and SCL should have external pull-up resistors (to 3.3V). 164K7 resistors are on the LSM9DS1+MS5611 Teensy 3.1 add-on shield/breakout board. 17Hardware setup: 18LSM9DS1Breakout --------- Arduino 19VDD ---------------------- 3.3V 20VDDI --------------------- 3.3V 21SDA ----------------------- A4 22SCL ----------------------- A5 23GND ---------------------- GND 24Note: The LSM9DS1 is an I2C sensor and can use the Arduino Wire library. 25Because the sensor is not 5V tolerant, we are using either a 3.3 V 8 MHz Pro Mini or a 3.3 V Teensy 3.1. 26We have disabled the internal pull-ups used by the Wire library in the Wire.h/twi.c utility file. 27We are also using the 400 kHz fast I2C mode by setting the TWI_FREQ to 400000L /twi.h utility file. 28 29Modified by Ruud Kapteijn 3016-Jan-2021 31Moulded into class AHRS 32taken out NOKIA 5110 monochrome display code 33code: https://github.com/j-mcc1993/LSM9DS1/blob/master/LSM9DS1_BasicAHRS_Nano33.ino 34discussion threa: https://github.com/kriswiner/LSM9DS1/issues/14 35 */ 36#include <Arduino.h> 37#include <Arduino_LSM9DS1.h> 38#include <Arduino_LPS22HB.h> //Include library to read Pressure 39#include <Wire.h> 40#include <SPI.h> 41#include "AHRS.h" 42 43boolean AHRS_setup() { 44 Wire1.begin(); 45 // TWBR = 12; // 400 kbit/sec I2C speed for Pro Mini 46 // Setup for Master mode, pins 16/17, external pullups, 400kHz for Teensy 3.1 47 //Wire.begin(I2C_MASTER, 0x00, I2C_PINS_16_17, I2C_PULLUP_EXT, I2C_RATE_400); 48 // reset 49 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG8, 0x05); 50 writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG2_M, 0x0c); 51 delay(100); 52 53 // Initialize LED pin 54 pinMode(myLed, OUTPUT); 55 digitalWrite(myLed, HIGH); 56 57 // Read the WHO_AM_I registers, this is a good test of communication 58 Serial.println("LSM9DS1 9-axis motion sensor..."); 59 byte c = readByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_WHO_AM_I); // Read WHO_AM_I register for LSM9DS1 accel/gyro 60 Serial.print("LSM9DS1 accel/gyro"); Serial.print("I AM "); Serial.print(c, HEX); Serial.print(" I should be "); Serial.println(0x68, HEX); 61 byte d = readByte(LSM9DS1M_ADDRESS, LSM9DS1M_WHO_AM_I); // Read WHO_AM_I register for LSM9DS1 magnetometer 62 Serial.print("LSM9DS1 magnetometer"); Serial.print("I AM "); Serial.print(d, HEX); Serial.print(" I should be "); Serial.println(0x3D, HEX); 63 64 if (c == 0x68 && d == 0x3D) // WHO_AM_I should always be 0x0E for the accel/gyro and 0x3C for the mag 65 { 66 Serial.println("LSM9DS1 is online..."); 67 68 // get sensor resolutions, only need to do this once 69 getAres(); 70 getGres(); 71 getMres(); 72 Serial.print("accel sensitivity is "); Serial.print(1./(1000.*aRes)); Serial.println(" LSB/mg"); 73 Serial.print("gyro sensitivity is "); Serial.print(1./(1000.*gRes)); Serial.println(" LSB/mdps"); 74 Serial.print("mag sensitivity is "); Serial.print(1./(1000.*mRes)); Serial.println(" LSB/mGauss"); 75 76 Serial.println("Perform gyro and accel self test"); 77 selftestLSM9DS1(); // check function of gyro and accelerometer via self test 78 79 Serial.println(" Calibrate gyro and accel"); 80 accelgyrocalLSM9DS1(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers 81 Serial.println("accel biases (mg)"); Serial.println(1000.*accelBias[0]); Serial.println(1000.*accelBias[1]); Serial.println(1000.*accelBias[2]); 82 Serial.println("gyro biases (dps)"); Serial.println(gyroBias[0]); Serial.println(gyroBias[1]); Serial.println(gyroBias[2]); 83 84 magcalLSM9DS1(magBias); 85 Serial.println("mag biases (mG)"); Serial.println(1000.*magBias[0]); Serial.println(1000.*magBias[1]); Serial.println(1000.*magBias[2]); 86 delay(2000); // add delay to see results before serial spew of data 87 88 initLSM9DS1(); 89 Serial.println("LSM9DS1 initialized for active data mode...."); // Initialize device for active mode read of acclerometer, gyroscope, and temperature 90 return true; 91 } 92 else 93 { 94 Serial.print("Could not connect to LSM9DS1: 0x"); 95 Serial.println(c, HEX); 96 return false; 97 } 98} 99 100void AHRS_update() { 101 int DEBUG = 0; 102 103 if (readByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_STATUS_REG) & 0x01) { // check if new accel data is ready 104 readAccelData(accelCount); // Read the x/y/z adc values 105 106 // Now we'll calculate the accleration value into actual g's 107 ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set 108 ay = (float)accelCount[1]*aRes - accelBias[1]; 109 az = (float)accelCount[2]*aRes - accelBias[2]; 110 } 111 112 if (readByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_STATUS_REG) & 0x02) { // check if new gyro data is ready 113 readGyroData(gyroCount); // Read the x/y/z adc values 114 115 // Calculate the gyro value into actual degrees per second 116 gx = (float)gyroCount[0]*gRes - gyroBias[0]; // get actual gyro value, this depends on scale being set 117 gy = (float)gyroCount[1]*gRes - gyroBias[1]; 118 gz = (float)gyroCount[2]*gRes - gyroBias[2]; 119 } 120 121 if (readByte(LSM9DS1M_ADDRESS, LSM9DS1M_STATUS_REG_M) & 0x08) { // check if new mag data is ready 122 readMagData(magCount); // Read the x/y/z adc values 123 124 // Calculate the magnetometer values in milliGauss 125 // Include factory calibration per data sheet and user environmental corrections 126 mx = (float)magCount[0]*mRes; // - magBias[0]; // get actual magnetometer value, this depends on scale being set 127 my = (float)magCount[1]*mRes; // - magBias[1]; 128 mz = (float)magCount[2]*mRes; // - magBias[2]; 129 } 130 131 Now = micros(); 132 deltat = ((Now - lastUpdate)/1000000.0f); // set integration time by time elapsed since last filter update 133 lastUpdate = Now; 134 135 sum += deltat; // sum for averaging filter update rate 136 sumCount++; 137 138 // Sensors x, y, and z axes of the accelerometer and gyro are aligned. The magnetometer 139 // the magnetometer z-axis (+ up) is aligned with the z-axis (+ up) of accelerometer and gyro, but the magnetometer 140 // x-axis is aligned with the -x axis of the gyro and the magnetometer y axis is aligned with the y axis of the gyro! 141 // We have to make some allowance for this orientation mismatch in feeding the output to the quaternion filter. 142 // For the LSM9DS1, we have chosen a magnetic rotation that keeps the sensor forward along the x-axis just like 143 // in the LSM9DS0 sensor. This rotation can be modified to allow any convenient orientation convention. 144 // This is ok by aircraft orientation standards! 145 // Pass gyro rate as rad/s 146 MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, -mx, -my, mz); 147 // MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, -mx, my, mz); 148 149 // Serial print and/or display at 0.5 s rate independent of data rates 150 delt_t = millis() - count; 151 if (delt_t > 500) { // update LCD once per half-second independent of read rate 152 153 if(SerialDebug && DEBUG > 0) { 154 Serial.print("lib -> ax = "); Serial.print((int)1000*ax); 155 Serial.print(" ay = "); Serial.print((int)1000*ay); 156 Serial.print(" az = "); Serial.print((int)1000*az); Serial.println(" mg"); 157 Serial.print("gx = "); Serial.print( gx, 2); 158 Serial.print(" gy = "); Serial.print( gy, 2); 159 Serial.print(" gz = "); Serial.print( gz, 2); Serial.println(" deg/s"); 160 Serial.print("mx = "); Serial.print( (int)1000*mx ); 161 Serial.print(" my = "); Serial.print( (int)1000*my ); 162 Serial.print(" mz = "); Serial.print( (int)1000*mz ); Serial.println(" mG"); 163 164 Serial.print("q0 = "); Serial.print(q[0]); 165 Serial.print(" qx = "); Serial.print(q[1]); 166 Serial.print(" qy = "); Serial.print(q[2]); 167 Serial.print(" qz = "); Serial.println(q[3]); 168 } 169 tempCount = readTempData(); // Read the gyro adc values 170 temperature = ((float) tempCount/256. + 25.0); // Gyro chip temperature in degrees Centigrade 171 // Print temperature in degrees Centigrade 172 if (SerialDebug && DEBUG > 0) { 173 Serial.print("Gyro temperature is "); Serial.print(temperature, 1); Serial.println(" degrees C"); // Print T values to tenths of s degree C 174 } 175 176 // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation. 177 // In this coordinate system, the positive z-axis is down toward Earth. 178 // Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination, looking down on the sensor positive yaw is counterclockwise. 179 // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative. 180 // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll. 181 // These arise from the definition of the homogeneous rotation matrix constructed from quaternions. 182 // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be 183 // applied in the correct order which for this configuration is yaw, pitch, and then roll. 184 // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links. 185 yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]); 186 pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2])); 187 roll = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]); 188 pitch *= 180.0f / PI; 189 yaw *= 180.0f / PI; 190 yaw -= 1.0f; // Declination at Amsterdam in 2021 191 roll *= 180.0f / PI; 192 193 if(SerialDebug && DEBUG > 0) { 194 Serial.print("lib -> Yaw, Pitch, Roll: "); 195 Serial.print(yaw, 2); 196 Serial.print(", "); 197 Serial.print(pitch, 2); 198 Serial.print(", "); 199 Serial.println(roll, 2); 200 // Serial.print("rate = "); Serial.print((float)sumCount/sum, 2); Serial.println(" Hz\ 201"); 202 } 203 // With these settings the filter is updating at a ~145 Hz rate using the Madgwick scheme and 204 // >200 Hz using the Mahony scheme even though the display refreshes at only 2 Hz. 205 // The filter update rate is determined mostly by the mathematical steps in the respective algorithms, 206 // the processor speed (8 MHz for the 3.3V Pro Mini), and the magnetometer ODR: 207 // an ODR of 10 Hz for the magnetometer produce the above rates, maximum magnetometer ODR of 100 Hz produces 208 // filter update rates of 36 - 145 and ~38 Hz for the Madgwick and Mahony schemes, respectively. 209 // This is presumably because the magnetometer read takes longer than the gyro or accelerometer reads. 210 // This filter update rate should be fast enough to maintain accurate platform orientation for 211 // stabilization control of a fast-moving robot or quadcopter. Compare to the update rate of 200 Hz 212 // produced by the on-board Digital Motion Processor of Invensense's MPU6050 6 DoF and MPU9150 9DoF sensors. 213 // The 3.3 V 8 MHz Pro Mini is doing pretty well! 214 215 digitalWrite(myLed, !digitalRead(myLed)); 216 count = millis(); 217 sumCount = 0; 218 sum = 0; 219 } 220 } 221 222//=================================================================================================================== 223//====== Set of useful function to access acceleration. gyroscope, magnetometer, and temperature data 224//=================================================================================================================== 225 226void getMres() { 227 switch (Mscale) 228 { 229 // Possible magnetometer scales (and their register bit settings) are: 230 // 4 Gauss (00), 8 Gauss (01), 12 Gauss (10) and 16 Gauss (11) 231 case MFS_4G: 232 mRes = 4.0/32768.0; 233 break; 234 case MFS_8G: 235 mRes = 8.0/32768.0; 236 break; 237 case MFS_12G: 238 mRes = 12.0/32768.0; 239 break; 240 case MFS_16G: 241 mRes = 16.0/32768.0; 242 break; 243 } 244} 245 246void getGres() { 247 switch (Gscale) 248 { 249 // Possible gyro scales (and their register bit settings) are: 250 // 245 DPS (00), 500 DPS (01), and 2000 DPS (11). 251 case GFS_245DPS: 252 gRes = 245.0/32768.0; 253 break; 254 case GFS_500DPS: 255 gRes = 500.0/32768.0; 256 break; 257 case GFS_2000DPS: 258 gRes = 2000.0/32768.0; 259 break; 260 } 261} 262 263void getAres() { 264 switch (Ascale) 265 { 266 // Possible accelerometer scales (and their register bit settings) are: 267 // 2 Gs (00), 16 Gs (01), 4 Gs (10), and 8 Gs (11). 268 case AFS_2G: 269 aRes = 2.0/32768.0; 270 break; 271 case AFS_16G: 272 aRes = 16.0/32768.0; 273 break; 274 case AFS_4G: 275 aRes = 4.0/32768.0; 276 break; 277 case AFS_8G: 278 aRes = 8.0/32768.0; 279 break; 280 } 281} 282 283 284void readAccelData(int16_t * destination) 285{ 286 uint8_t rawData[6]; // x/y/z accel register data stored here 287 readBytes(LSM9DS1XG_ADDRESS, LSM9DS1XG_OUT_X_L_XL, 6, &rawData[0]); // Read the six raw data registers into data array 288 destination[0] = ((int16_t)rawData[1] << 8) | rawData[0] ; // Turn the MSB and LSB into a signed 16-bit value 289 destination[1] = ((int16_t)rawData[3] << 8) | rawData[2] ; 290 destination[2] = ((int16_t)rawData[5] << 8) | rawData[4] ; 291} 292 293 294void readGyroData(int16_t * destination) 295{ 296 uint8_t rawData[6]; // x/y/z gyro register data stored here 297 readBytes(LSM9DS1XG_ADDRESS, LSM9DS1XG_OUT_X_L_G, 6, &rawData[0]); // Read the six raw data registers sequentially into data array 298 destination[0] = ((int16_t)rawData[1] << 8) | rawData[0] ; // Turn the MSB and LSB into a signed 16-bit value 299 destination[1] = ((int16_t)rawData[3] << 8) | rawData[2] ; 300 destination[2] = ((int16_t)rawData[5] << 8) | rawData[4] ; 301} 302 303void readMagData(int16_t * destination) 304{ 305 uint8_t rawData[6]; // x/y/z gyro register data stored here 306 readBytes(LSM9DS1M_ADDRESS, LSM9DS1M_OUT_X_L_M, 6, &rawData[0]); // Read the six raw data registers sequentially into data array 307 destination[0] = ((int16_t)rawData[1] << 8) | rawData[0] ; // Turn the MSB and LSB into a signed 16-bit value 308 destination[1] = ((int16_t)rawData[3] << 8) | rawData[2] ; // Data stored as little Endian 309 destination[2] = ((int16_t)rawData[5] << 8) | rawData[4] ; 310} 311 312int16_t readTempData() 313{ 314 uint8_t rawData[2]; // x/y/z gyro register data stored here 315 readBytes(LSM9DS1XG_ADDRESS, LSM9DS1XG_OUT_TEMP_L, 2, &rawData[0]); // Read the two raw data registers sequentially into data array 316 return (((int16_t)rawData[1] << 8) | rawData[0]); // Turn the MSB and LSB into a 16-bit signed value 317} 318 319 320void initLSM9DS1() 321{ 322 // enable the 3-axes of the gyroscope 323 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG4, 0x38); 324 // configure the gyroscope 325 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG1_G, Godr << 5 | Gscale << 3 | Gbw); 326 delay(200); 327 // enable the three axes of the accelerometer 328 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG5_XL, 0x38); 329 // configure the accelerometer-specify bandwidth selection with Abw 330 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG6_XL, Aodr << 5 | Ascale << 3 | 0x04 |Abw); 331 delay(200); 332 // enable block data update, allow auto-increment during multiple byte read 333 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG8, 0x44); 334 // configure the magnetometer-enable temperature compensation of mag data 335 writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG1_M, 0x80 | Mmode << 5 | Modr << 2); // select x,y-axis mode 336 writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG2_M, Mscale << 5 ); // select mag full scale 337 writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG3_M, 0x00 ); // continuous conversion mode 338 writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG4_M, Mmode << 2 ); // select z-axis mode 339 writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG5_M, 0x40 ); // select block update mode 340} 341 342 343void selftestLSM9DS1() 344{ 345 float accel_noST[3] = {0., 0., 0.}, accel_ST[3] = {0., 0., 0.}; 346 float gyro_noST[3] = {0., 0., 0.}, gyro_ST[3] = {0., 0., 0.}; 347 348 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG10, 0x00); // disable self test 349 accelgyrocalLSM9DS1(gyro_noST, accel_noST); 350 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG10, 0x05); // enable gyro/accel self test 351 accelgyrocalLSM9DS1(gyro_ST, accel_ST); 352 353 float gyrodx = (gyro_ST[0] - gyro_noST[0]); 354 float gyrody = (gyro_ST[1] - gyro_noST[1]); 355 float gyrodz = (gyro_ST[2] - gyro_noST[2]); 356 357 Serial.println("Gyro self-test results: "); 358 Serial.print("x-axis = "); Serial.print(gyrodx); Serial.print(" dps"); Serial.println(" should be between 20 and 250 dps"); 359 Serial.print("y-axis = "); Serial.print(gyrody); Serial.print(" dps"); Serial.println(" should be between 20 and 250 dps"); 360 Serial.print("z-axis = "); Serial.print(gyrodz); Serial.print(" dps"); Serial.println(" should be between 20 and 250 dps"); 361 362 float accdx = 1000.*(accel_ST[0] - accel_noST[0]); 363 float accdy = 1000.*(accel_ST[1] - accel_noST[1]); 364 float accdz = 1000.*(accel_ST[2] - accel_noST[2]); 365 366 Serial.println("Accelerometer self-test results: "); 367 Serial.print("x-axis = "); Serial.print(accdx); Serial.print(" mg"); Serial.println(" should be between 60 and 1700 mg"); 368 Serial.print("y-axis = "); Serial.print(accdy); Serial.print(" mg"); Serial.println(" should be between 60 and 1700 mg"); 369 Serial.print("z-axis = "); Serial.print(accdz); Serial.print(" mg"); Serial.println(" should be between 60 and 1700 mg"); 370 371 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG10, 0x00); // disable self test 372 delay(200); 373} 374// Function which accumulates gyro and accelerometer data after device initialization. It calculates the average 375// of the at-rest readings and then loads the resulting offsets into accelerometer and gyro bias registers. 376void accelgyrocalLSM9DS1(float * dest1, float * dest2) 377{ 378 uint8_t data[6] = {0, 0, 0, 0, 0, 0}; 379 int32_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0}; 380 uint16_t samples, ii; 381 382 // enable the 3-axes of the gyroscope 383 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG4, 0x38); 384 // configure the gyroscope 385 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG1_G, Godr << 5 | Gscale << 3 | Gbw); 386 delay(200); 387 // enable the three axes of the accelerometer 388 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG5_XL, 0x38); 389 // configure the accelerometer-specify bandwidth selection with Abw 390 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG6_XL, Aodr << 5 | Ascale << 3 | 0x04 |Abw); 391 delay(200); 392 // enable block data update, allow auto-increment during multiple byte read 393 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG8, 0x44); 394 395 // First get gyro bias 396 byte c = readByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG9); 397 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG9, c | 0x02); // Enable gyro FIFO 398 delay(50); // Wait for change to take effect 399 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_FIFO_CTRL, 0x20 | 0x1F); // Enable gyro FIFO stream mode and set watermark at 32 samples 400 delay(1000); // delay 1000 milliseconds to collect FIFO samples 401 402 samples = (readByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_FIFO_SRC) & 0x2F); // Read number of stored samples 403 404 for(ii = 0; ii < samples ; ii++) { // Read the gyro data stored in the FIFO 405 int16_t gyro_temp[3] = {0, 0, 0}; 406 readBytes(LSM9DS1XG_ADDRESS, LSM9DS1XG_OUT_X_L_G, 6, &data[0]); 407 gyro_temp[0] = (int16_t) (((int16_t)data[1] << 8) | data[0]); // Form signed 16-bit integer for each sample in FIFO 408 gyro_temp[1] = (int16_t) (((int16_t)data[3] << 8) | data[2]); 409 gyro_temp[2] = (int16_t) (((int16_t)data[5] << 8) | data[4]); 410 411 gyro_bias[0] += (int32_t) gyro_temp[0]; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases 412 gyro_bias[1] += (int32_t) gyro_temp[1]; 413 gyro_bias[2] += (int32_t) gyro_temp[2]; 414 } 415 416 gyro_bias[0] /= samples; // average the data 417 gyro_bias[1] /= samples; 418 gyro_bias[2] /= samples; 419 420 dest1[0] = (float)gyro_bias[0]*gRes; // Properly scale the data to get deg/s 421 dest1[1] = (float)gyro_bias[1]*gRes; 422 dest1[2] = (float)gyro_bias[2]*gRes; 423 424 c = readByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG9); 425 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG9, c & ~0x02); //Disable gyro FIFO 426 delay(50); 427 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_FIFO_CTRL, 0x00); // Enable gyro bypass mode 428 429 // now get the accelerometer bias 430 c = readByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG9); 431 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG9, c | 0x02); // Enable accel FIFO 432 delay(50); // Wait for change to take effect 433 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_FIFO_CTRL, 0x20 | 0x1F); // Enable accel FIFO stream mode and set watermark at 32 samples 434 delay(1000); // delay 1000 milliseconds to collect FIFO samples 435 436 samples = (readByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_FIFO_SRC) & 0x2F); // Read number of stored samples 437 438 for(ii = 0; ii < samples ; ii++) { // Read the accel data stored in the FIFO 439 int16_t accel_temp[3] = {0, 0, 0}; 440 readBytes(LSM9DS1XG_ADDRESS, LSM9DS1XG_OUT_X_L_XL, 6, &data[0]); 441 accel_temp[0] = (int16_t) (((int16_t)data[1] << 8) | data[0]); // Form signed 16-bit integer for each sample in FIFO 442 accel_temp[1] = (int16_t) (((int16_t)data[3] << 8) | data[2]); 443 accel_temp[2] = (int16_t) (((int16_t)data[5] << 8) | data[4]); 444 445 accel_bias[0] += (int32_t) accel_temp[0]; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases 446 accel_bias[1] += (int32_t) accel_temp[1]; 447 accel_bias[2] += (int32_t) accel_temp[2]; 448 } 449 450 accel_bias[0] /= samples; // average the data 451 accel_bias[1] /= samples; 452 accel_bias[2] /= samples; 453 454 if(accel_bias[2] > 0L) {accel_bias[2] -= (int32_t) (1.0/aRes);} // Remove gravity from the z-axis accelerometer bias calculation 455 else {accel_bias[2] += (int32_t) (1.0/aRes);} 456 457 dest2[0] = (float)accel_bias[0]*aRes; // Properly scale the data to get g 458 dest2[1] = (float)accel_bias[1]*aRes; 459 dest2[2] = (float)accel_bias[2]*aRes; 460 461 c = readByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG9); 462 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG9, c & ~0x02); //Disable accel FIFO 463 delay(50); 464 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_FIFO_CTRL, 0x00); // Enable accel bypass mode 465} 466 467void magcalLSM9DS1(float * dest1) 468{ 469 uint8_t data[6]; // data array to hold mag x, y, z, data 470 uint16_t ii = 0, sample_count = 0; 471 int32_t mag_bias[3] = {0, 0, 0}; 472 int16_t mag_max[3] = {0, 0, 0}, mag_min[3] = {0, 0, 0}; 473 474 // configure the magnetometer-enable temperature compensation of mag data 475 writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG1_M, 0x80 | Mmode << 5 | Modr << 2); // select x,y-axis mode 476 writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG2_M, Mscale << 5 ); // select mag full scale 477 writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG3_M, 0x00 ); // continuous conversion mode 478 writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG4_M, Mmode << 2 ); // select z-axis mode 479 writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG5_M, 0x40 ); // select block update mode 480 481 Serial.println("Mag Calibration: Wave device in a figure eight until done!"); 482 delay(4000); 483 484 sample_count = 128; 485 for(ii = 0; ii < sample_count; ii++) { 486 int16_t mag_temp[3] = {0, 0, 0}; 487 readBytes(LSM9DS1M_ADDRESS, LSM9DS1M_OUT_X_L_M, 6, &data[0]); // Read the six raw data registers into data array 488 mag_temp[0] = (int16_t) (((int16_t)data[1] << 8) | data[0]) ; // Form signed 16-bit integer for each sample in FIFO 489 mag_temp[1] = (int16_t) (((int16_t)data[3] << 8) | data[2]) ; 490 mag_temp[2] = (int16_t) (((int16_t)data[5] << 8) | data[4]) ; 491 for (int jj = 0; jj < 3; jj++) { 492 if(mag_temp[jj] > mag_max[jj]) mag_max[jj] = mag_temp[jj]; 493 if(mag_temp[jj] < mag_min[jj]) mag_min[jj] = mag_temp[jj]; 494 } 495 delay(105); // at 10 Hz ODR, new mag data is available every 100 ms 496 } 497 498 // Serial.println("mag x min/max:"); Serial.println(mag_max[0]); Serial.println(mag_min[0]); 499 // Serial.println("mag y min/max:"); Serial.println(mag_max[1]); Serial.println(mag_min[1]); 500 // Serial.println("mag z min/max:"); Serial.println(mag_max[2]); Serial.println(mag_min[2]); 501 502 mag_bias[0] = (mag_max[0] + mag_min[0])/2; // get average x mag bias in counts 503 mag_bias[1] = (mag_max[1] + mag_min[1])/2; // get average y mag bias in counts 504 mag_bias[2] = (mag_max[2] + mag_min[2])/2; // get average z mag bias in counts 505 506 dest1[0] = (float) mag_bias[0]*mRes; // save mag biases in G for main program 507 dest1[1] = (float) mag_bias[1]*mRes; 508 dest1[2] = (float) mag_bias[2]*mRes; 509 510 //write biases to accelerometermagnetometer offset registers as counts); 511 writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_OFFSET_X_REG_L_M, (int16_t) mag_bias[0] & 0xFF); 512 writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_OFFSET_X_REG_H_M, ((int16_t)mag_bias[0] >> 8) & 0xFF); 513 writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_OFFSET_Y_REG_L_M, (int16_t) mag_bias[1] & 0xFF); 514 writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_OFFSET_Y_REG_H_M, ((int16_t)mag_bias[1] >> 8) & 0xFF); 515 writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_OFFSET_Z_REG_L_M, (int16_t) mag_bias[2] & 0xFF); 516 writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_OFFSET_Z_REG_H_M, ((int16_t)mag_bias[2] >> 8) & 0xFF); 517 518 Serial.println("Mag Calibration done!"); 519} 520 521// I2C read/write functions for the LSM9DS1and AK8963 sensors 522 523void writeByte(uint8_t address, uint8_t subAddress, uint8_t data) 524{ 525 Wire1.beginTransmission(address); // Initialize the Tx buffer 526 Wire1.write(subAddress); // Put slave register address in Tx buffer 527 Wire1.write(data); // Put data in Tx buffer 528 Wire1.endTransmission(); // Send the Tx buffer 529} 530 531uint8_t readByte(uint8_t address, uint8_t subAddress) 532{ 533 uint8_t data; // `data` will store the register data 534 Wire1.beginTransmission(address); // Initialize the Tx buffer 535 Wire1.write(subAddress); // Put slave register address in Tx buffer 536 // Wire.endTransmission(I2C_NOSTOP); // Send the Tx buffer, but send a restart to keep connection alive 537 Wire1.endTransmission(false); // Send the Tx buffer, but send a restart to keep connection alive 538 // Wire.requestFrom(address, 1); // Read one byte from slave register address 539 Wire1.requestFrom(address, (size_t) 1); // Read one byte from slave register address 540 data = Wire1.read(); // Fill Rx buffer with result 541 return data; // Return data read from slave register 542} 543 544void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest) 545{ 546 Wire1.beginTransmission(address); // Initialize the Tx buffer 547 Wire1.write(subAddress); // Put slave register address in Tx buffer 548 // Wire.endTransmission(I2C_NOSTOP); // Send the Tx buffer, but send a restart to keep connection alive 549 Wire1.endTransmission(false); // Send the Tx buffer, but send a restart to keep connection alive 550 uint8_t i = 0; 551 Wire1.requestFrom(address, count); // Read bytes from slave register address 552 // Wire.requestFrom(address, (size_t) count); // Read bytes from slave register address 553 while (Wire1.available()) { 554 dest[i++] = Wire1.read(); } // Put read results in the Rx buffer 555} 556 557void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz) 558{ 559 float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3]; // short name local variable for readability 560 float norm; 561 float hx, hy, _2bx, _2bz; 562 float s1, s2, s3, s4; 563 float qDot1, qDot2, qDot3, qDot4; 564 565 // Auxiliary variables to avoid repeated arithmetic 566 float _2q1mx; 567 float _2q1my; 568 float _2q1mz; 569 float _2q2mx; 570 float _4bx; 571 float _4bz; 572 float _2q1 = 2.0f * q1; 573 float _2q2 = 2.0f * q2; 574 float _2q3 = 2.0f * q3; 575 float _2q4 = 2.0f * q4; 576 float _2q1q3 = 2.0f * q1 * q3; 577 float _2q3q4 = 2.0f * q3 * q4; 578 float q1q1 = q1 * q1; 579 float q1q2 = q1 * q2; 580 float q1q3 = q1 * q3; 581 float q1q4 = q1 * q4; 582 float q2q2 = q2 * q2; 583 float q2q3 = q2 * q3; 584 float q2q4 = q2 * q4; 585 float q3q3 = q3 * q3; 586 float q3q4 = q3 * q4; 587 float q4q4 = q4 * q4; 588 589 // Normalise accelerometer measurement 590 norm = sqrt(ax * ax + ay * ay + az * az); 591 if (norm == 0.0f) return; // handle NaN 592 norm = 1.0f/norm; 593 ax *= norm; 594 ay *= norm; 595 az *= norm; 596 597 // Normalise magnetometer measurement 598 norm = sqrt(mx * mx + my * my + mz * mz); 599 if (norm == 0.0f) return; // handle NaN 600 norm = 1.0f/norm; 601 mx *= norm; 602 my *= norm; 603 mz *= norm; 604 605 // Reference direction of Earth's magnetic field 606 _2q1mx = 2.0f * q1 * mx; 607 _2q1my = 2.0f * q1 * my; 608 _2q1mz = 2.0f * q1 * mz; 609 _2q2mx = 2.0f * q2 * mx; 610 hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4; 611 hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4; 612 _2bx = sqrt(hx * hx + hy * hy); 613 _2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4; 614 _4bx = 2.0f * _2bx; 615 _4bz = 2.0f * _2bz; 616 617 // Gradient decent algorithm corrective step 618 s1 = -_2q3 * (2.0f * q2q4 - _2q1q3 - ax) + _2q2 * (2.0f * q1q2 + _2q3q4 - ay) - _2bz * q3 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q4 + _2bz * q2) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz); 619 s2 = _2q4 * (2.0f * q2q4 - _2q1q3 - ax) + _2q1 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q2 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + _2bz * q4 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q4 - _4bz * q2) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz); 620 s3 = -_2q1 * (2.0f * q2q4 - _2q1q3 - ax) + _2q4 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q3 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz); 621 s4 = _2q2 * (2.0f * q2q4 - _2q1q3 - ax) + _2q3 * (2.0f * q1q2 + _2q3q4 - ay) + (-_4bx * q4 + _2bz * q2) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q1 + _2bz * q3) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz); 622 norm = sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4); // normalise step magnitude 623 norm = 1.0f/norm; 624 s1 *= norm; 625 s2 *= norm; 626 s3 *= norm; 627 s4 *= norm; 628 629 // Compute rate of change of quaternion 630 qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - beta * s1; 631 qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - beta * s2; 632 qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - beta * s3; 633 qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - beta * s4; 634 635 // Integrate to yield quaternion 636 q1 += qDot1 * deltat; 637 q2 += qDot2 * deltat; 638 q3 += qDot3 * deltat; 639 q4 += qDot4 * deltat; 640 norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); // normalise quaternion 641 norm = 1.0f/norm; 642 q[0] = q1 * norm; 643 q[1] = q2 * norm; 644 q[2] = q3 * norm; 645 q[3] = q4 * norm; 646 647} 648
AHRS_gvars.h
c_cpp
1uint8_t OSR = ADC_4096; // set pressure amd temperature oversample 2 rate 3uint8_t Gscale = GFS_245DPS; // gyro full scale 4uint8_t Godr = GODR_238Hz; 5 // gyro data sample rate 6uint8_t Gbw = GBW_med; // gyro data bandwidth 7uint8_t 8 Ascale = AFS_2G; // accel full scale 9uint8_t Aodr = AODR_238Hz; // accel 10 data sample rate 11uint8_t Abw = ABW_50Hz; // accel data bandwidth 12uint8_t 13 Mscale = MFS_4G; // mag full scale 14uint8_t Modr = MODR_10Hz; // mag data 15 sample rate 16uint8_t Mmode = MMode_HighPerformance; // magnetometer operation 17 mode 18float aRes, gRes, mRes; // scale resolutions per LSB for the sensors 19int 20 myLed = 13; 21uint16_t Pcal[8]; // calibration constants from MS5611 PROM 22 registers 23unsigned char nCRC; // calculated check sum to ensure PROM integrity 24//-(rk) 25 uint32_t D1 = 0, D2 = 0; // raw MS5611 pressure and temperature data 26double 27 dT, OFFSET, SENS, T2, OFFSET2, SENS2; // First order and second order corrections 28 for raw S5637 temperature and pressure data 29int16_t accelCount[3], gyroCount[3], 30 magCount[3]; // Stores the 16-bit signed accelerometer, gyro, and mag sensor output 31float 32 gyroBias[3] = {0, 0, 0}, accelBias[3] = {0, 0, 0}, magBias[3] = {0, 0, 0}; // Bias 33 corrections for gyro, accelerometer, and magnetometer 34int16_t tempCount; // 35 temperature raw count output 36float temperature; // Stores the LSM9DS1gyro 37 internal chip temperature in degrees Celsius 38double Temperature, Pressure; // 39 stores MS5611 pressures sensor pressure and temperature 40 41// global constants 42 for 9 DoF fusion and AHRS (Attitude and Heading Reference System) 43float GyroMeasError 44 = PI * (40.0f / 180.0f); // gyroscope measurement error in rads/s (start at 40 45 deg/s) 46float GyroMeasDrift = PI * (0.0f / 180.0f); // gyroscope measurement 47 drift in rad/s/s (start at 0.0 deg/s/s) 48// There is a tradeoff in the beta parameter 49 between accuracy and response speed. 50// In the original Madgwick study, beta 51 of 0.041 (corresponding to GyroMeasError of 2.7 degrees/s) was found to give optimal 52 accuracy. 53// However, with this value, the LSM9SD0 response time is about 10 54 seconds to a stable initial quaternion. 55// Subsequent changes also require a 56 longish lag time to a stable output, not fast enough for a quadcopter or robot car! 57// 58 By increasing beta (GyroMeasError) by about a factor of fifteen, the response time 59 constant is reduced to ~2 sec 60// I haven't noticed any reduction in solution 61 accuracy. This is essentially the I coefficient in a PID control sense; 62// the 63 bigger the feedback coefficient, the faster the solution converges, usually at the 64 expense of accuracy. 65// In any case, this is the free parameter in the Madgwick 66 filtering and fusion scheme. 67float beta = sqrt(3.0f / 4.0f) * GyroMeasError; 68 // compute beta 69float zeta = sqrt(3.0f / 4.0f) * GyroMeasDrift; // compute 70 zeta, the other free parameter in the Madgwick scheme usually set to a small or 71 zero value 72#define Kp 2.0f * 5.0f // these are the free parameters in the Mahony 73 filter and fusion scheme, Kp for proportional feedback, Ki for integral 74#define 75 Ki 0.0f 76 77uint32_t delt_t = 0, count = 0, sumCount = 0; // used to control 78 display output rate 79float pitch, yaw, roll; 80float deltat = 0.0f, sum = 0.0f; 81 // integration interval for both filter schemes 82uint32_t lastUpdate 83 = 0, firstUpdate = 0; // used to calculate integration interval 84uint32_t Now 85 = 0; // used to calculate integration interval 86 87float 88 ax, ay, az, gx, gy, gz, mx, my, mz; // variables to hold latest sensor data values 89 90float q[4] = {1.0f, 0.0f, 0.0f, 0.0f}; // vector to hold quaternion 91float 92 eInt[3] = {0.0f, 0.0f, 0.0f}; // vector to hold integral error for Mahony 93 method 94
DataLogger program files
Files which make up the DataLogger application: DataLogger.ino: main Arduino Nano 33 BLE Sense program AHRS.h: header file for AHRS/Madgwick library AHRS.cpp: code file for AHRS/Madgwick library AHRS_gvars.h: header file with global variables for AHRS/Madgwick library Ublox_NMEA_GPS.h: header file for Ublox NMEA GPS Class Ublox_NMEA_GPS.cpp: code file for Ublox NMEA GPS Class
Ublox_NMEA_GPS.h
c_cpp
Header file for Ublox_NMEA class
1class Ublox_NMEA_GPS { 2 private: 3 String _sentence = ""; 4 String _dat = ""; 5 String _tim = ""; 6 String _lon = ""; 7 String _lat = ""; 8 String _cog = ""; 9 String _sog = ""; 10 String _alt = ""; 11 String _fix = ""; 12 String _siv = ""; 13 14 public: 15 Ublox_NMEA_GPS(); 16 void init(); 17 void update(); 18 String getDAT(); 19 String getTIM(); 20 String getLON(); 21 String getLAT(); 22 String getCOG(); 23 String getSOG(); 24 String getALT(); 25 String getFIX(); 26 String getSIV(); 27}; 28 29extern long count_RMC; 30extern long count_GGA; 31
AHRS_gvars.h
c_cpp
Global variable for AHRS algoritm
1uint8_t OSR = ADC_4096; // set pressure amd temperature oversample 2 rate 3uint8_t Gscale = GFS_245DPS; // gyro full scale 4uint8_t Godr = GODR_238Hz; 5 // gyro data sample rate 6uint8_t Gbw = GBW_med; // gyro data bandwidth 7uint8_t 8 Ascale = AFS_2G; // accel full scale 9uint8_t Aodr = AODR_238Hz; // accel 10 data sample rate 11uint8_t Abw = ABW_50Hz; // accel data bandwidth 12uint8_t 13 Mscale = MFS_4G; // mag full scale 14uint8_t Modr = MODR_10Hz; // mag data 15 sample rate 16uint8_t Mmode = MMode_HighPerformance; // magnetometer operation 17 mode 18float aRes, gRes, mRes; // scale resolutions per LSB for the sensors 19int 20 myLed = 13; 21uint16_t Pcal[8]; // calibration constants from MS5611 PROM 22 registers 23unsigned char nCRC; // calculated check sum to ensure PROM integrity 24//-(rk) 25 uint32_t D1 = 0, D2 = 0; // raw MS5611 pressure and temperature data 26double 27 dT, OFFSET, SENS, T2, OFFSET2, SENS2; // First order and second order corrections 28 for raw S5637 temperature and pressure data 29int16_t accelCount[3], gyroCount[3], 30 magCount[3]; // Stores the 16-bit signed accelerometer, gyro, and mag sensor output 31float 32 gyroBias[3] = {0, 0, 0}, accelBias[3] = {0, 0, 0}, magBias[3] = {0, 0, 0}; // Bias 33 corrections for gyro, accelerometer, and magnetometer 34int16_t tempCount; // 35 temperature raw count output 36float temperature; // Stores the LSM9DS1gyro 37 internal chip temperature in degrees Celsius 38double Temperature, Pressure; // 39 stores MS5611 pressures sensor pressure and temperature 40 41// global constants 42 for 9 DoF fusion and AHRS (Attitude and Heading Reference System) 43float GyroMeasError 44 = PI * (40.0f / 180.0f); // gyroscope measurement error in rads/s (start at 40 45 deg/s) 46float GyroMeasDrift = PI * (0.0f / 180.0f); // gyroscope measurement 47 drift in rad/s/s (start at 0.0 deg/s/s) 48// There is a tradeoff in the beta parameter 49 between accuracy and response speed. 50// In the original Madgwick study, beta 51 of 0.041 (corresponding to GyroMeasError of 2.7 degrees/s) was found to give optimal 52 accuracy. 53// However, with this value, the LSM9SD0 response time is about 10 54 seconds to a stable initial quaternion. 55// Subsequent changes also require a 56 longish lag time to a stable output, not fast enough for a quadcopter or robot car! 57// 58 By increasing beta (GyroMeasError) by about a factor of fifteen, the response time 59 constant is reduced to ~2 sec 60// I haven't noticed any reduction in solution 61 accuracy. This is essentially the I coefficient in a PID control sense; 62// the 63 bigger the feedback coefficient, the faster the solution converges, usually at the 64 expense of accuracy. 65// In any case, this is the free parameter in the Madgwick 66 filtering and fusion scheme. 67float beta = sqrt(3.0f / 4.0f) * GyroMeasError; 68 // compute beta 69float zeta = sqrt(3.0f / 4.0f) * GyroMeasDrift; // compute 70 zeta, the other free parameter in the Madgwick scheme usually set to a small or 71 zero value 72#define Kp 2.0f * 5.0f // these are the free parameters in the Mahony 73 filter and fusion scheme, Kp for proportional feedback, Ki for integral 74#define 75 Ki 0.0f 76 77uint32_t delt_t = 0, count = 0, sumCount = 0; // used to control 78 display output rate 79float pitch, yaw, roll; 80float deltat = 0.0f, sum = 0.0f; 81 // integration interval for both filter schemes 82uint32_t lastUpdate 83 = 0, firstUpdate = 0; // used to calculate integration interval 84uint32_t Now 85 = 0; // used to calculate integration interval 86 87float 88 ax, ay, az, gx, gy, gz, mx, my, mz; // variables to hold latest sensor data values 89 90float q[4] = {1.0f, 0.0f, 0.0f, 0.0f}; // vector to hold quaternion 91float 92 eInt[3] = {0.0f, 0.0f, 0.0f}; // vector to hold integral error for Mahony 93 method 94
AHRS.cpp
c_cpp
AHRS / Madgwick library
1/* LSM9DS1_MS5611_t3 Basic Example Code 2by: Kris Winer 3date: November 4 1, 2014 5license: Beerware - Use this code however you'd like. If you 6find 7 it useful you can buy me a beer some time. 8Demonstrate basic LSM9DS1 functionality 9 including parameterizing the register addresses, initializing the sensor, 10getting 11 properly scaled accelerometer, gyroscope, and magnetometer data out. Added display 12 functions to 13allow display to on breadboard monitor. Addition of 9 DoF sensor 14 fusion using open source Madgwick and 15Mahony filter algorithms. Sketch runs 16 on the 3.3 V 8 MHz Pro Mini and the Teensy 3.1. 17This sketch is intended specifically 18 for the LSM9DS1+MS5611 Add-on shield for the Teensy 3.1. 19It uses SDA/SCL on pins 20 17/16, respectively, and it uses the Teensy 3.1-specific Wire library i2c_t3.h. 21The 22 MS5611 is a simple but high resolution pressure sensor, which can be used in its 23 high resolution 24mode but with power consumption od 20 microAmp, or in a lower 25 resolution mode with power consumption of 26only 1 microAmp. The choice will depend 27 on the application. 28SDA and SCL should have external pull-up resistors (to 3.3V). 294K7 30 resistors are on the LSM9DS1+MS5611 Teensy 3.1 add-on shield/breakout board. 31Hardware 32 setup: 33LSM9DS1Breakout --------- Arduino 34VDD ---------------------- 3.3V 35VDDI 36 --------------------- 3.3V 37SDA ----------------------- A4 38SCL ----------------------- 39 A5 40GND ---------------------- GND 41Note: The LSM9DS1 is an I2C sensor and can 42 use the Arduino Wire library. 43Because the sensor is not 5V tolerant, we are 44 using either a 3.3 V 8 MHz Pro Mini or a 3.3 V Teensy 3.1. 45We have disabled the 46 internal pull-ups used by the Wire library in the Wire.h/twi.c utility file. 47We 48 are also using the 400 kHz fast I2C mode by setting the TWI_FREQ to 400000L /twi.h 49 utility file. 50 51Modified by Ruud Kapteijn 5216-Jan-2021 53Moulded into class 54 AHRS 55taken out NOKIA 5110 monochrome display code 56code: https://github.com/j-mcc1993/LSM9DS1/blob/master/LSM9DS1_BasicAHRS_Nano33.ino 57discussion 58 threa: https://github.com/kriswiner/LSM9DS1/issues/14 59 */ 60#include <Arduino.h> 61#include 62 <Arduino_LSM9DS1.h> 63#include <Arduino_LPS22HB.h> //Include library to read Pressure 64#include 65 <Wire.h> 66#include <SPI.h> 67#include "AHRS.h" 68 69boolean AHRS_setup() 70 { 71 Wire1.begin(); 72 // TWBR = 12; // 400 kbit/sec I2C speed for Pro Mini 73 74 // Setup for Master mode, pins 16/17, external pullups, 400kHz for Teensy 3.1 75 76 //Wire.begin(I2C_MASTER, 0x00, I2C_PINS_16_17, I2C_PULLUP_EXT, I2C_RATE_400); 77 78 // reset 79 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG8, 0x05); 80 writeByte(LSM9DS1M_ADDRESS, 81 LSM9DS1M_CTRL_REG2_M, 0x0c); 82 delay(100); 83 84 // Initialize LED pin 85 86 pinMode(myLed, OUTPUT); 87 digitalWrite(myLed, HIGH); 88 89 // Read the WHO_AM_I 90 registers, this is a good test of communication 91 Serial.println("LSM9DS1 9-axis 92 motion sensor..."); 93 byte c = readByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_WHO_AM_I); 94 // Read WHO_AM_I register for LSM9DS1 accel/gyro 95 Serial.print("LSM9DS1 accel/gyro"); 96 Serial.print("I AM "); Serial.print(c, HEX); Serial.print(" I should be "); 97 Serial.println(0x68, HEX); 98 byte d = readByte(LSM9DS1M_ADDRESS, LSM9DS1M_WHO_AM_I); 99 // Read WHO_AM_I register for LSM9DS1 magnetometer 100 Serial.print("LSM9DS1 101 magnetometer"); Serial.print("I AM "); Serial.print(d, HEX); Serial.print(" 102 I should be "); Serial.println(0x3D, HEX); 103 104 if (c == 0x68 && d == 0x3D) 105 // WHO_AM_I should always be 0x0E for the accel/gyro and 0x3C for the mag 106 { 107 108 Serial.println("LSM9DS1 is online..."); 109 110 // get sensor resolutions, 111 only need to do this once 112 getAres(); 113 getGres(); 114 getMres(); 115 116 Serial.print("accel sensitivity is "); Serial.print(1./(1000.*aRes)); Serial.println(" 117 LSB/mg"); 118 Serial.print("gyro sensitivity is "); Serial.print(1./(1000.*gRes)); 119 Serial.println(" LSB/mdps"); 120 Serial.print("mag sensitivity is "); Serial.print(1./(1000.*mRes)); 121 Serial.println(" LSB/mGauss"); 122 123 Serial.println("Perform gyro and accel 124 self test"); 125 selftestLSM9DS1(); // check function of gyro and accelerometer 126 via self test 127 128 Serial.println(" Calibrate gyro and accel"); 129 accelgyrocalLSM9DS1(gyroBias, 130 accelBias); // Calibrate gyro and accelerometers, load biases in bias registers 131 132 Serial.println("accel biases (mg)"); Serial.println(1000.*accelBias[0]); Serial.println(1000.*accelBias[1]); 133 Serial.println(1000.*accelBias[2]); 134 Serial.println("gyro biases (dps)"); 135 Serial.println(gyroBias[0]); Serial.println(gyroBias[1]); Serial.println(gyroBias[2]); 136 137 138 magcalLSM9DS1(magBias); 139 Serial.println("mag biases (mG)"); Serial.println(1000.*magBias[0]); 140 Serial.println(1000.*magBias[1]); Serial.println(1000.*magBias[2]); 141 delay(2000); 142 // add delay to see results before serial spew of data 143 144 initLSM9DS1(); 145 146 Serial.println("LSM9DS1 initialized for active data mode...."); // Initialize 147 device for active mode read of acclerometer, gyroscope, and temperature 148 return 149 true; 150 } 151 else 152 { 153 Serial.print("Could not connect to LSM9DS1: 154 0x"); 155 Serial.println(c, HEX); 156 return false; 157 } 158} 159 160void 161 AHRS_update() { 162 int DEBUG = 0; 163 164 if (readByte(LSM9DS1XG_ADDRESS, 165 LSM9DS1XG_STATUS_REG) & 0x01) { // check if new accel data is ready 166 readAccelData(accelCount); 167 // Read the x/y/z adc values 168 169 // Now we'll calculate the accleration 170 value into actual g's 171 ax = (float)accelCount[0]*aRes - accelBias[0]; // 172 get actual g value, this depends on scale being set 173 ay = (float)accelCount[1]*aRes 174 - accelBias[1]; 175 az = (float)accelCount[2]*aRes - accelBias[2]; 176 177 } 178 179 if (readByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_STATUS_REG) & 0x02) 180 { // check if new gyro data is ready 181 readGyroData(gyroCount); // Read 182 the x/y/z adc values 183 184 // Calculate the gyro value into actual degrees 185 per second 186 gx = (float)gyroCount[0]*gRes - gyroBias[0]; // get actual 187 gyro value, this depends on scale being set 188 gy = (float)gyroCount[1]*gRes 189 - gyroBias[1]; 190 gz = (float)gyroCount[2]*gRes - gyroBias[2]; 191 } 192 193 194 if (readByte(LSM9DS1M_ADDRESS, LSM9DS1M_STATUS_REG_M) & 0x08) { // check if 195 new mag data is ready 196 readMagData(magCount); // Read the x/y/z adc values 197 198 199 // Calculate the magnetometer values in milliGauss 200 // Include factory 201 calibration per data sheet and user environmental corrections 202 mx = (float)magCount[0]*mRes; 203 // - magBias[0]; // get actual magnetometer value, this depends on scale being 204 set 205 my = (float)magCount[1]*mRes; // - magBias[1]; 206 mz = (float)magCount[2]*mRes; 207 // - magBias[2]; 208 } 209 210 Now = micros(); 211 deltat = ((Now - 212 lastUpdate)/1000000.0f); // set integration time by time elapsed since last filter 213 update 214 lastUpdate = Now; 215 216 sum += deltat; // sum for averaging filter 217 update rate 218 sumCount++; 219 220 // Sensors x, y, and z axes of the accelerometer 221 and gyro are aligned. The magnetometer 222 // the magnetometer z-axis (+ up) 223 is aligned with the z-axis (+ up) of accelerometer and gyro, but the magnetometer 224 225 // x-axis is aligned with the -x axis of the gyro and the magnetometer y axis 226 is aligned with the y axis of the gyro! 227 // We have to make some allowance 228 for this orientation mismatch in feeding the output to the quaternion filter. 229 230 // For the LSM9DS1, we have chosen a magnetic rotation that keeps the sensor 231 forward along the x-axis just like 232 // in the LSM9DS0 sensor. This rotation 233 can be modified to allow any convenient orientation convention. 234 // This is 235 ok by aircraft orientation standards! 236 // Pass gyro rate as rad/s 237 MadgwickQuaternionUpdate(ax, 238 ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, -mx, -my, mz); 239 // MahonyQuaternionUpdate(ax, 240 ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, -mx, my, mz); 241 242 // Serial 243 print and/or display at 0.5 s rate independent of data rates 244 delt_t = millis() 245 - count; 246 if (delt_t > 500) { // update LCD once per half-second independent 247 of read rate 248 249 if(SerialDebug && DEBUG > 0) { 250 Serial.print("lib 251 -> ax = "); Serial.print((int)1000*ax); 252 Serial.print(" ay = "); 253 Serial.print((int)1000*ay); 254 Serial.print(" az = "); Serial.print((int)1000*az); 255 Serial.println(" mg"); 256 Serial.print("gx = "); Serial.print( gx, 2); 257 258 Serial.print(" gy = "); Serial.print( gy, 2); 259 Serial.print(" 260 gz = "); Serial.print( gz, 2); Serial.println(" deg/s"); 261 Serial.print("mx 262 = "); Serial.print( (int)1000*mx ); 263 Serial.print(" my = "); Serial.print( 264 (int)1000*my ); 265 Serial.print(" mz = "); Serial.print( (int)1000*mz 266 ); Serial.println(" mG"); 267 268 Serial.print("q0 = "); Serial.print(q[0]); 269 270 Serial.print(" qx = "); Serial.print(q[1]); 271 Serial.print(" 272 qy = "); Serial.print(q[2]); 273 Serial.print(" qz = "); Serial.println(q[3]); 274 275 } 276 tempCount = readTempData(); // Read the gyro 277 adc values 278 temperature = ((float) tempCount/256. + 25.0); // Gyro chip 279 temperature in degrees Centigrade 280 // Print temperature in degrees Centigrade 281 282 if (SerialDebug && DEBUG > 0) { 283 Serial.print("Gyro temperature 284 is "); Serial.print(temperature, 1); Serial.println(" degrees C"); // Print 285 T values to tenths of s degree C 286 } 287 288 // Define output variables 289 from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft 290 orientation. 291 // In this coordinate system, the positive z-axis is down 292 toward Earth. 293 // Yaw is the angle between Sensor x-axis and Earth magnetic 294 North (or true North if corrected for local declination, looking down on the sensor 295 positive yaw is counterclockwise. 296 // Pitch is angle between sensor x-axis 297 and Earth ground plane, toward the Earth is positive, up toward the sky is negative. 298 299 // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is 300 positive roll. 301 // These arise from the definition of the homogeneous rotation 302 matrix constructed from quaternions. 303 // Tait-Bryan angles as well as Euler 304 angles are non-commutative; that is, the get the correct orientation the rotations 305 must be 306 // applied in the correct order which for this configuration is 307 yaw, pitch, and then roll. 308 // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles 309 which has additional links. 310 yaw = atan2(2.0f * (q[1] * q[2] + q[0] * 311 q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]); 312 pitch 313 = -asin(2.0f * (q[1] * q[3] - q[0] * q[2])); 314 roll = atan2(2.0f * (q[0] 315 * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]); 316 317 pitch *= 180.0f / PI; 318 yaw *= 180.0f / PI; 319 yaw -= 1.0f; 320 // Declination at Amsterdam in 2021 321 roll *= 180.0f / PI; 322 323 if(SerialDebug 324 && DEBUG > 0) { 325 Serial.print("lib -> Yaw, Pitch, Roll: "); 326 Serial.print(yaw, 327 2); 328 Serial.print(", "); 329 Serial.print(pitch, 2); 330 Serial.print(", 331 "); 332 Serial.println(roll, 2); 333 // Serial.print("rate = "); 334 Serial.print((float)sumCount/sum, 2); Serial.println(" Hz\ 335"); 336 } 337 338 // With these settings the filter is updating at a ~145 Hz rate using the 339 Madgwick scheme and 340 // >200 Hz using the Mahony scheme even though the 341 display refreshes at only 2 Hz. 342 // The filter update rate is determined 343 mostly by the mathematical steps in the respective algorithms, 344 // the 345 processor speed (8 MHz for the 3.3V Pro Mini), and the magnetometer ODR: 346 // 347 an ODR of 10 Hz for the magnetometer produce the above rates, maximum magnetometer 348 ODR of 100 Hz produces 349 // filter update rates of 36 - 145 and ~38 Hz for 350 the Madgwick and Mahony schemes, respectively. 351 // This is presumably because 352 the magnetometer read takes longer than the gyro or accelerometer reads. 353 // 354 This filter update rate should be fast enough to maintain accurate platform orientation 355 for 356 // stabilization control of a fast-moving robot or quadcopter. Compare 357 to the update rate of 200 Hz 358 // produced by the on-board Digital Motion 359 Processor of Invensense's MPU6050 6 DoF and MPU9150 9DoF sensors. 360 // The 361 3.3 V 8 MHz Pro Mini is doing pretty well! 362 363 digitalWrite(myLed, !digitalRead(myLed)); 364 365 count = millis(); 366 sumCount = 0; 367 sum = 0; 368 } 369 370 } 371 372//=================================================================================================================== 373//====== 374 Set of useful function to access acceleration. gyroscope, magnetometer, and temperature 375 data 376//=================================================================================================================== 377 378void 379 getMres() { 380 switch (Mscale) 381 { 382 // Possible magnetometer scales (and 383 their register bit settings) are: 384 // 4 Gauss (00), 8 Gauss (01), 12 Gauss 385 (10) and 16 Gauss (11) 386 case MFS_4G: 387 mRes = 4.0/32768.0; 388 break; 389 390 case MFS_8G: 391 mRes = 8.0/32768.0; 392 break; 393 case MFS_12G: 394 395 mRes = 12.0/32768.0; 396 break; 397 case MFS_16G: 398 mRes = 399 16.0/32768.0; 400 break; 401 } 402} 403 404void getGres() { 405 switch (Gscale) 406 407 { 408 // Possible gyro scales (and their register bit settings) are: 409 // 410 245 DPS (00), 500 DPS (01), and 2000 DPS (11). 411 case GFS_245DPS: 412 gRes 413 = 245.0/32768.0; 414 break; 415 case GFS_500DPS: 416 gRes = 500.0/32768.0; 417 418 break; 419 case GFS_2000DPS: 420 gRes = 2000.0/32768.0; 421 break; 422 423 } 424} 425 426void getAres() { 427 switch (Ascale) 428 { 429 // Possible 430 accelerometer scales (and their register bit settings) are: 431 // 2 Gs (00), 432 16 Gs (01), 4 Gs (10), and 8 Gs (11). 433 case AFS_2G: 434 aRes = 2.0/32768.0; 435 436 break; 437 case AFS_16G: 438 aRes = 16.0/32768.0; 439 break; 440 441 case AFS_4G: 442 aRes = 4.0/32768.0; 443 break; 444 case AFS_8G: 445 446 aRes = 8.0/32768.0; 447 break; 448 } 449} 450 451 452void readAccelData(int16_t 453 * destination) 454{ 455 uint8_t rawData[6]; // x/y/z accel register data stored 456 here 457 readBytes(LSM9DS1XG_ADDRESS, LSM9DS1XG_OUT_X_L_XL, 6, &rawData[0]); // 458 Read the six raw data registers into data array 459 destination[0] = ((int16_t)rawData[1] 460 << 8) | rawData[0] ; // Turn the MSB and LSB into a signed 16-bit value 461 destination[1] 462 = ((int16_t)rawData[3] << 8) | rawData[2] ; 463 destination[2] = ((int16_t)rawData[5] 464 << 8) | rawData[4] ; 465} 466 467 468void readGyroData(int16_t * destination) 469{ 470 471 uint8_t rawData[6]; // x/y/z gyro register data stored here 472 readBytes(LSM9DS1XG_ADDRESS, 473 LSM9DS1XG_OUT_X_L_G, 6, &rawData[0]); // Read the six raw data registers sequentially 474 into data array 475 destination[0] = ((int16_t)rawData[1] << 8) | rawData[0] ; 476 // Turn the MSB and LSB into a signed 16-bit value 477 destination[1] = ((int16_t)rawData[3] 478 << 8) | rawData[2] ; 479 destination[2] = ((int16_t)rawData[5] << 8) | rawData[4] 480 ; 481} 482 483void readMagData(int16_t * destination) 484{ 485 uint8_t rawData[6]; 486 // x/y/z gyro register data stored here 487 readBytes(LSM9DS1M_ADDRESS, LSM9DS1M_OUT_X_L_M, 488 6, &rawData[0]); // Read the six raw data registers sequentially into data array 489 490 destination[0] = ((int16_t)rawData[1] << 8) | rawData[0] ; // Turn the MSB and 491 LSB into a signed 16-bit value 492 destination[1] = ((int16_t)rawData[3] << 8) 493 | rawData[2] ; // Data stored as little Endian 494 destination[2] = ((int16_t)rawData[5] 495 << 8) | rawData[4] ; 496} 497 498int16_t readTempData() 499{ 500 uint8_t rawData[2]; 501 // x/y/z gyro register data stored here 502 readBytes(LSM9DS1XG_ADDRESS, LSM9DS1XG_OUT_TEMP_L, 503 2, &rawData[0]); // Read the two raw data registers sequentially into data array 504 505 return (((int16_t)rawData[1] << 8) | rawData[0]); // Turn the MSB and LSB 506 into a 16-bit signed value 507} 508 509 510void initLSM9DS1() 511{ 512 // enable 513 the 3-axes of the gyroscope 514 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG4, 515 0x38); 516 // configure the gyroscope 517 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG1_G, 518 Godr << 5 | Gscale << 3 | Gbw); 519 delay(200); 520 // enable the three axes of 521 the accelerometer 522 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG5_XL, 0x38); 523 524 // configure the accelerometer-specify bandwidth selection with Abw 525 writeByte(LSM9DS1XG_ADDRESS, 526 LSM9DS1XG_CTRL_REG6_XL, Aodr << 5 | Ascale << 3 | 0x04 |Abw); 527 delay(200); 528 529 // enable block data update, allow auto-increment during multiple byte read 530 531 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG8, 0x44); 532 // configure the 533 magnetometer-enable temperature compensation of mag data 534 writeByte(LSM9DS1M_ADDRESS, 535 LSM9DS1M_CTRL_REG1_M, 0x80 | Mmode << 5 | Modr << 2); // select x,y-axis mode 536 537 writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG2_M, Mscale << 5 ); // select mag 538 full scale 539 writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG3_M, 0x00 ); // continuous 540 conversion mode 541 writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG4_M, Mmode << 542 2 ); // select z-axis mode 543 writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG5_M, 544 0x40 ); // select block update mode 545} 546 547 548void selftestLSM9DS1() 549{ 550 551 float accel_noST[3] = {0., 0., 0.}, accel_ST[3] = {0., 0., 0.}; 552 float gyro_noST[3] 553 = {0., 0., 0.}, gyro_ST[3] = {0., 0., 0.}; 554 555 writeByte(LSM9DS1XG_ADDRESS, 556 LSM9DS1XG_CTRL_REG10, 0x00); // disable self test 557 accelgyrocalLSM9DS1(gyro_noST, 558 accel_noST); 559 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG10, 0x05); // 560 enable gyro/accel self test 561 accelgyrocalLSM9DS1(gyro_ST, accel_ST); 562 563 564 float gyrodx = (gyro_ST[0] - gyro_noST[0]); 565 float gyrody = (gyro_ST[1] - 566 gyro_noST[1]); 567 float gyrodz = (gyro_ST[2] - gyro_noST[2]); 568 569 Serial.println("Gyro 570 self-test results: "); 571 Serial.print("x-axis = "); Serial.print(gyrodx); 572 Serial.print(" dps"); Serial.println(" should be between 20 and 250 dps"); 573 574 Serial.print("y-axis = "); Serial.print(gyrody); Serial.print(" dps"); Serial.println(" 575 should be between 20 and 250 dps"); 576 Serial.print("z-axis = "); Serial.print(gyrodz); 577 Serial.print(" dps"); Serial.println(" should be between 20 and 250 dps"); 578 579 580 float accdx = 1000.*(accel_ST[0] - accel_noST[0]); 581 float accdy = 1000.*(accel_ST[1] 582 - accel_noST[1]); 583 float accdz = 1000.*(accel_ST[2] - accel_noST[2]); 584 585 586 Serial.println("Accelerometer self-test results: "); 587 Serial.print("x-axis 588 = "); Serial.print(accdx); Serial.print(" mg"); Serial.println(" should be between 589 60 and 1700 mg"); 590 Serial.print("y-axis = "); Serial.print(accdy); Serial.print(" 591 mg"); Serial.println(" should be between 60 and 1700 mg"); 592 Serial.print("z-axis 593 = "); Serial.print(accdz); Serial.print(" mg"); Serial.println(" should be between 594 60 and 1700 mg"); 595 596 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG10, 0x00); 597 // disable self test 598 delay(200); 599} 600// Function which accumulates gyro 601 and accelerometer data after device initialization. It calculates the average 602// 603 of the at-rest readings and then loads the resulting offsets into accelerometer 604 and gyro bias registers. 605void accelgyrocalLSM9DS1(float * dest1, float * dest2) 606{ 607 608 uint8_t data[6] = {0, 0, 0, 0, 0, 0}; 609 int32_t gyro_bias[3] = {0, 0, 610 0}, accel_bias[3] = {0, 0, 0}; 611 uint16_t samples, ii; 612 613 // enable the 614 3-axes of the gyroscope 615 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG4, 0x38); 616 617 // configure the gyroscope 618 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG1_G, 619 Godr << 5 | Gscale << 3 | Gbw); 620 delay(200); 621 // enable the three axes of 622 the accelerometer 623 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG5_XL, 0x38); 624 625 // configure the accelerometer-specify bandwidth selection with Abw 626 writeByte(LSM9DS1XG_ADDRESS, 627 LSM9DS1XG_CTRL_REG6_XL, Aodr << 5 | Ascale << 3 | 0x04 |Abw); 628 delay(200); 629 630 // enable block data update, allow auto-increment during multiple byte read 631 632 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG8, 0x44); 633 634 // First get 635 gyro bias 636 byte c = readByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG9); 637 writeByte(LSM9DS1XG_ADDRESS, 638 LSM9DS1XG_CTRL_REG9, c | 0x02); // Enable gyro FIFO 639 delay(50); // 640 Wait for change to take effect 641 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_FIFO_CTRL, 642 0x20 | 0x1F); // Enable gyro FIFO stream mode and set watermark at 32 samples 643 644 delay(1000); // delay 1000 milliseconds to collect FIFO samples 645 646 samples 647 = (readByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_FIFO_SRC) & 0x2F); // Read number of stored 648 samples 649 650 for(ii = 0; ii < samples ; ii++) { // Read the gyro 651 data stored in the FIFO 652 int16_t gyro_temp[3] = {0, 0, 0}; 653 readBytes(LSM9DS1XG_ADDRESS, 654 LSM9DS1XG_OUT_X_L_G, 6, &data[0]); 655 gyro_temp[0] = (int16_t) (((int16_t)data[1] 656 << 8) | data[0]); // Form signed 16-bit integer for each sample in FIFO 657 gyro_temp[1] 658 = (int16_t) (((int16_t)data[3] << 8) | data[2]); 659 gyro_temp[2] = (int16_t) 660 (((int16_t)data[5] << 8) | data[4]); 661 662 gyro_bias[0] += (int32_t) gyro_temp[0]; 663 // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases 664 665 gyro_bias[1] += (int32_t) gyro_temp[1]; 666 gyro_bias[2] += (int32_t) gyro_temp[2]; 667 668 } 669 670 gyro_bias[0] /= samples; // average the data 671 gyro_bias[1] 672 /= samples; 673 gyro_bias[2] /= samples; 674 675 dest1[0] = (float)gyro_bias[0]*gRes; 676 // Properly scale the data to get deg/s 677 dest1[1] = (float)gyro_bias[1]*gRes; 678 679 dest1[2] = (float)gyro_bias[2]*gRes; 680 681 c = readByte(LSM9DS1XG_ADDRESS, 682 LSM9DS1XG_CTRL_REG9); 683 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG9, c & 684 ~0x02); //Disable gyro FIFO 685 delay(50); 686 writeByte(LSM9DS1XG_ADDRESS, 687 LSM9DS1XG_FIFO_CTRL, 0x00); // Enable gyro bypass mode 688 689 // now get the 690 accelerometer bias 691 c = readByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG9); 692 693 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG9, c | 0x02); // Enable accel 694 FIFO 695 delay(50); // 696 Wait for change to take effect 697 writeByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_FIFO_CTRL, 698 0x20 | 0x1F); // Enable accel FIFO stream mode and set watermark at 32 samples 699 700 delay(1000); // delay 1000 milliseconds to collect FIFO samples 701 702 samples 703 = (readByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_FIFO_SRC) & 0x2F); // Read number of stored 704 samples 705 706 for(ii = 0; ii < samples ; ii++) { // Read the accel 707 data stored in the FIFO 708 int16_t accel_temp[3] = {0, 0, 0}; 709 readBytes(LSM9DS1XG_ADDRESS, 710 LSM9DS1XG_OUT_X_L_XL, 6, &data[0]); 711 accel_temp[0] = (int16_t) (((int16_t)data[1] 712 << 8) | data[0]); // Form signed 16-bit integer for each sample in FIFO 713 accel_temp[1] 714 = (int16_t) (((int16_t)data[3] << 8) | data[2]); 715 accel_temp[2] = (int16_t) 716 (((int16_t)data[5] << 8) | data[4]); 717 718 accel_bias[0] += (int32_t) accel_temp[0]; 719 // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases 720 721 accel_bias[1] += (int32_t) accel_temp[1]; 722 accel_bias[2] += (int32_t) 723 accel_temp[2]; 724 } 725 726 accel_bias[0] /= samples; // average the data 727 728 accel_bias[1] /= samples; 729 accel_bias[2] /= samples; 730 731 if(accel_bias[2] 732 > 0L) {accel_bias[2] -= (int32_t) (1.0/aRes);} // Remove gravity from the z-axis 733 accelerometer bias calculation 734 else {accel_bias[2] += (int32_t) (1.0/aRes);} 735 736 737 dest2[0] = (float)accel_bias[0]*aRes; // Properly scale the data to get g 738 739 dest2[1] = (float)accel_bias[1]*aRes; 740 dest2[2] = (float)accel_bias[2]*aRes; 741 742 743 c = readByte(LSM9DS1XG_ADDRESS, LSM9DS1XG_CTRL_REG9); 744 writeByte(LSM9DS1XG_ADDRESS, 745 LSM9DS1XG_CTRL_REG9, c & ~0x02); //Disable accel FIFO 746 delay(50); 747 writeByte(LSM9DS1XG_ADDRESS, 748 LSM9DS1XG_FIFO_CTRL, 0x00); // Enable accel bypass mode 749} 750 751void magcalLSM9DS1(float 752 * dest1) 753{ 754 uint8_t data[6]; // data array to hold mag x, y, z, data 755 756 uint16_t ii = 0, sample_count = 0; 757 int32_t mag_bias[3] = {0, 0, 0}; 758 int16_t 759 mag_max[3] = {0, 0, 0}, mag_min[3] = {0, 0, 0}; 760 761 // configure the magnetometer-enable 762 temperature compensation of mag data 763 writeByte(LSM9DS1M_ADDRESS, LSM9DS1M_CTRL_REG1_M, 764 0x80 | Mmode << 5 | Modr << 2); // select x,y-axis mode 765 writeByte(LSM9DS1M_ADDRESS, 766 LSM9DS1M_CTRL_REG2_M, Mscale << 5 ); // select mag full scale 767 writeByte(LSM9DS1M_ADDRESS, 768 LSM9DS1M_CTRL_REG3_M, 0x00 ); // continuous conversion mode 769 writeByte(LSM9DS1M_ADDRESS, 770 LSM9DS1M_CTRL_REG4_M, Mmode << 2 ); // select z-axis mode 771 writeByte(LSM9DS1M_ADDRESS, 772 LSM9DS1M_CTRL_REG5_M, 0x40 ); // select block update mode 773 774 Serial.println("Mag 775 Calibration: Wave device in a figure eight until done!"); 776 delay(4000); 777 778 779 sample_count = 128; 780 for(ii = 0; ii < sample_count; ii++) { 781 int16_t 782 mag_temp[3] = {0, 0, 0}; 783 readBytes(LSM9DS1M_ADDRESS, LSM9DS1M_OUT_X_L_M, 784 6, &data[0]); // Read the six raw data registers into data array 785 mag_temp[0] 786 = (int16_t) (((int16_t)data[1] << 8) | data[0]) ; // Form signed 16-bit integer 787 for each sample in FIFO 788 mag_temp[1] = (int16_t) (((int16_t)data[3] << 8) 789 | data[2]) ; 790 mag_temp[2] = (int16_t) (((int16_t)data[5] << 8) | data[4]) 791 ; 792 for (int jj = 0; jj < 3; jj++) { 793 if(mag_temp[jj] > mag_max[jj]) 794 mag_max[jj] = mag_temp[jj]; 795 if(mag_temp[jj] < mag_min[jj]) mag_min[jj] 796 = mag_temp[jj]; 797 } 798 delay(105); // at 10 Hz ODR, new mag data is available 799 every 100 ms 800 } 801 802 // Serial.println("mag x min/max:"); Serial.println(mag_max[0]); 803 Serial.println(mag_min[0]); 804 // Serial.println("mag y min/max:"); Serial.println(mag_max[1]); 805 Serial.println(mag_min[1]); 806 // Serial.println("mag z min/max:"); Serial.println(mag_max[2]); 807 Serial.println(mag_min[2]); 808 809 mag_bias[0] = (mag_max[0] + mag_min[0])/2; 810 // get average x mag bias in counts 811 mag_bias[1] = (mag_max[1] + mag_min[1])/2; 812 // get average y mag bias in counts 813 mag_bias[2] = (mag_max[2] + mag_min[2])/2; 814 // get average z mag bias in counts 815 816 dest1[0] = (float) mag_bias[0]*mRes; 817 // save mag biases in G for main program 818 dest1[1] = (float) mag_bias[1]*mRes; 819 820 dest1[2] = (float) mag_bias[2]*mRes; 821 822 //write biases 823 to accelerometermagnetometer offset registers as counts); 824 writeByte(LSM9DS1M_ADDRESS, 825 LSM9DS1M_OFFSET_X_REG_L_M, (int16_t) mag_bias[0] & 0xFF); 826 writeByte(LSM9DS1M_ADDRESS, 827 LSM9DS1M_OFFSET_X_REG_H_M, ((int16_t)mag_bias[0] >> 8) & 0xFF); 828 writeByte(LSM9DS1M_ADDRESS, 829 LSM9DS1M_OFFSET_Y_REG_L_M, (int16_t) mag_bias[1] & 0xFF); 830 writeByte(LSM9DS1M_ADDRESS, 831 LSM9DS1M_OFFSET_Y_REG_H_M, ((int16_t)mag_bias[1] >> 8) & 0xFF); 832 writeByte(LSM9DS1M_ADDRESS, 833 LSM9DS1M_OFFSET_Z_REG_L_M, (int16_t) mag_bias[2] & 0xFF); 834 writeByte(LSM9DS1M_ADDRESS, 835 LSM9DS1M_OFFSET_Z_REG_H_M, ((int16_t)mag_bias[2] >> 8) & 0xFF); 836 837 Serial.println("Mag 838 Calibration done!"); 839} 840 841// I2C read/write functions for the LSM9DS1and 842 AK8963 sensors 843 844void writeByte(uint8_t address, uint8_t subAddress, uint8_t 845 data) 846{ 847 Wire1.beginTransmission(address); // Initialize the Tx buffer 848 849 Wire1.write(subAddress); // Put slave register address in Tx buffer 850 851 Wire1.write(data); // Put data in Tx buffer 852 Wire1.endTransmission(); 853 // Send the Tx buffer 854} 855 856uint8_t readByte(uint8_t address, uint8_t 857 subAddress) 858{ 859 uint8_t data; // `data` will store the register data 860 861 Wire1.beginTransmission(address); // Initialize the Tx buffer 862 Wire1.write(subAddress); 863 // Put slave register address in Tx buffer 864 // Wire.endTransmission(I2C_NOSTOP); 865 // Send the Tx buffer, but send a restart to keep connection alive 866 Wire1.endTransmission(false); 867 // Send the Tx buffer, but send a restart to keep connection alive 868 869 // Wire.requestFrom(address, 1); // Read one byte from slave register address 870 871 Wire1.requestFrom(address, (size_t) 1); // Read one byte from slave register 872 address 873 data = Wire1.read(); // Fill Rx buffer with result 874 875 return data; // Return data read from slave register 876} 877 878void 879 readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest) 880{ 881 882 Wire1.beginTransmission(address); // Initialize the Tx buffer 883 Wire1.write(subAddress); 884 // Put slave register address in Tx buffer 885 // Wire.endTransmission(I2C_NOSTOP); 886 // Send the Tx buffer, but send a restart to keep connection alive 887 Wire1.endTransmission(false); 888 // Send the Tx buffer, but send a restart to keep connection alive 889 uint8_t 890 i = 0; 891 Wire1.requestFrom(address, count); // Read bytes from slave register 892 address 893 // Wire.requestFrom(address, (size_t) count); // Read bytes 894 from slave register address 895 while (Wire1.available()) { 896 dest[i++] = 897 Wire1.read(); } // Put read results in the Rx buffer 898} 899 900void MadgwickQuaternionUpdate(float 901 ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float 902 mz) 903{ 904 float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3]; // short name 905 local variable for readability 906 float norm; 907 float hx, hy, _2bx, _2bz; 908 909 float s1, s2, s3, s4; 910 float qDot1, qDot2, qDot3, qDot4; 911 912 // Auxiliary 913 variables to avoid repeated arithmetic 914 float _2q1mx; 915 float _2q1my; 916 917 float _2q1mz; 918 float _2q2mx; 919 float _4bx; 920 float _4bz; 921 float 922 _2q1 = 2.0f * q1; 923 float _2q2 = 2.0f * q2; 924 float _2q3 = 2.0f * q3; 925 926 float _2q4 = 2.0f * q4; 927 float _2q1q3 = 2.0f * q1 * q3; 928 float _2q3q4 929 = 2.0f * q3 * q4; 930 float q1q1 = q1 * q1; 931 float q1q2 = q1 * q2; 932 float 933 q1q3 = q1 * q3; 934 float q1q4 = q1 * q4; 935 float q2q2 = q2 * q2; 936 float 937 q2q3 = q2 * q3; 938 float q2q4 = q2 * q4; 939 float q3q3 = q3 * q3; 940 float 941 q3q4 = q3 * q4; 942 float q4q4 = q4 * q4; 943 944 // Normalise accelerometer measurement 945 946 norm = sqrt(ax * ax + ay * ay + az * az); 947 if (norm == 0.0f) return; // handle 948 NaN 949 norm = 1.0f/norm; 950 ax *= norm; 951 ay *= norm; 952 az *= norm; 953 954 955 // Normalise magnetometer measurement 956 norm = sqrt(mx * mx + my * my + mz 957 * mz); 958 if (norm == 0.0f) return; // handle NaN 959 norm = 1.0f/norm; 960 mx 961 *= norm; 962 my *= norm; 963 mz *= norm; 964 965 // Reference direction of Earth's 966 magnetic field 967 _2q1mx = 2.0f * q1 * mx; 968 _2q1my = 2.0f * q1 * my; 969 _2q1mz 970 = 2.0f * q1 * mz; 971 _2q2mx = 2.0f * q2 * mx; 972 hx = mx * q1q1 - _2q1my * q4 973 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4; 974 975 hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 976 + _2q3 * mz * q4 - my * q4q4; 977 _2bx = sqrt(hx * hx + hy * hy); 978 _2bz = -_2q1mx 979 * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * 980 q3q3 + mz * q4q4; 981 _4bx = 2.0f * _2bx; 982 _4bz = 2.0f * _2bz; 983 984 // 985 Gradient decent algorithm corrective step 986 s1 = -_2q3 * (2.0f * q2q4 - _2q1q3 987 - ax) + _2q2 * (2.0f * q1q2 + _2q3q4 - ay) - _2bz * q3 * (_2bx * (0.5f - q3q3 - 988 q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q4 + _2bz * q2) * (_2bx * (q2q3 - 989 q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * 990 (0.5f - q2q2 - q3q3) - mz); 991 s2 = _2q4 * (2.0f * q2q4 - _2q1q3 - ax) + _2q1 992 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q2 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - 993 az) + _2bz * q4 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx 994 * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * 995 q4 - _4bz * q2) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz); 996 997 s3 = -_2q1 * (2.0f * q2q4 - _2q1q3 - ax) + _2q4 * (2.0f * q1q2 + _2q3q4 - ay) 998 - 4.0f * q3 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + (-_4bx * q3 - _2bz * q1) 999 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q2 + _2bz 1000 * q4) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q1 - _4bz * 1001 q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz); 1002 s4 = _2q2 1003 * (2.0f * q2q4 - _2q1q3 - ax) + _2q3 * (2.0f * q1q2 + _2q3q4 - ay) + (-_4bx * q4 1004 + _2bz * q2) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx 1005 * q1 + _2bz * q3) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * 1006 q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz); 1007 norm = sqrt(s1 1008 * s1 + s2 * s2 + s3 * s3 + s4 * s4); // normalise step magnitude 1009 norm = 1010 1.0f/norm; 1011 s1 *= norm; 1012 s2 *= norm; 1013 s3 *= norm; 1014 s4 *= norm; 1015 1016 1017 // Compute rate of change of quaternion 1018 qDot1 = 0.5f * (-q2 * gx - q3 * gy 1019 - q4 * gz) - beta * s1; 1020 qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - beta 1021 * s2; 1022 qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - beta * s3; 1023 qDot4 1024 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - beta * s4; 1025 1026 // Integrate to yield 1027 quaternion 1028 q1 += qDot1 * deltat; 1029 q2 += qDot2 * deltat; 1030 q3 += qDot3 1031 * deltat; 1032 q4 += qDot4 * deltat; 1033 norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 1034 + q4 * q4); // normalise quaternion 1035 norm = 1.0f/norm; 1036 q[0] = q1 * norm; 1037 1038 q[1] = q2 * norm; 1039 q[2] = q3 * norm; 1040 q[3] = q4 * norm; 1041 1042} 1043
DataLogger.ino
c_cpp
Main Adruino program for Arduino Nano 33 BLE Sense
1/**************************************************************************** 2 3 * Data Logger v30 4 * 28-Jan-21 5 * Ruud Kapteijn 6 * Available under GPL3+ 7 8 * 9 * Uses the AHRS Madgwick code from Kris Winer (ref AHRS.h) 10 * Original 11 code by Kris Winer 12 * date: Nov 1, 2014 13 * code: https://github.com/j-mcc1993/LSM9DS1/blob/master/LSM9DS1_BasicAHRS_Nano33.ino 14 15 * discussion threa: https://github.com/kriswiner/LSM9DS1/issues/14 16 */ 17#include 18 <Arduino_LSM9DS1.h> 19#include <Arduino_LPS22HB.h> //Include library to read Pressure 20#include 21 <SPI.h> 22#include <SD.h> 23// #include <RF24.h> 24 25#include "Ublox_NMEA_GPS.h" 26#include 27 "AHRS.h" 28#include "AHRS_gvars.h" 29 30#define LEDR (22u) 31#define 32 LEDG (23u) 33#define LEDB (24u) 34#define LEDPWR (25u) 35 36// 37 GPS Vars 38Ublox_NMEA_GPS myGPS; 39boolean fix = false; 40int satellites, prev_satellites; 41 42// 43 potentionmeter 44int potmeter = 0; 45 46// IMU Vars 47// float ax, ay, az; 48// 49 float gx, gy, gz; 50// float mx, my, mz; 51 52// Baro vars 53float pressure; 54 55// 56 SD Vars 57File dataFile; 58int chipSelect = 10; 59int seqnr = 0; 60String fileName; 61int 62 counter = 0; 63int byteCounter = 0; 64String dataString; 65long last_write = 66 0; 67 68/* 69// transmitter vars 70RF24 radio(9, 10); // CE, CSN 71const byte 72 address[6] = "00001"; //Byte of array representing the address. This is the 73 address where we will send the data. This should be same on the receiving side. 74long 75 last_write = 0; 76char tx_buffer[256]; 77String dataString; 78int counter = 0; 79int 80 stringLength = 0; 81const char ping[] = "ping"; 82*/ 83 84// statistic vars 85boolean 86 first_fix = true; 87long start_loops; 88long count_loops=0; 89long count_RMC 90 = 0; 91long count_GGA = 0; 92 93void setup() { // 94 switch buildin LED off 95 digitalWrite(LEDPWR, HIGH); digitalWrite(LEDR, HIGH); 96 digitalWrite(LEDG, HIGH); digitalWrite(LEDB, HIGH); 97 digitalWrite(LEDR, LOW); 98 // switch buildin LED to red -> not initialized. 99 100 101 Serial.begin(9600); // Open serial communications 102 and wait for port to open: 103 // while (!Serial); // 104 wait until monitor is opened 105 Serial.println("INFO: ** Telemetry V10 **"); 106 107 108 myGPS.init(); 109 Serial.println("INFO: GPS Started"); 110 111 if (!IMU.begin()) 112 { 113 Serial.println("ERROR: Failed to initialize IMU! halted."); 114 while 115 (true); 116 } 117 Serial.println("INFO: IMU Started!"); 118 119 if (!BARO.begin()) 120 { //Initialize Pressure sensor 121 Serial.println("ERROR: Failed to initialize 122 Pressure Sensor! halted."); 123 while (1); 124 } 125 Serial.println("INFO: 126 BARO Started!"); 127 128 if (AHRS_setup()) 129 Serial.println("INFO: AHRS 130 setup ok"); 131 else { 132 Serial.println("ERROR AHRS setup failed. Halted!"); 133 134 while(true); 135 } 136 137/* 138 radio.begin(); //Starting 139 the Wireless communication 140 radio.openWritingPipe(address); //Setting the address 141 where we will send the data 142 radio.setPALevel(RF24_PA_MIN); //You can set it 143 as minimum or maximum depending on the distance between the transmitter and receiver. 144 145 radio.stopListening(); //This sets the module as transmitter 146 Serial.println("INFO: 147 Transmitter started"); 148*/ 149 150 if (!SD.begin(chipSelect)) { // 151 Setup SD Card 152 Serial.println("ERROR: SD Card failed or not present! Halted."); 153 154 while (1); 155 } 156 Serial.println("INFO: SD card initialized"); 157 seqnr 158 = 1; // Check on existing files 159 fileName = "DL" 160 + String(seqnr) + ".CSV"; 161 while(SD.exists(fileName)) { 162 seqnr++; 163 164 fileName = "DL" + String(seqnr) + ".CSV"; 165 delay(200); // Make sure 166 file access is done 167 } 168 dataFile = SD.open(fileName, FILE_WRITE); // Create 169 new CSV file with appropriate headers 170 dataFile.println("Nr,Mills,Date,UTC,Lat,Lon,Sog,Cog,Alt,Sat,Fix,Pm,Ax,Ay,Az,Gx,Gy,Gz,Mx,My,Mz,Pres,Pitch,Roll,Yaw"); 171 172 Serial.println("INFO: new data file created, ready for logging."); // Done 173 with SD Card Init 174 175 digitalWrite(LEDR, HIGH); // switch 176 buildin LED off 177 digitalWrite(LEDB, LOW); // switch buildin 178 LED to blue -> looking for satellites. 179 Serial.println("Initialization completed"); 180} 181 182void 183 loop() { // run continuously 184 myGPS.update(); 185 186 prev_satellites = satellites; 187 satellites = myGPS.getSIV().toInt(); 188 if 189 (satellites != prev_satellites) 190 Serial.println("SIV: " + String(satellites) 191 + " Pitch: " + String(pitch)); 192 193 if (satellites > 2) { // 194 return TRUE if fix 195 if (!fix) Serial.println("INFO: Fix created!"); 196 197 fix = true; 198 digitalWrite(LEDB, HIGH); // switch 199 buildin LED off 200 digitalWrite(LEDG, LOW); // switch 201 buildin LED to green -> (potential) fix. 202 if (first_fix) { 203 first_fix 204 = false; 205 start_loops = millis(); 206 count_loops = 0; 207 count_RMC 208 = 0; 209 count_GGA = 0; 210 } 211 } else { 212 if (fix) Serial.println("INFO: 213 Fix lost!"); 214 fix = false; 215 digitalWrite(LEDG, HIGH); // 216 switch buildin LED off 217 digitalWrite(LEDB, LOW); // 218 switch buildin LED to blue -> looking for satellites. 219 } 220 221 potmeter = 222 analogRead(7); // read value of potential meter via ADC 223 pin 7 224 getIMUData(); 225 pressure = BARO.readPressure(); 226 227 AHRS_update(); 228 229 230 // write line to datafile 231 if (dataFile && fix && millis() - last_write > 232 150) { 233 counter += 1; 234 dataString = createDataString(); 235 // dataString 236 = "dit is een lange test data string met een hele boel bytes"; 237 // Serial.print("INFO: 238 write to SD card: "); 239 // Serial.println(dataString); 240 dataFile.println(dataString); 241 242 byteCounter += dataString.length(); 243 if (byteCounter > 1024) { 244 // 245 Serial.println("INFO: flush buffer to SD card"); 246 dataFile.flush(); 247 248 byteCounter = 0; 249 } 250 } 251 252/* 253 if (fix && millis() - last_write 254 > 250) { // send data string 255 last_write = millis(); 256 counter += 257 1; 258 dataString = createDataString() + '\ 259'; 260 // Serial.print("INFO: 261 write to SD card: "); 262 for (int i = 0; i < dataString.length(); i+=32) { 263 264 String section = dataString.substring(i, i + 32); 265 section.toCharArray(tx_buffer, 266 sizeof(tx_buffer)); 267 radio.write(&tx_buffer, section.length()); //Sending 268 the message to receiver 269 } 270 } 271*/ 272 273 count_loops++; 274 if 275 (count_loops % 1000 == 0) { 276 Serial.println("loops: "+String(count_loops)+", 277 satellites: "+myGPS.getSIV()+", pitch: "+String(pitch)); 278 } 279 if (count_loops 280 % 10000 ==0) { 281 Serial.println("loops: "+String(count_loops)+", avg loop: 282 "+String((millis() - start_loops)/count_loops)+" ms, avg RMC: "+String((millis() 283 - start_loops)/count_RMC)+" ms, avg GGA: "+String((millis() - start_loops)/count_GGA)); 284 285 // radio.write(&ping, sizeof(ping)); //Sending the message to 286 receiver 287 } 288} 289 290boolean getIMUData() { 291 //Accelerometer values 292 293 if (IMU.accelerationAvailable()) { 294 IMU.readAcceleration(ax, ay, az); 295 296 } 297 //Gyroscope values 298 if (IMU.gyroscopeAvailable()) { 299 IMU.readGyroscope(gx, 300 gy, gz); 301 } 302 //Magnetometer values 303 if (IMU.magneticFieldAvailable()) 304 { 305 IMU.readMagneticField(mx, my, mz); 306 } 307} 308 309String createDataString() 310 { 311 String ds = ""; 312 313 ds += String(counter) + ","; 314 ds += String(millis()) 315 + ","; 316 ds += myGPS.getDAT() + ","; 317 ds += myGPS.getTIM() + ","; 318 319 ds += myGPS.getLAT() + ","; 320 ds += myGPS.getLON() + ","; 321 ds += myGPS.getSOG() 322 + ","; 323 ds += myGPS.getCOG() + ","; 324 ds += myGPS.getALT() + ","; 325 326 ds += myGPS.getSIV() + ","; 327 ds += myGPS.getFIX() + ","; 328 329 ds += 330 String(potmeter) + ","; 331 332 ds += String(ax) + ","; 333 ds += String(ay) 334 + ","; 335 ds += String(az) + ","; 336 ds += String(gx) + ","; 337 ds += 338 String(gy) + ","; 339 ds += String(gz) + ","; 340 ds += String(mx) + ","; 341 342 ds += String(my) + ","; 343 ds += String(mz) + ","; 344 345 ds += String(pressure) 346 + ","; 347 348 ds += String(pitch) + ","; 349 ds += String(roll) + ","; 350 351 ds += String(yaw); 352 353 return(ds); 354} 355
DataLogger repository
AHRS.h
c_cpp
Header file for the AHRS / Madgwick library
1/* LSM9DS1_MS5611_t3 Basic Example Code 2by: Kris Winer 3date: November 1, 2014 4license: Beerware - Use this code however you'd like. If you 5find it useful you can buy me a beer some time. 6Demonstrate basic LSM9DS1 functionality including parameterizing the register addresses, initializing the sensor, 7getting properly scaled accelerometer, gyroscope, and magnetometer data out. Added display functions to 8allow display to on breadboard monitor. Addition of 9 DoF sensor fusion using open source Madgwick and 9Mahony filter algorithms. Sketch runs on the 3.3 V 8 MHz Pro Mini and the Teensy 3.1. 10This sketch is intended specifically for the LSM9DS1+MS5611 Add-on shield for the Teensy 3.1. 11It uses SDA/SCL on pins 17/16, respectively, and it uses the Teensy 3.1-specific Wire library i2c_t3.h. 12The MS5611 is a simple but high resolution pressure sensor, which can be used in its high resolution 13mode but with power consumption od 20 microAmp, or in a lower resolution mode with power consumption of 14only 1 microAmp. The choice will depend on the application. 15SDA and SCL should have external pull-up resistors (to 3.3V). 164K7 resistors are on the LSM9DS1+MS5611 Teensy 3.1 add-on shield/breakout board. 17Hardware setup: 18LSM9DS1Breakout --------- Arduino 19VDD ---------------------- 3.3V 20VDDI --------------------- 3.3V 21SDA ----------------------- A4 22SCL ----------------------- A5 23GND ---------------------- GND 24Note: The LSM9DS1 is an I2C sensor and can use the Arduino Wire library. 25Because the sensor is not 5V tolerant, we are using either a 3.3 V 8 MHz Pro Mini or a 3.3 V Teensy 3.1. 26We have disabled the internal pull-ups used by the Wire library in the Wire.h/twi.c utility file. 27We are also using the 400 kHz fast I2C mode by setting the TWI_FREQ to 400000L /twi.h utility file. 28 29Modified by Ruud Kapteijn 3016-Jan-2021 31Moulded into class AHRS 32taken out NOKIA 5110 monochrome display code 33code: https://github.com/j-mcc1993/LSM9DS1/blob/master/LSM9DS1_BasicAHRS_Nano33.ino 34discussion threa: https://github.com/kriswiner/LSM9DS1/issues/14 35 */ 36 37// See MS5611-02BA03 Low Voltage Barometric Pressure Sensor Data Sheet 38#define MS5611_RESET 0x1E 39#define MS5611_CONVERT_D1 0x40 40#define MS5611_CONVERT_D2 0x50 41#define MS5611_ADC_READ 0x00 42 43// See also LSM9DS1 Register Map and Descriptions, http://www.st.com/st-web-ui/static/active/en/resource/technical/document/datasheet/DM00103319.pdf 44// Accelerometer and Gyroscope registers 45#define LSM9DS1XG_ACT_THS 0x04 46#define LSM9DS1XG_ACT_DUR 0x05 47#define LSM9DS1XG_INT_GEN_CFG_XL 0x06 48#define LSM9DS1XG_INT_GEN_THS_X_XL 0x07 49#define LSM9DS1XG_INT_GEN_THS_Y_XL 0x08 50#define LSM9DS1XG_INT_GEN_THS_Z_XL 0x09 51#define LSM9DS1XG_INT_GEN_DUR_XL 0x0A 52#define LSM9DS1XG_REFERENCE_G 0x0B 53#define LSM9DS1XG_INT1_CTRL 0x0C 54#define LSM9DS1XG_INT2_CTRL 0x0D 55#define LSM9DS1XG_WHO_AM_I 0x0F // should return 0x68 56#define LSM9DS1XG_CTRL_REG1_G 0x10 57#define LSM9DS1XG_CTRL_REG2_G 0x11 58#define LSM9DS1XG_CTRL_REG3_G 0x12 59#define LSM9DS1XG_ORIENT_CFG_G 0x13 60#define LSM9DS1XG_INT_GEN_SRC_G 0x14 61#define LSM9DS1XG_OUT_TEMP_L 0x15 62#define LSM9DS1XG_OUT_TEMP_H 0x16 63#define LSM9DS1XG_STATUS_REG 0x17 64#define LSM9DS1XG_OUT_X_L_G 0x18 65#define LSM9DS1XG_OUT_X_H_G 0x19 66#define LSM9DS1XG_OUT_Y_L_G 0x1A 67#define LSM9DS1XG_OUT_Y_H_G 0x1B 68#define LSM9DS1XG_OUT_Z_L_G 0x1C 69#define LSM9DS1XG_OUT_Z_H_G 0x1D 70#define LSM9DS1XG_CTRL_REG4 0x1E 71#define LSM9DS1XG_CTRL_REG5_XL 0x1F 72#define LSM9DS1XG_CTRL_REG6_XL 0x20 73#define LSM9DS1XG_CTRL_REG7_XL 0x21 74#define LSM9DS1XG_CTRL_REG8 0x22 75#define LSM9DS1XG_CTRL_REG9 0x23 76#define LSM9DS1XG_CTRL_REG10 0x24 77#define LSM9DS1XG_INT_GEN_SRC_XL 0x26 78//#define LSM9DS1XG_STATUS_REG 0x27 // duplicate of 0x17! 79#define LSM9DS1XG_OUT_X_L_XL 0x28 80#define LSM9DS1XG_OUT_X_H_XL 0x29 81#define LSM9DS1XG_OUT_Y_L_XL 0x2A 82#define LSM9DS1XG_OUT_Y_H_XL 0x2B 83#define LSM9DS1XG_OUT_Z_L_XL 0x2C 84#define LSM9DS1XG_OUT_Z_H_XL 0x2D 85#define LSM9DS1XG_FIFO_CTRL 0x2E 86#define LSM9DS1XG_FIFO_SRC 0x2F 87#define LSM9DS1XG_INT_GEN_CFG_G 0x30 88#define LSM9DS1XG_INT_GEN_THS_XH_G 0x31 89#define LSM9DS1XG_INT_GEN_THS_XL_G 0x32 90#define LSM9DS1XG_INT_GEN_THS_YH_G 0x33 91#define LSM9DS1XG_INT_GEN_THS_YL_G 0x34 92#define LSM9DS1XG_INT_GEN_THS_ZH_G 0x35 93#define LSM9DS1XG_INT_GEN_THS_ZL_G 0x36 94#define LSM9DS1XG_INT_GEN_DUR_G 0x37 95// 96// Magnetometer registers 97#define LSM9DS1M_OFFSET_X_REG_L_M 0x05 98#define LSM9DS1M_OFFSET_X_REG_H_M 0x06 99#define LSM9DS1M_OFFSET_Y_REG_L_M 0x07 100#define LSM9DS1M_OFFSET_Y_REG_H_M 0x08 101#define LSM9DS1M_OFFSET_Z_REG_L_M 0x09 102#define LSM9DS1M_OFFSET_Z_REG_H_M 0x0A 103#define LSM9DS1M_WHO_AM_I 0x0F // should be 0x3D 104#define LSM9DS1M_CTRL_REG1_M 0x20 105#define LSM9DS1M_CTRL_REG2_M 0x21 106#define LSM9DS1M_CTRL_REG3_M 0x22 107#define LSM9DS1M_CTRL_REG4_M 0x23 108#define LSM9DS1M_CTRL_REG5_M 0x24 109#define LSM9DS1M_STATUS_REG_M 0x27 110#define LSM9DS1M_OUT_X_L_M 0x28 111#define LSM9DS1M_OUT_X_H_M 0x29 112#define LSM9DS1M_OUT_Y_L_M 0x2A 113#define LSM9DS1M_OUT_Y_H_M 0x2B 114#define LSM9DS1M_OUT_Z_L_M 0x2C 115#define LSM9DS1M_OUT_Z_H_M 0x2D 116#define LSM9DS1M_INT_CFG_M 0x30 117#define LSM9DS1M_INT_SRC_M 0x31 118#define LSM9DS1M_INT_THS_L_M 0x32 119#define LSM9DS1M_INT_THS_H_M 0x33 120 121// Using the LSM9DS1+MS5611 Teensy 3.1 Add-On shield, ADO is set to 1 122// Seven-bit device address of accel/gyro is 110101 for ADO = 0 and 110101 for ADO = 1 123#define ADO 1 124#if ADO 125#define LSM9DS1XG_ADDRESS 0x6B // Device address when ADO = 1 126#define LSM9DS1M_ADDRESS 0x1E // Address of magnetometer 127#define MS5611_ADDRESS 0x77 // Address of altimeter 128#else 129#define LSM9DS1XG_ADDRESS 0x6A // Device address when ADO = 0 130#define LSM9DS1M_ADDRESS 0x1D // Address of magnetometer 131#define MS5611_ADDRESS 0x77 // Address of altimeter 132#endif 133 134#define SerialDebug true // set to true to get Serial output for debugging 135 136// Set initial input parameters 137enum Ascale { // set of allowable accel full scale settings 138 AFS_2G = 0, 139 AFS_16G, 140 AFS_4G, 141 AFS_8G 142}; 143 144enum Aodr { // set of allowable gyro sample rates 145 AODR_PowerDown = 0, 146 AODR_10Hz, 147 AODR_50Hz, 148 AODR_119Hz, 149 AODR_238Hz, 150 AODR_476Hz, 151 AODR_952Hz 152}; 153 154enum Abw { // set of allowable accewl bandwidths 155 ABW_408Hz = 0, 156 ABW_211Hz, 157 ABW_105Hz, 158 ABW_50Hz 159}; 160 161enum Gscale { // set of allowable gyro full scale settings 162 GFS_245DPS = 0, 163 GFS_500DPS, 164 GFS_NoOp, 165 GFS_2000DPS 166}; 167 168enum Godr { // set of allowable gyro sample rates 169 GODR_PowerDown = 0, 170 GODR_14_9Hz, 171 GODR_59_5Hz, 172 GODR_119Hz, 173 GODR_238Hz, 174 GODR_476Hz, 175 GODR_952Hz 176}; 177 178enum Gbw { // set of allowable gyro data bandwidths 179 GBW_low = 0, // 14 Hz at Godr = 238 Hz, 33 Hz at Godr = 952 Hz 180 GBW_med, // 29 Hz at Godr = 238 Hz, 40 Hz at Godr = 952 Hz 181 GBW_high, // 63 Hz at Godr = 238 Hz, 58 Hz at Godr = 952 Hz 182 GBW_highest // 78 Hz at Godr = 238 Hz, 100 Hz at Godr = 952 Hz 183}; 184 185enum Mscale { // set of allowable mag full scale settings 186 MFS_4G = 0, 187 MFS_8G, 188 MFS_12G, 189 MFS_16G 190}; 191 192enum Mmode { 193 MMode_LowPower = 0, 194 MMode_MedPerformance, 195 MMode_HighPerformance, 196 MMode_UltraHighPerformance 197}; 198 199enum Modr { // set of allowable mag sample rates 200 MODR_0_625Hz = 0, 201 MODR_1_25Hz, 202 MODR_2_5Hz, 203 MODR_5Hz, 204 MODR_10Hz, 205 MODR_20Hz, 206 MODR_80Hz 207}; 208 209#define ADC_256 0x00 // define pressure and temperature conversion rates 210#define ADC_512 0x02 211#define ADC_1024 0x04 212#define ADC_2048 0x06 213#define ADC_4096 0x08 214#define ADC_D1 0x40 215#define ADC_D2 0x50 216 217// Specify sensor full scale 218extern uint8_t OSR; // set pressure amd temperature oversample rate 219extern uint8_t Gscale; // gyro full scale 220extern uint8_t Godr; // gyro data sample rate 221extern uint8_t Gbw; // gyro data bandwidth 222extern uint8_t Ascale; // accel full scale 223extern uint8_t Aodr; // accel data sample rate 224extern uint8_t Abw; // accel data bandwidth 225extern uint8_t Mscale; // mag full scale 226extern uint8_t Modr; // mag data sample rate 227extern uint8_t Mmode; // magnetometer operation mode 228extern float aRes, gRes, mRes; // scale resolutions per LSB for the sensors 229 230// Pin definitions 231extern int myLed; 232 233extern uint16_t Pcal[8]; // calibration constants from MS5611 PROM registers 234extern unsigned char nCRC; // calculated check sum to ensure PROM integrity 235//-(rk) uint32_t D1 = 0, D2 = 0; // raw MS5611 pressure and temperature data 236extern double dT, OFFSET, SENS, T2, OFFSET2, SENS2; // First order and second order corrections for raw S5637 temperature and pressure data 237extern int16_t accelCount[3], gyroCount[3], magCount[3]; // Stores the 16-bit signed accelerometer, gyro, and mag sensor output 238extern float gyroBias[3], accelBias[3], magBias[3]; // Bias corrections for gyro, accelerometer, and magnetometer 239extern int16_t tempCount; // temperature raw count output 240extern float temperature; // Stores the LSM9DS1gyro internal chip temperature in degrees Celsius 241extern double Temperature, Pressure; // stores MS5611 pressures sensor pressure and temperature 242 243// global constants for 9 DoF fusion and AHRS (Attitude and Heading Reference System) 244extern float GyroMeasError; // gyroscope measurement error in rads/s (start at 40 deg/s) 245extern float GyroMeasDrift; // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s) 246// There is a tradeoff in the beta parameter between accuracy and response speed. 247// In the original Madgwick study, beta of 0.041 (corresponding to GyroMeasError of 2.7 degrees/s) was found to give optimal accuracy. 248// However, with this value, the LSM9SD0 response time is about 10 seconds to a stable initial quaternion. 249// Subsequent changes also require a longish lag time to a stable output, not fast enough for a quadcopter or robot car! 250// By increasing beta (GyroMeasError) by about a factor of fifteen, the response time constant is reduced to ~2 sec 251// I haven't noticed any reduction in solution accuracy. This is essentially the I coefficient in a PID control sense; 252// the bigger the feedback coefficient, the faster the solution converges, usually at the expense of accuracy. 253// In any case, this is the free parameter in the Madgwick filtering and fusion scheme. 254extern float beta; // compute beta 255extern float zeta; // compute zeta, the other free parameter in the Madgwick scheme usually set to a small or zero value 256#define Kp 2.0f * 5.0f // these are the free parameters in the Mahony filter and fusion scheme, Kp for proportional feedback, Ki for integral 257#define Ki 0.0f 258 259extern uint32_t delt_t, count, sumCount; // used to control display output rate 260extern float pitch, yaw, roll; 261extern float deltat, sum; // integration interval for both filter schemes 262extern uint32_t lastUpdate, firstUpdate; // used to calculate integration interval 263extern uint32_t Now; // used to calculate integration interval 264 265extern float ax, ay, az, gx, gy, gz, mx, my, mz; // variables to hold latest sensor data values 266extern float q[4]; // vector to hold quaternion 267extern float eInt[3]; // vector to hold integral error for Mahony method 268 269//-(rk) Prototypes of AHRS.cpp 270void getMres(); 271void getGres(); 272void getAres(); 273void readAccelData(int16_t * destination); 274void readGyroData(int16_t * destination); 275void readMagData(int16_t * destination); 276int16_t readTempData(); 277void initLSM9DS1(); 278void selftestLSM9DS1(); 279void accelgyrocalLSM9DS1(float * dest1, float * dest2); 280void magcalLSM9DS1(float * dest1); 281void writeByte(uint8_t address, uint8_t subAddress, uint8_t data); 282uint8_t readByte(uint8_t address, uint8_t subAddress); 283void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest); 284void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz); 285 286boolean AHRS_setup(); 287void AHRS_update(); 288
AHRS_gvars.h
c_cpp
Global variable for AHRS algoritm
1uint8_t OSR = ADC_4096; // set pressure amd temperature oversample rate 2uint8_t Gscale = GFS_245DPS; // gyro full scale 3uint8_t Godr = GODR_238Hz; // gyro data sample rate 4uint8_t Gbw = GBW_med; // gyro data bandwidth 5uint8_t Ascale = AFS_2G; // accel full scale 6uint8_t Aodr = AODR_238Hz; // accel data sample rate 7uint8_t Abw = ABW_50Hz; // accel data bandwidth 8uint8_t Mscale = MFS_4G; // mag full scale 9uint8_t Modr = MODR_10Hz; // mag data sample rate 10uint8_t Mmode = MMode_HighPerformance; // magnetometer operation mode 11float aRes, gRes, mRes; // scale resolutions per LSB for the sensors 12int myLed = 13; 13uint16_t Pcal[8]; // calibration constants from MS5611 PROM registers 14unsigned char nCRC; // calculated check sum to ensure PROM integrity 15//-(rk) uint32_t D1 = 0, D2 = 0; // raw MS5611 pressure and temperature data 16double dT, OFFSET, SENS, T2, OFFSET2, SENS2; // First order and second order corrections for raw S5637 temperature and pressure data 17int16_t accelCount[3], gyroCount[3], magCount[3]; // Stores the 16-bit signed accelerometer, gyro, and mag sensor output 18float gyroBias[3] = {0, 0, 0}, accelBias[3] = {0, 0, 0}, magBias[3] = {0, 0, 0}; // Bias corrections for gyro, accelerometer, and magnetometer 19int16_t tempCount; // temperature raw count output 20float temperature; // Stores the LSM9DS1gyro internal chip temperature in degrees Celsius 21double Temperature, Pressure; // stores MS5611 pressures sensor pressure and temperature 22 23// global constants for 9 DoF fusion and AHRS (Attitude and Heading Reference System) 24float GyroMeasError = PI * (40.0f / 180.0f); // gyroscope measurement error in rads/s (start at 40 deg/s) 25float GyroMeasDrift = PI * (0.0f / 180.0f); // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s) 26// There is a tradeoff in the beta parameter between accuracy and response speed. 27// In the original Madgwick study, beta of 0.041 (corresponding to GyroMeasError of 2.7 degrees/s) was found to give optimal accuracy. 28// However, with this value, the LSM9SD0 response time is about 10 seconds to a stable initial quaternion. 29// Subsequent changes also require a longish lag time to a stable output, not fast enough for a quadcopter or robot car! 30// By increasing beta (GyroMeasError) by about a factor of fifteen, the response time constant is reduced to ~2 sec 31// I haven't noticed any reduction in solution accuracy. This is essentially the I coefficient in a PID control sense; 32// the bigger the feedback coefficient, the faster the solution converges, usually at the expense of accuracy. 33// In any case, this is the free parameter in the Madgwick filtering and fusion scheme. 34float beta = sqrt(3.0f / 4.0f) * GyroMeasError; // compute beta 35float zeta = sqrt(3.0f / 4.0f) * GyroMeasDrift; // compute zeta, the other free parameter in the Madgwick scheme usually set to a small or zero value 36#define Kp 2.0f * 5.0f // these are the free parameters in the Mahony filter and fusion scheme, Kp for proportional feedback, Ki for integral 37#define Ki 0.0f 38 39uint32_t delt_t = 0, count = 0, sumCount = 0; // used to control display output rate 40float pitch, yaw, roll; 41float deltat = 0.0f, sum = 0.0f; // integration interval for both filter schemes 42uint32_t lastUpdate = 0, firstUpdate = 0; // used to calculate integration interval 43uint32_t Now = 0; // used to calculate integration interval 44 45float ax, ay, az, gx, gy, gz, mx, my, mz; // variables to hold latest sensor data values 46float q[4] = {1.0f, 0.0f, 0.0f, 0.0f}; // vector to hold quaternion 47float eInt[3] = {0.0f, 0.0f, 0.0f}; // vector to hold integral error for Mahony method 48
DataLogger.ino
c_cpp
Main Adruino program for Arduino Nano 33 BLE Sense
1/**************************************************************************** 2 * Data Logger v30 3 * 28-Jan-21 4 * Ruud Kapteijn 5 * Available under GPL3+ 6 * 7 * Uses the AHRS Madgwick code from Kris Winer (ref AHRS.h) 8 * Original code by Kris Winer 9 * date: Nov 1, 2014 10 * code: https://github.com/j-mcc1993/LSM9DS1/blob/master/LSM9DS1_BasicAHRS_Nano33.ino 11 * discussion threa: https://github.com/kriswiner/LSM9DS1/issues/14 12 */ 13#include <Arduino_LSM9DS1.h> 14#include <Arduino_LPS22HB.h> //Include library to read Pressure 15#include <SPI.h> 16#include <SD.h> 17// #include <RF24.h> 18 19#include "Ublox_NMEA_GPS.h" 20#include "AHRS.h" 21#include "AHRS_gvars.h" 22 23#define LEDR (22u) 24#define LEDG (23u) 25#define LEDB (24u) 26#define LEDPWR (25u) 27 28// GPS Vars 29Ublox_NMEA_GPS myGPS; 30boolean fix = false; 31int satellites, prev_satellites; 32 33// potentionmeter 34int potmeter = 0; 35 36// IMU Vars 37// float ax, ay, az; 38// float gx, gy, gz; 39// float mx, my, mz; 40 41// Baro vars 42float pressure; 43 44// SD Vars 45File dataFile; 46int chipSelect = 10; 47int seqnr = 0; 48String fileName; 49int counter = 0; 50int byteCounter = 0; 51String dataString; 52long last_write = 0; 53 54/* 55// transmitter vars 56RF24 radio(9, 10); // CE, CSN 57const byte address[6] = "00001"; //Byte of array representing the address. This is the address where we will send the data. This should be same on the receiving side. 58long last_write = 0; 59char tx_buffer[256]; 60String dataString; 61int counter = 0; 62int stringLength = 0; 63const char ping[] = "ping"; 64*/ 65 66// statistic vars 67boolean first_fix = true; 68long start_loops; 69long count_loops=0; 70long count_RMC = 0; 71long count_GGA = 0; 72 73void setup() { // switch buildin LED off 74 digitalWrite(LEDPWR, HIGH); digitalWrite(LEDR, HIGH); digitalWrite(LEDG, HIGH); digitalWrite(LEDB, HIGH); 75 digitalWrite(LEDR, LOW); // switch buildin LED to red -> not initialized. 76 77 Serial.begin(9600); // Open serial communications and wait for port to open: 78 // while (!Serial); // wait until monitor is opened 79 Serial.println("INFO: ** Telemetry V10 **"); 80 81 myGPS.init(); 82 Serial.println("INFO: GPS Started"); 83 84 if (!IMU.begin()) { 85 Serial.println("ERROR: Failed to initialize IMU! halted."); 86 while (true); 87 } 88 Serial.println("INFO: IMU Started!"); 89 90 if (!BARO.begin()) { //Initialize Pressure sensor 91 Serial.println("ERROR: Failed to initialize Pressure Sensor! halted."); 92 while (1); 93 } 94 Serial.println("INFO: BARO Started!"); 95 96 if (AHRS_setup()) 97 Serial.println("INFO: AHRS setup ok"); 98 else { 99 Serial.println("ERROR AHRS setup failed. Halted!"); 100 while(true); 101 } 102 103/* 104 radio.begin(); //Starting the Wireless communication 105 radio.openWritingPipe(address); //Setting the address where we will send the data 106 radio.setPALevel(RF24_PA_MIN); //You can set it as minimum or maximum depending on the distance between the transmitter and receiver. 107 radio.stopListening(); //This sets the module as transmitter 108 Serial.println("INFO: Transmitter started"); 109*/ 110 111 if (!SD.begin(chipSelect)) { // Setup SD Card 112 Serial.println("ERROR: SD Card failed or not present! Halted."); 113 while (1); 114 } 115 Serial.println("INFO: SD card initialized"); 116 seqnr = 1; // Check on existing files 117 fileName = "DL" + String(seqnr) + ".CSV"; 118 while(SD.exists(fileName)) { 119 seqnr++; 120 fileName = "DL" + String(seqnr) + ".CSV"; 121 delay(200); // Make sure file access is done 122 } 123 dataFile = SD.open(fileName, FILE_WRITE); // Create new CSV file with appropriate headers 124 dataFile.println("Nr,Mills,Date,UTC,Lat,Lon,Sog,Cog,Alt,Sat,Fix,Pm,Ax,Ay,Az,Gx,Gy,Gz,Mx,My,Mz,Pres,Pitch,Roll,Yaw"); 125 Serial.println("INFO: new data file created, ready for logging."); // Done with SD Card Init 126 127 digitalWrite(LEDR, HIGH); // switch buildin LED off 128 digitalWrite(LEDB, LOW); // switch buildin LED to blue -> looking for satellites. 129 Serial.println("Initialization completed"); 130} 131 132void loop() { // run continuously 133 myGPS.update(); 134 prev_satellites = satellites; 135 satellites = myGPS.getSIV().toInt(); 136 if (satellites != prev_satellites) 137 Serial.println("SIV: " + String(satellites) + " Pitch: " + String(pitch)); 138 139 if (satellites > 2) { // return TRUE if fix 140 if (!fix) Serial.println("INFO: Fix created!"); 141 fix = true; 142 digitalWrite(LEDB, HIGH); // switch buildin LED off 143 digitalWrite(LEDG, LOW); // switch buildin LED to green -> (potential) fix. 144 if (first_fix) { 145 first_fix = false; 146 start_loops = millis(); 147 count_loops = 0; 148 count_RMC = 0; 149 count_GGA = 0; 150 } 151 } else { 152 if (fix) Serial.println("INFO: Fix lost!"); 153 fix = false; 154 digitalWrite(LEDG, HIGH); // switch buildin LED off 155 digitalWrite(LEDB, LOW); // switch buildin LED to blue -> looking for satellites. 156 } 157 158 potmeter = analogRead(7); // read value of potential meter via ADC pin 7 159 getIMUData(); 160 pressure = BARO.readPressure(); 161 162 AHRS_update(); 163 164 // write line to datafile 165 if (dataFile && fix && millis() - last_write > 150) { 166 counter += 1; 167 dataString = createDataString(); 168 // dataString = "dit is een lange test data string met een hele boel bytes"; 169 // Serial.print("INFO: write to SD card: "); 170 // Serial.println(dataString); 171 dataFile.println(dataString); 172 byteCounter += dataString.length(); 173 if (byteCounter > 1024) { 174 // Serial.println("INFO: flush buffer to SD card"); 175 dataFile.flush(); 176 byteCounter = 0; 177 } 178 } 179 180/* 181 if (fix && millis() - last_write > 250) { // send data string 182 last_write = millis(); 183 counter += 1; 184 dataString = createDataString() + '\n'; 185 // Serial.print("INFO: write to SD card: "); 186 for (int i = 0; i < dataString.length(); i+=32) { 187 String section = dataString.substring(i, i + 32); 188 section.toCharArray(tx_buffer, sizeof(tx_buffer)); 189 radio.write(&tx_buffer, section.length()); //Sending the message to receiver 190 } 191 } 192*/ 193 194 count_loops++; 195 if (count_loops % 1000 == 0) { 196 Serial.println("loops: "+String(count_loops)+", satellites: "+myGPS.getSIV()+", pitch: "+String(pitch)); 197 } 198 if (count_loops % 10000 ==0) { 199 Serial.println("loops: "+String(count_loops)+", avg loop: "+String((millis() - start_loops)/count_loops)+" ms, avg RMC: "+String((millis() - start_loops)/count_RMC)+" ms, avg GGA: "+String((millis() - start_loops)/count_GGA)); 200 // radio.write(&ping, sizeof(ping)); //Sending the message to receiver 201 } 202} 203 204boolean getIMUData() { 205 //Accelerometer values 206 if (IMU.accelerationAvailable()) { 207 IMU.readAcceleration(ax, ay, az); 208 } 209 //Gyroscope values 210 if (IMU.gyroscopeAvailable()) { 211 IMU.readGyroscope(gx, gy, gz); 212 } 213 //Magnetometer values 214 if (IMU.magneticFieldAvailable()) { 215 IMU.readMagneticField(mx, my, mz); 216 } 217} 218 219String createDataString() { 220 String ds = ""; 221 222 ds += String(counter) + ","; 223 ds += String(millis()) + ","; 224 ds += myGPS.getDAT() + ","; 225 ds += myGPS.getTIM() + ","; 226 ds += myGPS.getLAT() + ","; 227 ds += myGPS.getLON() + ","; 228 ds += myGPS.getSOG() + ","; 229 ds += myGPS.getCOG() + ","; 230 ds += myGPS.getALT() + ","; 231 ds += myGPS.getSIV() + ","; 232 ds += myGPS.getFIX() + ","; 233 234 ds += String(potmeter) + ","; 235 236 ds += String(ax) + ","; 237 ds += String(ay) + ","; 238 ds += String(az) + ","; 239 ds += String(gx) + ","; 240 ds += String(gy) + ","; 241 ds += String(gz) + ","; 242 ds += String(mx) + ","; 243 ds += String(my) + ","; 244 ds += String(mz) + ","; 245 246 ds += String(pressure) + ","; 247 248 ds += String(pitch) + ","; 249 ds += String(roll) + ","; 250 ds += String(yaw); 251 252 return(ds); 253} 254
Downloadable files
Wiring diagram
Shows the connections on the breadboard
Wiring diagram
Wiring diagram
Shows the connections on the breadboard
Wiring diagram
Comments
Only logged in users can leave comments
rkapteyn
0 Followers
•0 Projects
Table of contents
Intro
6
0