diff --git a/flight/modules/Telemetry/telemetry.c b/flight/modules/Telemetry/telemetry.c index 2ddb3f5aa..2749121a2 100644 --- a/flight/modules/Telemetry/telemetry.c +++ b/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); + 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, - }; - EventPeriodicQueueCreate(&ev, queue, 0); - } + // Setup object for periodic updates + UAVObjEvent ev = { + .obj = obj, + .instId = UAVOBJ_ALL_INSTANCES, + .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)) { @@ -222,46 +220,78 @@ static void updateObject(UAVObjHandle obj, int32_t eventType) // Get metadata UAVObjGetMetadata(obj, &metadata); - updateMode = UAVObjGetTelemetryUpdateMode(&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; - UAVObjConnectQueue(obj, priorityQueue, eventMask); + eventMask |= EV_UPDATED_PERIODIC | EV_UPDATED_MANUAL | EV_UPDATE_REQ; break; case UPDATEMODE_ONCHANGE: // Set update period setUpdatePeriod(obj, 0); // Connect queue - eventMask = EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ; - UAVObjConnectQueue(obj, priorityQueue, eventMask); + eventMask |= EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ; 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; - UAVObjConnectQueue(obj, priorityQueue, eventMask); + eventMask |= EV_UPDATED_MANUAL | EV_UPDATE_REQ; 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 diff --git a/flight/uavobjects/inc/uavobjectmanager.h b/flight/uavobjects/inc/uavobjectmanager.h index a7af203fb..349d7c3dd 100644 --- a/flight/uavobjects/inc/uavobjectmanager.h +++ b/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 diff --git a/flight/uavobjects/uavobjectmanager.c b/flight/uavobjects/uavobjectmanager.c index 8b2120a4b..66117a1da 100644 --- a/flight/uavobjects/uavobjectmanager.c +++ b/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);