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

Merge remote-tracking branch 'refs/remotes/origin/Compass'

Browse Source
This commit is contained in:
Lennart Eriksson 2016-12-05 08:47:01 +01:00
commit 12278e3866
18 changed files with 510 additions and 103 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
UAV-ControlSystem/.custom.cfg Normal file
View File

@ -0,0 +1,13 @@
# This is an .custom board with a single STM32F411RETx chip.
# Generated by System Workbench for STM32
source [find interface/stlink-v2-1.cfg]
set WORKAREASIZE 0x20000
transport select "hla_swd"
source [find target/stm32f4x_stlink.cfg]
# use hardware reset, connect under reset
reset_config srst_only srst_nogate

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

@ -70,6 +70,8 @@ extern accel_t accelProfile;
extern float throttleRate; extern float throttleRate;
extern int HoverForce;/*Struct profile for input data from sensor*/ extern int HoverForce;/*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_RX_CLI,
TASK_SERIAL, TASK_SERIAL,
TASK_BATTERY, TASK_BATTERY,
TASK_ARDUINO,
#ifdef BARO #ifdef BARO
TASK_BARO, TASK_BARO,
#endif #endif

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

@ -38,6 +38,7 @@ void systemTaskRxCli(void);
bool systemTaskRxCliCheck(uint32_t currentDeltaTime); bool systemTaskRxCliCheck(uint32_t currentDeltaTime);
void systemTaskSerial(void); void systemTaskSerial(void);
void systemTaskBattery(void); void systemTaskBattery(void);
void systemTaskArduino(void);
void systemTaskBaro(void); void systemTaskBaro(void);
void systemTaskCompass(void); void systemTaskCompass(void);
void systemTaskGps(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_RX_CLI,
EEPROM_PERIOD_SERIAL, EEPROM_PERIOD_SERIAL,
EEPROM_PERIOD_BATTERY, EEPROM_PERIOD_BATTERY,
EEPROM_PERIOD_ARDUINO,
#ifdef BARO #ifdef BARO
EEPROM_PERIOD_BARO, EEPROM_PERIOD_BARO,
#endif #endif

50
UAV-ControlSystem/inc/drivers/arduino_com.h
View File

@ -19,11 +19,11 @@
* INFORMATION: Contains the whole compass message * * INFORMATION: Contains the whole compass message *
***********************************************************************/ ***********************************************************************/
typedef struct compass_data_t { typedef struct compass_data_t {
uint8_t header; uint8_t header __attribute__((packed));
int16_t x; int16_t x __attribute__((packed));
int16_t y; int16_t y __attribute__((packed));
int16_t z; int16_t z __attribute__((packed));
uint8_t crc; uint8_t crc __attribute__((packed));
} compass_data_t; } compass_data_t;
/*********************************************************************** /***********************************************************************
@ -31,10 +31,11 @@ typedef struct compass_data_t {
* INFORMATION: Contains the whole gps data message * * INFORMATION: Contains the whole gps data message *
***********************************************************************/ ***********************************************************************/
typedef struct gps_data_t { typedef struct gps_data_t {
uint8_t header; uint8_t header __attribute__((packed));
float latitude; float latitude __attribute__((packed));
float longitude; float longitude __attribute__((packed));
uint8_t crc; uint8_t num_of_sats __attribute__((packed));
uint8_t crc __attribute__((packed));
} gps_data_t; } gps_data_t;
/*********************************************************************** /***********************************************************************
@ -42,9 +43,9 @@ typedef struct gps_data_t {
* INFORMATION: Contains the whole ping sensor data message * * INFORMATION: Contains the whole ping sensor data message *
***********************************************************************/ ***********************************************************************/
typedef struct ping_data_t { typedef struct ping_data_t {
uint8_t header; uint8_t header __attribute__((packed));
uint16_t distance_mm; uint16_t distance_mm __attribute__((packed));
uint8_t crc; uint8_t crc __attribute__((packed));
}ping_data_t; }ping_data_t;
/* An instance of the GPS data read from Arduino Com */ /* An instance of the GPS data read from Arduino Com */
@ -74,13 +75,36 @@ bool arduino_frame_available();
***********************************************************************/ ***********************************************************************/
void arduino_read(); void arduino_read();
/*********************************************************************** /***********************************************************************
* BRIEF: Update the output sensor values and sends them to the Arduino * * BRIEF: Update the output sensor values and sends them to the Arduino *
* INFORMATION: * * INFORMATION: *
***********************************************************************/ ***********************************************************************/
void arduino_send_sensor_values(); void arduino_send_sensor_values();
/***********************************************************************
* BRIEF: Check so that the heartbeat messages are comming with a *
* steady stream *
* INFORMATION: Check the last time a heart beat message was received *
* and checks against a pre defined time before declaring *
* the communication as dead *
***********************************************************************/
bool arduino_com_alive();
/***********************************************************************
* BRIEF: Set a color on a specific led on the neo ledstrip attached *
* to the arduino *
* INFORMATION: Send a command with the led index and RGB color to the *
* Arduino *
***********************************************************************/
void arduino_set_led_color(uint8_t index, uint8_t r, uint8_t g, uint8_t b);
/***********************************************************************
* BRIEF: Tell the arduino that the FC system is OK and everything is *
* working *
* INFORMATION: Set the datavalue to 0xBA1DFACE for the led data *
***********************************************************************/
void arduino_im_ok();
#endif /* DRIVERS_ARDUINO_COM_H_ */ #endif /* DRIVERS_ARDUINO_COM_H_ */

