/**
  ******************************************************************************
  * @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 "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(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();
	}
	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;
}