# HG changeset patch
# User heinrichsweikamp <bitbucket@heinrichsweikamp.com>
# Date 1574610418 0
# Node ID 0dbb74be972f9b4c699ad0caab1eb03163332110
# Parent  c6a084d1433f23a41a8a7231567293a298848b2d# Parent  39c147e47c1c29a1201996d6b7346379f76f8976
Merged in Ideenmodellierer/ostc4/MotionDetection (pull request #34)

MotionDetection

diff -r 39c147e47c1c -r 0dbb74be972f Current build/OSTC4update_191009.bin
Binary file Current build/OSTC4update_191009.bin has changed
diff -r 39c147e47c1c -r 0dbb74be972f Current build/OSTC4update_191124.bin
Binary file Current build/OSTC4update_191124.bin has changed
diff -r 39c147e47c1c -r 0dbb74be972f Small_CPU/Inc/compass_LSM303D.h
--- a/Small_CPU/Inc/compass_LSM303D.h	Mon Oct 21 21:16:53 2019 +0200
+++ b/Small_CPU/Inc/compass_LSM303D.h	Sun Nov 24 15:46:58 2019 +0000
@@ -27,7 +27,7 @@
 /* Exported constants --------------------------------------------------------*/
 
 #define WHO_AM_I						0x0F 		// device identification register - default value
-#define WHOIAM_VALUE				(0x49)	// Who Am I default value
+#define WHOIAM_VALUE_LSM303D	0x49	// Who Am I default value
 
 #define ADDR_OUT_TEMP_L			0x05
 #define ADDR_OUT_TEMP_H			0x06
diff -r 39c147e47c1c -r 0dbb74be972f Small_CPU/Inc/i2c.h
--- a/Small_CPU/Inc/i2c.h	Mon Oct 21 21:16:53 2019 +0200
+++ b/Small_CPU/Inc/i2c.h	Sun Nov 24 15:46:58 2019 +0000
@@ -3,14 +3,25 @@
 #define I2C_H
 
 /* Pressure Sensor */
-#define DEVICE_PRESSURE        			0xEE 	// 2019 hardware (gen 3) will use 0xEC (MS5837), all other use 0xEE (MS5803)
+#define DEVICE_PRESSURE_MS5803     		0xEE 	// gen 1 and gen 2 use 0xEE (MS5803)
+#define	DEVICE_PRESSURE_MS5837   		0xEC	// end-2019 hardware (gen 3) uses 0xEC (MS5837)
 
 /* Compass/Accelerometer */
+#define COMPASS_NOT_RECOGNIZED  		0xAA	///< id used with hardwareCompass
+#define	compass_generation1				0x01	// Hardware gen 1 (Two chip solution with MMA8452Q and HMC5883L)
+#define	compass_generation2				0x02	// Hardware gen 2 (Single chip solution LSM303D)
+#define	compass_generation3				0x03	// Hardware gen 3 (Single chip solution LSM303AGR)
+
 #define DEVICE_ACCELARATOR_MMA8452Q 	0x38	// Hardware gen 1 (Two chip solution with MMA8452Q and HMC5883L)
-#define DEVICE_COMPASS_HMC5883L			0x3C	// Hardware gen 1
+#define DEVICE_COMPASS_HMC5883L			0x3C	// Hardware gen 1 (Two chip solution with MMA8452Q and HMC5883L)
 
 #define DEVICE_COMPASS_303D				0x3C 	// Hardware gen 2 (Single chip solution LSM303D)
+
 #define	DEVICE_COMPASS_303AGR			0x3C	// Hardware gen 3 (Single chip solution LSM303AGR)
+#define	DEVICE_ACCELARATOR_303AGR		0x32	// Hardware gen 3 (Single chip solution LSM303AGR)
+
+// Compass 3 defines (Can move in separate .h file...)
+#define WHOIAM_VALUE_LSM303AGR			0x33
 
 /* Battery Gas Gauge */
 #define DEVICE_BATTERYGAUGE 			0xC8 	// LTC2941 battery gauge
diff -r 39c147e47c1c -r 0dbb74be972f Small_CPU/Src/baseCPU2.c
--- a/Small_CPU/Src/baseCPU2.c	Mon Oct 21 21:16:53 2019 +0200
+++ b/Small_CPU/Src/baseCPU2.c	Sun Nov 24 15:46:58 2019 +0000
@@ -162,7 +162,7 @@
 // See CPU2-RTE.ld
 const SFirmwareData cpu2_FirmwareData __attribute__(( section(".firmware_data") ))= {
 		.versionFirst = 2,
-		.versionSecond = 0,
+		.versionSecond = 1,
 		.versionThird = 0,
 		.versionBeta = 0,
 
@@ -170,13 +170,13 @@
 		.signature = "mh",
 
 		.release_year = 19,
-		.release_month = 10,
-		.release_day = 6,
+		.release_month = 11,
+		.release_day = 23,
 		.release_sub = 0,
 
 		/* max 48 with trailing 0 */
 		//release_info ="12345678901234567890123456789012345678901"
-		.release_info = "stable Aug'19",
+		.release_info = "stable Nov'19",
 
 		/* for safety reasons and coming functions */
 		.magic[0] = FIRMWARE_MAGIC_FIRST, .magic[1] = FIRMWARE_MAGIC_SECOND,
diff -r 39c147e47c1c -r 0dbb74be972f Small_CPU/Src/compass.c
--- a/Small_CPU/Src/compass.c	Mon Oct 21 21:16:53 2019 +0200
+++ b/Small_CPU/Src/compass.c	Sun Nov 24 15:46:58 2019 +0000
@@ -39,9 +39,6 @@
 
 #include "stm32f4xx_hal.h"
 
-#define	TEST_IF_HMC5883L
-//#define COMPASS_DEACTIVATE
-
 /// split byte to bits
 typedef struct{ 
 uint8_t bit0:1; ///< split byte to bits
@@ -109,9 +106,6 @@
 #define Q_PI    (18000)
 #define Q_PIO2  (9000)
 
-#define HMC5883L (1)	///< id used with hardwareCompass 
-#define LSM303D  (2)	///< id used with hardwareCompass
-#define COMPASS_NOT_RECOGNIZED  (4)	///< id used with hardwareCompass
 
 
 //////////////////////////////////////////////////////////////////////////////
@@ -154,24 +148,12 @@
 
 uint8_t compass_gain; ///< 7 on start, can be reduced during calibration
 
-uint8_t  hardwareCompass = 0;	///< either  HMC5883L or LSM303D or not defined yet ( = 0 )
+uint8_t  hardwareCompass = 0;	///< either HMC5883L (=1) or LSM303D (=2) or LSM303AGR (=3) or not defined yet (=0)
 
 /// LSM303D variables
 uint8_t magDataBuffer[6];	///< here raw data from LSM303D is stored, can be local
 uint8_t accDataBuffer[6];	///< here raw data from LSM303D is stored, can be local
 
-//uint16_t velMag = 0;
-//uint16_t velAcc = 0;
-
-//uint16_t magODR[] = {31,62,125,250,500,1000,2000};
-//uint16_t accODR[] = {0,31,62,125,250,500,1000,2000,4000,8000,16000};
-//uint8_t fastest = 10; //no sensor is the fastest
-//uint8_t datas1 = 0;
-//uint8_t zoffFlag = 0;
-//uint8_t sendFlag = 0;
-
-
-// all by pixhawk code:
 
 //	struct accel_scale	_accel_scale;
 unsigned		_accel_range_m_s2;
@@ -223,6 +205,11 @@
 void compass_read_LSM303D(void);
 void acceleration_read_LSM303D(void);
 
+void compass_init_LSM303AGR(uint8_t fast, uint8_t gain);
+void compass_sleep_LSM303AGR(void);
+void compass_read_LSM303AGR(void);
+void acceleration_read_LSM303AGR(void);
+
 int LSM303D_accel_set_onchip_lowpass_filter_bandwidth(unsigned bandwidth);
 int compass_calib_common(void);
 
@@ -243,14 +230,8 @@
 /// @param 	gain: 7 is max gain, compass_calib() might reduce it
 //  ===============================================================================
 
-uint8_t testCompassTypeDebug = 0xFF;
-
 void compass_init(uint8_t fast, uint8_t gain)
 {
-// quick off
-#ifdef COMPASS_DEACTIVATE
-	hardwareCompass = COMPASS_NOT_RECOGNIZED;
-#endif
 
 // don't call again with fast, gain in calib mode etc.
 // if unknown
@@ -265,10 +246,13 @@
 		uint8_t data = WHO_AM_I;
 		I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1);
 		I2C_Master_Receive(  DEVICE_COMPASS_303D, &data, 1);
-		if(data == WHOIAM_VALUE)
-			hardwareCompass = LSM303D;
-		else
-			hardwareCompass = HMC5883L;
+		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 */
@@ -277,47 +261,47 @@
 		uint8_t data = WHO_AM_I;
 		I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1);
 		I2C_Master_Receive(  DEVICE_COMPASS_303D, &data, 1);
-		if(data == WHOIAM_VALUE)
-			hardwareCompass = LSM303D;
-		else
-			hardwareCompass = HMC5883L;
+		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 = HMC5883L;	
+		hardwareCompass = compass_generation1;				//HMC5883L;
 	
-#ifdef TEST_IF_HMC5883L
 	HAL_StatusTypeDef resultOfOperationHMC_MMA = HAL_TIMEOUT;
 
-	if(hardwareCompass == HMC5883L)
+	// 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 = HMC5883L; // all fine, keep it
+			hardwareCompass = compass_generation1;			//HMC5883L; // all fine, keep it
 		}
 		else
 		{
 			hardwareCompass = COMPASS_NOT_RECOGNIZED;
-			testCompassTypeDebug = 0xEC;
 		}
 	}
