# HG changeset patch # User heinrichsweikamp # Date 1574530790 -3600 # Node ID c3d511365552ff0166bd1cf06c28407a95bdc3f4 # Parent cb3870f79e9d449f8d5c76e432a7e8b6e5b56dc0 Add Support for new end-2019 hardware: support LSM303AGR compass (Not yet working!) cleanup compass code a bit diff -r cb3870f79e9d -r c3d511365552 Small_CPU/Inc/compass_LSM303D.h --- a/Small_CPU/Inc/compass_LSM303D.h Sat Nov 23 15:36:38 2019 +0100 +++ b/Small_CPU/Inc/compass_LSM303D.h Sat Nov 23 18:39:50 2019 +0100 @@ -27,7 +27,7 @@ /* Exported constants --------------------------------------------------------*/ #define WHO_AM_I 0x0F // device identification register - default value -#define WHOIAM_VALUE (0x49) // Who Am I default value +#define WHOIAM_VALUE_LSM303D 0x49 // Who Am I default value #define ADDR_OUT_TEMP_L 0x05 #define ADDR_OUT_TEMP_H 0x06 diff -r cb3870f79e9d -r c3d511365552 Small_CPU/Inc/i2c.h --- a/Small_CPU/Inc/i2c.h Sat Nov 23 15:36:38 2019 +0100 +++ b/Small_CPU/Inc/i2c.h Sat Nov 23 18:39:50 2019 +0100 @@ -3,15 +3,25 @@ #define I2C_H /* Pressure Sensor */ -#define DEVICE_PRESSURE_MS5803 0xEE // gen 1 and gen 2 use 0xEE (MS5803) -#define DEVICE_PRESSURE_MS5837 0xEC // end-2019 hardware (gen 3) uses 0xEC (MS5837) +#define DEVICE_PRESSURE_MS5803 0xEE // gen 1 and gen 2 use 0xEE (MS5803) +#define DEVICE_PRESSURE_MS5837 0xEC // end-2019 hardware (gen 3) uses 0xEC (MS5837) /* Compass/Accelerometer */ +#define COMPASS_NOT_RECOGNIZED 0xAA ///< id used with hardwareCompass +#define compass_generation1 0x01 // Hardware gen 1 (Two chip solution with MMA8452Q and HMC5883L) +#define compass_generation2 0x02 // Hardware gen 2 (Single chip solution LSM303D) +#define compass_generation3 0x03 // Hardware gen 3 (Single chip solution LSM303AGR) + #define DEVICE_ACCELARATOR_MMA8452Q 0x38 // Hardware gen 1 (Two chip solution with MMA8452Q and HMC5883L) -#define DEVICE_COMPASS_HMC5883L 0x3C // Hardware gen 1 +#define DEVICE_COMPASS_HMC5883L 0x3C // Hardware gen 1 (Two chip solution with MMA8452Q and HMC5883L) #define DEVICE_COMPASS_303D 0x3C // Hardware gen 2 (Single chip solution LSM303D) + #define DEVICE_COMPASS_303AGR 0x3C // Hardware gen 3 (Single chip solution LSM303AGR) +#define DEVICE_ACCELARATOR_303AGR 0x32 // Hardware gen 3 (Single chip solution LSM303AGR) + +// Compass 3 defines (Can move in separate .h file...) +#define WHOIAM_VALUE_LSM303AGR 0x33 /* Battery Gas Gauge */ #define DEVICE_BATTERYGAUGE 0xC8 // LTC2941 battery gauge diff -r cb3870f79e9d -r c3d511365552 Small_CPU/Src/compass.c --- a/Small_CPU/Src/compass.c Sat Nov 23 15:36:38 2019 +0100 +++ b/Small_CPU/Src/compass.c Sat Nov 23 18:39:50 2019 +0100 @@ -109,9 +109,6 @@ #define Q_PI (18000) #define Q_PIO2 (9000) -#define HMC5883L (1) ///< id used with hardwareCompass -#define LSM303D (2) ///< id used with hardwareCompass -#define COMPASS_NOT_RECOGNIZED (4) ///< id used with hardwareCompass ////////////////////////////////////////////////////////////////////////////// @@ -154,24 +151,12 @@ uint8_t compass_gain; ///< 7 on start, can be reduced during calibration -uint8_t hardwareCompass = 0; ///< either HMC5883L or LSM303D or not defined yet ( = 0 ) +uint8_t hardwareCompass = 0; ///< either HMC5883L (=1) or LSM303D (=2) or LSM303AGR (=3) or not defined yet (=0) /// LSM303D variables uint8_t magDataBuffer[6]; ///< here raw data from LSM303D is stored, can be local uint8_t accDataBuffer[6]; ///< here raw data from LSM303D is stored, can be local -//uint16_t velMag = 0; -//uint16_t velAcc = 0; - -//uint16_t magODR[] = {31,62,125,250,500,1000,2000}; -//uint16_t accODR[] = {0,31,62,125,250,500,1000,2000,4000,8000,16000}; -//uint8_t fastest = 10; //no sensor is the fastest -//uint8_t datas1 = 0; -//uint8_t zoffFlag = 0; -//uint8_t sendFlag = 0; - - -// all by pixhawk code: // struct accel_scale _accel_scale; unsigned _accel_range_m_s2; @@ -223,6 +208,11 @@ void compass_read_LSM303D(void); void acceleration_read_LSM303D(void); +void compass_init_LSM303AGR(uint8_t fast, uint8_t gain); +void compass_sleep_LSM303AGR(void); +void compass_read_LSM303AGR(void); +void acceleration_read_LSM303AGR(void); + int LSM303D_accel_set_onchip_lowpass_filter_bandwidth(unsigned bandwidth); int compass_calib_common(void); @@ -265,10 +255,12 @@ uint8_t data = WHO_AM_I; I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1); I2C_Master_Receive( DEVICE_COMPASS_303D, &data, 1); - if(data == WHOIAM_VALUE) - hardwareCompass = LSM303D; + if(data == WHOIAM_VALUE_LSM303D) + hardwareCompass = compass_generation2; //LSM303D; else - hardwareCompass = HMC5883L; + hardwareCompass = compass_generation1; //HMC5883L + if(data == WHOIAM_VALUE_LSM303AGR) + hardwareCompass = compass_generation3; //LSM303AGR; } /* No compass identified => Retry */ @@ -277,26 +269,28 @@ uint8_t data = WHO_AM_I; I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1); I2C_Master_Receive( DEVICE_COMPASS_303D, &data, 1); - if(data == WHOIAM_VALUE) - hardwareCompass = LSM303D; + if(data == WHOIAM_VALUE_LSM303D) + hardwareCompass = compass_generation2; //LSM303D; else - hardwareCompass = HMC5883L; + hardwareCompass = compass_generation1; //HMC5883L; + if(data == WHOIAM_VALUE_LSM303AGR) + hardwareCompass = compass_generation3; //LSM303AGR; } /* Assume that a HMC5883L is equipped by default if detection still failed */ if(hardwareCompass == 0) - hardwareCompass = HMC5883L; + hardwareCompass = compass_generation1; //HMC5883L; #ifdef TEST_IF_HMC5883L HAL_StatusTypeDef resultOfOperationHMC_MMA = HAL_TIMEOUT; - if(hardwareCompass == HMC5883L) + if(hardwareCompass == compass_generation1) // HMC5883L) { uint8_t data = 0x2A; // CTRL_REG1 of DEVICE_ACCELARATOR_MMA8452Q resultOfOperationHMC_MMA = I2C_Master_Transmit( DEVICE_ACCELARATOR_MMA8452Q, &data, 1); if(resultOfOperationHMC_MMA == HAL_OK) { - hardwareCompass = HMC5883L; // all fine, keep it + hardwareCompass = compass_generation1; //HMC5883L; // all fine, keep it } else { @@ -306,18 +300,15 @@ } #endif - if(hardwareCompass == LSM303D) - { + if(hardwareCompass == compass_generation2) //LSM303D) compass_init_LSM303D(fast, gain); - } - else - if(hardwareCompass == HMC5883L) - { + if(hardwareCompass == compass_generation3) //LSM303AGR) + compass_init_LSM303AGR(fast, gain); + if(hardwareCompass == compass_generation1) //HMC5883L) compass_init_HMC5883L(fast, gain); - } - tfull32 dataBlock[4]; - if(BFA_readLastDataBlock((uint32_t *)dataBlock) == BFA_OK) + tfull32 dataBlock[4]; + if(BFA_readLastDataBlock((uint32_t *)dataBlock) == BFA_OK) { compass_CX_f = dataBlock[0].Word16.low16; compass_CY_f = dataBlock[0].Word16.hi16; @@ -333,7 +324,7 @@ // =============================================================================== int compass_calib(void) { - if(hardwareCompass == LSM303D) + if(hardwareCompass == compass_generation2) //LSM303D) { LSM303D_accel_set_onchip_lowpass_filter_bandwidth(773); int out = compass_calib_common(); @@ -341,7 +332,12 @@ return out; } else - if(hardwareCompass == HMC5883L) + if(hardwareCompass == compass_generation1) //HMC5883L) + { + return compass_calib_common(); + } + else + if(hardwareCompass == compass_generation3) //LSM303AGR) { return compass_calib_common(); } @@ -360,12 +356,12 @@ // =============================================================================== void compass_sleep(void) { - if(hardwareCompass == LSM303D) + if(hardwareCompass == compass_generation2) //LSM303D) { compass_sleep_LSM303D(); } else - if(hardwareCompass == HMC5883L) + if(hardwareCompass == compass_generation1) //HMC5883L) { compass_sleep_HMC5883L(); } @@ -379,15 +375,13 @@ // =============================================================================== void compass_read(void) { - if(hardwareCompass == LSM303D) - { + if(hardwareCompass == compass_generation2) //LSM303D) compass_read_LSM303D(); - } - else - if(hardwareCompass == HMC5883L) - { + if(hardwareCompass == compass_generation1) //HMC5883L) compass_read_HMC5883L(); - } + if(hardwareCompass == compass_generation3) //LSM303AGR) + compass_read_LSM303AGR(); + } @@ -397,7 +391,7 @@ // =============================================================================== void accelerator_init(void) { - if(hardwareCompass == HMC5883L) + if(hardwareCompass == compass_generation1) //HMC5883L) accelerator_init_MMA8452Q(); } @@ -408,7 +402,7 @@ // =============================================================================== void accelerator_sleep(void) { - if(hardwareCompass == HMC5883L) + if(hardwareCompass == compass_generation1) //HMC5883L) accelerator_sleep_MMA8452Q(); } @@ -419,23 +413,73 @@ // =============================================================================== void acceleration_read(void) { - if(hardwareCompass == LSM303D) - { + if(hardwareCompass == compass_generation2) //LSM303D) acceleration_read_LSM303D(); - } - else - if(hardwareCompass == HMC5883L) - { + if(hardwareCompass == compass_generation1) //HMC5883L) acceleration_read_MMA8452Q(); - } + if(hardwareCompass == compass_generation3) //LSM303AGR) + acceleration_read_LSM303AGR(); } /* Private functions ---------------------------------------------------------*/ // =============================================================================== +// LSM303AGR_read_reg +// =============================================================================== +uint8_t LSM303AGR_read_reg(uint8_t addr) +{ + uint8_t data; + + I2C_Master_Transmit( DEVICE_COMPASS_303AGR, &addr, 1); + I2C_Master_Receive( DEVICE_COMPASS_303AGR, &data, 1); + return data; +} + + +// =============================================================================== +// LSM303AGR_write_reg +// =============================================================================== +void LSM303AGR_write_reg(uint8_t addr, uint8_t value) +{ + uint8_t data[2]; + + data[0] = addr; + data[1] = value; + I2C_Master_Transmit( DEVICE_COMPASS_303AGR, data, 2); +} + +// =============================================================================== +// LSM303AGR_acc_write_reg +// =============================================================================== +void LSM303AGR_acc_write_reg(uint8_t addr, uint8_t value) +{ + uint8_t data[2]; + + data[0] = addr; + data[1] = value; + I2C_Master_Transmit( DEVICE_ACCELARATOR_303AGR, data, 2); +} + + +// =============================================================================== +// LSM303AGR_write_checked_reg +// =============================================================================== +void LSM303AGR_write_checked_reg(uint8_t addr, uint8_t value) +{ + LSM303AGR_write_reg(addr, value); +} + +// =============================================================================== +// LSM303AGR_acc_write_checked_reg +// =============================================================================== +void LSM303AGR_acc_write_checked_reg(uint8_t addr, uint8_t value) +{ + LSM303AGR_acc_write_reg(addr, value); +} + +// =============================================================================== // LSM303D_read_reg -/// @brief tiny helpers by pixhawk // =============================================================================== uint8_t LSM303D_read_reg(uint8_t addr) { @@ -449,7 +493,6 @@ // =============================================================================== // LSM303D_write_reg -/// @brief tiny helpers by pixhawk // =============================================================================== void LSM303D_write_reg(uint8_t addr, uint8_t value) { @@ -464,7 +507,6 @@ // =============================================================================== // LSM303D_write_checked_reg -/// @brief tiny helpers by pixhawk. This runs unchecked at the moment. // =============================================================================== void LSM303D_write_checked_reg(uint8_t addr, uint8_t value) { @@ -474,7 +516,6 @@ // =============================================================================== // LSM303D_modify_reg -/// @brief tiny helpers by pixhawk // =============================================================================== void LSM303D_modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) { @@ -486,108 +527,8 @@ LSM303D_write_checked_reg(reg, val); } -/* -// =============================================================================== -// LSM303D_accel_set_range -/// @brief tiny helpers by pixhawk -// =============================================================================== -int LSM303D_accel_set_range(unsigned max_g) -{ - uint8_t setbits = 0; - uint8_t clearbits = REG2_FULL_SCALE_BITS_A; - float new_scale_g_digit = 0.0f; - - if (max_g == 0) { - max_g = 16; - } - - if (max_g <= 2) { - _accel_range_m_s2 = 2.0f * LSM303D_ONE_G; - setbits |= REG2_FULL_SCALE_2G_A; - new_scale_g_digit = 0.061e-3f; - - } else if (max_g <= 4) { - _accel_range_m_s2 = 4.0f * LSM303D_ONE_G; - setbits |= REG2_FULL_SCALE_4G_A; - new_scale_g_digit = 0.122e-3f; - - } else if (max_g <= 6) { - _accel_range_m_s2 = 6.0f * LSM303D_ONE_G; - setbits |= REG2_FULL_SCALE_6G_A; - new_scale_g_digit = 0.183e-3f; - - } else if (max_g <= 8) { - _accel_range_m_s2 = 8.0f * LSM303D_ONE_G; - setbits |= REG2_FULL_SCALE_8G_A; - new_scale_g_digit = 0.244e-3f; - - } else if (max_g <= 16) { - _accel_range_m_s2 = 16.0f * LSM303D_ONE_G; - setbits |= REG2_FULL_SCALE_16G_A; - new_scale_g_digit = 0.732e-3f; - - } else { - return -1; - } - - _accel_range_scale = new_scale_g_digit * LSM303D_ONE_G; - - - LSM303D_modify_reg(ADDR_CTRL_REG2, clearbits, setbits); - - return 0; -} -*/ -/* -// =============================================================================== -// LSM303D_mag_set_range -/// @brief tiny helpers by pixhawk -// =============================================================================== -int LSM303D_mag_set_range(unsigned max_ga) -{ - uint8_t setbits = 0; - uint8_t clearbits = REG6_FULL_SCALE_BITS_M; - float new_scale_ga_digit = 0.0f; - - if (max_ga == 0) { - max_ga = 12; - } - - if (max_ga <= 2) { - _mag_range_ga = 2; - setbits |= REG6_FULL_SCALE_2GA_M; - new_scale_ga_digit = 0.080e-3f; - - } else if (max_ga <= 4) { - _mag_range_ga = 4; - setbits |= REG6_FULL_SCALE_4GA_M; - new_scale_ga_digit = 0.160e-3f; - - } else if (max_ga <= 8) { - _mag_range_ga = 8; - setbits |= REG6_FULL_SCALE_8GA_M; - new_scale_ga_digit = 0.320e-3f; - - } else if (max_ga <= 12) { - _mag_range_ga = 12; - setbits |= REG6_FULL_SCALE_12GA_M; - new_scale_ga_digit = 0.479e-3f; - - } else { - return -1; - } - - _mag_range_scale = new_scale_ga_digit; - - LSM303D_modify_reg(ADDR_CTRL_REG6, clearbits, setbits); - - return 0; -} -*/ - // =============================================================================== // LSM303D_accel_set_onchip_lowpass_filter_bandwidth -/// @brief tiny helpers by pixhawk // =============================================================================== int LSM303D_accel_set_onchip_lowpass_filter_bandwidth(unsigned bandwidth) { @@ -626,7 +567,6 @@ // =============================================================================== // LSM303D_accel_set_driver_lowpass_filter -/// @brief tiny helpers by pixhawk. This one is not used at the moment! // =============================================================================== int LSM303D_accel_set_driver_lowpass_filter(float samplerate, float bandwidth) { @@ -638,113 +578,6 @@ return 0; } -/* unused 170821 -// =============================================================================== -// LSM303D_accel_set_samplerate -/// @brief tiny helpers by pixhawk -// =============================================================================== -int LSM303D_accel_set_samplerate(unsigned frequency) -{ - uint8_t setbits = 0; - uint8_t clearbits = REG1_RATE_BITS_A; - -// if (frequency == 0 || frequency == ACCEL_SAMPLERATE_DEFAULT) { - frequency = 1600; -// } - - if (frequency <= 3) { - setbits |= REG1_RATE_3_125HZ_A; - _accel_samplerate = 3; - - } else if (frequency <= 6) { - setbits |= REG1_RATE_6_25HZ_A; - _accel_samplerate = 6; - - } else if (frequency <= 12) { - setbits |= REG1_RATE_12_5HZ_A; - _accel_samplerate = 12; - - } else if (frequency <= 25) { - setbits |= REG1_RATE_25HZ_A; - _accel_samplerate = 25; - - } else if (frequency <= 50) { - setbits |= REG1_RATE_50HZ_A; - _accel_samplerate = 50; - - } else if (frequency <= 100) { - setbits |= REG1_RATE_100HZ_A; - _accel_samplerate = 100; - - } else if (frequency <= 200) { - setbits |= REG1_RATE_200HZ_A; - _accel_samplerate = 200; - - } else if (frequency <= 400) { - setbits |= REG1_RATE_400HZ_A; - _accel_samplerate = 400; - - } else if (frequency <= 800) { - setbits |= REG1_RATE_800HZ_A; - _accel_samplerate = 800; - - } else if (frequency <= 1600) { - setbits |= REG1_RATE_1600HZ_A; - _accel_samplerate = 1600; - - } else { - return -1; - } - - LSM303D_modify_reg(ADDR_CTRL_REG1, clearbits, setbits); - return 0; -} -// =============================================================================== -// LSM303D_mag_set_samplerate -/// @brief tiny helpers by pixhawk -// =============================================================================== -int LSM303D_mag_set_samplerate(unsigned frequency) -{ - uint8_t setbits = 0; - uint8_t clearbits = REG5_RATE_BITS_M; - - if (frequency == 0) { - frequency = 100; - } - - if (frequency <= 3) { - setbits |= REG5_RATE_3_125HZ_M; - _mag_samplerate = 25; - - } else if (frequency <= 6) { - setbits |= REG5_RATE_6_25HZ_M; - _mag_samplerate = 25; - - } else if (frequency <= 12) { - setbits |= REG5_RATE_12_5HZ_M; - _mag_samplerate = 25; - - } else if (frequency <= 25) { - setbits |= REG5_RATE_25HZ_M; - _mag_samplerate = 25; - - } else if (frequency <= 50) { - setbits |= REG5_RATE_50HZ_M; - _mag_samplerate = 50; - - } else if (frequency <= 100) { - setbits |= REG5_RATE_100HZ_M; - _mag_samplerate = 100; - - } else { - return -1; - } - - LSM303D_modify_reg(ADDR_CTRL_REG5, clearbits, setbits); - return 0; -} -*/ - // rotate_mag_3f: nicht genutzt aber praktisch; rotate_accel_3f wird benutzt // =============================================================================== @@ -786,9 +619,7 @@ // =============================================================================== -// compass_init_LSM303D by PIXhawk (LSM303D::reset()) -// https://raw.githubusercontent.com/PX4/Firmware/master/src/drivers/lsm303d/lsm303d.cpp -/// @brief The new ST 303D +// compass_init_LSM303D /// This might be called several times with different gain values during calibration /// but gain change is not supported at the moment. /// @@ -799,7 +630,6 @@ void compass_init_LSM303D(uint8_t fast, uint8_t gain) { -// matthias version 160620 if(fast == 0) { LSM303D_write_checked_reg(ADDR_CTRL_REG0, 0x00); @@ -821,61 +651,13 @@ LSM303D_write_checked_reg(ADDR_CTRL_REG6, 0x00); LSM303D_write_checked_reg(ADDR_CTRL_REG7, 0x00); - /* -uint8_t data; -for(int i=0;i<11;i++) -{ - data = ADDR_INT_THS_L_M + i; - I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1); - I2C_Master_Receive( DEVICE_COMPASS_303D, &testCompassLS303D[i], 1); -} -*/ - return; -/* - LSM303D_accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G); // modifies ADDR_CTRL_REG2 - LSM303D_accel_set_samplerate(LSM303D_ACCEL_DEFAULT_RATE); // modifies ADDR_CTRL_REG1 - - LSM303D_mag_set_range(LSM303D_MAG_DEFAULT_RANGE_GA); - LSM303D_mag_set_samplerate(LSM303D_MAG_DEFAULT_RATE); -*/ - -/* -// my stuff hw - // enable accel - LSM303D_write_checked_reg(ADDR_CTRL_REG1, - REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE | REG1_RATE_800HZ_A); - - // enable mag - LSM303D_write_checked_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M); - LSM303D_write_checked_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M | REG5_ENABLE_T); - LSM303D_write_checked_reg(ADDR_CTRL_REG3, 0x04); // DRDY on ACCEL on INT1 - LSM303D_write_checked_reg(ADDR_CTRL_REG4, 0x04); // DRDY on MAG on INT2 - - LSM303D_accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G); - LSM303D_accel_set_samplerate(LSM303D_ACCEL_DEFAULT_RATE); - LSM303D_accel_set_driver_lowpass_filter((float)LSM303D_ACCEL_DEFAULT_RATE, (float)LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ); - //LSM303D_accel_set_onchip_lowpass_filter_bandwidth(773); // factory setting - - // we setup the anti-alias on-chip filter as 50Hz. We believe - // this operates in the analog domain, and is critical for - // anti-aliasing. The 2 pole software filter is designed to - // operate in conjunction with this on-chip filter - if(fast) - LSM303D_accel_set_onchip_lowpass_filter_bandwidth(773); // factory setting - else - LSM303D_accel_set_onchip_lowpass_filter_bandwidth(LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ); - - - LSM303D_mag_set_range(LSM303D_MAG_DEFAULT_RANGE_GA); - LSM303D_mag_set_samplerate(LSM303D_MAG_DEFAULT_RATE); -*/ } // =============================================================================== // compass_sleep_LSM303D -/// @brief The new compass chip, hopefully this works! +// @brief Gen 2 chip // =============================================================================== void compass_sleep_LSM303D(void) { @@ -886,9 +668,7 @@ // =============================================================================== // acceleration_read_LSM303D -/// @brief The new LSM303D, code by pixhawk -/// -/// output is accel_DX_f, accel_DY_f, accel_DZ_f +// output is accel_DX_f, accel_DY_f, accel_DZ_f // =============================================================================== void acceleration_read_LSM303D(void) { @@ -920,66 +700,14 @@ accel_report_y = yraw_f; accel_report_z = zraw_f; - // my stuff -/* - accel_report_x = ((xraw_f * _accel_range_scale) - _accel_scale_x_offset) * _accel_scale_x_scale; - accel_report_y = ((yraw_f * _accel_range_scale) - _accel_scale_y_offset) * _accel_scale_y_scale; - accel_report_z = ((zraw_f * _accel_range_scale) - _accel_scale_z_offset) * _accel_scale_z_scale; -*/ accel_DX_f = ((int16_t)(accel_report_x)); accel_DY_f = ((int16_t)(accel_report_y)); accel_DZ_f = ((int16_t)(accel_report_z)); } -/* special code after accel_report_z = ... - * prior to output -// we have logs where the accelerometers get stuck at a fixed -// large value. We want to detect this and mark the sensor as -// being faulty - - if (fabsf(_last_accel[0] - x_in_new) < 0.001f && - fabsf(_last_accel[1] - y_in_new) < 0.001f && - fabsf(_last_accel[2] - z_in_new) < 0.001f && - fabsf(x_in_new) > 20 && - fabsf(y_in_new) > 20 && - fabsf(z_in_new) > 20) { - _constant_accel_count += 1; - - } else { - _constant_accel_count = 0; - } - - if (_constant_accel_count > 100) { - // we've had 100 constant accel readings with large - // values. The sensor is almost certainly dead. We - // will raise the error_count so that the top level - // flight code will know to avoid this sensor, but - // we'll still give the data so that it can be logged - // and viewed - perf_count(_bad_values); - _constant_accel_count = 0; - } - - _last_accel[0] = x_in_new; - _last_accel[1] = y_in_new; - _last_accel[2] = z_in_new; - - accel_report.x = _accel_filter_x.apply(x_in_new); - accel_report.y = _accel_filter_y.apply(y_in_new); - accel_report.z = _accel_filter_z.apply(z_in_new); - - math::Vector<3> aval(x_in_new, y_in_new, z_in_new); - math::Vector<3> aval_integrated; - - bool accel_notify = _accel_int.put(accel_report.timestamp, aval, aval_integrated, accel_report.integral_dt); - accel_report.x_integral = aval_integrated(0); - accel_report.y_integral = aval_integrated(1); - accel_report.z_integral = aval_integrated(2); -*/ // =============================================================================== // compass_read_LSM303D -/// @brief The new LSM303D, code by pixhawk /// /// output is compass_DX_f, compass_DY_f, compass_DZ_f // =============================================================================== @@ -1008,33 +736,129 @@ compass_DZ_f = (((int16_t)((magDataBuffer[5] << 8) | (magDataBuffer[4])))); // no rotation return; -/* - // my stuff - compass_DX_f = (((int16_t)((magDataBuffer[1] << 8) | (magDataBuffer[0]))) / 10) - 200; - compass_DY_f = (((int16_t)((magDataBuffer[3] << 8) | (magDataBuffer[2]))) / 10) - 200; - compass_DZ_f = (((int16_t)((magDataBuffer[5] << 8) | (magDataBuffer[4]))) / 10) - 200; -*/ - // old - /* - xraw_f = ((float)( (int16_t)((magDataBuffer[1] << 8) | (magDataBuffer[0])))); - yraw_f = ((float)( (int16_t)((magDataBuffer[3] << 8) | (magDataBuffer[2])))); - zraw_f = ((float)( (int16_t)((magDataBuffer[5] << 8) | (magDataBuffer[4])))); +} + + +// =============================================================================== +// compass_init_LSM303AGR +/// This might be called several times with different gain values during calibration +/// but gain change is not supported at the moment. +/// +/// @param gain: 7 is max gain and set with here, compass_calib() might reduce it +// =============================================================================== - rotate_mag_3f(&xraw_f, &yraw_f, &zraw_f); +void compass_init_LSM303AGR(uint8_t fast, uint8_t gain) +{ + if(fast == 0) + { + LSM303AGR_write_checked_reg(0x60, 0x80); // 10Hz + LSM303AGR_write_checked_reg(0x61, 0x03); // CFG_REG_B_M + LSM303AGR_write_checked_reg(0x62, 0x10); // CFG_REG_C_M + } + else + { + LSM303AGR_write_checked_reg(0x60, 0x80); // 10Hz + LSM303AGR_write_checked_reg(0x61, 0x03); // CFG_REG_B_M + LSM303AGR_write_checked_reg(0x62, 0x10); // CFG_REG_C_M + } + // init accel (Same chip, but different address...) + LSM303AGR_acc_write_checked_reg(0x1F, 0x00); // TEMP_CFG_REG_A (Temp sensor off) + LSM303AGR_acc_write_checked_reg(0x20, 0x4F); // CTRL_REG1_A (10Hz, x,y,z = ON) + LSM303AGR_acc_write_checked_reg(0x21, 0x00); // CTRL_REG2_A + LSM303AGR_acc_write_checked_reg(0x22, 0x00); // CTRL_REG3_A + LSM303AGR_acc_write_checked_reg(0x23, 0x04); // CTRL_REG4_A + + return; +} + + +// =============================================================================== +// compass_sleep_LSM303D +// @brief Gen 2 chip +// =============================================================================== +void compass_sleep_LSM303AGR(void) +{ + LSM303AGR_write_checked_reg(0x60, 0x03); // + LSM303AGR_write_checked_reg(0x61, 0x04); // + LSM303AGR_write_checked_reg(0x62, 0x51); // + LSM303AGR_write_checked_reg(0x63, 0x00); // + + + LSM303AGR_acc_write_checked_reg(0x1F, 0x00); // + LSM303AGR_acc_write_checked_reg(0x20, 0x00); // +} + - compass_DX_f = (int16_t)((xraw_f * 0.1f) - 200.0f); - compass_DY_f = (int16_t)((yraw_f * 0.1f) - 200.0f); - compass_DZ_f = (int16_t)((zraw_f * 0.1f) - 200.0f); -*/ -/* - mag_report_x = ((xraw_f * _mag_range_scale) - _mag_scale_x_offset) * _mag_scale_x_scale; - mag_report_y = ((yraw_f * _mag_range_scale) - _mag_scale_y_offset) * _mag_scale_y_scale; - mag_report_z = ((zraw_f * _mag_range_scale) - _mag_scale_z_offset) * _mag_scale_z_scale; - - compass_DX_f = (int16_t)(mag_report_x * 1000.0f); // 1000.0 is just a wild guess by hw - compass_DY_f = (int16_t)(mag_report_y * 1000.0f); - compass_DZ_f = (int16_t)(mag_report_z * 1000.0f); -*/ +// =============================================================================== +// acceleration_read_LSM303AGR +// output is accel_DX_f, accel_DY_f, accel_DZ_f +// =============================================================================== +void acceleration_read_LSM303AGR(void) +{ + uint8_t data; + float xraw_f, yraw_f, zraw_f; + float accel_report_x, accel_report_y, accel_report_z; + + memset(accDataBuffer,0,6); + + accel_DX_f = 0; + accel_DY_f = 0; + accel_DZ_f = 0; + + for(int i=0;i<6;i++) + { + data = ADDR_OUT_X_L_A + i; // ADDR_OUT_X_L_A is the same as in the LSM303D (luckily) + I2C_Master_Transmit( DEVICE_ACCELARATOR_303AGR, &data, 1); + I2C_Master_Receive( DEVICE_ACCELARATOR_303AGR, &accDataBuffer[i], 1); + } + + xraw_f = ((float)( (int16_t)((accDataBuffer[1] << 8) | (accDataBuffer[0])))); + yraw_f = ((float)( (int16_t)((accDataBuffer[3] << 8) | (accDataBuffer[2])))); + zraw_f = ((float)( (int16_t)((accDataBuffer[5] << 8) | (accDataBuffer[4])))); + + rotate_accel_3f(&xraw_f, &yraw_f, &zraw_f); + + // mh + accel_report_x = xraw_f; + accel_report_y = yraw_f; + accel_report_z = zraw_f; + + accel_DX_f = ((int16_t)(accel_report_x)); + accel_DY_f = ((int16_t)(accel_report_y)); + accel_DZ_f = ((int16_t)(accel_report_z)); +} + + +// =============================================================================== +// compass_read_LSM303AGR +/// +/// output is compass_DX_f, compass_DY_f, compass_DZ_f +// =============================================================================== +void compass_read_LSM303AGR(void) +{ + uint8_t data; +// float xraw_f, yraw_f, zraw_f; +// float mag_report_x, mag_report_y, mag_report_z; + + memset(magDataBuffer,0,6); + + compass_DX_f = 0; + compass_DY_f = 0; + compass_DZ_f = 0; + + for(int i=0;i<6;i++) + { + data = 0x68 + i; // OUTX_L_REG_M + I2C_Master_Transmit( DEVICE_COMPASS_303AGR, &data, 1); + I2C_Master_Receive( DEVICE_COMPASS_303AGR, &magDataBuffer[i], 1); + } + + // mh 160620 flip x and y if flip display + compass_DX_f = (((int16_t)((magDataBuffer[1] << 8) | (magDataBuffer[0])))); + compass_DY_f = (((int16_t)((magDataBuffer[3] << 8) | (magDataBuffer[2])))); + compass_DZ_f = (((int16_t)((magDataBuffer[5] << 8) | (magDataBuffer[4])))); + // no rotation + return; } @@ -1340,34 +1164,6 @@ } -/* -// =============================================================================== -// compass_calc_mini_during_calibration -/// @brief all the fancy stuff first implemented in OSTC3 -/// -/// input is accel_DX_f, accel_DY_f, accel_DZ_f -/// output is compass_pitch and compass_roll -// =============================================================================== -void compass_calc_mini_during_calibration(void) -{ - float sinPhi, cosPhi; - float Phi, Teta; - - //---- Calculate sine and cosine of roll angle Phi ----------------------- - //sincos(accel_DZ_f, accel_DY_f, &sin, &cos); - Phi= atan2f(accel_DY_f, accel_DZ_f) ; - compass_roll = Phi * 180.0f /PI; - sinPhi = sinf(Phi); - cosPhi = cosf(Phi); - - //---- calculate sin and cosine of pitch angle Theta --------------------- - //sincos(Gz, -accel_DX_f, &sin, &cos); // NOTE: changed sin sign. - Teta = atanf(-(float)accel_DX_f/(accel_DY_f * sinPhi + accel_DZ_f * cosPhi)); - compass_pitch = Teta * 180.0f /PI; -} -*/ - - // ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // // - Calibration - /////////////////////////////////////////////////////////////////////////////////////////////////////// // ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// @@ -1565,7 +1361,7 @@ acceleration_read(); compass_calc_roll_pitch_only(); - if((hardwareCompass == HMC5883L) + if((hardwareCompass == compass_generation1 ) //HMC5883L) &&((compass_DX_f == -4096) || (compass_DY_f == -4096) || (compass_DZ_f == -4096) )) @@ -1607,326 +1403,3 @@ return 0; } -// //////////////////////////// TEST CODE ///////////////////////////////////// - - - -//#include -//#include -//#include -/*#include - -short compass_DX_f, compass_DY_f, compass_DZ_f; -short compass_CX_f, compass_CY_f, compass_CZ_f; - -inline float uniform(void) { - return (rand() & 0xFFFF) / 65536.0f; -} -inline float sqr(float x) { - return x*x; -} - -static const float radius = 0.21f; -static const float cx = 0.79f, cy = -0.46f, cz = 0.24f; -// const float cx = 0, cy = 0, cz = 0; - -float check_compass_calib(void) -{ - - // Starts with no calibration at all: - compass_CX_f = compass_CY_f = compass_CZ_f = 0; - - // Try 10 recalibration passes: - for(int p=0; p<10; ++p) - { - compass_reset_calibration(); - - //---- Generates random points on a sphere ------------------------------- - // of radius,center (cx, cy, cz): - for(int i=0; i<100; ++i) - { - float theta = uniform()*360.0f; - float phi = uniform()*180.0f - 90.0f; - - float x = cx + radius * cosf(phi)*cosf(theta); - float y = cy + radius * cosf(phi)*sinf(theta); - float z = cz + radius * sinf(phi); - - compass_DX_f = (short)(32768 * x); - compass_DY_f = (short)(32768 * y); - compass_DZ_f = (short)(32768 * z); - compass_add_calibration(); - } - - compass_solve_calibration(); - //qDebug() << "Center =" -// << compass_CX_f/32768.0f -// << compass_CY_f/32768.0f -// << compass_CZ_f/32768.0f; - - float r2 = sqr(compass_CX_f/32768.0f - cx) - + sqr(compass_CY_f/32768.0f - cy) - + sqr(compass_CZ_f/32768.0f - cz); - if( r2 > 0.01f*0.01f ) - return sqrtf(r2); - } - return 0; -}*/ - - - -/* -void compass_read_LSM303D_v3(void) -{ - uint8_t data; - - memset(magDataBuffer,0,6); - - compass_DX_f = 0; - compass_DY_f = 0; - compass_DZ_f = 0; - - //magnetometer multi read, order xl,xh, yl,yh, zl, zh - data = REG_MAG_DATA_ADDR; - I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1); - I2C_Master_Receive( DEVICE_COMPASS_303D, magDataBuffer, 6); - - compass_DX_f = ((int16_t)( (int16_t)((magDataBuffer[1] << 8) | (magDataBuffer[0])))); - compass_DY_f = ((int16_t)( (int16_t)((magDataBuffer[3] << 8) | (magDataBuffer[2])))); - compass_DZ_f = ((int16_t)( (int16_t)((magDataBuffer[5] << 8) | (magDataBuffer[4])))); - -// compass_DX_f = compass_DX_f * stat->sensitivity_mag; -// compass_DY_f = compass_DY_f * stat->sensitivity_mag; -// compass_DZ_f = compass_DZ_f * stat->sensitivity_mag; -} - - -// =============================================================================== -// compass_init_LSM303D by STMicroelectronics 2013 V1.0.5 2013/Oct/23 -/// @brief The new ST 303D -/// This might be called several times with different gain values during calibration -/// -/// @param gain: 7 is max gain and set with here, compass_calib() might reduce it -// =============================================================================== - -void compass_init_LSM303D_v3(uint8_t gain) -{ - uint8_t data[10]; - - // CNTRL1 - // 0011 acceleration data rate 0011 = 12.5 Hz (3.125 Hz - 1600 Hz) - // 0xxx block data update off - // x111 enable all three axes - - // CNTRL5 - // 0xxx xxxx temp sensor off - // x00x xxxx magnetic resolution - // xxx0 1xxx magentic data rate 01 = 6,25 Hz (3.125 Hz - 50 Hz (100 Hz)) - // xxxx xx00 latch irq requests off - - // CNTRL7 - // 00xx high pass filter mode, 00 normal mode - // xx0x filter for acceleration data bypassed - // xxx0 temperature sensor mode only off - // x0xx magnetic data low-power mode off - // xx00 magnetic sensor mode 00 = continous-conversion mode (default 10 power-down) - - data[0] = CNTRL0; - data[1] = 0x00; - I2C_Master_Transmit( DEVICE_COMPASS_303D, data, 2); - - // acc - data[0] = CNTRL1; - data[1] = 0x00; - data[2] = 0x0F; - data[3] = 0x00; - data[4] = 0x00; - I2C_Master_Transmit( DEVICE_COMPASS_303D, data, 5); - - // mag - data[0] = CNTRL3; - data[1] = 0x00; - data[2] = 0x00; - data[3] = 0x18; - data[4] = 0x20; - I2C_Master_Transmit( DEVICE_COMPASS_303D, data, 5); - - data[0] = CNTRL7; - data[1] = ((MSMS_MASK & CONTINUOS_CONVERSION) | - ((~MSMS_MASK) & CNTRL7_RESUME_VALUE)); - I2C_Master_Transmit( DEVICE_COMPASS_303D, data, 2); - - HAL_Delay(100); -} - - -// =============================================================================== -// compass_init_LSM303D by nordevx for arduion -/// @brief The new ST 303D -/// This might be called several times with different gain values during calibration -/// -/// @param gain: 7 is max gain and set with here, compass_calib() might reduce it -// =============================================================================== -void compass_init_LSM303D_v2(uint8_t gain) -{ - uint8_t data[2]; - - // CNTRL1 - // 0011 acceleration data rate 0011 = 12.5 Hz (3.125 Hz - 1600 Hz) - // 0xxx block data update off - // x111 enable all three axes - - // CNTRL5 - // 0xxx xxxx temp sensor off - // x00x xxxx magnetic resolution - // xxx0 1xxx magentic data rate 01 = 6,25 Hz (3.125 Hz - 50 Hz (100 Hz)) - // xxxx xx00 latch irq requests off - - // CNTRL7 - // 00xx high pass filter mode, 00 normal mode - // xx0x filter for acceleration data bypassed - // xxx0 temperature sensor mode only off - // x0xx magnetic data low-power mode off - // xx00 magnetic sensor mode 00 = continous-conversion mode (default 10 power-down) - - data[0] = CNTRL1; - data[1] = 0x37; //0b 0011 0111 - I2C_Master_Transmit( DEVICE_COMPASS_303D, data, 2); - - data[0] = CNTRL5; - data[1] = 0x08; // 0b 0000 1000 - I2C_Master_Transmit( DEVICE_COMPASS_303D, data, 2); - - data[0] = CNTRL7; - data[1] = 0x00; // 0b 0000 0000 - I2C_Master_Transmit( DEVICE_COMPASS_303D, data, 2); - - HAL_Delay(100); -} - - -// =============================================================================== -// compass_init_LSM303D_v1 by ST lsm303d.c -/// @brief The new ST 303D -/// This might be called several times with different gain values during calibration -/// -/// @param gain: 7 is max gain and set with here, compass_calib() might reduce it -// =============================================================================== -void compass_init_LSM303D_v1(uint8_t gain) -{ - uint8_t data; - - compass_gain = gain; - - memset(magDataBuffer,0,6); - memset(accDataBuffer,0,6); - - data = CNTRL5; - I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1); - I2C_Master_Receive( DEVICE_COMPASS_303D, &data, 1); - data = (data & 0x1c) >> 2; - velMag = magODR[data]; - - data = CNTRL1; - I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1); - I2C_Master_Receive( DEVICE_COMPASS_303D, &data, 1); - data = (data & 0xf0) >> 4; - velAcc = accODR[data]; - - data = CNTRL7; - I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1); - I2C_Master_Receive( DEVICE_COMPASS_303D, &datas1, 1); - datas1 = (datas1 & 0x02); - - //if mag is not pd - //mag is bigger than gyro - if( (velMag < velAcc) || datas1 != 0 ) { - //acc is the biggest - fastest = ACC_IS_FASTEST; - } - else { - //acc is the biggest - fastest = MAG_IS_FASTEST; - } - - zoffFlag = 1; - - if( fastest == MAG_IS_FASTEST) - { - data = STATUS_REG_M; - I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1); - I2C_Master_Receive( DEVICE_COMPASS_303D, &data, 1); - -// if(ValBit(data, ZYXMDA)) { - sendFlag = 1; -// } - - } - else if(fastest == ACC_IS_FASTEST) - { - data = STATUS_REG_A; - I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1); - I2C_Master_Receive( DEVICE_COMPASS_303D, &data, 1); -// if(ValBit(data, DATAREADY_BIT)) { - sendFlag = 1; -// } - } -} - -// =============================================================================== -// compass_read_LSM303D -/// @brief The new LSM303D :-) -/// -/// output is compass_DX_f, compass_DY_f, compass_DZ_f, accel_DX_f, accel_DY_f, accel_DZ_f -// =============================================================================== -void compass_read_LSM303D_v2(void) -{ - uint8_t data; - - memset(magDataBuffer,0,6); - memset(accDataBuffer,0,6); - - compass_DX_f = 0; - compass_DY_f = 0; - compass_DZ_f = 0; - - accel_DX_f = 0; - accel_DY_f = 0; - accel_DZ_f = 0; - - //Accelerometer multi read, order xl,xh, yl,yh, zl, zh - data = REG_ACC_DATA_ADDR; - I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1); - I2C_Master_Receive( DEVICE_COMPASS_303D, accDataBuffer, 6); - - //magnetometer multi read, order xl,xh, yl,yh, zl, zh - data = OUT_X_L_M; - I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1); - I2C_Master_Receive( DEVICE_COMPASS_303D, magDataBuffer, 6); - - accel_DX_f = ((int16_t)( (int16_t)((accDataBuffer[1] << 8) | (accDataBuffer[0])))); - accel_DY_f = ((int16_t)( (int16_t)((accDataBuffer[3] << 8) | (accDataBuffer[2])))); - accel_DZ_f = ((int16_t)( (int16_t)((accDataBuffer[5] << 8) | (accDataBuffer[4])))); - -// accel_DX_f = accel_DX_f * stat->sensitivity_acc; -// accel_DY_f = accel_DY_f * stat->sensitivity_acc; -// accel_DZ_f = accel_DZ_f * stat->sensitivity_acc; - - - compass_DX_f = magDataBuffer[1]; - compass_DX_f *= 256; - compass_DX_f += magDataBuffer[0]; - - compass_DY_f = magDataBuffer[3]; - compass_DY_f *= 256; - compass_DY_f += magDataBuffer[2]; - - compass_DY_f = magDataBuffer[5]; - compass_DY_f *= 256; - compass_DY_f += magDataBuffer[4]; - -} - - -*/ -