14
UAV-ControlSystem/inc/drivers/compass.h Normal file
View File

@ -0,0 +1,14 @@
#ifndef DRIVERS_COMPASS_H
#define DRIVERS_COMPASS_H
bool initialize_compass();
void calibrate_compass();
void calculate_heading();
#endif //DRIVERS_COMPASS_H

9
UAV-ControlSystem/inc/drivers/usart.h
View File

@ -46,6 +46,14 @@ typedef enum parity
PARITY_ODD = 0x3 PARITY_ODD = 0x3
} parity; } parity;
typedef enum usart_index
{
USART1_IDX = 0,
USART3_IDX,
USART6_IDX,
USART_INDEX_COUNT,
}usart_index;
// Struct to be used for regular USART with polling // Struct to be used for regular USART with polling
typedef struct usart_profile typedef struct usart_profile
{ {
@ -61,6 +69,7 @@ typedef struct usart_dma_profile
uint8_t* dma_rx_buffer1; // The first rx buffer used in double buffering uint8_t* dma_rx_buffer1; // The first rx buffer used in double buffering
uint8_t* dma_rx_buffer2; // The second rx buffer used in double buffering uint8_t* dma_rx_buffer2; // The second rx buffer used in double buffering
uint8_t* dma_tx_buffer; // The tx buffer used for sending messages uint8_t* dma_tx_buffer; // The tx buffer used for sending messages
void* dma_rx_prev_buffer; // Keep track of the previous read buffer
} usart_dma_profile; } usart_dma_profile;

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

