diff --git a/UAV-ControlSystem/inc/drivers/arduino_com.h b/UAV-ControlSystem/inc/drivers/arduino_com.h index cbeabd1..18b6b82 100644 --- a/UAV-ControlSystem/inc/drivers/arduino_com.h +++ b/UAV-ControlSystem/inc/drivers/arduino_com.h @@ -31,10 +31,11 @@ typedef struct compass_data_t { * INFORMATION: Contains the whole gps data message * ***********************************************************************/ typedef struct gps_data_t { - uint8_t header __attribute__((packed)); - float latitude; __attribute__((packed)) - float longitude __attribute__((packed)); - uint8_t crc __attribute__((packed)); + 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; /*********************************************************************** diff --git a/UAV-ControlSystem/inc/drivers/compass.h b/UAV-ControlSystem/inc/drivers/compass.h index 69b1141..4201866 100644 --- a/UAV-ControlSystem/inc/drivers/compass.h +++ b/UAV-ControlSystem/inc/drivers/compass.h @@ -7,5 +7,8 @@ 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 5da5b03..e15b1cd 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -24,8 +24,6 @@ #include "drivers/barometer.h" #include "drivers/system_clock.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*/ @@ -75,13 +73,6 @@ 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 * @@ -199,63 +190,7 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile) break; case PID_ID_COMPASS: - { - 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> 24) -#define BYTE2_32BITS_VALUE(x) ((x & 0x00FF0000) >> 16) -#define BYTE3_32BITS_VALUE(x) ((x & 0x0000FF00) >> 8) -#define BYTE4_32BITS_VALUE(x) ((x & 0x000000FF) >> 0) #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) -const uint32_t heartbeat_msg = 0xDEADBEEF; -const uint32_t heartbeat_rsp = 0xBA1DFACE; + +#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 { uint8_t ID __attribute__((packed)); uint32_t value __attribute__((packed)); @@ -47,12 +52,51 @@ 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_3 = 0xA1, - FSS_SENSOR_2 = 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 @@ -61,9 +105,26 @@ enum smartport_packets_e { }; enum smartportID { - BAROMETER_SENSOR_ID = 0, - 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, @@ -77,7 +138,7 @@ arduino_sensor_t sensors[SENSOR_COUNT]; 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 enum packet_ids { @@ -127,14 +188,34 @@ void arduinoCom_init(USART_TypeDef* usart_inst) 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[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; +#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[LED_STRIP_ID].ID = LED_STRIP_DATA; sensors[LED_STRIP_ID].value = 0; @@ -148,8 +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(&usartdmaHandler); - 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; } /*********************************************************************** @@ -199,7 +280,7 @@ void arduino_parse_message(uint8_t data) static uint8_t crc = 0; static uint8_t heartbeatiterator = 0; - if (heartbeatiterator == 0 && data == BYTE1_32BITS_VALUE(heartbeat_msg)) + if (heartbeatiterator == 0 && data == heartbeat_msg[0]) heartbeatiterator = 1; if (heartbeatiterator > 0) @@ -210,14 +291,14 @@ void arduino_parse_message(uint8_t data) heartbeatiterator = 2; break; case 2: - heartbeatiterator = (data == BYTE2_32BITS_VALUE(heartbeat_msg)) ? 3 : 0; + heartbeatiterator = (data == heartbeat_msg[1]) ? 3 : 0; break; case 3: - heartbeatiterator = (data == BYTE3_32BITS_VALUE(heartbeat_msg)) ? 4 : 0; + heartbeatiterator = (data == heartbeat_msg[2]) ? 4 : 0; break; case 4: heartbeatiterator = 0; - if(data == BYTE4_32BITS_VALUE(heartbeat_msg)) + if(data == heartbeat_msg[3]) { usart_transmit(&usartdmaHandler.usart_pro, (uint8_t *) &heartbeat_rsp, 4, 10000); time_since_heartbeat = HAL_GetTick(); @@ -299,15 +380,15 @@ void arduino_parse_message(uint8_t data) ***********************************************************************/ void arduino_read() { - raw_dma_data_t = usart_get_dma_buffer(&usartdmaHandler); + 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]); } } } @@ -327,16 +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[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 = 0; - sensors[TUNE_PITCH_ID].crc = calculate_crc(&sensors[TUNE_PITCH_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[TUNE_ROLL_ID].value = 0; - sensors[TUNE_ROLL_ID].crc = calculate_crc(&sensors[TUNE_ROLL_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 = 0; + sensors[CURR_LON_ID].crc = calculate_crc(&sensors[CURR_LON_ID], ARDUINO_SENSOR_SIZE - 1); + + sensors[CURR_LAT_ID].value = 0; + 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 = 0; + sensors[ALTITUDE_ID].crc = calculate_crc(&sensors[ALTITUDE_ID], ARDUINO_SENSOR_SIZE - 1); +#endif } @@ -346,11 +449,11 @@ 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(&usartdmaHandler.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; } /*********************************************************************** diff --git a/UAV-ControlSystem/src/drivers/compass.c b/UAV-ControlSystem/src/drivers/compass.c index 79f196c..c8910c3 100644 --- a/UAV-ControlSystem/src/drivers/compass.c +++ b/UAV-ControlSystem/src/drivers/compass.c @@ -3,6 +3,19 @@ #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() { @@ -12,3 +25,62 @@ 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; }