diff --git a/UAV-ControlSystem/inc/drivers/arduino_com.h b/UAV-ControlSystem/inc/drivers/arduino_com.h index 64ce846..e274513 100644 --- a/UAV-ControlSystem/inc/drivers/arduino_com.h +++ b/UAV-ControlSystem/inc/drivers/arduino_com.h @@ -37,12 +37,24 @@ typedef struct gps_data_t { uint8_t crc; } gps_data_t; +/*********************************************************************** +* BRIEF: RX packet structure from arduino com * +* INFORMATION: Contains the whole ping sensor data message * +***********************************************************************/ +typedef struct ping_data_t { + uint8_t header; + uint16_t distance_mm; + uint8_t crc; +}ping_data_t; + /* An instance of the GPS data read from Arduino Com */ gps_data_t gps_data; /* An instance of the compass data read from Arduino Com */ compass_data_t compass_data; +/* An instance of the ping data read from Arduino Com */ +ping_data_t ping_data; /*********************************************************************** * BRIEF: Initializes the UART for Arduino com * @@ -62,6 +74,13 @@ 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(); + #endif /* DRIVERS_ARDUINO_COM_H_ */ diff --git a/UAV-ControlSystem/src/drivers/arduino_com.c b/UAV-ControlSystem/src/drivers/arduino_com.c index 594ff57..6860400 100644 --- a/UAV-ControlSystem/src/drivers/arduino_com.c +++ b/UAV-ControlSystem/src/drivers/arduino_com.c @@ -13,6 +13,41 @@ #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)); + 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 + FSS_SENSOR_CURRENT = 0xA1, + FSS_SENSOR_VOLTAGE = 0x22, + FSS_SENSOR_BAROMETER = 0x1B, + FSS_TUNE_PITCH = 0x0D, + FSS_TUNE_ROLL = 0x34, + FSS_SENSOR_6 = 0x67, +}; + +enum smartportID { + CURRENT_SENSOR_ID = 0, + VOLTAGE_SENSOR_ID, + BAROMETER_SENSOR_ID, + TUNE_PITCH_ID, + TUNE_ROLL_ID, + SENSOR_COUNT, +}; + + +arduino_sensor_t sensors[SENSOR_COUNT]; usart_dma_profile dmaHandler; @@ -22,6 +57,7 @@ dma_usart_return raw_dma_data_t; enum packet_ids { COMPASS_PACKET_ID = 0xA1, GPS_PACKET_ID = 0xB1, + PING_PACKET_ID = 0xC1, }; // Structure used to hold the data for "data_arr" @@ -34,6 +70,7 @@ typedef struct arduino_data_t { enum arduino_data_e { COMPASS_DATA_ID, GPS_DATA_ID, + PING_DATA_ID, ARDUINO_DATA_COUNT, }; @@ -47,6 +84,10 @@ arduino_data_t data_arr[ARDUINO_DATA_COUNT] = { .size = GPS_PACKET_SIZE, .dataPtr = &gps_data, }, + [PING_DATA_ID] = { + .size = PING_PACKET_SIZE, + .dataPtr = &ping_data, + }, }; @@ -58,7 +99,22 @@ 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_transmit(&dmaHandler.usart_pro, "data", 4, 100000); + + /*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; + + 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; } @@ -95,6 +151,9 @@ arduino_data_t find_packet_from_header(uint8_t header) 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; } @@ -198,3 +257,73 @@ void arduino_read() } } } + +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*/ + 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); + + sensors[BAROMETER_SENSOR_ID].value += 3; + sensors[BAROMETER_SENSOR_ID].crc = calculate_crc(&sensors[BAROMETER_SENSOR_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); +} + + +/*********************************************************************** +* BRIEF: Update the output sensor values and sends them to the Arduino * +* INFORMATION: * +***********************************************************************/ +void arduino_send_sensor_values() +{ + update_sensor_values(); + for (int i = 0; i < SENSOR_COUNT; i++) + { + usart_transmit(&dmaHandler.usart_pro, (uint8_t *) &sensors[i], 6, 10000); + } +} + + + + + + + + + + + + + + + + + + + + + + + diff --git a/UAV-ControlSystem/src/main.c b/UAV-ControlSystem/src/main.c index 4a1317b..510f9bd 100644 --- a/UAV-ControlSystem/src/main.c +++ b/UAV-ControlSystem/src/main.c @@ -66,8 +66,6 @@ void init_system() sbus_init(); - - #ifdef USE_LEDS //Initialize the on board leds ledReavoEnable();