/**
* Register a new object, adds object to local list and connects the queue depending on the object's
* telemetry settings.
* \param[in] obj Object to connect
*/
static void registerObject(UAVObjHandle obj)
{
if (UAVObjIsMetaobject(obj)) {
/* Only connect change notifications for meta objects. No periodic updates */
UAVObjConnectQueue(obj, priorityQueue, EV_MASK_ALL_UPDATES);
return;
} else {
UAVObjMetadata metadata;
UAVObjUpdateMode updateMode;
UAVObjGetMetadata(obj, &metadata);
updateMode = UAVObjGetTelemetryUpdateMode(&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 telemetry updates
updateObject(obj, EV_NONE);
}
/**
* Processes queue events
*/
static void processObjEvent(UAVObjEvent * ev)
{
UAVObjMetadata metadata;
UAVObjUpdateMode updateMode;
FlightTelemetryStatsData flightStats;
int32_t retries;
int32_t success;
if (ev->obj == 0) {
updateTelemetryStats();
} else if (ev->obj == GCSTelemetryStatsHandle()) {
gcsTelemetryStatsUpdated();
} else {
// Only process event if connected to GCS or if object FlightTelemetryStats is updated
FlightTelemetryStatsGet(&flightStats);
// Get object metadata
UAVObjGetMetadata(ev->obj, &metadata);
updateMode = UAVObjGetTelemetryUpdateMode(&metadata);
if (flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_CONNECTED || ev->obj == FlightTelemetryStatsHandle()) {
// 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))) {
// 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
++retries;
}
// Update stats
txRetries += (retries - 1);
if (success == -1) {
++txErrors;
}
} else if (ev->event == EV_UPDATE_REQ) {
// Request object update from GCS (with retries)
while (retries < MAX_RETRIES && success == -1) {
success = UAVTalkSendObjectRequest(uavTalkCon, ev->obj, ev->instId, REQ_TIMEOUT_MS); // call blocks until update is received or timeout
++retries;
}
// Update stats
txRetries += (retries - 1);
if (success == -1) {
++txErrors;
}
}
// If this is a metaobject then make necessary telemetry updates
if (UAVObjIsMetaobject(ev->obj)) {
updateObject(UAVObjGetLinkedObj(ev->obj), EV_NONE); // linked object will be the actual object the metadata are for
}
}
if((updateMode == UPDATEMODE_THROTTLED) && !UAVObjIsMetaobject(ev->obj)) {
// If this is UPDATEMODE_THROTTLED, the event mask changes on every event.
updateObject(ev->obj, ev->event);
}
}
}
/**
* Update object's queue connections and timer, depending on object's settings
* \param[in] obj Object to updates
*/
static void updateObject(UAVObjHandle obj, int32_t eventType)
{
UAVObjMetadata metadata;
UAVObjUpdateMode updateMode;
int32_t eventMask;
// Get metadata
UAVObjGetMetadata(obj, &metadata);
updateMode = UAVObjGetTelemetryUpdateMode(&metadata);
// Setup object depending on update mode
if (updateMode == UPDATEMODE_PERIODIC) {
// Set update period
setUpdatePeriod(obj, metadata.telemetryUpdatePeriod);
// Connect queue
eventMask = EV_UPDATED_PERIODIC | EV_UPDATED_MANUAL | EV_UPDATE_REQ;
if (UAVObjIsMetaobject(obj)) {
eventMask |= EV_UNPACKED; // we also need to act on remote updates (unpack events)
}
UAVObjConnectQueue(obj, priorityQueue, eventMask);
} else if (updateMode == UPDATEMODE_ONCHANGE) {
// Set update period
setUpdatePeriod(obj, 0);
// Connect queue
eventMask = EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ;
if (UAVObjIsMetaobject(obj)) {
eventMask |= EV_UNPACKED; // we also need to act on remote updates (unpack events)
}
UAVObjConnectQueue(obj, priorityQueue, eventMask);
} else if (updateMode == 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;
// 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;
}
if (UAVObjIsMetaobject(obj)) {
eventMask |= EV_UNPACKED; // we also need to act on remote updates (unpack events)
}
UAVObjConnectQueue(obj, priorityQueue, eventMask);
} else if (updateMode == UPDATEMODE_MANUAL) {
// Set update period
setUpdatePeriod(obj, 0);
// Connect queue
eventMask = EV_UPDATED_MANUAL | EV_UPDATE_REQ;
if (UAVObjIsMetaobject(obj)) {
eventMask |= EV_UNPACKED; // we also need to act on remote updates (unpack events)
}
UAVObjConnectQueue(obj, priorityQueue, eventMask);
}
}