@ -20,10 +20,12 @@
#include "drivers/failsafe_toggles.h" #include "drivers/failsafe_toggles.h"
#include "drivers/motormix.h" #include "drivers/motormix.h"
#include "utilities.h" #include "utilities.h"
#include "drivers/arduino_com.h"
#include "drivers/barometer.h" #include "drivers/barometer.h"
#include "drivers/system_clock.h" #include "drivers/system_clock.h"
#define PTERM_SCALE 0.032029f /*P-term used as a scale value to the PID controller*/ #define PTERM_SCALE 0.032029f /*P-term used as a scale value to the PID controller*/
#define ITERM_SCALE 0.0012f /*I-term used as a scale value to the PID controller*/ #define ITERM_SCALE 0.0012f /*I-term used as a scale value to the PID controller*/
#define DTERM_SCALE 0.0529f /*D-term used as a scale value to the PID controller*/ #define DTERM_SCALE 0.0529f /*D-term used as a scale value to the PID controller*/
@ -68,6 +70,44 @@ pt1Filter_t accelFilter[2] = {0};
float accRollFineTune = 0; float accRollFineTune = 0;
float accPitchFineTune = 0; float accPitchFineTune = 0;
float oldSensorValue[2] = {0};
float oldSensorValueRoll[12] = {0};
float oldSensorValuePitch[12] = {0};
/**************************************************************************
* BRIEF: Calculates angle from accelerometer *
* INFORMATION: *
**************************************************************************/
float calcAngle(const uint8_t axis, const float x_axis, const float y_axis, const float z_axis)
{
float angle;
switch (axis)
{
case ROLL:
angle = atan2(x_axis, sqrt(y_axis*y_axis + z_axis*z_axis))*180/M_PI;
angle = -1*((angle > 0)? (z_axis < 0 )? 180 - angle: angle : (z_axis < 0 )? - 180 - angle: angle);
break;
case PITCH:
angle = atan2( y_axis, sqrt(z_axis*z_axis + x_axis*x_axis))*180/M_PI; /*down (the front down against ground) = pos angle*/
angle = (angle > 0)? ((z_axis < 0))? 180 - angle: angle : (z_axis < 0 )? - 180 - angle: angle;
break;
default:
angle = 0;
break;
}
return angle;
}
float calcGravity(accel_t profile ) //const float x_axis, const float y_axis, const float z_axis)
{
return sqrt(profile.accelXconv*profile.accelXconv + profile.accelYconv*profile.accelYconv + profile.accelZconv*profile.accelZconv);
}
float throttleRate = 1; float throttleRate = 1;
int HoverForce = 1475; /*Struct profile for input data from sensor*/ int HoverForce = 1475; /*Struct profile for input data from sensor*/
@ -80,6 +120,23 @@ float convertData(int inputRange, int outputRange, int offset, float value)
return ((float)outputRange/(float)inputRange)*(value-(float)offset); return ((float)outputRange/(float)inputRange)*(value-(float)offset);
} }
/**************************************************************************
* BRIEF: Constrain float values within a defined limit *
* INFORMATION: Used in PID loop to limit values *
**************************************************************************/
float constrainfu(float amt, int low, int high)
{
if (amt < (float)low)
return (float)low;
else if (amt > (float)high)
return (float)high;
else
return amt;
}
int i = 0;
uint8_t FlagVelocityLimit = 0; uint8_t FlagVelocityLimit = 0;
float VelocityCompensation = 0; float VelocityCompensation = 0;
@ -115,14 +172,6 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile)
/*Checks the biggest angle */ /*Checks the biggest angle */
throttleRate = cos(ABS_FLOAT(sensorValues[PITCH])*M_PI/180)*cos(ABS_FLOAT(sensorValues[ROLL])*M_PI/180); throttleRate = cos(ABS_FLOAT(sensorValues[PITCH])*M_PI/180)*cos(ABS_FLOAT(sensorValues[ROLL])*M_PI/180);
break;
case PID_ID_COMPASS:
sensorValues[ROLL] = 0;
sensorValues[PITCH] = 0;
sensorValues[YAW] = 0;
break; break;
case PID_ID_BAROMETER: case PID_ID_BAROMETER:
@ -139,6 +188,8 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile)
oldHeightValue = current_height; oldHeightValue = current_height;
sensorValues[0]*=BAROMETER_SCALE; sensorValues[0]*=BAROMETER_SCALE;
break;
case PID_ID_COMPASS:
break; break;
default: default:
current_micros = clock_get_us(); current_micros = clock_get_us();
@ -239,7 +290,7 @@ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff,
/*Limits the PTerm of the Yaw axis */ /*Limits the PTerm of the Yaw axis */
if (pidProfile->yaw_p_limit) if (pidProfile->yaw_p_limit)
{ {
PTerm = constrainf(PTerm, -pidProfile->yaw_p_limit, pidProfile->yaw_p_limit); PTerm = constrainfu(PTerm, -pidProfile->yaw_p_limit, pidProfile->yaw_p_limit);
} }
} }
@ -247,12 +298,12 @@ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff,
float ITerm = pidProfileBuff->lastITerm[axis] + ITERM_SCALE * rateError * pidProfileBuff->dT * (float)pidProfile->I[axis]; float ITerm = pidProfileBuff->lastITerm[axis] + ITERM_SCALE * rateError * pidProfileBuff->dT * (float)pidProfile->I[axis];
// limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated. // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
ITerm = constrainf(ITerm, -(int)PID_MAX_I, (int)PID_MAX_I); ITerm = constrainfu(ITerm, -(int)PID_MAX_I, (int)PID_MAX_I);
// Anti windup protection // Anti windup protection
if (motorLimitReached) if (motorLimitReached)
{ {
ITerm = constrainf(ITerm, -pidProfileBuff->ITermLimit[axis], pidProfileBuff->ITermLimit[axis]); ITerm = constrainfu(ITerm, -pidProfileBuff->ITermLimit[axis], pidProfileBuff->ITermLimit[axis]);
} }
else else
{ {
@ -288,7 +339,7 @@ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff,
} }
DTerm = DTERM_SCALE * delta * (float)pidProfile->D[axis] * (float)pidProfile->PIDweight[axis] / 100.0; DTerm = DTERM_SCALE * delta * (float)pidProfile->D[axis] * (float)pidProfile->PIDweight[axis] / 100.0;
DTerm = constrainf(DTerm, -PID_MAX_D, PID_MAX_D); DTerm = constrainfu(DTerm, -PID_MAX_D, PID_MAX_D);
} }
@ -299,7 +350,7 @@ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff,
pidProfileBuff->lastITerm[axis] = 0; pidProfileBuff->lastITerm[axis] = 0;
pidProfileBuff->ITermLimit[axis] = 0; pidProfileBuff->ITermLimit[axis] = 0;
} }
pidProfile->PID_Out[axis] = constrainf(PTerm + ITerm + DTerm, -(int)pidProfile->pid_out_limit, (int)pidProfile->pid_out_limit); pidProfile->PID_Out[axis] = constrainfu(PTerm + ITerm + DTerm, -(int)pidProfile->pid_out_limit, (int)pidProfile->pid_out_limit);
} }
/************************************************************************** /**************************************************************************
@ -401,11 +452,11 @@ void pidRun(uint8_t ID)
else else
{ {
pidUAV(&PidProfile[PID_ID_BAROMETER], &PidProfileBuff[PID_ID_BAROMETER]); pidUAV(&PidProfile[PID_ID_BAROMETER], &PidProfileBuff[PID_ID_BAROMETER]);
PidProfile[PID_ID_BAROMETER].PID_Out[0] = constrainf(PidProfile[PID_ID_BAROMETER].PID_Out[0], -20, (int)PidProfile[PID_ID_BAROMETER].pid_out_limit); PidProfile[PID_ID_BAROMETER].PID_Out[0] = constrainfu(PidProfile[PID_ID_BAROMETER].PID_Out[0], -20, (int)PidProfile[PID_ID_BAROMETER].pid_out_limit);
//PidProfile[PID_ID_BAROMETER].PID_Out[0] = constrainf(PidProfile[PID_ID_BAROMETER].PID_Out[0], -15, (int)PidProfile[PID_ID_BAROMETER].pid_out_limit); //PidProfile[PID_ID_BAROMETER].PID_Out[0] = constrainfu(PidProfile[PID_ID_BAROMETER].PID_Out[0], -15, (int)PidProfile[PID_ID_BAROMETER].pid_out_limit);
//PidProfile[PID_ID_BAROMETER].PID_Out[0] = constrainf(PidProfile[PID_ID_BAROMETER].PID_Out[0], -(int)PidProfile[PID_ID_BAROMETER].pid_out_limit, (int)PidProfile[PID_ID_BAROMETER].pid_out_limit); //PidProfile[PID_ID_BAROMETER].PID_Out[0] = constrainfu(PidProfile[PID_ID_BAROMETER].PID_Out[0], -(int)PidProfile[PID_ID_BAROMETER].pid_out_limit, (int)PidProfile[PID_ID_BAROMETER].pid_out_limit);
} }
break; break;

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

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

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

@ -100,6 +100,14 @@ task_t SystemTasks[TASK_COUNT] =
.staticPriority = PRIORITY_MEDIUM, .staticPriority = PRIORITY_MEDIUM,
}, },
[TASK_ARDUINO] =
{
.taskName = "ARDUINO",
.taskFunction = systemTaskArduino,
.desiredPeriod = GetUpdateRateHz(50), //50 hz update rate (20 ms)
.staticPriority = PRIORITY_MEDIUM,
},
#ifdef BARO #ifdef BARO
[TASK_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_RX_CLI,
COMMAND_ID_PERIOD_SERIAL, COMMAND_ID_PERIOD_SERIAL,
COMMAND_ID_PERIOD_BATTERY, COMMAND_ID_PERIOD_BATTERY,
COMMAND_ID_PERIOD_ARDUINO,
#ifdef BARO #ifdef BARO
COMMAND_ID_PERIOD_BARO, COMMAND_ID_PERIOD_BARO,
#endif #endif
@ -384,6 +385,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} "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 #ifdef BARO
[COMMAND_ID_PERIOD_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), .dataPtr = &(SystemTasks[TASK_BATTERY].desiredPeriod),
}, },
[EEPROM_PERIOD_ARDUINO] =
{
.size = sizeof(SystemTasks[TASK_ARDUINO].desiredPeriod),
.dataPtr = &(SystemTasks[TASK_ARDUINO].desiredPeriod),
},
#ifdef BARO #ifdef BARO
[EEPROM_PERIOD_BARO] = [EEPROM_PERIOD_BARO] =
{ {

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

@ -10,11 +10,34 @@
#include "utilities.h" #include "utilities.h"
#include "string.h" #include "string.h"
#include "stm32f4xx_revo.h" #include "stm32f4xx_revo.h"
#include "Flight/pid.h"
#define COMPASS_PACKET_SIZE 8 #define COMPASS_PACKET_SIZE 8
#define GPS_PACKET_SIZE 10 #define GPS_PACKET_SIZE 11
#define PING_PACKET_SIZE 4 #define PING_PACKET_SIZE 4
#define ARDUINO_SENSOR_SIZE 6 #define ARDUINO_SENSOR_SIZE 6
#define TIME_BEFORE_DEATH_MS 500
#define SET_BYTE1_32BITS_VALUE(x) ((x & 0xFF) << 24)
#define SET_BYTE2_32BITS_VALUE(x) ((x & 0xFF) << 16)
#define SET_BYTE3_32BITS_VALUE(x) ((x & 0xFF) << 8)
#define SET_BYTE4_32BITS_VALUE(x) ((x & 0xFF) << 0)
#define USE_STORED_WP
#define USE_CURR_POS
#define USE_CURR_HEADING
#define USE_DISTANCE_TO_HOME
#define USE_CURRENT_SPEED
#define USE_CURRENT_ALTITUDE
const uint8_t heartbeat_msg[4] = { 0xDE, 0xAD, 0xBE, 0xEF };
const uint8_t heartbeat_rsp[4] = { 0xBA, 0x1D, 0xFA, 0xCE };
uint32_t time_since_heartbeat = 0;
typedef struct arduino_sensor_t { typedef struct arduino_sensor_t {
uint8_t ID __attribute__((packed)); uint8_t ID __attribute__((packed));
@ -29,20 +52,84 @@ enum smartport_packets_e {
FSSP_DATA_FRAME = 0x10, // Sensor replies with this as start byte FSSP_DATA_FRAME = 0x10, // Sensor replies with this as start byte
// ID of sensors. Must be something polled by FrSky RX // ID of sensors. Must be something polled by FrSky RX
FSS_SENSOR_CURRENT = 0xA1, #ifdef USE_STORED_WP
FSS_SENSOR_VOLTAGE = 0x22, FSS_WP_LON = 0xA1, //Physical 2
FSS_SENSOR_BAROMETER = 0x1B, FSS_WP_LAT = 0x22, //Physical 3
FSS_TUNE_PITCH = 0x0D, #endif
FSS_TUNE_ROLL = 0x34,
FSS_SENSOR_6 = 0x67, #ifdef USE_CURR_POS
FSS_CURR_POS_LON = 0x83, //Physical 4
FSS_CURR_POS_LAT = 0xE4, //Physical 5
#endif
#ifdef USE_CURR_HEADING
FSS_CURR_HEADING = 0x45, //Physical 6
#endif
#ifdef USE_DISTANCE_TO_HOME
FSS_DIST_HOME = 0xC6, //Physical 7
#endif
#ifdef USE_CURRENT_SPEED
FSS_SPEED = 0x67, //Physical 8
#endif
#ifdef USE_CURRENT_ALTITUDE
FSS_ALTITUDE = 0x48, //Physical 9
#endif
FSS_SENSOR_10 = 0xE9, //Physical 10
FSS_SENSOR_11 = 0x6A, //Physical 11
FSS_SENSOR_12 = 0xCB, //Physical 12
FSS_SENSOR_13 = 0xAC, //Physical 13
FSS_SENSOR_14 = 0x0D, //Physical 14
FSS_SENSOR_15 = 0x8E, //Physical 15
FSS_SENSOR_16 = 0x2F, //Physical 16
FSS_SENSOR_17 = 0xD0, //Physical 17
FSS_SENSOR_18 = 0x71, //Physical 18
FSS_SENSOR_19 = 0xF2, //Physical 19
FSS_SENSOR_20 = 0x53, //Physical 20
FSS_SENSOR_21 = 0x34, //Physical 21
FSS_SENSOR_22 = 0x95, //Physical 22
FSS_SENSOR_23 = 0x16, //Physical 23
FSS_SENSOR_24 = 0xB7, //Physical 24
FSS_SENSOR_25 = 0x98, //Physical 25
FSS_SENSOR_26 = 0x39, //Physical 26
FSS_SENSOR_27 = 0xBA, //Physical 27
FSS_SENSOR_28 = 0x1B, //Physical 28
//This is for handeling the LED strip and has nothing to do with the smartport
//It is only handled here since the information comes from the FC and is sent
//over the same bus as the rest of the messages
LED_STRIP_DATA = 0xED
}; };
enum smartportID { enum smartportID {
CURRENT_SENSOR_ID = 0, #ifdef USE_STORED_WP
VOLTAGE_SENSOR_ID, WP_LON_ID,
BAROMETER_SENSOR_ID, WP_LAT_ID,
TUNE_PITCH_ID, #endif
TUNE_ROLL_ID, #ifdef USE_CURR_POS
CURR_LON_ID,
CURR_LAT_ID,
#endif
#ifdef USE_CURR_HEADING
CURR_HEADING_ID,
#endif
#ifdef USE_DISTANCE_TO_HOME
DIST_HOME_ID,
#endif
#ifdef USE_CURRENT_SPEED
SPEED_ID,
#endif
#ifdef USE_CURRENT_ALTITUDE
ALTITUDE_ID,
#endif
//LED_STRIP_ID should only be on the flight controller side
LED_STRIP_ID,
//the number of sensors to send data from .
SENSOR_COUNT, SENSOR_COUNT,
}; };
@ -50,8 +137,8 @@ enum smartportID {
arduino_sensor_t sensors[SENSOR_COUNT]; arduino_sensor_t sensors[SENSOR_COUNT];
usart_dma_profile dmaHandler; usart_dma_profile usartdmaHandler;
dma_usart_return raw_dma_data_t; dma_usart_return raw_arduino_dma_data_t;
// enumeration to hold the id:s of the different packages // enumeration to hold the id:s of the different packages
enum packet_ids { enum packet_ids {
@ -98,23 +185,40 @@ arduino_data_t data_arr[ARDUINO_DATA_COUNT] = {
void arduinoCom_init(USART_TypeDef* usart_inst) void arduinoCom_init(USART_TypeDef* usart_inst)
{ {
/* initialize the USART with a dma buffer */ /* 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*/ /*Initialize the sensors to be sent over smartport*/
sensors[CURRENT_SENSOR_ID].ID = FSS_SENSOR_CURRENT; #ifdef USE_STORED_WP
sensors[CURRENT_SENSOR_ID].value = 0; sensors[WP_LON_ID].ID = FSS_WP_LON;
sensors[WP_LON_ID].value = 0;
sensors[WP_LAT_ID].ID = FSS_WP_LAT;
sensors[WP_LAT_ID].value = 0;
#endif
#ifdef USE_CURR_POS
sensors[CURR_LON_ID].ID = FSS_CURR_POS_LON;
sensors[CURR_LON_ID].value = 0;
sensors[CURR_LAT_ID].ID = FSS_CURR_POS_LAT;
sensors[CURR_LAT_ID].value = 0;
#endif
#ifdef USE_CURR_HEADING
sensors[CURR_HEADING_ID].ID = FSS_CURR_HEADING;
sensors[CURR_HEADING_ID].value = 0;
#endif
#ifdef USE_DISTANCE_TO_HOME
sensors[DIST_HOME_ID].ID = FSS_DIST_HOME;
sensors[DIST_HOME_ID].value = 0;
#endif
#ifdef USE_CURRENT_SPEED
sensors[SPEED_ID].ID = FSS_SPEED;
sensors[SPEED_ID].value = 0;
#endif
#ifdef USE_CURRENT_ALTITUDE
sensors[ALTITUDE_ID].ID = FSS_ALTITUDE;
sensors[ALTITUDE_ID].value = 0;
#endif
sensors[VOLTAGE_SENSOR_ID].ID = FSS_SENSOR_VOLTAGE; sensors[LED_STRIP_ID].ID = LED_STRIP_DATA;
sensors[VOLTAGE_SENSOR_ID].value = 0; sensors[LED_STRIP_ID].value = 0;
sensors[BAROMETER_SENSOR_ID].ID = FSS_SENSOR_BAROMETER;
sensors[BAROMETER_SENSOR_ID].value = 0;
sensors[TUNE_PITCH_ID].ID = FSS_TUNE_PITCH;
sensors[TUNE_PITCH_ID].value = 0;
sensors[TUNE_ROLL_ID].ID = FSS_TUNE_ROLL;
sensors[TUNE_ROLL_ID].value = 0;
} }
@ -125,9 +229,8 @@ void arduinoCom_init(USART_TypeDef* usart_inst)
bool arduino_frame_available() bool arduino_frame_available()
{ {
/* We read data from DMA */ /* We read data from DMA */
raw_dma_data_t = usart_get_dma_buffer(&dmaHandler); raw_arduino_dma_data_t = usart_get_dma_buffer(&usartdmaHandler);
return raw_arduino_dma_data_t.new_data;
return raw_dma_data_t.new_data;
} }
/*********************************************************************** /***********************************************************************
@ -175,6 +278,36 @@ void arduino_parse_message(uint8_t data)
static uint8_t secondary_message_it = 0; static uint8_t secondary_message_it = 0;
static arduino_data_t msg_header_and_size = { .size = 0, .dataPtr = NULL }; static arduino_data_t msg_header_and_size = { .size = 0, .dataPtr = NULL };
static uint8_t crc = 0; static uint8_t crc = 0;
static uint8_t heartbeatiterator = 0;
if (heartbeatiterator == 0 && data == heartbeat_msg[0])
heartbeatiterator = 1;
if (heartbeatiterator > 0)
{
switch (heartbeatiterator)
{
case 1:
heartbeatiterator = 2;
break;
case 2:
heartbeatiterator = (data == heartbeat_msg[1]) ? 3 : 0;
break;
case 3:
heartbeatiterator = (data == heartbeat_msg[2]) ? 4 : 0;
break;
case 4:
heartbeatiterator = 0;
if(data == heartbeat_msg[3])
{
usart_transmit(&usartdmaHandler.usart_pro, (uint8_t *) &heartbeat_rsp, 4, 10000);
time_since_heartbeat = HAL_GetTick();
}
break;
default:
break;
}
}
if(find_header) if(find_header)
{ {
@ -247,13 +380,15 @@ void arduino_parse_message(uint8_t data)
***********************************************************************/ ***********************************************************************/
void arduino_read() void arduino_read()
{ {
raw_arduino_dma_data_t = usart_get_dma_buffer(&usartdmaHandler);
//If the DMA has come to a new buffer //If the DMA has come to a new buffer
if (raw_dma_data_t.new_data) if (raw_arduino_dma_data_t.new_data)
{ {
// parse the entire message to the gps_data and compass_data // parse the entire message to the gps_data and compass_data
for (int i = 0; i < ARDUINO_DMA_SIZE; i++) for (int i = 0; i < ARDUINO_DMA_SIZE; i++)
{ {
arduino_parse_message(raw_dma_data_t.buff[i]); arduino_parse_message(raw_arduino_dma_data_t.buff[i]);
} }
} }
} }
@ -273,22 +408,38 @@ uint8_t calculate_crc(uint8_t *data, uint8_t length)
***********************************************************************/ ***********************************************************************/
void update_sensor_values() void update_sensor_values()
{ {
/* TODO: Add the correct data to the value parameters here*/ /* TODO: Add the correct data to the value parameters here*/
sensors[CURRENT_SENSOR_ID].value += 1;
sensors[CURRENT_SENSOR_ID].crc = calculate_crc(&sensors[CURRENT_SENSOR_ID], ARDUINO_SENSOR_SIZE - 1);
sensors[VOLTAGE_SENSOR_ID].value += 2; #ifdef USE_STORED_WP
sensors[VOLTAGE_SENSOR_ID].crc = calculate_crc(&sensors[VOLTAGE_SENSOR_ID], ARDUINO_SENSOR_SIZE - 1); sensors[WP_LON_ID].value = 0;
sensors[WP_LON_ID].crc = calculate_crc(&sensors[WP_LON_ID], ARDUINO_SENSOR_SIZE - 1);
sensors[BAROMETER_SENSOR_ID].value += 3; sensors[WP_LAT_ID].value = 0;
sensors[BAROMETER_SENSOR_ID].crc = calculate_crc(&sensors[BAROMETER_SENSOR_ID], ARDUINO_SENSOR_SIZE - 1); sensors[WP_LAT_ID].crc = calculate_crc(&sensors[WP_LAT_ID], ARDUINO_SENSOR_SIZE - 1);
#endif
#ifdef USE_CURR_POS
sensors[CURR_LON_ID].value = gps_data.longitude;
sensors[CURR_LON_ID].crc = calculate_crc(&sensors[CURR_LON_ID], ARDUINO_SENSOR_SIZE - 1);
sensors[TUNE_PITCH_ID].value += 4; sensors[CURR_LAT_ID].value = gps_data.latitude;
sensors[TUNE_PITCH_ID].crc = calculate_crc(&sensors[TUNE_PITCH_ID], ARDUINO_SENSOR_SIZE - 1); sensors[CURR_LAT_ID].crc = calculate_crc(&sensors[CURR_LAT_ID], ARDUINO_SENSOR_SIZE - 1);
#endif
sensors[TUNE_ROLL_ID].value += 5; #ifdef USE_CURR_HEADING
sensors[TUNE_ROLL_ID].crc = calculate_crc(&sensors[TUNE_ROLL_ID], ARDUINO_SENSOR_SIZE - 1); sensors[CURR_HEADING_ID].value = 0;
sensors[CURR_HEADING_ID].crc = calculate_crc(&sensors[CURR_HEADING_ID], ARDUINO_SENSOR_SIZE - 1);
#endif
#ifdef USE_DISTANCE_TO_HOME
sensors[DIST_HOME_ID].value = 0;
sensors[DIST_HOME_ID].crc = calculate_crc(&sensors[DIST_HOME_ID], ARDUINO_SENSOR_SIZE - 1);
#endif
#ifdef USE_CURRENT_SPEED
sensors[SPEED_ID].value = 0;
sensors[SPEED_ID].crc = calculate_crc(&sensors[SPEED_ID], ARDUINO_SENSOR_SIZE - 1);
#endif
#ifdef USE_CURRENT_ALTITUDE
sensors[ALTITUDE_ID].value = ping_data.distance_mm / 100;
sensors[ALTITUDE_ID].crc = calculate_crc(&sensors[ALTITUDE_ID], ARDUINO_SENSOR_SIZE - 1);
#endif
} }
@ -298,32 +449,55 @@ void update_sensor_values()
***********************************************************************/ ***********************************************************************/
void arduino_send_sensor_values() void arduino_send_sensor_values()
{ {
static int sensor_send_index = 0;
update_sensor_values(); update_sensor_values();
for (int i = 0; i < SENSOR_COUNT; i++) usart_transmit(&usartdmaHandler.usart_pro, (uint8_t *) &sensors[sensor_send_index], 6, 10000);
{ sensor_send_index = (sensor_send_index + 1) % SENSOR_COUNT;
usart_transmit(&dmaHandler.usart_pro, (uint8_t *) &sensors[i], 6, 10000); }
}
/***********************************************************************
* BRIEF: Check so that the heartbeat messages are comming with a *
* steady stream *
* INFORMATION: Check the last time a heart beat message was received *
* and checks against a pre defined time before declaring *
* the communication as dead *
***********************************************************************/
bool arduino_com_alive()
{
return (HAL_GetTick() < (time_since_heartbeat + TIME_BEFORE_DEATH_MS));
}
/***********************************************************************
* BRIEF: Set a color on a specific led on the neo ledstrip attached *
* to the arduino *
* INFORMATION: Send a command with the led index and RGB color to the *
* Arduino *
***********************************************************************/
void arduino_set_led_color(uint8_t index, uint8_t r, uint8_t g, uint8_t b)
{
sensors[LED_STRIP_ID].value = SET_BYTE1_32BITS_VALUE(index) |
SET_BYTE2_32BITS_VALUE(r) |
SET_BYTE3_32BITS_VALUE(g) |
SET_BYTE4_32BITS_VALUE(b);
sensors[LED_STRIP_ID].crc = calculate_crc(&sensors[LED_STRIP_ID], ARDUINO_SENSOR_SIZE - 1);
}
/***********************************************************************
* BRIEF: Tell the arduino that the FC system is OK and everything is *
* working *
* INFORMATION: Set the datavalue to 0xBA1DFACE for the led data *
***********************************************************************/
void arduino_im_ok()
{
sensors[LED_STRIP_ID].value = heartbeat_rsp;
sensors[LED_STRIP_ID].crc = calculate_crc(&sensors[LED_STRIP_ID], ARDUINO_SENSOR_SIZE - 1);
} }

