diff --git a/UAV-ControlSystem/.custom.cfg b/UAV-ControlSystem/.custom.cfg new file mode 100644 index 0000000..c77723e --- /dev/null +++ b/UAV-ControlSystem/.custom.cfg @@ -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 diff --git a/UAV-ControlSystem/inc/Flight/pid.h b/UAV-ControlSystem/inc/Flight/pid.h index fd6d62a..5eda143 100644 --- a/UAV-ControlSystem/inc/Flight/pid.h +++ b/UAV-ControlSystem/inc/Flight/pid.h @@ -70,6 +70,8 @@ extern accel_t accelProfile; extern float throttleRate; extern int HoverForce;/*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 b6f5797..c29afba 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 5570534..9cd0e7f 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/drivers/arduino_com.h b/UAV-ControlSystem/inc/drivers/arduino_com.h index e274513..18b6b82 100644 --- a/UAV-ControlSystem/inc/drivers/arduino_com.h +++ b/UAV-ControlSystem/inc/drivers/arduino_com.h @@ -19,11 +19,11 @@ * INFORMATION: Contains the whole compass message * ***********************************************************************/ typedef struct compass_data_t { - uint8_t header; - int16_t x; - int16_t y; - int16_t z; - uint8_t crc; + uint8_t header __attribute__((packed)); + int16_t x __attribute__((packed)); + int16_t y __attribute__((packed)); + int16_t z __attribute__((packed)); + uint8_t crc __attribute__((packed)); } compass_data_t; /*********************************************************************** @@ -31,10 +31,11 @@ typedef struct compass_data_t { * INFORMATION: Contains the whole gps data message * ***********************************************************************/ typedef struct gps_data_t { - uint8_t header; - float latitude; - float longitude; - uint8_t crc; + uint8_t header __attribute__((packed)); + float latitude __attribute__((packed)); + float longitude __attribute__((packed)); + uint8_t num_of_sats __attribute__((packed)); + uint8_t crc __attribute__((packed)); } gps_data_t; /*********************************************************************** @@ -42,9 +43,9 @@ typedef struct gps_data_t { * INFORMATION: Contains the whole ping sensor data message * ***********************************************************************/ typedef struct ping_data_t { - uint8_t header; - uint16_t distance_mm; - uint8_t crc; + uint8_t header __attribute__((packed)); + uint16_t distance_mm __attribute__((packed)); + uint8_t crc __attribute__((packed)); }ping_data_t; /* An instance of the GPS data read from Arduino Com */ @@ -74,13 +75,36 @@ bool arduino_frame_available(); ***********************************************************************/ void arduino_read(); - /*********************************************************************** * BRIEF: Update the output sensor values and sends them to the Arduino * * INFORMATION: * ***********************************************************************/ 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_ */ diff --git a/UAV-ControlSystem/inc/drivers/compass.h b/UAV-ControlSystem/inc/drivers/compass.h new file mode 100644 index 0000000..4201866 --- /dev/null +++ b/UAV-ControlSystem/inc/drivers/compass.h @@ -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 diff --git a/UAV-ControlSystem/inc/drivers/usart.h b/UAV-ControlSystem/inc/drivers/usart.h index 4417cdf..42bf1a1 100644 --- a/UAV-ControlSystem/inc/drivers/usart.h +++ b/UAV-ControlSystem/inc/drivers/usart.h @@ -46,6 +46,14 @@ typedef enum parity PARITY_ODD = 0x3 } 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 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_buffer2; // The second rx buffer used in double buffering 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; diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index 5ae7201..8d84696 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -20,10 +20,12 @@ #include "drivers/failsafe_toggles.h" #include "drivers/motormix.h" #include "utilities.h" +#include "drivers/arduino_com.h" #include "drivers/barometer.h" #include "drivers/system_clock.h" + #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 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 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; 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); } +/************************************************************************** +* 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; float VelocityCompensation = 0; @@ -115,14 +172,6 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile) /*Checks the biggest angle */ 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; case PID_ID_BAROMETER: @@ -139,6 +188,8 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile) oldHeightValue = current_height; sensorValues[0]*=BAROMETER_SCALE; + break; + case PID_ID_COMPASS: break; default: current_micros = clock_get_us(); @@ -239,7 +290,7 @@ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff, /*Limits the PTerm of the Yaw axis */ 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]; // 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 if (motorLimitReached) { - ITerm = constrainf(ITerm, -pidProfileBuff->ITermLimit[axis], pidProfileBuff->ITermLimit[axis]); + ITerm = constrainfu(ITerm, -pidProfileBuff->ITermLimit[axis], pidProfileBuff->ITermLimit[axis]); } 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 = 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->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 { 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] = 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], -15, (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; diff --git a/UAV-ControlSystem/src/Scheduler/scheduler.c b/UAV-ControlSystem/src/Scheduler/scheduler.c index beb4b26..7d5a5cb 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 63a62d2..e731d12 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 @@ -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} }, + [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 bb5e39b..2569742 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 6860400..c0f1c42 100644 --- a/UAV-ControlSystem/src/drivers/arduino_com.c +++ b/UAV-ControlSystem/src/drivers/arduino_com.c @@ -10,11 +10,34 @@ #include "utilities.h" #include "string.h" #include "stm32f4xx_revo.h" +#include "Flight/pid.h" + +#define COMPASS_PACKET_SIZE 8 +#define GPS_PACKET_SIZE 11 +#define PING_PACKET_SIZE 4 +#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; + + -#define COMPASS_PACKET_SIZE 8 -#define GPS_PACKET_SIZE 10 -#define PING_PACKET_SIZE 4 -#define ARDUINO_SENSOR_SIZE 6 typedef struct arduino_sensor_t { uint8_t ID __attribute__((packed)); @@ -29,20 +52,84 @@ enum smartport_packets_e { FSSP_DATA_FRAME = 0x10, // Sensor replies with this as start byte // ID of sensors. Must be something polled by FrSky RX - FSS_SENSOR_CURRENT = 0xA1, - FSS_SENSOR_VOLTAGE = 0x22, - FSS_SENSOR_BAROMETER = 0x1B, - FSS_TUNE_PITCH = 0x0D, - FSS_TUNE_ROLL = 0x34, - FSS_SENSOR_6 = 0x67, +#ifdef USE_STORED_WP + FSS_WP_LON = 0xA1, //Physical 2 + FSS_WP_LAT = 0x22, //Physical 3 +#endif + +#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 { - CURRENT_SENSOR_ID = 0, - VOLTAGE_SENSOR_ID, - BAROMETER_SENSOR_ID, - TUNE_PITCH_ID, - TUNE_ROLL_ID, +#ifdef USE_STORED_WP + WP_LON_ID, + WP_LAT_ID, +#endif +#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, }; @@ -50,8 +137,8 @@ enum smartportID { arduino_sensor_t sensors[SENSOR_COUNT]; -usart_dma_profile dmaHandler; -dma_usart_return raw_dma_data_t; +usart_dma_profile usartdmaHandler; +dma_usart_return raw_arduino_dma_data_t; // enumeration to hold the id:s of the different packages enum packet_ids { @@ -98,23 +185,40 @@ 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; - sensors[CURRENT_SENSOR_ID].value = 0; +#ifdef USE_STORED_WP + 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[VOLTAGE_SENSOR_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; + sensors[LED_STRIP_ID].ID = LED_STRIP_DATA; + sensors[LED_STRIP_ID].value = 0; } @@ -125,9 +229,8 @@ 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); - - return raw_dma_data_t.new_data; + raw_arduino_dma_data_t = usart_get_dma_buffer(&usartdmaHandler); + return raw_arduino_dma_data_t.new_data; } /*********************************************************************** @@ -175,6 +278,36 @@ void arduino_parse_message(uint8_t data) static uint8_t secondary_message_it = 0; static arduino_data_t msg_header_and_size = { .size = 0, .dataPtr = NULL }; 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) { @@ -247,13 +380,15 @@ void arduino_parse_message(uint8_t data) ***********************************************************************/ void arduino_read() { + raw_arduino_dma_data_t = usart_get_dma_buffer(&usartdmaHandler); + //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 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() { - /* 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; - sensors[VOLTAGE_SENSOR_ID].crc = calculate_crc(&sensors[VOLTAGE_SENSOR_ID], ARDUINO_SENSOR_SIZE - 1); +#ifdef USE_STORED_WP + 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[BAROMETER_SENSOR_ID].crc = calculate_crc(&sensors[BAROMETER_SENSOR_ID], ARDUINO_SENSOR_SIZE - 1); + sensors[WP_LAT_ID].value = 0; + 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[TUNE_PITCH_ID].crc = calculate_crc(&sensors[TUNE_PITCH_ID], ARDUINO_SENSOR_SIZE - 1); - - sensors[TUNE_ROLL_ID].value += 5; - sensors[TUNE_ROLL_ID].crc = calculate_crc(&sensors[TUNE_ROLL_ID], ARDUINO_SENSOR_SIZE - 1); + sensors[CURR_LAT_ID].value = gps_data.latitude; + sensors[CURR_LAT_ID].crc = calculate_crc(&sensors[CURR_LAT_ID], ARDUINO_SENSOR_SIZE - 1); +#endif +#ifdef USE_CURR_HEADING + 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() { + static int sensor_send_index = 0; + 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[sensor_send_index], 6, 10000); + sensor_send_index = (sensor_send_index + 1) % SENSOR_COUNT; +} + +/*********************************************************************** +* 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); } - - - - - - - - - - - - - - - - diff --git a/UAV-ControlSystem/src/drivers/compass.c b/UAV-ControlSystem/src/drivers/compass.c new file mode 100644 index 0000000..c8910c3 --- /dev/null +++ b/UAV-ControlSystem/src/drivers/compass.c @@ -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 (xMagnetFiltereddma_usart_rx_instance = dma_rx_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 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.new_data = (data.buff != prevBuf); - prevBuf = data.buff; + data.new_data = (data.buff != profile->dma_rx_prev_buffer); + profile->dma_rx_prev_buffer = data.buff; return data; } diff --git a/UAV-ControlSystem/src/main.c b/UAV-ControlSystem/src/main.c index 510f9bd..25ad347 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" #include "drivers/beeper.h" /************************************************************************** @@ -65,6 +66,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 d5ab7b5..4115c84 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" #include "drivers/beeper.h" void systemTaskGyroPid(void) @@ -222,6 +223,12 @@ void systemTaskBattery(void) ledOffInverted(Led0_PIN, Led0_GPIO_PORT); } +void systemTaskArduino(void) +{ + arduino_read(); + arduino_send_sensor_values(); +} + void systemTaskBaro(void) { barometer_CaclulateValues();