Merge remote-tracking branch 'refs/remotes/origin/master' into baro2

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_ */
 
 

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);
+	}
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

UAV-ControlSystem/src/main.c

@@ -66,8 +66,6 @@ void init_system()
 	sbus_init();
 
 
-
-
 #ifdef USE_LEDS
 	//Initialize the on board leds
 	ledReavoEnable();