504 lines
15 KiB
C
504 lines
15 KiB
C
/*
|
|
* arduino_com.c
|
|
*
|
|
* Created on: 26 okt. 2016
|
|
* Author: Philip
|
|
*/
|
|
|
|
#include "drivers/arduino_com.h"
|
|
|
|
#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;
|
|
|
|
|
|
|
|
|
|
typedef struct arduino_sensor_t {
|
|
uint8_t ID __attribute__((packed));
|
|
uint32_t value __attribute__((packed));
|
|
uint8_t crc __attribute__((packed));
|
|
} arduino_sensor_t;
|
|
|
|
|
|
/* Some definitions of packets and sensor IDs */
|
|
enum smartport_packets_e {
|
|
FSSP_START_STOP = 0x7E, // Start/Stop bit sent from RX when polling
|
|
FSSP_DATA_FRAME = 0x10, // Sensor replies with this as start byte
|
|
|
|
// ID of sensors. Must be something polled by FrSky RX
|
|
#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 {
|
|
#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,
|
|
};
|
|
|
|
|
|
arduino_sensor_t sensors[SENSOR_COUNT];
|
|
|
|
|
|
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 {
|
|
COMPASS_PACKET_ID = 0xA1,
|
|
GPS_PACKET_ID = 0xB1,
|
|
PING_PACKET_ID = 0xC1,
|
|
};
|
|
|
|
// Structure used to hold the data for "data_arr"
|
|
typedef struct arduino_data_t {
|
|
uint8_t size; //Size of the data
|
|
void * dataPtr; //pointer to the data
|
|
} arduino_data_t ;
|
|
|
|
// An enumeration of the array positions of the data in the "data_arr"
|
|
enum arduino_data_e {
|
|
COMPASS_DATA_ID,
|
|
GPS_DATA_ID,
|
|
PING_DATA_ID,
|
|
ARDUINO_DATA_COUNT,
|
|
};
|
|
|
|
// An array to hold the pointers to the different data structures that are used in the rest of the system;
|
|
arduino_data_t data_arr[ARDUINO_DATA_COUNT] = {
|
|
[COMPASS_DATA_ID] = {
|
|
.size = COMPASS_PACKET_SIZE,
|
|
.dataPtr = &compass_data,
|
|
},
|
|
[GPS_DATA_ID] = {
|
|
.size = GPS_PACKET_SIZE,
|
|
.dataPtr = &gps_data,
|
|
},
|
|
[PING_DATA_ID] = {
|
|
.size = PING_PACKET_SIZE,
|
|
.dataPtr = &ping_data,
|
|
},
|
|
};
|
|
|
|
|
|
/***********************************************************************
|
|
* BRIEF: Initializes the UART for Arduino com *
|
|
* INFORMATION: A DMA Buffer starts storing the bytes received from RX *
|
|
***********************************************************************/
|
|
void arduinoCom_init(USART_TypeDef* usart_inst)
|
|
{
|
|
/* initialize the USART with a dma buffer */
|
|
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*/
|
|
#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;
|
|
}
|
|
|
|
|
|
/***********************************************************************
|
|
* BRIEF: Checks if new RX packet is available *
|
|
* INFORMATION: Is called by the scheduler *
|
|
***********************************************************************/
|
|
bool arduino_frame_available()
|
|
{
|
|
/* We read data from DMA */
|
|
raw_arduino_dma_data_t = usart_get_dma_buffer(&usartdmaHandler);
|
|
return raw_arduino_dma_data_t.new_data;
|
|
}
|
|
|
|
/***********************************************************************
|
|
* BRIEF: Private function that looks for the correct data struct from *
|
|
* the header data *
|
|
* INFORMATION: *
|
|
***********************************************************************/
|
|
arduino_data_t find_packet_from_header(uint8_t header)
|
|
{
|
|
arduino_data_t arduino_data = {
|
|
.dataPtr = NULL,
|
|
.size = 0,
|
|
};
|
|
|
|
/* Check what header it is and return the correct datapointer if correct header*/
|
|
switch (header)
|
|
{
|
|
case COMPASS_PACKET_ID:
|
|
arduino_data = data_arr[COMPASS_DATA_ID];
|
|
break;
|
|
case GPS_PACKET_ID:
|
|
arduino_data = data_arr[GPS_DATA_ID];
|
|
break;
|
|
case PING_PACKET_ID:
|
|
arduino_data = data_arr[PING_DATA_ID];
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
return arduino_data;
|
|
}
|
|
|
|
|
|
/***********************************************************************
|
|
* 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
|
|
* of the message
|
|
* INFORMATION: *
|
|
***********************************************************************/
|
|
void arduino_parse_message(uint8_t data)
|
|
{
|
|
static uint8_t arduino_arr[ARDUINO_DMA_SIZE];
|
|
static bool find_header = true;
|
|
static uint8_t message_it = 0;
|
|
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)
|
|
{
|
|
msg_header_and_size = find_packet_from_header(data);
|
|
|
|
if(msg_header_and_size.size != 0)
|
|
{
|
|
find_header = false;
|
|
arduino_arr[(message_it)] = data;
|
|
message_it++;
|
|
crc ^= data;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
/* If we find any new possible header then we should be able to return to that point in time */
|
|
if (((arduino_data_t)find_packet_from_header(data)).size != 0 && secondary_message_it == 0)
|
|
{
|
|
secondary_message_it = message_it; //save the value of the position in The buffer array, not the dma array index
|
|
}
|
|
|
|
/* Reading the message except the crc byte */
|
|
if ((message_it) < (msg_header_and_size.size - 1))
|
|
{
|
|
arduino_arr[(message_it)] = data;
|
|
crc ^= data;
|
|
message_it++;
|
|
}
|
|
else if ((message_it) == (msg_header_and_size.size - 1))
|
|
{
|
|
/* put the crc code into the data buffer as well */
|
|
arduino_arr[(message_it)] = data;
|
|
|
|
/* TODO: Replace with check for CRC */
|
|
if (crc == data)
|
|
{
|
|
/* Clear necessary variables in order to fill the buffer with new ones */
|
|
message_it = 0;
|
|
find_header = true;
|
|
secondary_message_it = 0;
|
|
crc = 0;
|
|
|
|
memcpy(msg_header_and_size.dataPtr, arduino_arr, msg_header_and_size.size);
|
|
}
|
|
else
|
|
{
|
|
int size = msg_header_and_size.size;
|
|
int new_iter = secondary_message_it;
|
|
|
|
crc = 0;
|
|
find_header = true;
|
|
message_it = 0;
|
|
secondary_message_it = 0;
|
|
|
|
if(new_iter > 0)
|
|
{
|
|
for(int i = new_iter; i < size; i++)
|
|
{
|
|
arduino_parse_message(arduino_arr[i]);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
/***********************************************************************
|
|
* BRIEF: Updates "gps_data" and "compass_data" *
|
|
* INFORMATION: Is called by the scheduler *
|
|
***********************************************************************/
|
|
void arduino_read()
|
|
{
|
|
raw_arduino_dma_data_t = usart_get_dma_buffer(&usartdmaHandler);
|
|
|
|
//If the DMA has come to a new buffer
|
|
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_arduino_dma_data_t.buff[i]);
|
|
}
|
|
}
|
|
}
|
|
|
|
uint8_t calculate_crc(uint8_t *data, uint8_t length)
|
|
{
|
|
uint8_t crc = 0;
|
|
for(int i = 0; i < length; i++)
|
|
crc ^= data[i];
|
|
|
|
return crc;
|
|
}
|
|
|
|
/***********************************************************************
|
|
* BRIEF: Update the output sensor values and calculate the crc *
|
|
* INFORMATION: *
|
|
***********************************************************************/
|
|
void update_sensor_values()
|
|
{
|
|
/* TODO: Add the correct data to the value parameters here*/
|
|
|
|
#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[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[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
|
|
}
|
|
|
|
|
|
/***********************************************************************
|
|
* BRIEF: Update the output sensor values and sends them to the Arduino *
|
|
* INFORMATION: *
|
|
***********************************************************************/
|
|
void arduino_send_sensor_values()
|
|
{
|
|
static int sensor_send_index = 0;
|
|
|
|
update_sensor_values();
|
|
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);
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|