Added color coding for cv gas list:
In previous version only a warning was highlighted using the red color. Now a better gas selection is highlighted in green. Gas without notification is blue while the active gas is shown in white
line source
/**+ −
******************************************************************************+ −
* @file compass.c + −
* @author heinrichs weikamp gmbh+ −
* @date 27-March-2014+ −
* @version V0.2.0+ −
* @since 21-April-2016+ −
* @brief for Honeywell Compass and ST LSM303D+ −
* + −
@verbatim + −
============================================================================== + −
##### How to use #####+ −
============================================================================== + −
V0.1.0 09-March-2016+ −
V0.2.0 21-April-2016 Orientation fixed for LSM303D,+ −
roll and pitch added to calibration output,+ −
orientation double checked with datasheets and layout+ −
as well as with value output during calibration+ −
V0.2.1 19-May-2016 New date rate config and full-scale selection+ −
+ −
@endverbatim+ −
******************************************************************************+ −
* @attention+ −
*+ −
* <h2><center>© COPYRIGHT(c) 2016 heinrichs weikamp</center></h2>+ −
*+ −
******************************************************************************+ −
*/ + −
+ −
#include <math.h>+ −
#include <string.h>+ −
+ −
#include "compass.h"+ −
#include "compass_LSM303D.h"+ −
+ −
#include "i2c.h"+ −
#include "spi.h"+ −
#include "RTE_FlashAccess.h" // to store compass_calib_data+ −
+ −
#include "stm32f4xx_hal.h"+ −
+ −
extern uint32_t time_elapsed_ms(uint32_t ticksstart,uint32_t ticksnow);+ −
+ −
/// split byte to bits+ −
typedef struct{ + −
uint8_t bit0:1; ///< split byte to bits+ −
uint8_t bit1:1; ///< split byte to bits+ −
uint8_t bit2:1; ///< split byte to bits+ −
uint8_t bit3:1; ///< split byte to bits+ −
uint8_t bit4:1; ///< split byte to bits+ −
uint8_t bit5:1; ///< split byte to bits+ −
uint8_t bit6:1; ///< split byte to bits+ −
uint8_t bit7:1; ///< split byte to bits+ −
} ubit8_t;+ −
+ −
+ −
/// split byte to bits+ −
typedef union{ + −
ubit8_t ub; ///< split byte to bits+ −
uint8_t uw; ///< split byte to bits+ −
} bit8_Type;+ −
+ −
+ −
/// split word to 2 bytes+ −
typedef struct{+ −
uint8_t low; ///< split word to 2 bytes+ −
uint8_t hi; ///< split word to 2 bytes+ −
} two_byte; + −
+ −
+ −
/// split word to 2 bytes+ −
typedef union{+ −
two_byte Byte; ///< split word to 2 bytes+ −
uint16_t Word; ///< split word to 2 bytes+ −
} tword; + −
+ −
+ −
/// split signed word to 2 bytes+ −
typedef union{+ −
two_byte Byte; ///< split signed word to 2 bytes+ −
int16_t Word; ///< split signed word to 2 bytes+ −
} signed_tword; + −
+ −
+ −
/// split full32 to 2 words+ −
typedef struct{+ −
uint16_t low16; ///< split word to 2 bytes+ −
uint16_t hi16; ///< split word to 2 bytes+ −
} two_word; + −
+ −
typedef union{+ −
two_word Word16; ///< split word to 2 bytes+ −
uint32_t Full32; ///< split word to 2 bytes+ −
} tfull32; + −
+ −
+ −
/// crazy compass calibration stuff+ −
typedef struct+ −
{+ −
unsigned short int compass_N; + −
float Su, Sv, Sw; + −
float Suu, Svv, Sww, Suv, Suw, Svw; + −
float Suuu, Svvv, Swww; + −
float Suuv, Suuw, Svvu, Svvw, Swwu, Swwv; + −
} SCompassCalib; + −
+ −
+ −
#define Q_PI (18000)+ −
#define Q_PIO2 (9000)+ −
+ −
+ −
+ −
//////////////////////////////////////////////////////////////////////////////+ −
// fifth order of polynomial approximation of atan(), giving 0.05 deg max error+ −
//+ −
#define K1 (5701) // Needs K1/2**16+ −
#define K2 (1645) // Needs K2/2**48 WAS NEGATIV+ −
#define K3 ( 446) // Needs K3/2**80+ −
+ −
const float PI = 3.14159265; ///< pi, used in compass_calc()+ −
+ −
typedef short int Int16;+ −
typedef signed char Int8;+ −
typedef Int16 Angle;+ −
+ −
+ −
/// The (filtered) components of the magnetometer sensor+ −
int16_t compass_DX_f; ///< output from sensor+ −
int16_t compass_DY_f; ///< output from sensor+ −
int16_t compass_DZ_f; ///< output from sensor+ −
+ −
+ −
/// Found soft-iron calibration values, deduced from already filtered values+ −
int16_t compass_CX_f; ///< calibration value+ −
int16_t compass_CY_f; ///< calibration value+ −
int16_t compass_CZ_f; ///< calibration value+ −
+ −
+ −
/// The (filtered) components of the accelerometer sensor+ −
int16_t accel_DX_f; ///< output from sensor+ −
int16_t accel_DY_f; ///< output from sensor+ −
int16_t accel_DZ_f; ///< output from sensor+ −
+ −
+ −
/// The compass result values+ −
float compass_heading; ///< the final result calculated in compass_calc()+ −
float compass_roll; ///< the final result calculated in compass_calc()+ −
float compass_pitch; ///< the final result calculated in compass_calc()+ −
+ −
+ −
uint8_t compass_gain; ///< 7 on start, can be reduced during calibration+ −
+ −
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+ −
+ −
+ −
// struct accel_scale _accel_scale;+ −
unsigned _accel_range_m_s2;+ −
float _accel_range_scale;+ −
unsigned _accel_samplerate;+ −
unsigned _accel_onchip_filter_bandwith;+ −
+ −
// struct mag_scale _mag_scale;+ −
unsigned _mag_range_ga;+ −
float _mag_range_scale;+ −
unsigned _mag_samplerate;+ −
+ −
// default scale factors+ −
float _accel_scale_x_offset = 0.0f;+ −
float _accel_scale_x_scale = 1.0f;+ −
float _accel_scale_y_offset = 0.0f;+ −
float _accel_scale_y_scale = 1.0f;+ −
float _accel_scale_z_offset = 0.0f;+ −
float _accel_scale_z_scale = 1.0f;+ −
+ −
float _mag_scale_x_offset = 0.0f;+ −
float _mag_scale_x_scale = 1.0f;+ −
float _mag_scale_y_offset = 0.0f;+ −
float _mag_scale_y_scale = 1.0f;+ −
float _mag_scale_z_offset = 0.0f;+ −
float _mag_scale_z_scale = 1.0f;+ −
+ −
+ −
/* External function prototypes ----------------------------------------------*/+ −
+ −
extern void copyCompassDataDuringCalibration(int16_t dx, int16_t dy, int16_t dz);+ −
+ −
/* Private function prototypes -----------------------------------------------*/+ −
+ −
void compass_reset_calibration(SCompassCalib *g);+ −
void compass_add_calibration(SCompassCalib *g);+ −
void compass_solve_calibration(SCompassCalib *g);+ −
+ −
void compass_init_HMC5883L(uint8_t fast, uint8_t gain);+ −
void compass_sleep_HMC5883L(void);+ −
void compass_read_HMC5883L(void);+ −
+ −
void accelerator_init_MMA8452Q(void);+ −
void accelerator_sleep_MMA8452Q(void);+ −
void acceleration_read_MMA8452Q(void);+ −
+ −
void compass_init_LSM303D(uint8_t fast, uint8_t gain);+ −
void compass_sleep_LSM303D(void);+ −
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);+ −
+ −
void compass_calc_roll_pitch_only(void);+ −
+ −
void rotate_mag_3f(float *x, float *y, float *z);+ −
void rotate_accel_3f(float *x, float *y, float *z);+ −
+ −
+ −
/* Exported functions --------------------------------------------------------*/+ −
+ −
+ −
// ===============================================================================+ −
// compass_init+ −
/// @brief This might be called several times with different gain values during calibration+ −
/// On first call it figures out which hardware is integrated+ −
///+ −
/// @param gain: 7 is max gain, compass_calib() might reduce it+ −
// ===============================================================================+ −
+ −
void compass_init(uint8_t fast, uint8_t gain)+ −
{+ −
+ −
// don't call again with fast, gain in calib mode etc.+ −
// if unknown+ −
if(hardwareCompass == COMPASS_NOT_RECOGNIZED)+ −
{+ −
return;+ −
}+ −
+ −
// old code but without else+ −
if(hardwareCompass == 0)+ −
{+ −
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_LSM303D)+ −
hardwareCompass = compass_generation2; //LSM303D;+ −
data = WHO_AM_I;+ −
I2C_Master_Transmit( DEVICE_ACCELARATOR_303AGR, &data, 1);+ −
I2C_Master_Receive( DEVICE_ACCELARATOR_303AGR, &data, 1);+ −
if(data == WHOIAM_VALUE_LSM303AGR)+ −
hardwareCompass = compass_generation3; //LSM303AGR;+ −
}+ −
+ −
/* No compass identified => Retry */+ −
if(hardwareCompass == 0)+ −
{+ −
I2C_DeInit();+ −
HAL_Delay(100);+ −
MX_I2C1_Init();+ −
HAL_Delay(100);+ −
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_LSM303D)+ −
hardwareCompass = compass_generation2; //LSM303D;+ −
data = WHO_AM_I;+ −
I2C_Master_Transmit( DEVICE_ACCELARATOR_303AGR, &data, 1);+ −
I2C_Master_Receive( DEVICE_ACCELARATOR_303AGR, &data, 1);+ −
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 = compass_generation1; //HMC5883L;+ −
+ −
HAL_StatusTypeDef resultOfOperationHMC_MMA = HAL_TIMEOUT;+ −
+ −
// test if both chips of the two-chip solution (gen 1) are present+ −
if(hardwareCompass == compass_generation1) // HMC5883L)+ −
{+ −
HAL_Delay(100);+ −
I2C_DeInit();+ −
HAL_Delay(100);+ −
MX_I2C1_Init();+ −
HAL_Delay(100);+ −
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 = compass_generation1; //HMC5883L; // all fine, keep it+ −
}+ −
else+ −
{+ −
hardwareCompass = COMPASS_NOT_RECOGNIZED;+ −
}+ −
}+ −
+ −
if(hardwareCompass == compass_generation2) //LSM303D)+ −
compass_init_LSM303D(fast, gain);+ −
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)+ −
{+ −
compass_CX_f = dataBlock[0].Word16.low16;+ −
compass_CY_f = dataBlock[0].Word16.hi16;+ −
compass_CZ_f = dataBlock[1].Word16.low16;+ −
}+ −
+ −
}+ −
+ −
+ −
// ===============================================================================+ −
// compass_calib+ −
/// @brief with onchip_lowpass_filter configuration for accelerometer of LSM303D+ −
// ===============================================================================+ −
int compass_calib(void)+ −
{+ −
if(hardwareCompass == compass_generation2) //LSM303D)+ −
{+ −
LSM303D_accel_set_onchip_lowpass_filter_bandwidth(773);+ −
int out = compass_calib_common();+ −
LSM303D_accel_set_onchip_lowpass_filter_bandwidth(LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ);+ −
return out;+ −
}+ −
else+ −
if(hardwareCompass == compass_generation1) //HMC5883L)+ −
{+ −
return compass_calib_common();+ −
}+ −
else+ −
if(hardwareCompass == compass_generation3) //LSM303AGR)+ −
{+ −
return compass_calib_common();+ −
}+ −
else+ −
{+ −
return 0; // standard answer of compass_calib_common();+ −
}+ −
+ −
+ −
}+ −
+ −
+ −
// ===============================================================================+ −
// compass_sleep+ −
/// @brief low power mode+ −
// ===============================================================================+ −
void compass_sleep(void)+ −
{+ −
if(hardwareCompass == compass_generation2) //LSM303D+ −
compass_sleep_LSM303D();+ −
if(hardwareCompass == compass_generation1) //HMC5883L+ −
compass_sleep_HMC5883L();+ −
if(hardwareCompass == compass_generation3) //LSM303AGR+ −
compass_sleep_LSM303AGR();+ −
}+ −
+ −
+ −
// ===============================================================================+ −
// compass_read+ −
/// @brief reads magnetometer and accelerometer for LSM303D,+ −
/// otherwise magnetometer only+ −
// ===============================================================================+ −
void compass_read(void)+ −
{+ −
if(hardwareCompass == compass_generation2) //LSM303D)+ −
compass_read_LSM303D();+ −
if(hardwareCompass == compass_generation1) //HMC5883L)+ −
compass_read_HMC5883L();+ −
if(hardwareCompass == compass_generation3) //LSM303AGR)+ −
compass_read_LSM303AGR();+ −
+ −
}+ −
+ −
+ −
// ===============================================================================+ −
// accelerator_init+ −
/// @brief empty for for LSM303D+ −
// ===============================================================================+ −
void accelerator_init(void)+ −
{+ −
if(hardwareCompass == compass_generation1) //HMC5883L)+ −
accelerator_init_MMA8452Q();+ −
}+ −
+ −
+ −
// ===============================================================================+ −
// accelerator_sleep+ −
/// @brief empty for for LSM303D+ −
// ===============================================================================+ −
void accelerator_sleep(void)+ −
{+ −
if(hardwareCompass == compass_generation1) //HMC5883L)+ −
accelerator_sleep_MMA8452Q();+ −
}+ −
+ −
+ −
// ===============================================================================+ −
// acceleration_read+ −
/// @brief empty for for LSM303D+ −
// ===============================================================================+ −
void acceleration_read(void)+ −
{+ −
if(hardwareCompass == compass_generation2) //LSM303D)+ −
acceleration_read_LSM303D();+ −
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+ −
// ===============================================================================+ −
uint8_t LSM303D_read_reg(uint8_t addr)+ −
{+ −
uint8_t data;+ −
+ −
I2C_Master_Transmit( DEVICE_COMPASS_303D, &addr, 1);+ −
I2C_Master_Receive( DEVICE_COMPASS_303D, &data, 1);+ −
return data;+ −
}+ −
+ −
+ −
// ===============================================================================+ −
// LSM303D_write_reg+ −
// ===============================================================================+ −
void LSM303D_write_reg(uint8_t addr, uint8_t value)+ −
{+ −
uint8_t data[2];+ −
+ −
/* enable accel*/+ −
data[0] = addr;+ −
data[1] = value;+ −
I2C_Master_Transmit( DEVICE_COMPASS_303D, data, 2);+ −
}+ −
+ −
+ −
// ===============================================================================+ −
// LSM303D_write_checked_reg+ −
// ===============================================================================+ −
void LSM303D_write_checked_reg(uint8_t addr, uint8_t value)+ −
{+ −
LSM303D_write_reg(addr, value);+ −
}+ −
+ −
+ −
// ===============================================================================+ −
// LSM303D_modify_reg+ −
// ===============================================================================+ −
void LSM303D_modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)+ −
{+ −
uint8_t val;+ −
+ −
val = LSM303D_read_reg(reg);+ −
val &= ~clearbits;+ −
val |= setbits;+ −
LSM303D_write_checked_reg(reg, val);+ −
}+ −
+ −
// ===============================================================================+ −
// LSM303D_accel_set_onchip_lowpass_filter_bandwidth+ −
// ===============================================================================+ −
int LSM303D_accel_set_onchip_lowpass_filter_bandwidth(unsigned bandwidth)+ −
{+ −
uint8_t setbits = 0;+ −
uint8_t clearbits = REG2_ANTIALIAS_FILTER_BW_BITS_A;+ −
+ −
if (bandwidth == 0) {+ −
bandwidth = 773;+ −
}+ −
+ −
if (bandwidth <= 50) {+ −
setbits |= REG2_AA_FILTER_BW_50HZ_A;+ −
_accel_onchip_filter_bandwith = 50;+ −
+ −
} else if (bandwidth <= 194) {+ −
setbits |= REG2_AA_FILTER_BW_194HZ_A;+ −
_accel_onchip_filter_bandwith = 194;+ −
+ −
} else if (bandwidth <= 362) {+ −
setbits |= REG2_AA_FILTER_BW_362HZ_A;+ −
_accel_onchip_filter_bandwith = 362;+ −
+ −
} else if (bandwidth <= 773) {+ −
setbits |= REG2_AA_FILTER_BW_773HZ_A;+ −
_accel_onchip_filter_bandwith = 773;+ −
+ −
} else {+ −
return -1;+ −
}+ −
+ −
LSM303D_modify_reg(ADDR_CTRL_REG2, clearbits, setbits);+ −
+ −
return 0;+ −
}+ −
+ −
+ −
// ===============================================================================+ −
// LSM303D_accel_set_driver_lowpass_filter+ −
// ===============================================================================+ −
int LSM303D_accel_set_driver_lowpass_filter(float samplerate, float bandwidth)+ −
{+ −
/*+ −
_accel_filter_x_set_cutoff_frequency(samplerate, bandwidth);+ −
_accel_filter_y_set_cutoff_frequency(samplerate, bandwidth);+ −
_accel_filter_z_set_cutoff_frequency(samplerate, bandwidth);+ −
*/ + −
return 0;+ −
}+ −
+ −
+ −
// rotate_mag_3f: nicht genutzt aber praktisch; rotate_accel_3f wird benutzt+ −
// ===============================================================================+ −
// rotate_mag_3f+ −
/// @brief swap axis in convient way, by hw+ −
/// @param *x raw input is set to *y input+ −
/// @param *y raw input is set to -*x input+ −
/// @param *z raw is not touched+ −
// ===============================================================================+ −
void rotate_mag_3f(float *x, float *y, float *z)+ −
{+ −
return;+ −
/* + −
*x = *x; // HMC: *x = -*y+ −
*y = *y; // HMC: *y = *x // change 20.04.2016: zuvor *y = -*y+ −
*z = *z; // HMC: *z = *z + −
*/ + −
}+ −
+ −
+ −
// ===============================================================================+ −
// rotate_accel_3f+ −
/// @brief swap axis in convient way, by hw, same as MMA8452Q+ −
/// @param *x raw input, output is with sign change+ −
/// @param *y raw input, output is with sign change+ −
/// @param *z raw input, output is with sign change+ −
// ===============================================================================+ −
void rotate_accel_3f(float *x, float *y, float *z)+ −
{+ −
*x = -*x;+ −
*y = -*y;+ −
*z = -*z;+ −
/* tested:+ −
x = x, y =-y, z=-z: does not work with roll+ −
x = x, y =y, z=-z: does not work with pitch+ −
x = x, y =y, z=z: does not work at all+ −
*/+ −
}+ −
+ −
+ −
// ===============================================================================+ −
// compass_init_LSM303D+ −
/// 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+ −
// ===============================================================================+ −
+ −
//uint8_t testCompassLS303D[11];+ −
+ −
void compass_init_LSM303D(uint8_t fast, uint8_t gain)+ −
{+ −
if(fast == 0)+ −
{+ −
LSM303D_write_checked_reg(ADDR_CTRL_REG0, 0x00);+ −
LSM303D_write_checked_reg(ADDR_CTRL_REG1, 0x3F); // mod 12,5 Hz 3 instead of 6,25 Hz 2+ −
LSM303D_write_checked_reg(ADDR_CTRL_REG2, 0xC0);+ −
LSM303D_write_checked_reg(ADDR_CTRL_REG3, 0x00);+ −
LSM303D_write_checked_reg(ADDR_CTRL_REG4, 0x00);+ −
LSM303D_write_checked_reg(ADDR_CTRL_REG5, 0x68); // mod 12,5 Hz 8 instead of 6,25 Hz 4+ −
}+ −
else+ −
{+ −
LSM303D_write_checked_reg(ADDR_CTRL_REG0, 0x00);+ −
LSM303D_write_checked_reg(ADDR_CTRL_REG1, 0x6F); // 100 Hz+ −
LSM303D_write_checked_reg(ADDR_CTRL_REG2, 0xC0);+ −
LSM303D_write_checked_reg(ADDR_CTRL_REG3, 0x00);+ −
LSM303D_write_checked_reg(ADDR_CTRL_REG4, 0x00);+ −
LSM303D_write_checked_reg(ADDR_CTRL_REG5, 0x74); // 100 Hz+ −
}+ −
LSM303D_write_checked_reg(ADDR_CTRL_REG6, 0x00);+ −
LSM303D_write_checked_reg(ADDR_CTRL_REG7, 0x00);+ −
+ −
return;+ −
}+ −
+ −
+ −
// ===============================================================================+ −
// compass_sleep_LSM303D+ −
// @brief Gen 2 chip+ −
// ===============================================================================+ −
void compass_sleep_LSM303D(void)+ −
{+ −
LSM303D_write_checked_reg(ADDR_CTRL_REG1, 0x00); // CNTRL1: acceleration sensor Power-down mode+ −
LSM303D_write_checked_reg(ADDR_CTRL_REG7, 0x02); // CNTRL7: magnetic sensor Power-down mode+ −
}+ −
+ −
+ −
// ===============================================================================+ −
// acceleration_read_LSM303D+ −
// output is accel_DX_f, accel_DY_f, accel_DZ_f+ −
// ===============================================================================+ −
void acceleration_read_LSM303D(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;+ −
I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1);+ −
I2C_Master_Receive( DEVICE_COMPASS_303D, &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_LSM303D+ −
///+ −
/// output is compass_DX_f, compass_DY_f, compass_DZ_f+ −
// ===============================================================================+ −
void compass_read_LSM303D(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 = ADDR_OUT_X_L_M + i;+ −
I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1);+ −
I2C_Master_Receive( DEVICE_COMPASS_303D, &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;+ −
}+ −
+ −
+ −
// ===============================================================================+ −
// 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+ −
// ===============================================================================+ −
+ −
void compass_init_LSM303AGR(uint8_t fast, uint8_t gain)+ −
{+ −
if(fast == 0)+ −
{+ −
// init compass+ −
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+ −
LSM303AGR_write_checked_reg(0x63, 0x00); // INT_CTRL_REG_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 (50Hz, 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, 0x08); // CTRL_REG4_A, High Resolution Mode enabled+ −
}+ −
else+ −
{+ −
// init compass+ −
LSM303AGR_write_checked_reg(0x60, 0x84); // 20Hz+ −
LSM303AGR_write_checked_reg(0x61, 0x03); // CFG_REG_B_M+ −
LSM303AGR_write_checked_reg(0x62, 0x10); // CFG_REG_C_M+ −
LSM303AGR_write_checked_reg(0x63, 0x00); // INT_CTRL_REG_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 (50Hz, 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, 0x0); // CTRL_REG4_A, High Resolution Mode enabled+ −
}+ −
+ −
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); //+ −
}+ −
+ −
+ −
// ===============================================================================+ −
// 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 = 0x28 + i; // OUT_X_L_A+ −
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; // flip Z in gen 2 hardware+ −
+ −
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]))));+ −
+ −
// align axis in gen 2 hardware+ −
compass_DZ_f *= -1;+ −
+ −
return;+ −
}+ −
+ −
+ −
// --------------------------------------------------------------------------------+ −
// ----------EARLIER COMPONENTS ---------------------------------------------------+ −
// --------------------------------------------------------------------------------+ −
+ −
// ===============================================================================+ −
// compass_init_HMC5883L+ −
/// @brief The horrible Honeywell compass chip+ −
/// This might be called several times during calibration+ −
///+ −
/// @param fast: 1 is fast mode, 0 is normal mode+ −
/// @param gain: 7 is max gain and set with here, compass_calib() might reduce it+ −
// ===============================================================================+ −
void compass_init_HMC5883L(uint8_t fast, uint8_t gain)+ −
{+ −
uint8_t write_buffer[4];+ −
+ −
compass_gain = gain;+ −
uint16_t length = 0;+ −
write_buffer[0] = 0x00; // 00 = config Register A+ −
+ −
if( fast )+ −
write_buffer[1] = 0x38; // 0b 0011 1000; // ConfigA: 75Hz, 2 Samples averaged+ −
else+ −
write_buffer[1] = 0x68; // 0b 0110 1000; // ConfigA: 3Hz, 8 Samples averaged+ −
+ −
switch(gain)+ −
{+ −
case 7:+ −
write_buffer[2] = 0xE0; //0b 1110 0000; // ConfigB: gain+ −
break;+ −
case 6:+ −
write_buffer[2] = 0xC0; //0b 1100 0000; // ConfigB: gain+ −
break;+ −
case 5:+ −
write_buffer[2] = 0xA0; //0b 1010 0000; // ConfigB: gain+ −
break;+ −
case 4:+ −
write_buffer[2] = 0x80; //0b 1000 0000; // ConfigB: gain+ −
break;+ −
case 3:+ −
write_buffer[2] = 0x60; //0b 0110 0000; // ConfigB: gain+ −
break;+ −
case 2:+ −
write_buffer[2] = 0x40; //0b 01000 0000; // ConfigB: gain+ −
break;+ −
case 1:+ −
write_buffer[2] = 0x20; //0b 00100 0000; // ConfigB: gain+ −
break;+ −
case 0:+ −
write_buffer[2] = 0x00; //0b 00000 0000; // ConfigB: gain+ −
break;+ −
}+ −
write_buffer[3] = 0x00; // Mode: continuous mode+ −
length = 4;+ −
//hmc_twi_write(0);+ −
I2C_Master_Transmit( DEVICE_COMPASS_HMC5883L, write_buffer, length);+ −
}+ −
+ −
+ −
+ −
// ===============================================================================+ −
// compass_sleep_HMC5883L+ −
/// @brief Power-down mode for Honeywell compass chip+ −
// ===============================================================================+ −
void compass_sleep_HMC5883L(void)+ −
{+ −
uint8_t write_buffer[4];+ −
uint16_t length = 0;+ −
+ −
write_buffer[0] = 0x00; // 00 = config Register A+ −
write_buffer[1] = 0x68; // 0b 0110 1000; // ConfigA+ −
write_buffer[2] = 0x20; // 0b 0010 0000; // ConfigB+ −
write_buffer[3] = 0x02; // 0b 0000 0010; // Idle Mode+ −
length = 4;+ −
I2C_Master_Transmit( DEVICE_COMPASS_HMC5883L, write_buffer, length);+ −
}+ −
+ −
+ −
// ===============================================================================+ −
// accelerator_init_MMA8452Q+ −
/// @brief Power-down mode for acceleration chip used in combination with Honeywell compass+ −
// ===============================================================================+ −
void accelerator_init_MMA8452Q(void)+ −
{+ −
uint8_t write_buffer[4];+ −
uint16_t length = 0;+ −
//HAL_Delay(1);+ −
//return;+ −
write_buffer[0] = 0x0E; // XYZ_DATA_CFG+ −
write_buffer[1] = 0x00;//0b00000000; // High pass Filter=0 , +/- 2g range+ −
length = 2;+ −
I2C_Master_Transmit( DEVICE_ACCELARATOR_MMA8452Q, write_buffer, length);+ −
//HAL_Delay(1);+ −
write_buffer[0] = 0x2A; // CTRL_REG1+ −
write_buffer[1] = 0x34; //0b00110100; // CTRL_REG1: 160ms data rate, St.By Mode, reduced noise mode+ −
write_buffer[2] = 0x02; //0b00000010; // CTRL_REG2: High Res in Active mode+ −
length = 3;+ −
I2C_Master_Transmit( DEVICE_ACCELARATOR_MMA8452Q, write_buffer, length);+ −
+ −
//HAL_Delay(1);+ −
//hw_delay_us(100);+ −
write_buffer[0] = 0x2A; // CTRL_REG1+ −
write_buffer[1] = 0x35; //0b00110101; // CTRL_REG1: ... Active Mode+ −
length = 2;+ −
I2C_Master_Transmit( DEVICE_ACCELARATOR_MMA8452Q, write_buffer, length);+ −
/* + −
HAL_Delay(6);+ −
compass_read();+ −
HAL_Delay(1);+ −
acceleration_read();+ −
+ −
compass_calc();+ −
*/ + −
}+ −
+ −
+ −
// ===============================================================================+ −
// accelerator_sleep_MMA8452Q+ −
/// @brief compass_sleep_HMC5883L+ −
// ===============================================================================+ −
void accelerator_sleep_MMA8452Q(void)+ −
{+ −
uint16_t length = 0;+ −
uint8_t write_buffer[4];+ −
+ −
write_buffer[0] = 0x2A; // CTRL_REG1+ −
write_buffer[1] = 0x00; //0b00000000; // CTRL_REG1: Standby Mode+ −
length = 2;+ −
I2C_Master_Transmit( DEVICE_ACCELARATOR_MMA8452Q, write_buffer, length);+ −
}+ −
+ −
+ −
// ===============================================================================+ −
// compass_read_HMC5883L+ −
/// @brief The new ST 303D - get ALL data and store in static variables+ −
///+ −
/// output is compass_DX_f, compass_DY_f, compass_DZ_f+ −
// ===============================================================================+ −
void compass_read_HMC5883L(void)+ −
{+ −
uint8_t buffer[20];+ −
compass_DX_f = 0;+ −
compass_DY_f = 0;+ −
compass_DZ_f = 0;+ −
uint8_t length = 0;+ −
uint8_t read_buffer[6];+ −
signed_tword data;+ −
for(int i = 0; i<6;i++)+ −
read_buffer[i] = 0;+ −
buffer[0] = 0x03; // 03 = Data Output X MSB Register+ −
length = 1;+ −
I2C_Master_Transmit( DEVICE_COMPASS_HMC5883L, buffer, length);+ −
length = 6;+ −
I2C_Master_Receive( DEVICE_COMPASS_HMC5883L, read_buffer, length);+ −
+ −
+ −
data.Byte.hi = read_buffer[0];+ −
data.Byte.low = read_buffer[1];+ −
//Y = Z+ −
compass_DY_f = - data.Word;+ −
+ −
data.Byte.hi = read_buffer[2];+ −
data.Byte.low = read_buffer[3];+ −
compass_DZ_f = data.Word;+ −
+ −
data.Byte.hi = read_buffer[4];+ −
data.Byte.low = read_buffer[5]; + −
//X = -Y+ −
compass_DX_f = data.Word;+ −
}+ −
+ −
+ −
// ===============================================================================+ −
// acceleration_read_MMA8452Q+ −
/// @brief The old MMA8452Q used with the Honeywell compass+ −
/// get the acceleration data and store in static variables+ −
///+ −
/// output is accel_DX_f, accel_DY_f, accel_DZ_f+ −
// ===============================================================================+ −
void acceleration_read_MMA8452Q(void)+ −
{+ −
uint8_t buffer[20];+ −
accel_DX_f = 0;+ −
accel_DY_f = 0;+ −
accel_DZ_f = 0;+ −
uint8_t length = 0;+ −
// bit8_Type status ;+ −
uint8_t read_buffer[7];+ −
signed_tword data;+ −
for(int i = 0; i<6;i++)+ −
read_buffer[i] = 0;+ −
buffer[0] = 0x00; // 03 = Data Output X MSB Register+ −
length = 1;+ −
I2C_Master_Transmit( DEVICE_ACCELARATOR_MMA8452Q, buffer, length);+ −
length = 7;+ −
I2C_Master_Receive( DEVICE_ACCELARATOR_MMA8452Q, read_buffer, length);+ −
+ −
// status.uw = read_buffer[0];+ −
data.Byte.hi = read_buffer[1];+ −
data.Byte.low = read_buffer[2];+ −
accel_DX_f =data.Word/16;+ −
data.Byte.hi = read_buffer[3];+ −
data.Byte.low = read_buffer[4];+ −
accel_DY_f =data.Word/16;+ −
data.Byte.hi = read_buffer[5];+ −
data.Byte.low = read_buffer[6];+ −
accel_DZ_f =data.Word/16;+ −
+ −
accel_DX_f *= -1;+ −
accel_DY_f *= -1;+ −
accel_DZ_f *= -1;+ −
}+ −
+ −
+ −
// ===============================================================================+ −
// compass_calc_roll_pitch_only+ −
/// @brief only the roll and pitch parts of compass_calc()+ −
///+ −
/// input is accel_DX_f, accel_DY_f, accel_DZ_f+ −
/// output is compass_pitch and compass_roll+ −
// ===============================================================================+ −
void compass_calc_roll_pitch_only(void)+ −
{+ −
float sinPhi, cosPhi;+ −
float Phi, Teta;+ −
+ −
//---- Calculate sine and cosine of roll angle Phi -----------------------+ −
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 ---------------------+ −
Teta = atanf(-(float)accel_DX_f/(accel_DY_f * sinPhi + accel_DZ_f * cosPhi));+ −
compass_pitch = Teta * 180.0f /PI;+ −
}+ −
+ −
+ −
// ===============================================================================+ −
// compass_calc+ −
/// @brief all the fancy stuff first implemented in OSTC3+ −
///+ −
/// input is compass_DX_f, compass_DY_f, compass_DZ_f, accel_DX_f, accel_DY_f, accel_DZ_f+ −
/// and compass_CX_f, compass_CY_f, compass_CZ_f+ −
/// output is compass_heading, compass_pitch and compass_roll+ −
// ===============================================================================+ −
void compass_calc(void)+ −
{+ −
float sinPhi, cosPhi, sinTeta, cosTeta;+ −
float Phi, Teta, Psi;+ −
int16_t iBfx, iBfy;+ −
int16_t iBpx, iBpy, iBpz;+ −
+ −
//---- Make hard iron correction -----------------------------------------+ −
// Measured magnetometer orientation, measured ok.+ −
// From matthias drawing: (X,Y,Z) --> (X,Y,Z) : no rotation.+ −
iBpx = compass_DX_f - compass_CX_f; // X+ −
iBpy = compass_DY_f - compass_CY_f; // Y+ −
iBpz = compass_DZ_f - compass_CZ_f; // Z+ −
+ −
//---- 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);+ −
+ −
//---- rotate by roll angle (-Phi) ---------------------------------------+ −
iBfy = iBpy * cosPhi - iBpz * sinPhi;+ −
iBpz = iBpy * sinPhi + iBpz * cosPhi;+ −
//Gz = imul(accel_DY_f, sin) + imul(accel_DZ_f, cos);+ −
+ −
//---- calculate sin and cosine of pitch angle Theta ---------------------+ −
//sincos(Gz, -accel_DX_f, &sin, &cos); // NOTE: changed sin sign.+ −
// Teta takes into account roll of computer and sends combination of Y and Z :-) understand now hw 160421+ −
Teta = atanf(-(float)accel_DX_f/(accel_DY_f * sinPhi + accel_DZ_f * cosPhi));+ −
compass_pitch = Teta * 180.0f /PI;+ −
sinTeta = sinf(Teta);+ −
cosTeta = cosf(Teta);+ −
/* correct cosine if pitch not in range -90 to 90 degrees */+ −
if( cosTeta < 0 ) cosTeta = -cosTeta;+ −
+ −
///---- de-rotate by pitch angle Theta -----------------------------------+ −
iBfx = iBpx * cosTeta + iBpz * sinTeta;+ −
+ −
//---- Detect uncalibrated compass ---------------------------------------+ −
if( !compass_CX_f && !compass_CY_f && !compass_CZ_f )+ −
{+ −
compass_heading = -1;+ −
return;+ −
}+ −
+ −
//---- calculate current yaw = e-compass angle Psi -----------------------+ −
// Result in degree (no need of 0.01 deg precision...+ −
Psi = atan2f(-iBfy,iBfx);+ −
compass_heading = Psi * 180.0f /PI;+ −
// Result in 0..360 range:+ −
if( compass_heading < 0 )+ −
compass_heading += 360;+ −
}+ −
+ −
+ −
// //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////+ −
// // - Calibration - ///////////////////////////////////////////////////////////////////////////////////////////////////////+ −
// //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////+ −
+ −
/* can be lost during sleep as those are reset with compass_reset_calibration() */+ −
+ −
// ===============================================================================+ −
// compass_reset_calibration+ −
/// @brief all the fancy stuff first implemented in OSTC3+ −
///+ −
/// output is struct g and compass_CX_f, compass_CY_f, compass_CZ_f+ −
///+ −
/// @param g: is a struct with crazy stuff like Suuu, Svvv, Svvu, etc.+ −
/// all is set to zero here+ −
// ===============================================================================+ −
void compass_reset_calibration(SCompassCalib *g)+ −
{+ −
g->compass_N = 0;+ −
g->Su = g->Sv = g->Sw = 0.0;+ −
g->Suu = g->Svv = g->Sww = g->Suv = g->Suw = g->Svw = 0.0;+ −
g->Suuu = g->Svvv = g->Swww = 0.0;+ −
g->Suuv = g->Suuw = g->Svvu = g->Svvw = g->Swwu = g->Swwv = 0.0;+ −
compass_CX_f = compass_CY_f = compass_CZ_f = 0.0;+ −
}+ −
+ −
+ −
// ===============================================================================+ −
// compass_add_calibration+ −
/// @brief all the fancy stuff first implemented in OSTC3+ −
///+ −
/// input is compass_DX_f, compass_DY_f, compass_DZ_f+ −
/// and compass_CX_f, compass_CY_f, compass_CZ_f+ −
/// output is struct g+ −
///+ −
/// @param g: is a struct with crazy stuff like Suuu, Svvv, Svvu, etc.+ −
// ===============================================================================+ −
void compass_add_calibration(SCompassCalib *g)+ −
{+ −
float u, v, w;+ −
+ −
u = (compass_DX_f - compass_CX_f) / 32768.0f;+ −
v = (compass_DY_f - compass_CY_f) / 32768.0f;+ −
w = (compass_DZ_f - compass_CZ_f) / 32768.0f;+ −
+ −
g->compass_N++;+ −
g->Su += u;+ −
g->Sv += v;+ −
g->Sw += w;+ −
g->Suv += u*v;+ −
g->Suw += u*w;+ −
g->Svw += v*w;+ −
g->Suu += u*u;+ −
g->Suuu += u*u*u;+ −
g->Suuv += v*u*u;+ −
g->Suuw += w*u*u;+ −
g->Svv += v*v;+ −
g->Svvv += v*v*v;+ −
g->Svvu += u*v*v;+ −
g->Svvw += w*v*v;+ −
g->Sww += w*w;+ −
g->Swww += w*w*w;+ −
g->Swwu += u*w*w;+ −
g->Swwv += v*w*w;+ −
}+ −
+ −
//////////////////////////////////////////////////////////////////////////////+ −
+ −
// ===============================================================================+ −
// compass_solve_calibration+ −
/// @brief all the fancy stuff first implemented in OSTC3+ −
///+ −
/// input is compass_CX_f, compass_CY_f, compass_CZ_f and g+ −
/// output is struct g+ −
///+ −
/// @param g: is a struct with crazy stuff like Suuu, Svvv, Svvu, etc.+ −
// ===============================================================================+ −
void compass_solve_calibration(SCompassCalib *g)+ −
{+ −
float yu, yv, yw;+ −
float delta;+ −
float uc, vc, wc;+ −
+ −
+ −
//---- Normalize partial sums --------------------------------------------+ −
//+ −
// u, v, w should be centered on the mean value um, vm, wm:+ −
// x = u + um, with um = Sx/N+ −
//+ −
// So:+ −
// (u + um)**2 = u**2 + 2u*um + um**2+ −
// Su = 0, um = Sx/N+ −
// Sxx = Suu + 2 um Su + N*(Sx/N)**2 = Suu + Sx**2/N+ −
// Suu = Sxx - Sx**2/N+ −
yu = g->Su/g->compass_N;+ −
yv = g->Sv/g->compass_N;+ −
yw = g->Sw/g->compass_N;+ −
+ −
g->Suu -= g->Su*yu;+ −
g->Svv -= g->Sv*yv;+ −
g->Sww -= g->Sw*yw;+ −
+ −
// (u + um)(v + vm) = uv + u vm + v um + um vm+ −
// Sxy = Suv + N * um vm+ −
// Suv = Sxy - N * (Sx/N)(Sy/N);+ −
g->Suv -= g->Su*yv;+ −
g->Suw -= g->Su*yw;+ −
g->Svw -= g->Sv*yw;+ −
+ −
// (u + um)**3 = u**3 + 3 u**2 um + 3 u um**2 + um**3+ −
// Sxxx = Suuu + 3 um Suu + 3 um**2 Su + N.um**3+ −
// Su = 0, um = Sx/N:+ −
// Suuu = Sxxx - 3 Sx*Suu/N - N.(Sx/N)**3+ −
// = Sxxx - 3 Sx*Suu/N - Sx**3/N**2+ −
+ −
// (u + um)**2 (v + vm) = (u**2 + 2 u um + um**2)(v + vm)+ −
// Sxxy = Suuv + vm Suu + 2 um (Suv + vm Su) + um**2 (Sv + N.vm)+ −
//+ −
// Su = 0, Sv = 0, vm = Sy/N:+ −
// Sxxy = Suuv + vm Suu + 2 um Suv + N um**2 vm+ −
//+ −
// Suuv = Sxxy - (Sy/N) Suu - 2 (Sx/N) Suv - (Sx/N)**2 Sy+ −
// = Sxxy - Suu*Sy/N - 2 Suv*Sx/N - Sx*Sx*Sy/N/N+ −
// = Sxxy - (Suu + Sx*Sx/N)*Sy/N - 2 Suv*Sx/N+ −
g->Suuu -= (3*g->Suu + g->Su*yu)*yu;+ −
g->Suuv -= (g->Suu + g->Su*yu)*yv + 2*g->Suv*yu;+ −
g->Suuw -= (g->Suu + g->Su*yu)*yw + 2*g->Suw*yu;+ −
+ −
g->Svvu -= (g->Svv + g->Sv*yv)*yu + 2*g->Suv*yv;+ −
g->Svvv -= (3*g->Svv + g->Sv*yv)*yv;+ −
g->Svvw -= (g->Svv + g->Sv*yv)*yw + 2*g->Svw*yv;+ −
+ −
g->Swwu -= (g->Sww + g->Sw*yw)*yu + 2*g->Suw*yw;+ −
g->Swwv -= (g->Sww + g->Sw*yw)*yv + 2*g->Svw*yw;+ −
g->Swww -= (3*g->Sww + g->Sw*yw)*yw;+ −
+ −
//---- Solve the system --------------------------------------------------+ −
// uc Suu + vc Suv + wc Suw = (Suuu + Svvu + Swwu) / 2+ −
// uc Suv + vc Svv + wc Svw = (Suuv + Svvv + Swwv) / 2+ −
// uc Suw + vc Svw + wc Sww = (Suuw + Svvw + Swww) / 2+ −
// Note this is symetric, with a positiv diagonal, hence+ −
// it always have a uniq solution.+ −
yu = 0.5f * (g->Suuu + g->Svvu + g->Swwu);+ −
yv = 0.5f * (g->Suuv + g->Svvv + g->Swwv);+ −
yw = 0.5f * (g->Suuw + g->Svvw + g->Swww);+ −
delta = g->Suu * (g->Svv * g->Sww - g->Svw * g->Svw)+ −
- g->Suv * (g->Suv * g->Sww - g->Svw * g->Suw)+ −
+ g->Suw * (g->Suv * g->Svw - g->Svv * g->Suw);+ −
+ −
uc = (yu * (g->Svv * g->Sww - g->Svw * g->Svw)+ −
- yv * (g->Suv * g->Sww - g->Svw * g->Suw)+ −
+ yw * (g->Suv * g->Svw - g->Svv * g->Suw) )/delta;+ −
vc = (g->Suu * ( yv * g->Sww - yw * g->Svw)+ −
- g->Suv * ( yu * g->Sww - yw * g->Suw)+ −
+ g->Suw * ( yu * g->Svw - yv * g->Suw) )/delta;+ −
wc = (g->Suu * (g->Svv * yw - g->Svw * yv )+ −
- g->Suv * (g->Suv * yw - g->Svw * yu )+ −
+ g->Suw * (g->Suv * yv - g->Svv * yu ) )/delta;+ −
+ −
// Back to uncentered coordinates:+ −
// xc = um + uc+ −
uc = g->Su/g->compass_N + compass_CX_f/32768.0f + uc;+ −
vc = g->Sv/g->compass_N + compass_CY_f/32768.0f + vc;+ −
wc = g->Sw/g->compass_N + compass_CZ_f/32768.0f + wc;+ −
+ −
// Then save the new calibrated center:+ −
compass_CX_f = (short)(32768 * uc);+ −
compass_CY_f = (short)(32768 * vc);+ −
compass_CZ_f = (short)(32768 * wc);+ −
}+ −
+ −
+ −
// ===============================================================================+ −
// compass_calib+ −
/// @brief the main loop for calibration+ −
/// output is compass_CX_f, compass_CY_f, compass_CZ_f and g+ −
/// 160704 removed -4096 limit for LSM303D+ −
///+ −
/// @return always 0+ −
// ===============================================================================+ −
int compass_calib_common(void)+ −
{+ −
SCompassCalib g;+ −
+ −
// Starts with no calibration at all:+ −
compass_reset_calibration(&g);+ −
uint32_t tickstart = 0;+ −
tickstart = HAL_GetTick();+ −
/* run calibration for one minute */+ −
while(time_elapsed_ms(tickstart,HAL_GetTick()) < 60000)+ −
{+ −
while((SPI_Evaluate_RX_Data() == 0) && (time_elapsed_ms(tickstart,HAL_GetTick()) < 60000))+ −
{+ −
HAL_Delay(1);+ −
}+ −
compass_read();+ −
acceleration_read();+ −
compass_calc_roll_pitch_only();+ −
+ −
if((hardwareCompass == compass_generation1 ) //HMC5883L)+ −
&&((compass_DX_f == -4096) ||+ −
(compass_DY_f == -4096) ||+ −
(compass_DZ_f == -4096) ))+ −
{+ −
if(compass_gain == 0)+ −
return -1;+ −
compass_gain--;+ −
compass_init(1, compass_gain);+ −
compass_reset_calibration(&g);+ −
continue;+ −
}+ −
+ −
copyCompassDataDuringCalibration(compass_DX_f,compass_DY_f,compass_DZ_f);+ −
compass_add_calibration(&g);+ −
}+ −
+ −
compass_solve_calibration(&g);+ −
+ −
tfull32 dataBlock[4];+ −
dataBlock[0].Word16.low16 = compass_CX_f;+ −
dataBlock[0].Word16.hi16 = compass_CY_f;+ −
dataBlock[1].Word16.low16 = compass_CZ_f;+ −
dataBlock[1].Word16.hi16 = 0xFFFF;+ −
dataBlock[2].Full32 = 0x7FFFFFFF;+ −
dataBlock[3].Full32 = 0x7FFFFFFF;+ −
BFA_writeDataBlock((uint32_t *)dataBlock);+ −
+ −
return 0;+ −
}+ −
+ −