telemetry: throttled obj can only be in queue once

This means that different objects using the link get a fair share when
there's congestion.

Author: mlyle

flight/Modules/System/systemmod.c

@@ -950,6 +950,7 @@ static int32_t eventPeriodicCreate(UAVObjEvent* ev, UAVObjEventCallback cb, stru
 	objEntry->evInfo.ev.obj = ev->obj;
 	objEntry->evInfo.ev.instId = ev->instId;
 	objEntry->evInfo.ev.event = ev->event;
+	objEntry->evInfo.ev.throttle = NULL;
 	objEntry->evInfo.cb = cb;
 	objEntry->evInfo.queue = queue;
 	objEntry->updatePeriodMs = periodMs;

flight/Modules/Telemetry/telemetry.c

@@ -599,6 +599,8 @@ static void processObjEvent(telem_t telem, UAVObjEvent * ev)
 					EV_NONE);
 		}
 	}
+
+	UAVObjUnblockThrottle(ev->throttle);
 }
 
 static bool sendRequestedObjs(telem_t telem)

flight/UAVObjects/inc/uavobjectmanager.h

@@ -116,10 +116,11 @@ typedef enum {
  */
 typedef struct {
 	UAVObjHandle obj;
-	uint16_t instId;
 	UAVObjEventType event;
-} UAVObjEvent;
+	struct ObjectEventEntryThrottled *throttle;
 
+	uint16_t instId;
+} UAVObjEvent;
 
 /**
  * Event callback, this function is called when an event is invoked. The function
@@ -205,6 +206,7 @@ int32_t UAVObjDisconnectQueue(UAVObjHandle obj_handle, struct pios_queue *queue)
 int32_t UAVObjConnectQueueThrottled(UAVObjHandle obj_handle, struct pios_queue *queue, uint8_t eventMask, uint16_t interval);
 int32_t UAVObjConnectCallback(UAVObjHandle obj_handle, UAVObjEventCallback cb, void *cbCtx, uint8_t eventMask);
 int32_t UAVObjConnectCallbackThrottled(UAVObjHandle obj_handle, UAVObjEventCallback cb, void *cbCtx, uint8_t eventMask, uint16_t interval);
+void UAVObjUnblockThrottle(struct ObjectEventEntryThrottled *throttled);
 int32_t UAVObjDisconnectCallback(UAVObjHandle obj_handle, UAVObjEventCallback cb, void *cbCtx);
 void UAVObjUpdated(UAVObjHandle obj);
 void UAVObjInstanceUpdated(UAVObjHandle obj_handle, uint16_t instId);

flight/UAVObjects/uavobjectmanager.c

@@ -57,7 +57,7 @@ extern uintptr_t pios_uavo_settings_fs_id;
  */
 
 /** opaque type for instances **/
-typedef void* InstanceHandle; 
+typedef void* InstanceHandle;
 
 struct ObjectEventEntry {
 	union {
@@ -76,6 +76,7 @@ struct ObjectEventEntryThrottled {
 
 	uint32_t                  due;
 	uint16_t                  interval;
+	volatile uint8_t          inhibited;
 };
 
 /*
@@ -130,7 +131,7 @@ struct UAVOSingle {
 	struct UAVOData   uavo;
 
 	uint8_t           instance0[];
-	/* 
+	/*
 	 * Additional space will be malloc'd here to hold the
 	 * the data for this instance.
 	 */
@@ -140,7 +141,7 @@ struct UAVOSingle {
 struct UAVOMultiInst {
 	struct UAVOMultiInst * next;
 	uint8_t                instance[];
-	/* 
+	/*
 	 * Additional space will be malloc'd here to hold the
 	 * the data for this instance.
 	 */
@@ -345,7 +346,7 @@ static struct UAVOData * UAVObjAllocMulti(uint32_t num_bytes)
  * \return Object handle, or NULL if failure.
  * \return
  */
-UAVObjHandle UAVObjRegister(uint32_t id, 
+UAVObjHandle UAVObjRegister(uint32_t id,
 			int32_t isSingleInstance, int32_t isSettings,
 			uint32_t num_bytes,
 			UAVObjInitializeCallback initCb)
@@ -1293,7 +1294,7 @@ int32_t UAVObjGetInstanceDataField(UAVObjHandle obj_handle, uint16_t instId, voi
 		if ((size + offset) > obj->instance_size) {
 			goto unlock_exit;
 		}
-		
+
 		// Set data
 		memcpy(dataOut, InstanceData(instEntry) + offset, size);
 	}
@@ -1485,8 +1486,8 @@ void UAVObjSetGcsTelemetryUpdateMode(UAVObjMetadata* metadata, UAVObjUpdateMode
 /**
  * Check if an object is read only
  * \param[in] obj The object handle
- * \return 
- *   \arg 0 if not read only 
+ * \return
+ *   \arg 0 if not read only
  *   \arg 1 if read only
  *   \arg -1 if unable to get meta data
  */
@@ -1724,31 +1725,48 @@ static int32_t pumpOneEvent(UAVObjEvent msg, void *obj_data, int len) {
 	struct ObjectEventEntry *event;
 	LL_FOREACH(msg.obj->next_event, event) {
 		if (event->eventMask == 0
-			|| (event->eventMask & msg.event) != 0) {
+				|| (event->eventMask & msg.event) != 0) {
+			struct ObjectEventEntryThrottled *throtInfo =
+				(struct ObjectEventEntryThrottled *) event;
+
 			if (event->hasThrottle) {
 				// This is a throttled event (triggered with a spacing of at least "interval" ms)
 				struct ObjectEventEntryThrottled *throtInfo =
 					(struct ObjectEventEntryThrottled *) event;
 
 				uint32_t now = PIOS_Thread_Systime();
-				if (throtInfo->due > now){
+				if (throtInfo->due > now) {
 					continue;
 				}
 
 				// Set time for next callback
 				throtInfo->due += ((now - throtInfo->due) / throtInfo->interval + 1) * throtInfo->interval;
+
+				if (throtInfo->inhibited) {
+					continue;
+				}
+
+				msg.throttle = throtInfo;
+			} else {
+				msg.throttle = NULL;
 			}
 
 			// Invoke callback (from event task) if a valid one is registered
 			if (event->cb) {
 				// invoke callback directly; callbacks must be well behaved
 				invokeCallback(event, &msg, obj_data, len);
 			} else if (event->cbInfo.queue) {
+				if (event->hasThrottle) {
+					throtInfo->inhibited = 1;
+				}
 				// Send to queue if a valid queue is registered
 				// will not block
 				if (PIOS_Queue_Send(event->cbInfo.queue, &msg, 0) != true) {
 					stats.lastQueueErrorID = UAVObjGetID(msg.obj);
 					++stats.eventQueueErrors;
+					if (event->hasThrottle) {
+						throtInfo->inhibited = 0;
+					}
 				}
 			}
 
@@ -2154,6 +2172,17 @@ uint32_t UAVObjIDByIndex(uint8_t index)
 	return 0;
 }
 
+/**
+ * Unblocks a throttled event-- allows it to be inserted into queues once
+ * again.
+ */
+void UAVObjUnblockThrottle(struct ObjectEventEntryThrottled *throttled)
+{
+	if (throttled) {
+		throttled->inhibited = 0;
+	}
+}
+
 /**
  * Registers a new UAVO instance created callback
  */