Added responding to OBJ_REQ and sending ACK/NACK on PipX.

flight/Modules/RadioComBridge/RadioComBridge.c

@@ -35,6 +35,7 @@
 #include <packet_handler.h>
 #include <gcsreceiver.h>
 #include <pipxstatus.h>
+#include <objectpersistence.h>
 #include <pipxsettings.h>
 #include <uavtalk_priv.h>
 #include <pios_rfm22b.h>
@@ -42,7 +43,7 @@
 
 #include <stdbool.h>
 
-//#undef PIOS_INCLUDE_USB
+#undef PIOS_INCLUDE_USB
 
 // ****************
 // Private constants
@@ -57,8 +58,10 @@
 #define RADIOSTATS_UPDATE_PERIOD_MS 500
 #define MAX_LOST_CONTACT_TIME 10
 #define PACKET_QUEUE_SIZE 5
-#define EV_PACKET_RECEIVED 0x10
 #define MAX_PORT_DELAY 200
+#define EV_PACKET_RECEIVED 0x10
+#define EV_SEND_ACK 0x20
+#define EV_SEND_NACK 0x40
 
 // ****************
 // Private types
@@ -104,15 +107,6 @@ typedef struct {
 	portTickType send_timeout;
 	uint16_t min_packet_size;
 
-	// Flag used to indicate an update of the UAVObjects.
-	bool send_gcsreceiver;
-	bool send_pipxstatus;
-
-	// Tracks the UAVTalk messages transmitted from radio to com.
-	bool uavtalk_idle;
-	int16_t uavtalk_packet_len;
-	int16_t uavtalk_packet_index;
-
 	// Track other radios that are in range.
 	PairStats pairStats[PIPXSTATUS_PAIRIDS_NUMELEM];
 
@@ -146,6 +140,7 @@ static void PPMHandler(uint16_t *channels);
 static void updateSettings();
 static BufferedReadHandle BufferedReadInit(uint32_t com_port, uint16_t buffer_length);
 static bool BufferedRead(BufferedReadHandle h, uint8_t *value, uint32_t timeout_ms);
+static void BufferedReadSetCom(BufferedReadHandle h, uint32_t com_port);
 
 // ****************
 // Private variables
@@ -194,8 +189,6 @@ static int32_t RadioComBridgeInitialize(void)
 	// Initialize the UAVObjects that we use
 	GCSReceiverInitialize();
 	PipXStatusInitialize();
-	data->send_gcsreceiver = false;
-	data->send_pipxstatus = false;
 
 	// TODO: Get from settings object
 	data->com_port = PIOS_COM_BRIDGE_COM;
@@ -230,11 +223,6 @@ static int32_t RadioComBridgeInitialize(void)
 	data->send_timeout = 25; // ms
 	data->min_packet_size = 50;
 
-	// Initialize the rado->com UAVTalk message tracker.
-	data->uavtalk_idle = true;
-	data->uavtalk_packet_len = 0;
-	data->uavtalk_packet_index = -1; // Looking for SYNC
-
 	// Initialize the detected device statistics.
 	for (uint8_t i = 0; i < PIPXSTATUS_PAIRIDS_NUMELEM; ++i)
 	{
@@ -270,6 +258,14 @@ static void comUAVTalkTask(void *parameters)
 		PIOS_WDG_UpdateFlag(PIOS_WDG_COMUAVTALK);
 #endif /* PIOS_INCLUDE_WDG */
 
+#if defined(PIOS_INCLUDE_USB)
+		// Determine input port (USB takes priority over telemetry port)
+		if (PIOS_USB_CheckAvailable(0) && PIOS_COM_TELEM_USB)
+			BufferedReadSetCom(f, PIOS_COM_TELEM_USB);
+		else
+#endif /* PIOS_INCLUDE_USB */
+			BufferedReadSetCom(f, data->com_port);
+
 		// Read the next byte
 		uint8_t rx_byte;
 		if(!BufferedRead(f, &rx_byte, MAX_PORT_DELAY))
@@ -309,11 +305,48 @@ static void comUAVTalkTask(void *parameters)
 
 		// Keep reading until we receive a completed packet.
 		UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(data->inUAVTalkCon, rx_byte);
+		UAVTalkInputProcessor *iproc = &(((UAVTalkConnectionData*)(data->inUAVTalkCon))->iproc);
 		if (state == UAVTALK_STATE_COMPLETE) {
 
+			// Is this a local UAVObject?
+			if (iproc->obj != NULL)
+			{
+				// We treat the ObjectPersistance object differently
+				if(iproc->objId == OBJECTPERSISTENCE_OBJID)
+				{
 					// Queue the packet for transmission.
-			xQueueSend(data->sendPacketQueue, &p, portMAX_DELAY);
+					xQueueSend(data->sendPacketQueue, &p, MAX_PORT_DELAY);
 					p = NULL;
+				}
+				else
+				{
+					UAVObjEvent ev;
+					ev.obj = iproc->obj;
+					switch (iproc->type)
+					{
+					case UAVTALK_TYPE_OBJ:
+						// Unpack object, if the instance does not exist it will be created!
+						UAVObjUnpack(iproc->obj, iproc->instId, p->data);
+						break;
+					case UAVTALK_TYPE_OBJ_REQ:
+						// Queue up an object send request.
+						ev.event = EV_UPDATE_REQ;
+						xQueueSend(data->objEventQueue, &ev, MAX_PORT_DELAY);
+						break;
+					case UAVTALK_TYPE_OBJ_ACK:
+						// Queue up an ACK
+						ev.event = EV_SEND_ACK;
+						xQueueSend(data->objEventQueue, &ev, MAX_PORT_DELAY);
+						break;
+					}
+				}
+			}
+			else
+			{
+				// Queue the packet for transmission.
+				xQueueSend(data->sendPacketQueue, &p, MAX_PORT_DELAY);
+				p = NULL;
+			}
 
 		} else if(state == UAVTALK_STATE_ERROR) {
 			DEBUG_PRINTF(1, "UAVTalk FAILED!\n\r");
@@ -321,6 +354,16 @@ static void comUAVTalkTask(void *parameters)
 			// Release the packet and start over again.
 			PHReleaseTXPacket(pios_packet_handler, p);
 			p = NULL;
+
+			// Send a NACK if required.
+			if((iproc->obj) && (iproc->type == UAVTALK_TYPE_ACK))
+			{
+				UAVObjEvent ev;
+				ev.obj = iproc->obj;
+				// Queue up a NACK
+				ev.event = EV_SEND_NACK;
+				xQueueSend(data->objEventQueue, &ev, MAX_PORT_DELAY);
+			}
 		}
 	}
 }
