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

Ping sensor implementation and moved compass code

Browse Source
This commit is contained in:
Lennart Eriksson 2016-12-02 17:23:32 +01:00
parent 26982bf938
commit c9b23acaa2
7 changed files with 245 additions and 123 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -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;
/***********************************************************************

3
UAV-ControlSystem/inc/drivers/compass.h
View File

@ -7,5 +7,8 @@ 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;
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;

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

@ -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<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);
}
break;
break;
default:
current_micros = clock_get_us();
current_micros = current_micros/1000000;

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

@ -12,28 +12,33 @@
#include "stm32f4xx_revo.h"
#include "Flight/pid.h"
#define COMPASS_PACKET_SIZE 8
#define GPS_PACKET_SIZE 10
#define PING_PACKET_SIZE 4
#define ARDUINO_SENSOR_SIZE 6
#define TIME_BEFORE_DEATH_MS 500
#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 BYTE1_32BITS_VALUE(x) ((x & 0xFF000000) >> 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;
}
/***********************************************************************

72
UAV-ControlSystem/src/drivers/compass.c
View File

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

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

@ -43,8 +43,6 @@
//BRR
#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
@ -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
profile_out->dma_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;
}