rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-29 14:52:12 +01:00
Code Issues Releases Activity

UAVObject logging through telemetry module and uavobjectmanager

Browse Source
This commit is contained in:
Corvus Corax 2013-11-17 17:15:23 +01:00
parent d9a9536b61
commit 243e261411
3 changed files with 132 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

116
flight/modules/Telemetry/telemetry.c
View File

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

1
flight/uavobjects/inc/uavobjectmanager.h
View File

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

44
flight/uavobjects/uavobjectmanager.c
View File

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