86
UAV-ControlSystem/src/drivers/compass.c Normal file
View File

@ -0,0 +1,86 @@
#include "drivers/compass.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
float MagnetFilteredOld[3];
float alphaMagnet = 0.4;
int MagnetMax[3];
int MagnetMin[3];
float MagnetMap[3];
float Yaw;
float YawU;
bool initialize_compass()
{
}
void calibrate_compass()
{
}
void calculate_heading()
{
// readAcc();
// 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(accelProfile.rollAngle) +
// MagnetMap[2] * sin(accelProfile.rollAngle)),
// MagnetMap[0] * cos(accelProfile.pitchAngle) +
// MagnetMap[1] * sin(accelProfile.pitchAngle) * sin(accelProfile.rollAngle) +
// MagnetMap[2] * sin(accelProfile.rollAngle) * cos(accelProfile.pitchAngle));
// YawU = atan2(MagnetMap[1], MagnetMap[0]);
//
// float XH;
// XH = (compass_data.y * cos(accelProfile.pitchAngle * (3.141592 / 180))) +
// (compass_data.x * sin(accelProfile.pitchAngle * (3.141592 / 180)) * sin(accelProfile.rollAngle * (3.141592 / 180))) +
// (compass_data.z * cos(accelProfile.pitchAngle * (3.141592 / 180)) * sin(accelProfile.rollAngle * (3.141592 / 180)));
// float YH;
// YH = (compass_data.x * cos(accelProfile.rollAngle * (3.141592 / 180))) +
// (compass_data.z * sin(accelProfile.rollAngle * (3.141592 / 180)));
//
// Yaw = atan2f(-YH, XH);
// YawU = atan2f(-compass_data.x, compass_data.y);
}

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

