UAVObject logging through telemetry module and uavobjectmanager

flight/modules/Telemetry/telemetry.c

@@ -88,6 +88,7 @@ static int32_t transmitData(uint8_t *data, int32_t length);
 static void registerObject(UAVObjHandle obj);
 static void updateObject(UAVObjHandle obj, int32_t eventType);
 static int32_t setUpdatePeriod(UAVObjHandle obj, int32_t updatePeriodMs);
+static int32_t setLoggingPeriod(UAVObjHandle obj, int32_t updatePeriodMs);
 static void processObjEvent(UAVObjEvent *ev);
 static void updateTelemetryStats();
 static void gcsTelemetryStatsUpdated();
@@ -181,21 +182,18 @@ static void registerObject(UAVObjHandle obj)
         return;
     } else {
         UAVObjMetadata metadata;
-        UAVObjUpdateMode updateMode;
+        UAVObjUpdateMode updateMode, loggingMode;
         UAVObjGetMetadata(obj, &metadata);
         updateMode  = UAVObjGetTelemetryUpdateMode(&metadata);
+        loggingMode = UAVObjGetLoggingUpdateMode(&metadata);
 
-        /* Only create a periodic event for objects that are periodic */
-        if ((updateMode == UPDATEMODE_PERIODIC) ||
-            (updateMode == UPDATEMODE_THROTTLED)) {
         // Setup object for periodic updates
         UAVObjEvent ev = {
             .obj    = obj,
             .instId = UAVOBJ_ALL_INSTANCES,
-                .event  = EV_UPDATED_PERIODIC,
+            .event  = (updateMode == UPDATEMODE_PERIODIC || updateMode == UPDATEMODE_THROTTLED) ? EV_UPDATED_PERIODIC : 0 | (loggingMode == UPDATEMODE_PERIODIC || loggingMode == UPDATEMODE_THROTTLED) ? EV_LOGGING_PERIODIC : 0,
         };
         EventPeriodicQueueCreate(&ev, queue, 0);
-        }
 
         // Setup object for telemetry updates
         updateObject(obj, EV_NONE);
@@ -209,7 +207,7 @@ static void registerObject(UAVObjHandle obj)
 static void updateObject(UAVObjHandle obj, int32_t eventType)
 {
     UAVObjMetadata metadata;
-    UAVObjUpdateMode updateMode;
+    UAVObjUpdateMode updateMode, loggingMode;
     int32_t eventMask;
 
     if (UAVObjIsMetaobject(obj)) {
@@ -223,45 +221,77 @@ static void updateObject(UAVObjHandle obj, int32_t eventType)
     // Get metadata
     UAVObjGetMetadata(obj, &metadata);
     updateMode  = UAVObjGetTelemetryUpdateMode(&metadata);
+    loggingMode = UAVObjGetLoggingUpdateMode(&metadata);
 
     // Setup object depending on update mode
+    eventMask   = 0;
     switch (updateMode) {
     case UPDATEMODE_PERIODIC:
         // Set update period
         setUpdatePeriod(obj, metadata.telemetryUpdatePeriod);
         // Connect queue
-        eventMask = EV_UPDATED_PERIODIC | EV_UPDATED_MANUAL | EV_UPDATE_REQ;
+        eventMask |= EV_UPDATED_PERIODIC | EV_UPDATED_MANUAL | EV_UPDATE_REQ;
-        UAVObjConnectQueue(obj, priorityQueue, eventMask);
         break;
     case UPDATEMODE_ONCHANGE:
         // Set update period
         setUpdatePeriod(obj, 0);
         // Connect queue
-        eventMask = EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ;
+        eventMask |= EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ;
-        UAVObjConnectQueue(obj, priorityQueue, eventMask);
         break;
     case UPDATEMODE_THROTTLED:
         if ((eventType == EV_UPDATED_PERIODIC) || (eventType == EV_NONE)) {
             // If we received a periodic update, we can change back to update on change
-            eventMask = EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ;
+            eventMask |= EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ;
             // Set update period on initialization and metadata change
             if (eventType == EV_NONE) {
                 setUpdatePeriod(obj, metadata.telemetryUpdatePeriod);
             }
         } else {
             // Otherwise, we just received an object update, so switch to periodic for the timeout period to prevent more updates
-            eventMask = EV_UPDATED_PERIODIC | EV_UPDATED_MANUAL | EV_UPDATE_REQ;
+            eventMask |= EV_UPDATED_PERIODIC | EV_UPDATED_MANUAL | EV_UPDATE_REQ;
         }
-        UAVObjConnectQueue(obj, priorityQueue, eventMask);
         break;
     case UPDATEMODE_MANUAL:
         // Set update period
         setUpdatePeriod(obj, 0);
         // Connect queue
-        eventMask = EV_UPDATED_MANUAL | EV_UPDATE_REQ;
+        eventMask |= EV_UPDATED_MANUAL | EV_UPDATE_REQ;
-        UAVObjConnectQueue(obj, priorityQueue, eventMask);
         break;
     }
+    switch (loggingMode) {
+    case UPDATEMODE_PERIODIC:
+        // Set update period
+        setLoggingPeriod(obj, metadata.loggingUpdatePeriod);
+        // Connect queue
+        eventMask |= EV_LOGGING_PERIODIC | EV_LOGGING_MANUAL;
+        break;
+    case UPDATEMODE_ONCHANGE:
+        // Set update period
+        setLoggingPeriod(obj, 0);
+        // Connect queue
+        eventMask |= EV_UPDATED | EV_LOGGING_MANUAL;
+        break;
+    case UPDATEMODE_THROTTLED:
+        if ((eventType == EV_LOGGING_PERIODIC) || (eventType == EV_NONE)) {
+            // If we received a periodic update, we can change back to update on change
+            eventMask |= EV_UPDATED | EV_LOGGING_MANUAL;
+            // Set update period on initialization and metadata change
+            if (eventType == EV_NONE) {
+                setLoggingPeriod(obj, metadata.loggingUpdatePeriod);
+            }
+        } else {
+            // Otherwise, we just received an object update, so switch to periodic for the timeout period to prevent more updates
+            eventMask |= EV_LOGGING_PERIODIC | EV_LOGGING_MANUAL;
+        }
+        break;
+    case UPDATEMODE_MANUAL:
+        // Set update period
+        setLoggingPeriod(obj, 0);
+        // Connect queue
+        eventMask |= EV_LOGGING_MANUAL;
+        break;
+    }
+    UAVObjConnectQueue(obj, priorityQueue, eventMask);
 }
 
 /**
@@ -288,7 +318,7 @@ static void processObjEvent(UAVObjEvent *ev)
         // Act on event
         retries    = 0;
         success    = -1;
-        if (ev->event == EV_UPDATED || ev->event == EV_UPDATED_MANUAL || ((ev->event == EV_UPDATED_PERIODIC) && (updateMode != UPDATEMODE_THROTTLED))) {
+        if ((ev->event == EV_UPDATED && (updateMode == UPDATEMODE_ONCHANGE || updateMode == UPDATEMODE_THROTTLED)) || ev->event == EV_UPDATED_MANUAL || ((ev->event == EV_UPDATED_PERIODIC) && (updateMode != UPDATEMODE_THROTTLED))) {
             // Send update to GCS (with retries)
             while (retries < MAX_RETRIES && success == -1) {
                 success = UAVTalkSendObject(uavTalkCon, ev->obj, ev->instId, UAVObjGetTelemetryAcked(&metadata), REQ_TIMEOUT_MS); // call blocks until ack is received or timeout
@@ -321,6 +351,24 @@ static void processObjEvent(UAVObjEvent *ev)
             }
         }
     }
+    // Log UAVObject if necessary
+    if (ev->obj) {
+        updateMode = UAVObjGetLoggingUpdateMode(&metadata);
+        if ((ev->event == EV_UPDATED && (updateMode == UPDATEMODE_ONCHANGE || updateMode == UPDATEMODE_THROTTLED)) || ev->event == EV_LOGGING_MANUAL || ((ev->event == EV_LOGGING_PERIODIC) && (updateMode != UPDATEMODE_THROTTLED))) {
+            if (ev->instId == UAVOBJ_ALL_INSTANCES) {
+                success = UAVObjGetNumInstances(ev->obj);
+                for (retries = 0; retries < success; retries++) {
+                    UAVObjInstanceWriteToLog(ev->obj, retries);
+                }
+            } else {
+                UAVObjInstanceWriteToLog(ev->obj, ev->instId);
+            }
+        }
+        if (updateMode == UPDATEMODE_THROTTLED) {
+            // If this is UPDATEMODE_THROTTLED, the event mask changes on every event.
+            updateObject(ev->obj, ev->event);
+        }
+    }
 }
 
 /**
@@ -447,6 +495,24 @@ static int32_t setUpdatePeriod(UAVObjHandle obj, int32_t updatePeriodMs)
     return EventPeriodicQueueUpdate(&ev, queue, updatePeriodMs);
 }
 
+/**
+ * Set logging update period of object (it must be already setup for periodic updates)
+ * \param[in] obj The object to update
+ * \param[in] updatePeriodMs The update period in ms, if zero then periodic updates are disabled
+ * \return 0 Success
+ * \return -1 Failure
+ */
+static int32_t setLoggingPeriod(UAVObjHandle obj, int32_t updatePeriodMs)
+{
+    UAVObjEvent ev;
+
+    // Add object for periodic updates
+    ev.obj    = obj;
+    ev.instId = UAVOBJ_ALL_INSTANCES;
+    ev.event  = EV_LOGGING_PERIODIC;
+    return EventPeriodicQueueUpdate(&ev, queue, updatePeriodMs);
+}
+
 /**
  * Called each time the GCS telemetry stats object is updated.
  * Trigger a flight telemetry stats update if a connection is not

flight/uavobjects/inc/uavobjectmanager.h

@@ -208,6 +208,7 @@ void UAVObjInstanceUpdated(UAVObjHandle obj_handle, uint16_t instId);
 void UAVObjLogging(UAVObjHandle obj);
 void UAVObjInstanceLogging(UAVObjHandle obj_handle, uint16_t instId);
 void UAVObjIterate(void (*iterator)(UAVObjHandle obj));
+void UAVObjInstanceWriteToLog(UAVObjHandle obj_handle, uint16_t instId);
 
 #endif // UAVOBJECTMANAGER_H
 

flight/uavobjects/uavobjectmanager.c

@@ -705,6 +705,43 @@ unlock_exit:
 }
 
 
+/**
+ * Actually write the object's data to the logfile
+ * \param[in] obj The object handle
+ * \param[in] instId The object instance ID
+ */
+void UAVObjInstanceWriteToLog(UAVObjHandle obj_handle, uint16_t instId)
+{
+    PIOS_Assert(obj_handle);
+
+    // Lock
+    xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
+
+    if (UAVObjIsMetaobject(obj_handle)) {
+        if (instId != 0) {
+            goto unlock_exit;
+        }
+        PIOS_DEBUGLOG_UAVObject(UAVObjGetID(obj_handle), instId, MetaNumBytes, (uint8_t *)MetaDataPtr((struct UAVOMeta *)obj_handle));
+    } else {
+        struct UAVOData *obj;
+        InstanceHandle instEntry;
+
+        // Cast handle to object
+        obj = (struct UAVOData *)obj_handle;
+
+        // Get the instance
+        instEntry = getInstance(obj, instId);
+        if (instEntry == NULL) {
+            goto unlock_exit;
+        }
+        // Pack data
+        PIOS_DEBUGLOG_UAVObject(UAVObjGetID(obj_handle), instId, obj->instance_size, (uint8_t *)InstanceData(instEntry));
+    }
+
+unlock_exit:
+    xSemaphoreGiveRecursive(mutex);
+}
+
 /**
  * Save the data of the specified object to the file system (SD card).
  * If the object contains multiple instances, all of them will be saved.
@@ -712,10 +749,9 @@ unlock_exit:
  * The object data can be restored using the UAVObjLoad function.
  * @param[in] obj The object handle.
  * @param[in] instId The instance ID
- * @param[in] file File to append to
  * @return 0 if success or -1 if failure
  */
-int32_t UAVObjSave(UAVObjHandle obj_handle, __attribute__((unused)) uint16_t instId)
+int32_t UAVObjSave(UAVObjHandle obj_handle, uint16_t instId)
 {
     PIOS_Assert(obj_handle);
 
@@ -754,7 +790,7 @@ int32_t UAVObjSave(UAVObjHandle obj_handle, __attribute__((unused)) uint16_t ins
  * @param[in] instId The object instance
  * @return 0 if success or -1 if failure
  */
-int32_t UAVObjLoad(UAVObjHandle obj_handle, __attribute__((unused)) uint16_t instId)
+int32_t UAVObjLoad(UAVObjHandle obj_handle, uint16_t instId)
 {
     PIOS_Assert(obj_handle);
 
@@ -794,7 +830,7 @@ int32_t UAVObjLoad(UAVObjHandle obj_handle, __attribute__((unused)) uint16_t ins
  * @param[in] instId The object instance
  * @return 0 if success or -1 if failure
  */
-int32_t UAVObjDelete(UAVObjHandle obj_handle, __attribute__((unused)) uint16_t instId)
+int32_t UAVObjDelete(UAVObjHandle obj_handle, uint16_t instId)
 {
     PIOS_Assert(obj_handle);
     PIOS_FLASHFS_ObjDelete(pios_uavo_settings_fs_id, UAVObjGetID(obj_handle), instId);