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)
{
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(10);
MX_I2C1_Init();
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();
}
else
if(hardwareCompass == compass_generation1) //HMC5883L)
{
compass_sleep_HMC5883L();
}
}
// ===============================================================================
// 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;
}