From e2c8ffee5ada53e1f08253f2dd82e2e55985ceda Mon Sep 17 00:00:00 2001
From: Lennart Eriksson <lennart.eriksson91@gmail.com>
Date: Wed, 23 Nov 2016 13:56:55 +0100
Subject: [PATCH] Got compass to work again

---
 UAV-ControlSystem/inc/Flight/pid.h          |  2 +
 UAV-ControlSystem/inc/Scheduler/scheduler.h |  1 +
 UAV-ControlSystem/inc/Scheduler/tasks.h     |  1 +
 UAV-ControlSystem/inc/config/eeprom.h       |  1 +
 UAV-ControlSystem/inc/system_variables.h    |  2 +-
 UAV-ControlSystem/src/Flight/pid.c          | 75 +++++++++++++++++++--
 UAV-ControlSystem/src/Scheduler/scheduler.c |  2 +
 UAV-ControlSystem/src/Scheduler/tasks.c     |  8 +++
 UAV-ControlSystem/src/config/cli.c          |  5 ++
 UAV-ControlSystem/src/config/eeprom.c       |  6 ++
 UAV-ControlSystem/src/drivers/arduino_com.c | 22 +++---
 UAV-ControlSystem/src/drivers/usart.c       |  2 +-
 UAV-ControlSystem/src/main.c                |  6 +-
 UAV-ControlSystem/src/tasks_main.c          |  7 ++
 14 files changed, 121 insertions(+), 19 deletions(-)

diff --git a/UAV-ControlSystem/inc/Flight/pid.h b/UAV-ControlSystem/inc/Flight/pid.h
index 7213f7d..ee9b57e 100644
--- a/UAV-ControlSystem/inc/Flight/pid.h
+++ b/UAV-ControlSystem/inc/Flight/pid.h
@@ -67,6 +67,8 @@ extern float accPitchFineTune;
 
 extern accel_t accelProfile;												/*Struct profile for input data from sensor*/
 
+extern float Yaw;
+extern float YawU;
 
 
 /**************************************************************************
diff --git a/UAV-ControlSystem/inc/Scheduler/scheduler.h b/UAV-ControlSystem/inc/Scheduler/scheduler.h
index 945f953..c1dae17 100644
--- a/UAV-ControlSystem/inc/Scheduler/scheduler.h
+++ b/UAV-ControlSystem/inc/Scheduler/scheduler.h
@@ -107,6 +107,7 @@ typedef enum
 	TASK_RX_CLI,
 	TASK_SERIAL,
 	TASK_BATTERY,
+	TASK_ARDUINO,
 #ifdef BARO
 	TASK_BARO,
 #endif
diff --git a/UAV-ControlSystem/inc/Scheduler/tasks.h b/UAV-ControlSystem/inc/Scheduler/tasks.h
index 2a6c79c..cbe0756 100644
--- a/UAV-ControlSystem/inc/Scheduler/tasks.h
+++ b/UAV-ControlSystem/inc/Scheduler/tasks.h
@@ -38,6 +38,7 @@ void systemTaskRxCli(void);
 bool systemTaskRxCliCheck(uint32_t currentDeltaTime);
 void systemTaskSerial(void);
 void systemTaskBattery(void);
+void systemTaskArduino(void);
 void systemTaskBaro(void);
 void systemTaskCompass(void);
 void systemTaskGps(void);
diff --git a/UAV-ControlSystem/inc/config/eeprom.h b/UAV-ControlSystem/inc/config/eeprom.h
index 1774a35..c1a66b0 100644
--- a/UAV-ControlSystem/inc/config/eeprom.h
+++ b/UAV-ControlSystem/inc/config/eeprom.h
@@ -159,6 +159,7 @@ typedef enum {
 	EEPROM_PERIOD_RX_CLI,
 	EEPROM_PERIOD_SERIAL,
 	EEPROM_PERIOD_BATTERY,
+	EEPROM_PERIOD_ARDUINO,
 #ifdef BARO
 	EEPROM_PERIOD_BARO,
 #endif
diff --git a/UAV-ControlSystem/inc/system_variables.h b/UAV-ControlSystem/inc/system_variables.h
index 0217fd9..b570055 100644
--- a/UAV-ControlSystem/inc/system_variables.h
+++ b/UAV-ControlSystem/inc/system_variables.h
@@ -14,7 +14,7 @@
 #ifndef SYSTEM_VARIABLES_H_
 #define SYSTEM_VARIABLES_H_
 
-#define EEPROM_SYS_VERSION 107
+#define EEPROM_SYS_VERSION 111
 
 #define ADC_STATE
 #include "stm32f4xx.h"
diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c
index c16c8ec..1d5d870 100644
--- a/UAV-ControlSystem/src/Flight/pid.c
+++ b/UAV-ControlSystem/src/Flight/pid.c
@@ -20,6 +20,10 @@
 #include "drivers/failsafe_toggles.h"
 #include "drivers/motormix.h"
 #include "utilities.h"
+#include "drivers/arduino_com.h"
+
+#define sq(x) ((x)*(x))
+#define map(x, in_min, in_max, out_min, out_max) (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min
 
 
 #define PTERM_SCALE 0.032029f										/*P-term used as a scale value to the PID controller*/
