view Small_CPU/Src/compass.c @ 909:6902bb4b6b2e Evo_2_23

Define state of new GPIO out pins: Some new GPIO pins were initialized but not used. This may cause some random activations e.g. of the red debug LED. To avoid this the output levels are set to inactive levels (e.g. to LED_OFF)
author Ideenmodellierer
date Tue, 15 Oct 2024 19:07:19 +0200
parents 08af5d707c5a
children
line wrap: on
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>&copy; 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 "scheduler.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);
extern SGlobal global;


/// 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 = compass_generation_undef;	///< 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;
	}
	
	if(hardwareCompass == compass_generation_undef)					/* check if compass had been detected before */
	{
		tfull32 dataBlock[4];
		if(BFA_readLastDataBlock(dataBlock) == BFA_OK)
		{
			if(dataBlock[3].Word16.hi16 == BFA_calc_Block_Checksum(dataBlock))
			{
				compass_CX_f = dataBlock[0].Word16.low16;
				compass_CY_f = dataBlock[0].Word16.hi16;
				compass_CZ_f = dataBlock[1].Word16.low16;
				hardwareCompass = dataBlock[1].Word16.hi16;
				if(hardwareCompass >= compass_generation_future)		/* no generation stored (including COMPASS_NOT_RECOGNIZED) */
				{
					hardwareCompass = compass_generation_undef;
				}
			}
		}
	}

// old code but without else
	if(hardwareCompass == compass_generation_undef)
	{
		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 == compass_generation_undef)
	{
		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;
		}
		else
		{
			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 == compass_generation_undef)
		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);
	
	if(global.deviceDataSendToMaster.hw_Info.compass == 0)
	{
		global.deviceDataSendToMaster.hw_Info.compass = hardwareCompass;
		global.deviceDataSendToMaster.hw_Info.checkCompass = 1;
	}
}


//  ===============================================================================
//	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 BDU and all axis
		LSM303D_write_checked_reg(ADDR_CTRL_REG2, 0xC0); // 50Hz anti alias filter
		LSM303D_write_checked_reg(ADDR_CTRL_REG3, 0x00); // no interrupts
		LSM303D_write_checked_reg(ADDR_CTRL_REG4, 0x00); // no interrupts
		LSM303D_write_checked_reg(ADDR_CTRL_REG5, 0x68); // mod 12,5 Hz 8 instead of 6,25 Hz 4 High resolution
	}
	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); // CFG_REG_A_M     10Hz continuous measurement
		LSM303AGR_write_checked_reg(0x61, 0x03); // CFG_REG_B_M		Enable offset cancellation and low pass filter
		LSM303AGR_write_checked_reg(0x62, 0x10); // CFG_REG_C_M		Avoid incoherence (BDU)
		LSM303AGR_write_checked_reg(0x63, 0x00); // INT_CTRL_REG_M	No interrupts

		// 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, 0x27); // CTRL_REG1_A (10Hz, x,y,z = ON, low power mode)
		LSM303AGR_acc_write_checked_reg(0x21, 0x00); // CTRL_REG2_A	(High pass filter normal mode)
		LSM303AGR_acc_write_checked_reg(0x22, 0x00); // CTRL_REG3_A	(no interrupts)
		LSM303AGR_acc_write_checked_reg(0x23, 0x88); // CTRL_REG4_A, High Resolution Mode enabled, enable BDU
	}
	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, 0x47); // 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, 0x88); // 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, 0x13); // low power and idle mode
	LSM303AGR_write_checked_reg(0x61, 0x04); // pulse only at power on
	LSM303AGR_write_checked_reg(0x62, 0x51); // int mag pin used (?), BDU and DRDY is output
	LSM303AGR_write_checked_reg(0x63, 0x00); // no interrupts


	LSM303AGR_acc_write_checked_reg(0x1F, 0x00); // temperature off
	LSM303AGR_acc_write_checked_reg(0x20, 0x00); //	power down
}


//  ===============================================================================
//	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);
		
    if((hardwareCompass != compass_generation_undef)		/* if compass is not know at this point in time storing data makes no sense */
    	&& (hardwareCompass != COMPASS_NOT_RECOGNIZED))
    {
		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 = hardwareCompass;
		dataBlock[2].Full32 = 0x7FFFFFFF;
		dataBlock[3].Word16.low16 = 0xFFFF;
		dataBlock[3].Word16.hi16 = BFA_calc_Block_Checksum(dataBlock);
		BFA_writeDataBlock(dataBlock);
    }
	return 0;
}