philip/MDH-UAV-Control-System
Archived
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Got compass to work again

Browse Source
This commit is contained in:
Lennart Eriksson 2016-11-23 13:56:55 +01:00
parent 3c10f7b4de
commit e2c8ffee5a
14 changed files with 121 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
UAV-ControlSystem/inc/Flight/pid.h
View File

@ -67,6 +67,8 @@ extern float accPitchFineTune;
extern accel_t accelProfile; /*Struct profile for input data from sensor*/
extern float Yaw;
extern float YawU;
/**************************************************************************

1
UAV-ControlSystem/inc/Scheduler/scheduler.h
View File

@ -107,6 +107,7 @@ typedef enum
TASK_RX_CLI,
TASK_SERIAL,
TASK_BATTERY,
TASK_ARDUINO,
#ifdef BARO
TASK_BARO,
#endif

1
UAV-ControlSystem/inc/Scheduler/tasks.h
View File

@ -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);

1
UAV-ControlSystem/inc/config/eeprom.h
View File

@ -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

2
UAV-ControlSystem/inc/system_variables.h
View File

@ -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"

75
UAV-ControlSystem/src/Flight/pid.c
View File

@ -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;
}

2
UAV-ControlSystem/src/Scheduler/scheduler.c
View File

@ -339,6 +339,8 @@ void initSchedulerTasks(void)
enableTask(TASK_BATTERY, true);
enableTask(TASK_ARDUINO, true);
#ifdef BARO
enableTask(TASK_BARO, true);
#endif

8
UAV-ControlSystem/src/Scheduler/tasks.c
View File

@ -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] =
{

5
UAV-ControlSystem/src/config/cli.c
View File

@ -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] =
{

6
UAV-ControlSystem/src/config/eeprom.c
View File

@ -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] =
{

22
UAV-ControlSystem/src/drivers/arduino_com.c
View File

@ -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);
}
}

2
UAV-ControlSystem/src/drivers/usart.c
View File

@ -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;
}

6
UAV-ControlSystem/src/main.c
View File

@ -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

7
UAV-ControlSystem/src/tasks_main.c
View File

@ -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();