@@ -63,7 +67,16 @@ pt1Filter_t accelFilter[2] = {0};
 float accRollFineTune = 0;
 float accPitchFineTune = 0;
 
-
+float oldSensorValue[2] = {0};
+float oldSensorValueRoll[12] = {0};
+float oldSensorValuePitch[12] = {0};
+float MagnetFilteredOld[3];
+float alphaMagnet = 0.4;
+int MagnetMax[3];
+int MagnetMin[3];
+float MagnetMap[3];
+float Yaw;
+float YawU;
 
 /**************************************************************************
 * BRIEF: Calculates angle from accelerometer   		                      *
@@ -124,9 +137,6 @@ float constrainf(float amt, int low, int high)
 }
 
 
-float oldSensorValue[2] = {0};
-float oldSensorValueRoll[12] = {0};
-float oldSensorValuePitch[12] = {0};
 
 int i = 0;
 
@@ -214,6 +224,59 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile)
 
 		break;
 	case PID_ID_COMPASS:
+	{
+		float xMagnetFiltered = 0;
+		float yMagnetFiltered = 0;
+		float zMagnetFiltered = 0;
+		xMagnetFiltered = MagnetFilteredOld[0] + alphaMagnet * (compass_data.x - MagnetFilteredOld[0]);
+		yMagnetFiltered = MagnetFilteredOld[1] + alphaMagnet * (compass_data.y - MagnetFilteredOld[1]);
+		zMagnetFiltered = MagnetFilteredOld[2] + alphaMagnet * (compass_data.z - MagnetFilteredOld[2]);
+
+		MagnetFilteredOld[0] = xMagnetFiltered;
+		MagnetFilteredOld[1] = yMagnetFiltered;
+		MagnetFilteredOld[2] = zMagnetFiltered;
+
+
+		//this part is required to normalize the magnetic vector
+		if (xMagnetFiltered>MagnetMax[0]) { MagnetMax[0] = xMagnetFiltered; }
+		if (yMagnetFiltered>MagnetMax[1]) { MagnetMax[1] = yMagnetFiltered; }
+		if (zMagnetFiltered>MagnetMax[2]) { MagnetMax[2] = zMagnetFiltered; }
+
+		if (xMagnetFiltered<MagnetMin[0]) { MagnetMin[0] = xMagnetFiltered; }
+		if (yMagnetFiltered<MagnetMin[1]) { MagnetMin[1] = yMagnetFiltered; }
+		if (zMagnetFiltered<MagnetMin[2]) { MagnetMin[2] = zMagnetFiltered; }
+
+		float norm;
+
+		MagnetMap[0] = (float)(map(xMagnetFiltered, MagnetMin[0], MagnetMax[0], -10000, 10000)) / 10000.0;
+		MagnetMap[1] = (float)(map(yMagnetFiltered, MagnetMin[1], MagnetMax[1], -10000, 10000)) / 10000.0;
+		MagnetMap[2] = (float)(map(zMagnetFiltered, MagnetMin[2], MagnetMax[2], -10000, 10000)) / 10000.0;
+
+		//normalize the magnetic vector
+		norm = sqrt(sq(MagnetMap[0]) + sq(MagnetMap[1]) + sq(MagnetMap[2]));
+		MagnetMap[0] /= norm;
+		MagnetMap[1] /= norm;
+		MagnetMap[2] /= norm;
+
+//		compare Applications of Magnetic Sensors for Low Cost Compass Systems by Michael J. Caruso
+//		for the compensated Yaw equations...
+//		http://www.ssec.honeywell.com/magnetic/datasheets/lowcost.pdf
+		Yaw = atan2(-(MagnetMap[1] * cos(oldSensorValue[0]) + MagnetMap[2] * sin(oldSensorValue[0])), MagnetMap[0] * cos(oldSensorValue[1]) + MagnetMap[1] * sin(oldSensorValue[1]) * sin(oldSensorValue[0]) + MagnetMap[2] * sin(oldSensorValue[1]) * cos(oldSensorValue[0]));
+		YawU = atan2(-MagnetMap[1], MagnetMap[0]);
+
+//		float XH;
+//		XH = compass_data.y * cos(oldSensorValue[1]) +
+//			 compass_data.x * sin(oldSensorValue[1]) * sin(oldSensorValue[0]) +
+//			 compass_data.z * sin(oldSensorValue[1]) * cos(oldSensorValue[0]);
+//		float YH;
+//		YH = compass_data.x * cos(oldSensorValue[0]) +
+//			 compass_data.z * sin(oldSensorValue[0]);
+//
+//		Yaw = atan2(-YH, XH);
+//		YawU = atan2(compass_data.y, compass_data.x);
+
+	}
+	break;
 	case PID_ID_BAROMETER:
 	default:
 
@@ -421,7 +484,7 @@ void pidRun(uint8_t ID)
 		break;
 	case PID_ID_ACCELEROMETER:
 
-		if (!(PidProfile[PID_ID_ACCELEROMETER].pidEnabled && flags_IsSet_ID(systemFlags_flightmode_acceleromter_id)))
+		if ((PidProfile[PID_ID_ACCELEROMETER].pidEnabled && flags_IsSet_ID(systemFlags_flightmode_acceleromter_id)))
 		{
 			PidProfile[PID_ID_ACCELEROMETER].PID_Out[ROLL] = rc_input.Roll;
 			PidProfile[PID_ID_ACCELEROMETER].PID_Out[PITCH] = rc_input.Pitch;
@@ -434,7 +497,7 @@ void pidRun(uint8_t ID)
 		break;
 	case PID_ID_COMPASS:
 
-		if (!PidProfile[PID_ID_COMPASS].pidEnabled)
+		if (PidProfile[PID_ID_COMPASS].pidEnabled)
 		{
 			PidProfile[PID_ID_COMPASS].PID_Out[0] = rc_input.Yaw;
 		}
diff --git a/UAV-ControlSystem/src/Scheduler/scheduler.c b/UAV-ControlSystem/src/Scheduler/scheduler.c
index 340687a..0086e91 100644
--- a/UAV-ControlSystem/src/Scheduler/scheduler.c
+++ b/UAV-ControlSystem/src/Scheduler/scheduler.c
@@ -339,6 +339,8 @@ void initSchedulerTasks(void)
 
 	enableTask(TASK_BATTERY, true);
 
+	enableTask(TASK_ARDUINO, true);
+
 #ifdef BARO
 	enableTask(TASK_BARO, true);
 #endif
diff --git a/UAV-ControlSystem/src/Scheduler/tasks.c b/UAV-ControlSystem/src/Scheduler/tasks.c
index f06d65d..a3f6a19 100644
--- a/UAV-ControlSystem/src/Scheduler/tasks.c
+++ b/UAV-ControlSystem/src/Scheduler/tasks.c
@@ -100,6 +100,14 @@ task_t SystemTasks[TASK_COUNT] =
 			.staticPriority = PRIORITY_MEDIUM,
 		},
 
+		[TASK_ARDUINO] =
+		{
+		     .taskName = "ARDUINO",
+		     .taskFunction = systemTaskArduino,
+		 	 .desiredPeriod = GetUpdateRateHz(50),			//50 hz update rate (20 ms)
+		 	 .staticPriority = PRIORITY_MEDIUM,
+		 },
+
 #ifdef BARO
 		[TASK_BARO] =
 		{
diff --git a/UAV-ControlSystem/src/config/cli.c b/UAV-ControlSystem/src/config/cli.c
index 1d7cb04..12b5d09 100644
--- a/UAV-ControlSystem/src/config/cli.c
+++ b/UAV-ControlSystem/src/config/cli.c
@@ -183,6 +183,7 @@ typedef enum
 	COMMAND_ID_PERIOD_RX_CLI,
 	COMMAND_ID_PERIOD_SERIAL,
 	COMMAND_ID_PERIOD_BATTERY,
+	COMMAND_ID_PERIOD_ARDUINO,
 #ifdef BARO
 	COMMAND_ID_PERIOD_BARO,
 #endif
@@ -371,6 +372,10 @@ const cliCommandConfig_t commandTable[COMMAND_ID_COUNT] = {
 		{
 			"task_period_battery", 	COMMAND_ID_PERIOD_BATTERY, EEPROM_PERIOD_BATTERY, EEPROM_VALUE_TYPE_SYSTEM, 0, VAL_UINT_32, .valueRange = {0, 1000000000}
 		},
+		[COMMAND_ID_PERIOD_ARDUINO] =
+		{
+			 "task_period_arduino", 	COMMAND_ID_PERIOD_ARDUINO, EEPROM_PERIOD_ARDUINO, EEPROM_VALUE_TYPE_SYSTEM, 0, VAL_UINT_32, .valueRange = {0, 1000000000}
+		},
 #ifdef BARO
 		[COMMAND_ID_PERIOD_BARO] =
 		{
diff --git a/UAV-ControlSystem/src/config/eeprom.c b/UAV-ControlSystem/src/config/eeprom.c
index 7852cc1..2d3c2c3 100644
--- a/UAV-ControlSystem/src/config/eeprom.c
+++ b/UAV-ControlSystem/src/config/eeprom.c
@@ -154,6 +154,12 @@ EEPROM_DATA_t eeprom_sys_Arr[EEPROM_SYS_COUNT] = {
 		.dataPtr = &(SystemTasks[TASK_BATTERY].desiredPeriod),
 	},
 
+	[EEPROM_PERIOD_ARDUINO] =
+	{
+	    .size = sizeof(SystemTasks[TASK_ARDUINO].desiredPeriod),
+	    .dataPtr = &(SystemTasks[TASK_ARDUINO].desiredPeriod),
+	},
+
 #ifdef BARO
 	[EEPROM_PERIOD_BARO] =
 	{
diff --git a/UAV-ControlSystem/src/drivers/arduino_com.c b/UAV-ControlSystem/src/drivers/arduino_com.c
index 0068ea9..75b9d14 100644
--- a/UAV-ControlSystem/src/drivers/arduino_com.c
+++ b/UAV-ControlSystem/src/drivers/arduino_com.c
@@ -10,6 +10,7 @@
 #include "utilities.h"
 #include "string.h"
 #include "stm32f4xx_revo.h"
+#include "Flight/pid.h"
 
 #define COMPASS_PACKET_SIZE 8
 #define GPS_PACKET_SIZE 10
@@ -49,7 +50,7 @@ enum smartportID {
 arduino_sensor_t sensors[SENSOR_COUNT];
 
 
-usart_dma_profile dmaHandler;
+usart_dma_profile usartdmaHandler;
 dma_usart_return raw_dma_data_t;
 
 // enumeration to hold the id:s of the different packages
@@ -91,7 +92,7 @@ arduino_data_t data_arr[ARDUINO_DATA_COUNT] = {
 void arduinoCom_init(USART_TypeDef* usart_inst)
 {
 	/* initialize the USART with a dma buffer */