-#endif
 	
-	if(hardwareCompass == LSM303D)
-	{
+	if(hardwareCompass == compass_generation2)			//LSM303D)
 		compass_init_LSM303D(fast, gain);
-	}
-	else
-	if(hardwareCompass == HMC5883L)
-	{
+	if(hardwareCompass == compass_generation3)			//LSM303AGR)
+		compass_init_LSM303AGR(fast, gain);
+	if(hardwareCompass == compass_generation1)			//HMC5883L)
 		compass_init_HMC5883L(fast, gain);
-	}
 	
-		tfull32 dataBlock[4];
-		if(BFA_readLastDataBlock((uint32_t *)dataBlock) == BFA_OK)
+	tfull32 dataBlock[4];
+	if(BFA_readLastDataBlock((uint32_t *)dataBlock) == BFA_OK)
 		{
 			compass_CX_f = dataBlock[0].Word16.low16;
 			compass_CY_f = dataBlock[0].Word16.hi16;
@@ -333,7 +317,7 @@
 //  ===============================================================================
 int compass_calib(void)
 {
-	if(hardwareCompass == LSM303D)
+	if(hardwareCompass == compass_generation2)			//LSM303D)
 	{
 		LSM303D_accel_set_onchip_lowpass_filter_bandwidth(773);
 		int out = compass_calib_common();
@@ -341,7 +325,12 @@
 		return out;
 	}
 	else
-	if(hardwareCompass == HMC5883L)
+	if(hardwareCompass == compass_generation1)			//HMC5883L)
+	{
+		return compass_calib_common();
+	}
+	else
+	if(hardwareCompass == compass_generation3)			//LSM303AGR)
 	{
 		return compass_calib_common();
 	}
@@ -360,12 +349,12 @@
 //  ===============================================================================
 void compass_sleep(void)
 {
-	if(hardwareCompass == LSM303D)
+	if(hardwareCompass == compass_generation2)			//LSM303D)
 	{
 		compass_sleep_LSM303D();
 	}
 	else
-	if(hardwareCompass == HMC5883L)
+	if(hardwareCompass == compass_generation1)			//HMC5883L)
 	{
 		compass_sleep_HMC5883L();
 	}
@@ -379,15 +368,13 @@
 //  ===============================================================================
 void compass_read(void)
 {
-	if(hardwareCompass == LSM303D)
-	{
+	if(hardwareCompass == compass_generation2)			//LSM303D)
 		compass_read_LSM303D();
-	}
-	else
-	if(hardwareCompass == HMC5883L)
-	{
+	if(hardwareCompass == compass_generation1)			//HMC5883L)
 		compass_read_HMC5883L();
-	}
+	if(hardwareCompass == compass_generation3)			//LSM303AGR)
+			compass_read_LSM303AGR();
+
 }
 
 
@@ -397,7 +384,7 @@
 //  ===============================================================================
 void accelerator_init(void)
 {
-	if(hardwareCompass == HMC5883L)
+	if(hardwareCompass == compass_generation1)			//HMC5883L)
 		accelerator_init_MMA8452Q();
 }
 
@@ -408,7 +395,7 @@
 //  ===============================================================================
 void accelerator_sleep(void)
 {
-	if(hardwareCompass == HMC5883L)
+	if(hardwareCompass == compass_generation1)			//HMC5883L)
 		accelerator_sleep_MMA8452Q();
 }
 
@@ -419,23 +406,73 @@
 //  ===============================================================================
 void  acceleration_read(void)
 {
-	if(hardwareCompass == LSM303D)
-	{
+	if(hardwareCompass == compass_generation2)			//LSM303D)
 		acceleration_read_LSM303D();
-	}
-	else
-	if(hardwareCompass == HMC5883L)
-	{
+	if(hardwareCompass == compass_generation1)			//HMC5883L)
 		acceleration_read_MMA8452Q();
-	}
+	if(hardwareCompass == compass_generation3)			//LSM303AGR)
+		acceleration_read_LSM303AGR();
 }
 
 
 /* Private functions ---------------------------------------------------------*/
 
 //  ===============================================================================
+//	LSM303AGR_read_reg
+//  ===============================================================================
+uint8_t LSM303AGR_read_reg(uint8_t addr)
+{
+  uint8_t data;
+
+	I2C_Master_Transmit( DEVICE_COMPASS_303AGR, &addr, 1);
+	I2C_Master_Receive(  DEVICE_COMPASS_303AGR, &data, 1);
+	return data;
+}
+
+
+//  ===============================================================================
+//	LSM303AGR_write_reg
+//  ===============================================================================
+void LSM303AGR_write_reg(uint8_t addr, uint8_t value)
+{
+  uint8_t data[2];
+
+	data[0] = addr;
+	data[1] = value;
+	I2C_Master_Transmit( DEVICE_COMPASS_303AGR, data, 2);
+}
+
+//  ===============================================================================
+//	LSM303AGR_acc_write_reg
+//  ===============================================================================
+void LSM303AGR_acc_write_reg(uint8_t addr, uint8_t value)
+{
+  uint8_t data[2];
+
+	data[0] = addr;
+	data[1] = value;
+	I2C_Master_Transmit( DEVICE_ACCELARATOR_303AGR, data, 2);
+}
+
+
+//  ===============================================================================
+//	LSM303AGR_write_checked_reg
+//  ===============================================================================
+void LSM303AGR_write_checked_reg(uint8_t addr, uint8_t value)
+{
+	LSM303AGR_write_reg(addr, value);
+}
+
+//  ===============================================================================
+//	LSM303AGR_acc_write_checked_reg
+//  ===============================================================================
+void LSM303AGR_acc_write_checked_reg(uint8_t addr, uint8_t value)
+{
+	LSM303AGR_acc_write_reg(addr, value);
+}
+
+//  ===============================================================================
 //	LSM303D_read_reg
-/// @brief	tiny helpers by pixhawk
 //  ===============================================================================
 uint8_t LSM303D_read_reg(uint8_t addr)
 {
@@ -449,7 +486,6 @@
 
 //  ===============================================================================
 //	LSM303D_write_reg
-/// @brief	tiny helpers by pixhawk
 //  ===============================================================================
 void LSM303D_write_reg(uint8_t addr, uint8_t value)
 {
@@ -464,7 +500,6 @@
 
 //  ===============================================================================
 //	LSM303D_write_checked_reg
-/// @brief	tiny helpers by pixhawk. This runs unchecked at the moment.
 //  ===============================================================================
 void LSM303D_write_checked_reg(uint8_t addr, uint8_t value)
 {
@@ -474,7 +509,6 @@
 
 //  ===============================================================================
 //	LSM303D_modify_reg
-/// @brief	tiny helpers by pixhawk
 //  ===============================================================================
 void LSM303D_modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
 {
@@ -486,108 +520,8 @@
 	LSM303D_write_checked_reg(reg, val);
 }
 
-/*
-//  ===============================================================================
-//	LSM303D_accel_set_range
-/// @brief	tiny helpers by pixhawk
-//  ===============================================================================
-int LSM303D_accel_set_range(unsigned max_g)
-{
-	uint8_t setbits = 0;
-	uint8_t clearbits = REG2_FULL_SCALE_BITS_A;
-	float new_scale_g_digit = 0.0f;
-
-	if (max_g == 0) {
-		max_g = 16;
-	}
-
-	if (max_g <= 2) {
-		_accel_range_m_s2 = 2.0f * LSM303D_ONE_G;
-		setbits |= REG2_FULL_SCALE_2G_A;
-		new_scale_g_digit = 0.061e-3f;
-
-	} else if (max_g <= 4) {
-		_accel_range_m_s2 = 4.0f * LSM303D_ONE_G;
-		setbits |= REG2_FULL_SCALE_4G_A;
-		new_scale_g_digit = 0.122e-3f;
-
-	} else if (max_g <= 6) {
-		_accel_range_m_s2 = 6.0f * LSM303D_ONE_G;
-		setbits |= REG2_FULL_SCALE_6G_A;
-		new_scale_g_digit = 0.183e-3f;
-
-	} else if (max_g <= 8) {
-		_accel_range_m_s2 = 8.0f * LSM303D_ONE_G;
-		setbits |= REG2_FULL_SCALE_8G_A;
-		new_scale_g_digit = 0.244e-3f;
-
-	} else if (max_g <= 16) {
-		_accel_range_m_s2 = 16.0f * LSM303D_ONE_G;
-		setbits |= REG2_FULL_SCALE_16G_A;
-		new_scale_g_digit = 0.732e-3f;
-
-	} else {
-		return -1;
-	}
-
-	_accel_range_scale = new_scale_g_digit * LSM303D_ONE_G;
-
-
-	LSM303D_modify_reg(ADDR_CTRL_REG2, clearbits, setbits);
-
-	return 0;
-}
-*/
-/*
-//  ===============================================================================
-//	LSM303D_mag_set_range
-/// @brief	tiny helpers by pixhawk
-//  ===============================================================================
-int LSM303D_mag_set_range(unsigned max_ga)
-{
-	uint8_t setbits = 0;
-	uint8_t clearbits = REG6_FULL_SCALE_BITS_M;
-	float new_scale_ga_digit = 0.0f;
-
-	if (max_ga == 0) {
-		max_ga = 12;
-	}
-
-	if (max_ga <= 2) {
-		_mag_range_ga = 2;
-		setbits |= REG6_FULL_SCALE_2GA_M;
-		new_scale_ga_digit = 0.080e-3f;
-
-	} else if (max_ga <= 4) {
-		_mag_range_ga = 4;
-		setbits |= REG6_FULL_SCALE_4GA_M;
-		new_scale_ga_digit = 0.160e-3f;
-
-	} else if (max_ga <= 8) {
-		_mag_range_ga = 8;
-		setbits |= REG6_FULL_SCALE_8GA_M;
-		new_scale_ga_digit = 0.320e-3f;
-
-	} else if (max_ga <= 12) {
-		_mag_range_ga = 12;
-		setbits |= REG6_FULL_SCALE_12GA_M;
-		new_scale_ga_digit = 0.479e-3f;
-
-	} else {
-		return -1;
-	}
-
-	_mag_range_scale = new_scale_ga_digit;
-
-	LSM303D_modify_reg(ADDR_CTRL_REG6, clearbits, setbits);
-
-	return 0;
-}
-*/
-
 //  ===============================================================================
 //	LSM303D_accel_set_onchip_lowpass_filter_bandwidth
-/// @brief	tiny helpers by pixhawk
 //  ===============================================================================
 int LSM303D_accel_set_onchip_lowpass_filter_bandwidth(unsigned bandwidth)
 {
@@ -626,7 +560,6 @@
 
 //  ===============================================================================
 //	LSM303D_accel_set_driver_lowpass_filter
-/// @brief	tiny helpers by pixhawk. This one is not used at the moment!
 //  ===============================================================================
 int LSM303D_accel_set_driver_lowpass_filter(float samplerate, float bandwidth)
 {
@@ -638,113 +571,6 @@
 	return 0;
 }
 
-/* unused 170821
-//  ===============================================================================
-//	LSM303D_accel_set_samplerate
-/// @brief	tiny helpers by pixhawk
-//  ===============================================================================
-int LSM303D_accel_set_samplerate(unsigned frequency)
-{
-	uint8_t setbits = 0;
-	uint8_t clearbits = REG1_RATE_BITS_A;
-
-//	if (frequency == 0 || frequency == ACCEL_SAMPLERATE_DEFAULT) {
-		frequency = 1600;
-//	}
-
-	if (frequency <= 3) {
-		setbits |= REG1_RATE_3_125HZ_A;
-		_accel_samplerate = 3;
-
-	} else if (frequency <= 6) {
-		setbits |= REG1_RATE_6_25HZ_A;
-		_accel_samplerate = 6;
-
-	} else if (frequency <= 12) {
-		setbits |= REG1_RATE_12_5HZ_A;
-		_accel_samplerate = 12;
-
-	} else if (frequency <= 25) {
-		setbits |= REG1_RATE_25HZ_A;
-		_accel_samplerate = 25;
-
-	} else if (frequency <= 50) {
-		setbits |= REG1_RATE_50HZ_A;
-		_accel_samplerate = 50;
-
-	} else if (frequency <= 100) {
-		setbits |= REG1_RATE_100HZ_A;
-		_accel_samplerate = 100;
-
-	} else if (frequency <= 200) {
-		setbits |= REG1_RATE_200HZ_A;
-		_accel_samplerate = 200;
-
-	} else if (frequency <= 400) {
-		setbits |= REG1_RATE_400HZ_A;
-		_accel_samplerate = 400;
-
-	} else if (frequency <= 800) {
-		setbits |= REG1_RATE_800HZ_A;
-		_accel_samplerate = 800;
-
-	} else if (frequency <= 1600) {
-		setbits |= REG1_RATE_1600HZ_A;
-		_accel_samplerate = 1600;
-
-	} else {
-		return -1;
-	}
-
-	LSM303D_modify_reg(ADDR_CTRL_REG1, clearbits, setbits);
-	return 0;
-}
-//  ===============================================================================
-//	LSM303D_mag_set_samplerate
-/// @brief	tiny helpers by pixhawk
-//  ===============================================================================
-int LSM303D_mag_set_samplerate(unsigned frequency)
-{
-	uint8_t setbits = 0;
-	uint8_t clearbits = REG5_RATE_BITS_M;
-
-	if (frequency == 0) {
-		frequency = 100;
-	}
-
-	if (frequency <= 3) {
-		setbits |= REG5_RATE_3_125HZ_M;
-		_mag_samplerate = 25;
-
-	} else if (frequency <= 6) {
-		setbits |= REG5_RATE_6_25HZ_M;
-		_mag_samplerate = 25;
-
-	} else if (frequency <= 12) {
-		setbits |= REG5_RATE_12_5HZ_M;
-		_mag_samplerate = 25;
-
-	} else if (frequency <= 25) {
-		setbits |= REG5_RATE_25HZ_M;
-		_mag_samplerate = 25;
-
-	} else if (frequency <= 50) {
-		setbits |= REG5_RATE_50HZ_M;
-		_mag_samplerate = 50;
-
-	} else if (frequency <= 100) {
-		setbits |= REG5_RATE_100HZ_M;
-		_mag_samplerate = 100;
-
-	} else {
-		return -1;
-	}
-
-	LSM303D_modify_reg(ADDR_CTRL_REG5, clearbits, setbits);
-	return 0;
-}
-*/
-
 
 // rotate_mag_3f: nicht genutzt aber praktisch; rotate_accel_3f wird benutzt
 //  ===============================================================================
@@ -786,9 +612,7 @@
 
 
 //  ===============================================================================
-//	compass_init_LSM303D by PIXhawk (LSM303D::reset())
-//  https://raw.githubusercontent.com/PX4/Firmware/master/src/drivers/lsm303d/lsm303d.cpp
-/// @brief	The new ST 303D 
+//	compass_init_LSM303D
 ///					This might be called several times with different gain values during calibration
 ///					but gain change is not supported at the moment.
 ///
@@ -799,7 +623,6 @@
 
 void compass_init_LSM303D(uint8_t fast, uint8_t gain)
 {
-// matthias version 160620
 	if(fast == 0)
 	{
 		LSM303D_write_checked_reg(ADDR_CTRL_REG0, 0x00);
@@ -821,61 +644,13 @@
 	LSM303D_write_checked_reg(ADDR_CTRL_REG6, 0x00);
 	LSM303D_write_checked_reg(ADDR_CTRL_REG7, 0x00);
 
-	/*
-uint8_t data;
-for(int i=0;i<11;i++)
-{
-	data = ADDR_INT_THS_L_M + i;
-	I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1);
-	I2C_Master_Receive(  DEVICE_COMPASS_303D, &testCompassLS303D[i], 1);
-}
-*/	
-	
 	return;
-/*
-	LSM303D_accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G); // modifies ADDR_CTRL_REG2
-	LSM303D_accel_set_samplerate(LSM303D_ACCEL_DEFAULT_RATE); // modifies ADDR_CTRL_REG1
-
-	LSM303D_mag_set_range(LSM303D_MAG_DEFAULT_RANGE_GA);
-	LSM303D_mag_set_samplerate(LSM303D_MAG_DEFAULT_RATE);
-*/
-	
-/*
-// my stuff hw
-	// enable accel
-	LSM303D_write_checked_reg(ADDR_CTRL_REG1,
-			  REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE | REG1_RATE_800HZ_A);
-	
-	// enable mag
-	LSM303D_write_checked_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M);
-	LSM303D_write_checked_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M | REG5_ENABLE_T);
-	LSM303D_write_checked_reg(ADDR_CTRL_REG3, 0x04); // DRDY on ACCEL on INT1
-	LSM303D_write_checked_reg(ADDR_CTRL_REG4, 0x04); // DRDY on MAG on INT2
-
-	LSM303D_accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G);
-	LSM303D_accel_set_samplerate(LSM303D_ACCEL_DEFAULT_RATE);
-	LSM303D_accel_set_driver_lowpass_filter((float)LSM303D_ACCEL_DEFAULT_RATE, (float)LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ);
-	//LSM303D_accel_set_onchip_lowpass_filter_bandwidth(773); // factory setting	
-	
-	// we setup the anti-alias on-chip filter as 50Hz. We believe
-	// this operates in the analog domain, and is critical for
-	// anti-aliasing. The 2 pole software filter is designed to
-	// operate in conjunction with this on-chip filter
-	if(fast)
-		LSM303D_accel_set_onchip_lowpass_filter_bandwidth(773); // factory setting
-	else
-		LSM303D_accel_set_onchip_lowpass_filter_bandwidth(LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ);
-
-
-	LSM303D_mag_set_range(LSM303D_MAG_DEFAULT_RANGE_GA);
-	LSM303D_mag_set_samplerate(LSM303D_MAG_DEFAULT_RATE);
-*/
 }
 
 
 //  ===============================================================================
 //	compass_sleep_LSM303D
-/// @brief	The new compass chip, hopefully this works!
+// 	@brief	Gen 2 chip
 //  ===============================================================================
 void compass_sleep_LSM303D(void)
 {
@@ -886,9 +661,7 @@
 
 //  ===============================================================================
 //	acceleration_read_LSM303D
-/// @brief	The new LSM303D, code by pixhawk
-///
-/// output is accel_DX_f, accel_DY_f, accel_DZ_f
+// 	output is accel_DX_f, accel_DY_f, accel_DZ_f
 //  ===============================================================================
 void acceleration_read_LSM303D(void)
 {
@@ -920,66 +693,14 @@
 	accel_report_y = yraw_f;
 	accel_report_z = zraw_f;
 
-	// my stuff
-/*	
-	accel_report_x = ((xraw_f * _accel_range_scale) - _accel_scale_x_offset) * _accel_scale_x_scale;
-	accel_report_y = ((yraw_f * _accel_range_scale) - _accel_scale_y_offset) * _accel_scale_y_scale;
-	accel_report_z = ((zraw_f * _accel_range_scale) - _accel_scale_z_offset) * _accel_scale_z_scale;
-*/	
 	accel_DX_f = ((int16_t)(accel_report_x));
 	accel_DY_f = ((int16_t)(accel_report_y));
 	accel_DZ_f = ((int16_t)(accel_report_z));
 }
-/* special code after accel_report_z = ...
- * prior to output
-//	  we have logs where the accelerometers get stuck at a fixed
-//	  large value. We want to detect this and mark the sensor as
-//	  being faulty
-
-	if (fabsf(_last_accel[0] - x_in_new) < 0.001f &&
-	    fabsf(_last_accel[1] - y_in_new) < 0.001f &&
-	    fabsf(_last_accel[2] - z_in_new) < 0.001f &&
-	    fabsf(x_in_new) > 20 &&
-	    fabsf(y_in_new) > 20 &&
-	    fabsf(z_in_new) > 20) {
-		_constant_accel_count += 1;
-
-	} else {
-		_constant_accel_count = 0;
-	}
-
-	if (_constant_accel_count > 100) {
-		// we've had 100 constant accel readings with large
-		// values. The sensor is almost certainly dead. We
-		// will raise the error_count so that the top level
-		// flight code will know to avoid this sensor, but
-		// we'll still give the data so that it can be logged
-		// and viewed
-		perf_count(_bad_values);
-		_constant_accel_count = 0;
-	}
-
-	_last_accel[0] = x_in_new;
-	_last_accel[1] = y_in_new;
-	_last_accel[2] = z_in_new;
-
-	accel_report.x = _accel_filter_x.apply(x_in_new);
-	accel_report.y = _accel_filter_y.apply(y_in_new);
-	accel_report.z = _accel_filter_z.apply(z_in_new);
-
-	math::Vector<3> aval(x_in_new, y_in_new, z_in_new);
-	math::Vector<3> aval_integrated;
-
-	bool accel_notify = _accel_int.put(accel_report.timestamp, aval, aval_integrated, accel_report.integral_dt);
-	accel_report.x_integral = aval_integrated(0);
-	accel_report.y_integral = aval_integrated(1);
-	accel_report.z_integral = aval_integrated(2);
-*/
 
 
 //  ===============================================================================
 //	compass_read_LSM303D
-/// @brief	The new LSM303D, code by pixhawk
 ///
 /// output is compass_DX_f, compass_DY_f, compass_DZ_f
 //  ===============================================================================
@@ -1008,33 +729,144 @@
 	compass_DZ_f = (((int16_t)((magDataBuffer[5] << 8) | (magDataBuffer[4]))));
 	// no rotation
 	return;
-/*	
-	// my stuff
-	compass_DX_f = (((int16_t)((magDataBuffer[1] << 8) | (magDataBuffer[0]))) / 10) - 200;
-	compass_DY_f = (((int16_t)((magDataBuffer[3] << 8) | (magDataBuffer[2]))) / 10) - 200;
-	compass_DZ_f = (((int16_t)((magDataBuffer[5] << 8) | (magDataBuffer[4]))) / 10) - 200;
-*/
-	// old
-	/*	
-	xraw_f = ((float)( (int16_t)((magDataBuffer[1] << 8) | (magDataBuffer[0]))));
-	yraw_f = ((float)( (int16_t)((magDataBuffer[3] << 8) | (magDataBuffer[2]))));
-	zraw_f = ((float)( (int16_t)((magDataBuffer[5] << 8) | (magDataBuffer[4]))));	
+}
+
+
+//  ===============================================================================
+//	compass_init_LSM303AGR
+///					This might be called several times with different gain values during calibration
+///					but gain change is not supported at the moment.
+///
+/// @param 	gain: 7 is max gain and set with here, compass_calib() might reduce it
+//  ===============================================================================
+
+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
 
-	rotate_mag_3f(&xraw_f, &yraw_f, &zraw_f); 
+		// 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); //
+}
+
 
-	compass_DX_f = (int16_t)((xraw_f * 0.1f) - 200.0f);
-	compass_DY_f = (int16_t)((yraw_f * 0.1f) - 200.0f);
-	compass_DZ_f = (int16_t)((zraw_f * 0.1f) - 200.0f);
-*/	
-/*	
-	mag_report_x = ((xraw_f * _mag_range_scale) - _mag_scale_x_offset) * _mag_scale_x_scale;
-	mag_report_y = ((yraw_f * _mag_range_scale) - _mag_scale_y_offset) * _mag_scale_y_scale;
-	mag_report_z = ((zraw_f * _mag_range_scale) - _mag_scale_z_offset) * _mag_scale_z_scale;
-	
-	compass_DX_f = (int16_t)(mag_report_x * 1000.0f); // 1000.0 is just a wild guess by hw
-	compass_DY_f = (int16_t)(mag_report_y * 1000.0f);
-	compass_DZ_f = (int16_t)(mag_report_z * 1000.0f);
-*/	
+//  ===============================================================================
+//	acceleration_read_LSM303AGR
+// 	output is accel_DX_f, accel_DY_f, accel_DZ_f
+//  ===============================================================================
+void acceleration_read_LSM303AGR(void)
+{
+  uint8_t data;
+	float xraw_f, yraw_f, zraw_f;
+	float accel_report_x, accel_report_y, accel_report_z;
+
+  memset(accDataBuffer,0,6);
+
+	accel_DX_f = 0;
+	accel_DY_f = 0;
+	accel_DZ_f = 0;
+
+	for(int i=0;i<6;i++)
+	{
+		data = 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;
 }
 
 
@@ -1340,34 +1172,6 @@
 }
 
 
-/*
-//  ===============================================================================
-//	compass_calc_mini_during_calibration
-/// @brief	all the fancy stuff first implemented in OSTC3
-///
-/// input is accel_DX_f, accel_DY_f, accel_DZ_f
-/// output is compass_pitch and compass_roll
-//  ===============================================================================
-void compass_calc_mini_during_calibration(void)
-{
-     float	sinPhi, cosPhi;
-		 float Phi, Teta;
-
-    //---- Calculate sine and cosine of roll angle Phi -----------------------
-    //sincos(accel_DZ_f, accel_DY_f, &sin, &cos);
-    Phi= atan2f(accel_DY_f, accel_DZ_f) ;
-		compass_roll = Phi * 180.0f /PI;
-		sinPhi = sinf(Phi);
-		cosPhi = cosf(Phi);
-		
-    //---- calculate sin and cosine of pitch angle Theta ---------------------
-    //sincos(Gz, -accel_DX_f, &sin, &cos);     // NOTE: changed sin sign.
-    Teta = atanf(-(float)accel_DX_f/(accel_DY_f * sinPhi + accel_DZ_f * cosPhi));
-		compass_pitch = Teta * 180.0f /PI;
-}
-*/
-
-
 // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 // // - Calibration - ///////////////////////////////////////////////////////////////////////////////////////////////////////
 // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -1565,7 +1369,7 @@
 				acceleration_read();
 				compass_calc_roll_pitch_only();
 			
-				if((hardwareCompass == HMC5883L)
+				if((hardwareCompass == compass_generation1 )		//HMC5883L)
 				&&((compass_DX_f == -4096) ||
 					 (compass_DY_f == -4096) ||
 					 (compass_DZ_f == -4096) ))
@@ -1607,326 +1411,3 @@
 		return 0;
 }
 
-// //////////////////////////// TEST CODE /////////////////////////////////////
-
-
-
-//#include <QtDebug>
-//#include <stdio.h>
-//#include <math.h>
-/*#include <stdlib.h>
-
-short compass_DX_f, compass_DY_f, compass_DZ_f;
-short compass_CX_f, compass_CY_f, compass_CZ_f;
-
-inline float uniform(void) {
-    return (rand() & 0xFFFF) / 65536.0f;
-}
-inline float sqr(float x) {
-    return x*x;
-}
-
-static const float radius = 0.21f;
-static const float cx = 0.79f, cy = -0.46f, cz = 0.24f;
-// const float cx = 0, cy = 0, cz = 0;
-
-float check_compass_calib(void)
-{
-
-    // Starts with no calibration at all:
-    compass_CX_f = compass_CY_f = compass_CZ_f = 0;
-
-    // Try 10 recalibration passes:
-    for(int p=0; p<10; ++p)
-    {
-        compass_reset_calibration();
-
-        //---- Generates random points on a sphere -------------------------------
-        // of radius,center (cx, cy, cz):
-        for(int i=0; i<100; ++i)
-        {
-            float theta = uniform()*360.0f;
-            float phi   = uniform()*180.0f - 90.0f;
-
-            float x = cx + radius * cosf(phi)*cosf(theta);
-            float y = cy + radius * cosf(phi)*sinf(theta);
-            float z = cz + radius * sinf(phi);
-
-            compass_DX_f = (short)(32768 * x);
-            compass_DY_f = (short)(32768 * y);
-            compass_DZ_f = (short)(32768 * z);
-            compass_add_calibration();
-        }
-
-        compass_solve_calibration();
-        //qDebug() << "Center ="
-//                 << compass_CX_f/32768.0f
-//                 << compass_CY_f/32768.0f
-//                 << compass_CZ_f/32768.0f;
-
-        float r2 = sqr(compass_CX_f/32768.0f - cx)
-                 + sqr(compass_CY_f/32768.0f - cy)
-                 + sqr(compass_CZ_f/32768.0f - cz);
-        if( r2 > 0.01f*0.01f )
-            return sqrtf(r2);
-    }
-		return 0;
-}*/
-
-
-
-/*
-void compass_read_LSM303D_v3(void)
-{
-  uint8_t data;
-	
-  memset(magDataBuffer,0,6);
-
-	compass_DX_f = 0;
-	compass_DY_f = 0;
-	compass_DZ_f = 0;
-	
-	//magnetometer multi read, order xl,xh, yl,yh, zl, zh
-	data = REG_MAG_DATA_ADDR;
-	I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1);
-	I2C_Master_Receive(  DEVICE_COMPASS_303D, magDataBuffer, 6);
-
-	compass_DX_f = ((int16_t)( (int16_t)((magDataBuffer[1] << 8) | (magDataBuffer[0]))));
-	compass_DY_f = ((int16_t)( (int16_t)((magDataBuffer[3] << 8) | (magDataBuffer[2]))));
-	compass_DZ_f = ((int16_t)( (int16_t)((magDataBuffer[5] << 8) | (magDataBuffer[4]))));	
-
-//	compass_DX_f = compass_DX_f * stat->sensitivity_mag;
-//	compass_DY_f = compass_DY_f * stat->sensitivity_mag;
-//	compass_DZ_f = compass_DZ_f * stat->sensitivity_mag;
-}
-
-
-//  ===============================================================================
-//	compass_init_LSM303D by STMicroelectronics 2013 V1.0.5 2013/Oct/23
-/// @brief	The new ST 303D 
-///					This might be called several times with different gain values during calibration
-///
-/// @param 	gain: 7 is max gain and set with here, compass_calib() might reduce it
-//  ===============================================================================
-
-void compass_init_LSM303D_v3(uint8_t gain)
-{
-  uint8_t data[10];
-
-	// CNTRL1
-	// 0011 acceleration data rate 0011 = 12.5 Hz (3.125 Hz - 1600 Hz)
-	// 0xxx block data update off
-	// x111 enable all three axes 
-	
-	// CNTRL5
-	// 0xxx xxxx temp sensor off
-	// x00x xxxx magnetic resolution
-	// xxx0 1xxx magentic data rate 01 = 6,25 Hz (3.125 Hz - 50 Hz (100 Hz))
-	// xxxx xx00 latch irq requests off
-	
-	// CNTRL7
-	// 00xx high pass filter mode, 00 normal mode
-	// xx0x filter for acceleration data bypassed
-	// xxx0 temperature sensor mode only off
-	// x0xx magnetic data low-power mode off
-	// xx00 magnetic sensor mode 00 = continous-conversion mode (default 10 power-down)
-	
-	data[0] = CNTRL0;
-	data[1] = 0x00;
-	I2C_Master_Transmit( DEVICE_COMPASS_303D, data, 2);
-
-	// acc
-	data[0] = CNTRL1;
-	data[1] = 0x00;
-	data[2] = 0x0F;
-	data[3] = 0x00;
-	data[4] = 0x00;
-	I2C_Master_Transmit( DEVICE_COMPASS_303D, data, 5);
-
-	// mag
-	data[0] = CNTRL3;
-	data[1] = 0x00;
-	data[2] = 0x00;
-	data[3] = 0x18;
-	data[4] = 0x20;
-	I2C_Master_Transmit( DEVICE_COMPASS_303D, data, 5);
-	
-	data[0] = CNTRL7;
-	data[1] = ((MSMS_MASK & CONTINUOS_CONVERSION) |
-			((~MSMS_MASK) & CNTRL7_RESUME_VALUE));
-	I2C_Master_Transmit( DEVICE_COMPASS_303D, data, 2);
-	
-	HAL_Delay(100);
-}
-
-
-//  ===============================================================================
-//	compass_init_LSM303D by nordevx for arduion
-/// @brief	The new ST 303D 
-///					This might be called several times with different gain values during calibration
-///
-/// @param 	gain: 7 is max gain and set with here, compass_calib() might reduce it
-//  ===============================================================================
-void compass_init_LSM303D_v2(uint8_t gain)
-{
-  uint8_t data[2];
-
-	// CNTRL1
-	// 0011 acceleration data rate 0011 = 12.5 Hz (3.125 Hz - 1600 Hz)
-	// 0xxx block data update off
-	// x111 enable all three axes 
-	
-	// CNTRL5
-	// 0xxx xxxx temp sensor off
-	// x00x xxxx magnetic resolution
-	// xxx0 1xxx magentic data rate 01 = 6,25 Hz (3.125 Hz - 50 Hz (100 Hz))
-	// xxxx xx00 latch irq requests off
-	
-	// CNTRL7
-	// 00xx high pass filter mode, 00 normal mode
-	// xx0x filter for acceleration data bypassed
-	// xxx0 temperature sensor mode only off
-	// x0xx magnetic data low-power mode off
-	// xx00 magnetic sensor mode 00 = continous-conversion mode (default 10 power-down)
-	
-	data[0] = CNTRL1;
-	data[1] = 0x37; //0b 0011 0111 
-	I2C_Master_Transmit( DEVICE_COMPASS_303D, data, 2);
-	
-	data[0] = CNTRL5;
-	data[1] = 0x08; // 0b 0000 1000
-	I2C_Master_Transmit( DEVICE_COMPASS_303D, data, 2);
-	
-	data[0] = CNTRL7;
-	data[1] = 0x00; // 0b 0000 0000
-	I2C_Master_Transmit( DEVICE_COMPASS_303D, data, 2);
-	
-	HAL_Delay(100);
-}
-
-
-//  ===============================================================================
-//	compass_init_LSM303D_v1 by ST lsm303d.c
-/// @brief	The new ST 303D 
-///					This might be called several times with different gain values during calibration
-///
-/// @param 	gain: 7 is max gain and set with here, compass_calib() might reduce it
-//  ===============================================================================
-void compass_init_LSM303D_v1(uint8_t gain)
-{
-  uint8_t data;
-
-	compass_gain = gain;
-
-  memset(magDataBuffer,0,6);
-  memset(accDataBuffer,0,6);
-
-	data = CNTRL5;
-	I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1);
-	I2C_Master_Receive(  DEVICE_COMPASS_303D, &data, 1);
-  data = (data & 0x1c) >> 2;  
-  velMag = magODR[data];  
-
-	data = CNTRL1;
-	I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1);
-	I2C_Master_Receive(  DEVICE_COMPASS_303D, &data, 1);
-  data = (data & 0xf0) >> 4;  
-  velAcc = accODR[data];
-  
-	data = CNTRL7;
-	I2C_Master_Transmit( DEVICE_COMPASS_303D, &data,   1);
-	I2C_Master_Receive(  DEVICE_COMPASS_303D, &datas1, 1);
-  datas1 = (datas1 & 0x02);
-  
-  //if mag is not pd
-  //mag is bigger than gyro
-  if( (velMag < velAcc) || datas1 != 0 ) {
-	//acc is the biggest
-	fastest = ACC_IS_FASTEST;	
-  }	
-  else {		
-	//acc is the biggest		
-	fastest = MAG_IS_FASTEST;
-  }
-  
-  zoffFlag = 1;
-	
-  if( fastest == MAG_IS_FASTEST)
-	{	
-		data = STATUS_REG_M;
-		I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1);
-		I2C_Master_Receive(  DEVICE_COMPASS_303D, &data, 1);
- 
-//    if(ValBit(data, ZYXMDA)) {
-      sendFlag = 1;      
-//    }
-    
-  }  
-  else if(fastest == ACC_IS_FASTEST)
-	{
-		data = STATUS_REG_A;
-		I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1);
-		I2C_Master_Receive(  DEVICE_COMPASS_303D, &data, 1);
-//      if(ValBit(data, DATAREADY_BIT)) {
-        sendFlag = 1;
-//      }    
-  }  
-}
-
-//  ===============================================================================
-//	compass_read_LSM303D
-/// @brief	The new LSM303D :-)
-///
-/// output is compass_DX_f, compass_DY_f, compass_DZ_f, accel_DX_f, accel_DY_f, accel_DZ_f
-//  ===============================================================================
-void compass_read_LSM303D_v2(void)
-{
-	  uint8_t data;
-	
-  memset(magDataBuffer,0,6);
-  memset(accDataBuffer,0,6);
-
-	compass_DX_f = 0;
-	compass_DY_f = 0;
-	compass_DZ_f = 0;
-
-	accel_DX_f = 0;
-	accel_DY_f = 0;
-	accel_DZ_f = 0;
-	
-	//Accelerometer multi read, order xl,xh, yl,yh, zl, zh
-	data = REG_ACC_DATA_ADDR;
-	I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1);
-	I2C_Master_Receive(  DEVICE_COMPASS_303D, accDataBuffer, 6);
-	
-	//magnetometer multi read, order xl,xh, yl,yh, zl, zh
-	data = OUT_X_L_M;
-	I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1);
-	I2C_Master_Receive(  DEVICE_COMPASS_303D, magDataBuffer, 6);
-	
-	accel_DX_f = ((int16_t)( (int16_t)((accDataBuffer[1] << 8) | (accDataBuffer[0]))));
-	accel_DY_f = ((int16_t)( (int16_t)((accDataBuffer[3] << 8) | (accDataBuffer[2]))));
-	accel_DZ_f = ((int16_t)( (int16_t)((accDataBuffer[5] << 8) | (accDataBuffer[4]))));	
-
-//	accel_DX_f = accel_DX_f * stat->sensitivity_acc;
-//	accel_DY_f = accel_DY_f * stat->sensitivity_acc;
-//	accel_DZ_f = accel_DZ_f * stat->sensitivity_acc;
-
-
-	compass_DX_f  = magDataBuffer[1];
-	compass_DX_f *= 256;
-	compass_DX_f += magDataBuffer[0];
-
-	compass_DY_f  = magDataBuffer[3];
-	compass_DY_f *= 256;
-	compass_DY_f += magDataBuffer[2];
-
-	compass_DY_f  = magDataBuffer[5];
-	compass_DY_f *= 256;
-	compass_DY_f += magDataBuffer[4];
-
-}
-
-
-*/
-
diff -r 39c147e47c1c -r 0dbb74be972f Small_CPU/Src/pressure.c
--- a/Small_CPU/Src/pressure.c	Mon Oct 21 21:16:53 2019 +0200
+++ b/Small_CPU/Src/pressure.c	Sun Nov 24 15:46:58 2019 +0000
@@ -61,11 +61,13 @@
 #define PRESSURE_SURFACE_DETECT_UNSTABLE_CNT	(3u)			/* Event count to detect unstable condition */
 
 
+static uint8_t PRESSURE_ADDRESS = DEVICE_PRESSURE_MS5803;					/* Default Address */
+
 static uint16_t get_ci_by_coef_num(uint8_t coef_num);
 //void pressure_calculation_new(void);
 //void pressure_calculation_old(void);
 static void pressure_calculation_AN520_004_mod_MS5803_30BA__09_2015(void);
-static uint8_t crc4(uint16_t n_prom[]);
+//static uint8_t crc4(uint16_t n_prom[]);
 
 static HAL_StatusTypeDef pressure_sensor_get_data(void);
 static uint32_t get_adc(void);
@@ -74,7 +76,7 @@
 static uint16_t C[8] = { 1 };
 static uint32_t D1 = 1;
 static uint32_t D2 = 1;
-static uint8_t n_crc;
+//static uint8_t n_crc;
 
 static int64_t C5_x_2p8 = 1;
 static int64_t C2_x_2p16 = 1;
@@ -338,25 +340,49 @@
 uint8_t init_pressure(void)
 {
 	uint8_t buffer[1];
-	buffer[0] = 0x1e;
+	buffer[0] = 0x1E;			// Reset Command
 	uint8_t retValue = 0xFF;
 	
 	pressureSensorInitSuccess = false;
 	init_pressure_history();
 
+/* Probe new sensor first */
+	retValue = I2C_Master_Transmit(  DEVICE_PRESSURE_MS5837, buffer, 1);
+	if(retValue != HAL_OK)
+	{
+		PRESSURE_ADDRESS = DEVICE_PRESSURE_MS5803;			// use old sensor
+		HAL_Delay(100);
+		MX_I2C1_Init();
+		if (global.I2C_SystemStatus != HAL_OK)
+		{
+			if (MX_I2C1_TestAndClear() == GPIO_PIN_RESET) {
+				MX_I2C1_TestAndClear(); // do it a second time
+			}
+			MX_I2C1_Init();
+		}
+	}
+	else
+	{
+		PRESSURE_ADDRESS = DEVICE_PRESSURE_MS5837;			// Success, use new sensor
+	}
+	HAL_Delay(3);		//2.8ms according to datasheet
+
+	buffer[0] = 0x1E;			// Reset Command
+	retValue = 0xFF;
+
 /* Send reset request to pressure sensor */
-	retValue = I2C_Master_Transmit(  DEVICE_PRESSURE, buffer, 1);
+	retValue = I2C_Master_Transmit(  PRESSURE_ADDRESS, buffer, 1);
 	if(retValue != HAL_OK)
 	{
 		return (HAL_StatusTypeDef)retValue;
 	}
-	HAL_Delay(3);
+	HAL_Delay(3);		//2.8ms according to datasheet
 	
-	for(uint8_t i=0;i<8;i++)
+	for(uint8_t i=0;i<7;i++)
 	{
 		C[i] = get_ci_by_coef_num(i);
 	}
-	n_crc = crc4(C); // no evaluation at the moment hw 151026
+	// n_crc = crc4(C); // no evaluation at the moment hw 151026
 
 	C5_x_2p8  = C[5] * 256;
 	C2_x_2p16 = C[2] * 65536;
@@ -379,8 +405,8 @@
 	uint32_t answer = 0;
 
 	buffer[0] = 0x00; // Get ADC
-	I2C_Master_Transmit( DEVICE_PRESSURE, buffer, 1);
-	I2C_Master_Receive(  DEVICE_PRESSURE, resivebuf, 4);
+	I2C_Master_Transmit( PRESSURE_ADDRESS, buffer, 1);
+	I2C_Master_Receive(  PRESSURE_ADDRESS, resivebuf, 4);
 	resivebuf[3] = 0;
 	answer = 256*256 *(uint32_t)resivebuf[0]  + 256 * (uint32_t)resivebuf[1] + (uint32_t)resivebuf[2];
 
@@ -393,8 +419,8 @@
 	uint8_t resivebuf[2];
 
 	uint8_t cmd = CMD_PROM_RD+coef_num*2; 
-	I2C_Master_Transmit( DEVICE_PRESSURE, &cmd, 1);
-	I2C_Master_Receive(  DEVICE_PRESSURE, resivebuf, 2);
+	I2C_Master_Transmit( PRESSURE_ADDRESS, &cmd, 1);
+	I2C_Master_Receive(  PRESSURE_ADDRESS, resivebuf, 2);
 	return (256*(uint16_t)resivebuf[0]) + (uint16_t)resivebuf[1];
 }
 
@@ -437,7 +463,7 @@
 	uint8_t command = CMD_ADC_CONV + cmd;
 	HAL_StatusTypeDef statusReturnTemp = HAL_TIMEOUT;
 	
-	statusReturnTemp = I2C_Master_Transmit( DEVICE_PRESSURE, &command, 1);
+	statusReturnTemp = I2C_Master_Transmit( PRESSURE_ADDRESS, &command, 1);
 
 	if(statusReturn)
 	{
@@ -705,7 +731,7 @@
 /* taken from AN520 by meas-spec.com dated 9. Aug. 2011
  * short and int are both 16bit according to AVR/GCC google results
  */
-static uint8_t crc4(uint16_t n_prom[])
+/*static uint8_t crc4(uint16_t n_prom[])
 {
 uint16_t cnt; // simple counter
 uint16_t n_rem; // crc reminder
@@ -734,7 +760,7 @@
 n_prom[7]=crc_read; // restore the crc_read to its original place
 return (n_rem ^ 0x00);
 }
-/*
+
 void test_calculation(void)
 {
 	C1 = 29112;
diff -r 39c147e47c1c -r 0dbb74be972f Small_CPU/Src/scheduler.c
--- a/Small_CPU/Src/scheduler.c	Mon Oct 21 21:16:53 2019 +0200
+++ b/Small_CPU/Src/scheduler.c	Sun Nov 24 15:46:58 2019 +0000
@@ -1030,7 +1030,7 @@
 		MX_I2C1_Init();
 		pressure_sensor_get_pressure_raw();
 
-/* check if I2C is not up an running and try to reactivate if necessary. Also do initialization if problem occured during startup */
+/* check if I2C is not up an running and try to reactivate if necessary. Also do initialization if problem occurred during startup */
 		if(global.I2C_SystemStatus != HAL_OK)
 		{
 			MX_I2C1_TestAndClear();