temp

UAV-ControlSystem/inc/drivers/arduino_com.h

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

UAV-ControlSystem/src/Flight/pid.c

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

UAV-ControlSystem/src/drivers/arduino_com.c

@@ -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_3           = 0xA1,
-	FSS_SENSOR_VOLTAGE     = 0x22,
+	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,
+	BAROMETER_SENSOR_ID = 0,
-	VOLTAGE_SENSOR_ID,
-	BAROMETER_SENSOR_ID,
 	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);
+}