-	usart_init_dma(usart_inst, &dmaHandler, ARDUINO_BAUD, STOP_BITS_1, PARITY_NONE, ARDUINO_DMA_SIZE, 0);
+	usart_init_dma(usart_inst, &usartdmaHandler, ARDUINO_BAUD, STOP_BITS_1, PARITY_NONE, ARDUINO_DMA_SIZE, 0);
 
 	/*Initialize the sensors to be sent over smartport*/
 	sensors[CURRENT_SENSOR_ID].ID = FSS_SENSOR_CURRENT;
@@ -118,8 +119,7 @@ void arduinoCom_init(USART_TypeDef* usart_inst)
 bool arduino_frame_available()
 {
 	/* We read data from DMA */
-	raw_dma_data_t = usart_get_dma_buffer(&dmaHandler);
-
+	raw_dma_data_t = usart_get_dma_buffer(&usartdmaHandler);
 	return raw_dma_data_t.new_data;
 }
 
@@ -237,6 +237,8 @@ void arduino_parse_message(uint8_t data)
 ***********************************************************************/
 void arduino_read()
 {
+	raw_dma_data_t = usart_get_dma_buffer(&usartdmaHandler);
+
 	//If the DMA has come to a new buffer
 	if (raw_dma_data_t.new_data)
 	{
@@ -265,19 +267,19 @@ void update_sensor_values()
 {
 
 	/* TODO: Add the correct data to the value parameters here*/
-	sensors[CURRENT_SENSOR_ID].value += 1;
+	sensors[CURRENT_SENSOR_ID].value = Yaw * (180 / 3.1415) + 180;
 	sensors[CURRENT_SENSOR_ID].crc = calculate_crc(&sensors[CURRENT_SENSOR_ID], ARDUINO_SENSOR_SIZE - 1);
 
-	sensors[VOLTAGE_SENSOR_ID].value += 2;
+	sensors[VOLTAGE_SENSOR_ID].value = YawU * (180 / 3.1415) + 180;
 	sensors[VOLTAGE_SENSOR_ID].crc = calculate_crc(&sensors[VOLTAGE_SENSOR_ID], ARDUINO_SENSOR_SIZE - 1);
 
-	sensors[BAROMETER_SENSOR_ID].value += 3;
+	sensors[BAROMETER_SENSOR_ID].value = 0;
 	sensors[BAROMETER_SENSOR_ID].crc = calculate_crc(&sensors[BAROMETER_SENSOR_ID], ARDUINO_SENSOR_SIZE - 1);
 
-	sensors[TUNE_PITCH_ID].value += 4;
+	sensors[TUNE_PITCH_ID].value = 0;
 	sensors[TUNE_PITCH_ID].crc = calculate_crc(&sensors[TUNE_PITCH_ID], ARDUINO_SENSOR_SIZE - 1);
 
-	sensors[TUNE_ROLL_ID].value += 5;
+	sensors[TUNE_ROLL_ID].value = 0;
 	sensors[TUNE_ROLL_ID].crc = calculate_crc(&sensors[TUNE_ROLL_ID], ARDUINO_SENSOR_SIZE - 1);
 }
 
@@ -291,7 +293,7 @@ void arduino_send_sensor_values()
 	update_sensor_values();
 	for (int i = 0; i < SENSOR_COUNT; i++)
 	{
-		usart_transmit(&dmaHandler.usart_pro, (uint8_t *) &sensors[i], 6, 10000);
+		usart_transmit(&usartdmaHandler.usart_pro, (uint8_t *) &sensors[i], 6, 10000);
 	}
 }
 
diff --git a/UAV-ControlSystem/src/drivers/usart.c b/UAV-ControlSystem/src/drivers/usart.c
index 437fadd..ac01f4c 100644
--- a/UAV-ControlSystem/src/drivers/usart.c
+++ b/UAV-ControlSystem/src/drivers/usart.c
@@ -84,7 +84,7 @@ bool usart_init_dma(USART_TypeDef* usart_inst,      // The USART instance to be
 	}
 	else if(usart_inst == USART6)
 	{
-		dma_rx_instance = DMA2_Stream2;
+		dma_rx_instance = DMA2_Stream1;
 		dma_tx_instance = DMA2_Stream6;
 		channel  = DMA_CHANNEL_5;
 	}
diff --git a/UAV-ControlSystem/src/main.c b/UAV-ControlSystem/src/main.c
index fb73cea..a2960e5 100644
--- a/UAV-ControlSystem/src/main.c
+++ b/UAV-ControlSystem/src/main.c
@@ -28,7 +28,8 @@
 #include "drivers/motormix.h"
 #include "drivers/motors.h"
 #include "Flight/pid.h"
-#include "drivers/barometer.h"#include "drivers/arduino_com.h"
+#include "drivers/barometer.h"
+#include "drivers/arduino_com.h"
 
 /**************************************************************************
 * BRIEF: Should contain all the initializations of the system, needs to
@@ -64,6 +65,9 @@ void init_system()
 	//init sbus, using USART1
 	sbus_init();
 
+	arduinoCom_init(USART6);
+//	uart1_rx_inverter_init(false);
+
 
 #ifdef USE_LEDS
 	//Initialize the on board leds
diff --git a/UAV-ControlSystem/src/tasks_main.c b/UAV-ControlSystem/src/tasks_main.c
index 4392405..12ef234 100644
--- a/UAV-ControlSystem/src/tasks_main.c
+++ b/UAV-ControlSystem/src/tasks_main.c
@@ -42,6 +42,7 @@
 #include "drivers/motormix.h"
 #include "Flight/pid.h"
 #include "drivers/barometer.h"
+#include "drivers/arduino_com.h"
 
 void systemTaskGyroPid(void)
 {
@@ -224,6 +225,12 @@ void systemTaskBattery(void)
 		ledOffInverted(Led0_PIN, Led0_GPIO_PORT);
 }
 
+void systemTaskArduino(void)
+{
+    arduino_read();
+    arduino_send_sensor_values();
+}
+
 void systemTaskBaro(void)
 {
 	//barometer_CaclulateValues();