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

temp

Browse Source
This commit is contained in:
Lennart Eriksson 2016-11-29 14:55:27 +01:00
parent ad58ecee85
commit d224a2f82d
3 changed files with 98 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -74,13 +74,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_ */

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

@ -481,7 +481,7 @@ void pidRun(uint8_t ID)
break;
case PID_ID_ACCELEROMETER:
if ((PidProfile[PID_ID_ACCELEROMETER].pidEnabled && flags_IsSet_ID(systemFlags_flightmode_acceleromter_id)))
if (!(PidProfile[PID_ID_ACCELEROMETER].pidEnabled && flags_IsSet_ID(systemFlags_flightmode_acceleromter_id)))
{
PidProfile[PID_ID_ACCELEROMETER].PID_Out[ROLL] = rc_input.Roll;
PidProfile[PID_ID_ACCELEROMETER].PID_Out[PITCH] = rc_input.Pitch;
@ -497,7 +497,7 @@ void pidRun(uint8_t ID)
break;
case PID_ID_COMPASS:
if (PidProfile[PID_ID_COMPASS].pidEnabled)
if (!PidProfile[PID_ID_COMPASS].pidEnabled)
{
PidProfile[PID_ID_COMPASS].PID_Out[0] = rc_input.Yaw;
}

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

@ -16,6 +16,23 @@
#define GPS_PACKET_SIZE 10
#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;
uint32_t time_since_heartbeat = 0;
typedef struct arduino_sensor_t {
uint8_t ID __attribute__((packed));
@ -30,20 +47,28 @@ 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_3 = 0xA1,
FSS_SENSOR_2 = 0x22,
FSS_SENSOR_BAROMETER = 0x1B,
FSS_TUNE_PITCH = 0x0D,
FSS_TUNE_ROLL = 0x34,
FSS_SENSOR_6 = 0x67,
//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,
BAROMETER_SENSOR_ID = 0,
TUNE_PITCH_ID,
TUNE_ROLL_ID,
//LED_STRIP_ID should only be on the flight controller side
LED_STRIP_ID,
//the number of sensors to send data from .
SENSOR_COUNT,
};
@ -102,12 +127,6 @@ 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[CURRENT_SENSOR_ID].ID = FSS_SENSOR_CURRENT;
sensors[CURRENT_SENSOR_ID].value = 0;
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;
@ -116,6 +135,9 @@ void arduinoCom_init(USART_TypeDef* usart_inst)
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;
}
@ -161,14 +183,6 @@ arduino_data_t find_packet_from_header(uint8_t header)
}
#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)
const uint32_t heartbeat_msg = 0xDEADBEEF;
const uint32_t heartbeat_rsp = 0xBA1DFACE;
/***********************************************************************
* BRIEF: A function that parses the message byte per byte and then
* if it was wrong it will do recursive call and parse the rest
@ -206,6 +220,7 @@ void arduino_parse_message(uint8_t data)
if(data == BYTE4_32BITS_VALUE(heartbeat_msg))
{
usart_transmit(&usartdmaHandler.usart_pro, (uint8_t *) &heartbeat_rsp, 4, 10000);
time_since_heartbeat = HAL_GetTick();
}
break;
default:
@ -314,12 +329,6 @@ void update_sensor_values()
{
/* TODO: Add the correct data to the value parameters here*/
sensors[CURRENT_SENSOR_ID].value = Yaw * (180 / 3.1415) + 180;
sensors[CURRENT_SENSOR_ID].crc = calculate_crc(&sensors[CURRENT_SENSOR_ID], ARDUINO_SENSOR_SIZE - 1);
sensors[VOLTAGE_SENSOR_ID].value = YawU * (180 / 3.1415) + 180;
sensors[VOLTAGE_SENSOR_ID].crc = calculate_crc(&sensors[VOLTAGE_SENSOR_ID], ARDUINO_SENSOR_SIZE - 1);
sensors[BAROMETER_SENSOR_ID].value = 0;
sensors[BAROMETER_SENSOR_ID].crc = calculate_crc(&sensors[BAROMETER_SENSOR_ID], ARDUINO_SENSOR_SIZE - 1);
@ -344,21 +353,44 @@ 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()
{
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);
}