@ -43,8 +43,6 @@
//BRR //BRR
#define USART_BRR(_PCLK_, _BAUD_) ((_PCLK_ /(_BAUD_ * 16)) * 16) // Calculate BRR from the desired baud rate #define USART_BRR(_PCLK_, _BAUD_) ((_PCLK_ /(_BAUD_ * 16)) * 16) // Calculate BRR from the desired baud rate
/* Stores last DMA buffer address from "usart_get_dma_buffer". Is used to compare if data read is new or old */
uint8_t * prevBuf = NULL;
/*********************************************************************** /***********************************************************************
* BRIEF: Initialize the USART with DMA reception of messages * BRIEF: Initialize the USART with DMA reception of messages
@ -84,7 +82,7 @@ bool usart_init_dma(USART_TypeDef* usart_inst, // The USART instance to be
} }
else if(usart_inst == USART6) else if(usart_inst == USART6)
{ {
dma_rx_instance = DMA2_Stream2; dma_rx_instance = DMA2_Stream1;
dma_tx_instance = DMA2_Stream6; dma_tx_instance = DMA2_Stream6;
channel = DMA_CHANNEL_5; channel = DMA_CHANNEL_5;
} }
@ -112,6 +110,7 @@ bool usart_init_dma(USART_TypeDef* usart_inst, // The USART instance to be
// Set the DMA instances in the USART profile // Set the DMA instances in the USART profile
profile_out->dma_usart_rx_instance = dma_rx_instance; profile_out->dma_usart_rx_instance = dma_rx_instance;
profile_out->dma_usart_tx_instance = dma_tx_instance; profile_out->dma_usart_tx_instance = dma_tx_instance;
profile_out->dma_rx_prev_buffer = NULL;
// Enable the DMA on the USARTon register level // Enable the DMA on the USARTon register level
profile_out->usart_pro.usart_instance->CR3 |= DMAR | DMAT; profile_out->usart_pro.usart_instance->CR3 |= DMAR | DMAT;
@ -404,8 +403,8 @@ dma_usart_return usart_get_dma_buffer(usart_dma_profile *profile)
{ {
data.buff = profile->dma_rx_buffer2; data.buff = profile->dma_rx_buffer2;
} }
data.new_data = (data.buff != prevBuf); data.new_data = (data.buff != profile->dma_rx_prev_buffer);
prevBuf = data.buff; profile->dma_rx_prev_buffer = data.buff;
return data; return data;
} }

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

@ -28,7 +28,8 @@
#include "drivers/motormix.h" #include "drivers/motormix.h"
#include "drivers/motors.h" #include "drivers/motors.h"
#include "Flight/pid.h" #include "Flight/pid.h"
#include "drivers/barometer.h"#include "drivers/arduino_com.h" #include "drivers/barometer.h"
#include "drivers/arduino_com.h"
#include "drivers/beeper.h" #include "drivers/beeper.h"
/************************************************************************** /**************************************************************************
@ -65,6 +66,9 @@ void init_system()
//init sbus, using USART1 //init sbus, using USART1
sbus_init(); sbus_init();
arduinoCom_init(USART6);
// uart1_rx_inverter_init(false);
#ifdef USE_LEDS #ifdef USE_LEDS
//Initialize the on board leds //Initialize the on board leds

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

@ -42,6 +42,7 @@
#include "drivers/motormix.h" #include "drivers/motormix.h"
#include "Flight/pid.h" #include "Flight/pid.h"
#include "drivers/barometer.h" #include "drivers/barometer.h"
#include "drivers/arduino_com.h"
#include "drivers/beeper.h" #include "drivers/beeper.h"
void systemTaskGyroPid(void) void systemTaskGyroPid(void)
@ -222,6 +223,12 @@ void systemTaskBattery(void)
ledOffInverted(Led0_PIN, Led0_GPIO_PORT); ledOffInverted(Led0_PIN, Led0_GPIO_PORT);
} }
void systemTaskArduino(void)
{
arduino_read();
arduino_send_sensor_values();
}
void systemTaskBaro(void) void systemTaskBaro(void)
{ {
barometer_CaclulateValues(); barometer_CaclulateValues();