@@ -347,6 +390,8 @@ static void radioReceiveTask(void *parameters)
 
 		// Receive data from the radio port
 		rx_bytes = PIOS_COM_ReceiveBuffer(data->radio_port, (uint8_t*)p, PIOS_PH_MAX_PACKET, MAX_PORT_DELAY);
+		if(rx_bytes == 0)
+			continue;
 
 		// Verify that the packet is valid and pass it on.
 		if(PHVerifyPacket(pios_packet_handler, p, rx_bytes) > 0) {
@@ -395,7 +440,7 @@ static void sendDataTask(void *parameters)
 #endif /* PIOS_INCLUDE_WDG */
 		// Wait for a packet on the queue.
 		if (xQueueReceive(data->objEventQueue, &ev, MAX_PORT_DELAY) == pdTRUE) {
-			if (ev.event == EV_UPDATED)
+			if ((ev.event == EV_UPDATED) || (ev.event == EV_UPDATE_REQ))
 			{
 				// Send update (with retries)
 				uint32_t retries = 0;
@@ -405,6 +450,16 @@ static void sendDataTask(void *parameters)
 					++retries;
 				}
 			}
+			else if(ev.event == EV_SEND_ACK)
+			{
+				// Send the ACK
+				UAVTalkSendAck(data->outUAVTalkCon, ev.obj, ev.instId);
+			}
+			else if(ev.event == EV_SEND_NACK)
+			{
+				// Send the ACK
+				UAVTalkSendNack(data->outUAVTalkCon, UAVObjGetID(ev.obj));
+			}
 			else if(ev.event == EV_PACKET_RECEIVED)
 			{
 				// Receive the packet.
@@ -543,7 +598,7 @@ static void radioStatusTask(void *parameters)
 				status_packet.source_id = pipxStatus.DeviceID;
 				status_packet.rssi = pipxStatus.RSSI;
 				PHPacketHandle sph = (PHPacketHandle)&status_packet;
-				xQueueSend(data->sendPacketQueue, &sph, portMAX_DELAY);
+				xQueueSend(data->sendPacketQueue, &sph, MAX_PORT_DELAY);
 				cntr = 0;
 			}
 		}
@@ -672,7 +727,6 @@ static void PPMHandler(uint16_t *channels)
 		UAVObjSetMetadata(GCSReceiverHandle(), &metadata);
 	}
 	GCSReceiverSet(&rcvr);
-	data->send_gcsreceiver = true;
 }
 
 static BufferedReadHandle BufferedReadInit(uint32_t com_port, uint16_t buffer_length)
@@ -710,6 +764,11 @@ static bool BufferedRead(BufferedReadHandle h, uint8_t *value, uint32_t timeout_
 	return true;
 }
 
+static void BufferedReadSetCom(BufferedReadHandle h, uint32_t com_port)
+{
+	h->com_port = com_port;
+}
+
 static void updateSettings()
 {
 	if (data->com_port) {