static void uavoRelayTask(void *parameters)
{
UAVObjEvent ev;
// Loop forever
while (1) {
PIOS_Thread_Sleep(50);
// Wait for queue message
if (PIOS_Queue_Receive(queue, &ev, 2) == true) {
// Process event. This calls transmitData
UAVTalkSendObject(uavTalkCon, ev.obj, ev.instId, false, 0);
}
// Process incoming data in sufficient chunks that we keep up
uint8_t serial_data[8];
uint16_t bytes_to_process;
bytes_to_process = PIOS_COM_ReceiveBuffer(uavorelay_com_id, serial_data, sizeof(serial_data), 0);
do {
bytes_to_process = PIOS_COM_ReceiveBuffer(uavorelay_com_id, serial_data, sizeof(serial_data), 0);
for (uint8_t i = 0; i < bytes_to_process; i++)
UAVTalkProcessInputStream(uavTalkCon,serial_data[i]);
} while (bytes_to_process > 0);
}
}
/**
* 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);
}
}
}
/**
* Processes queue events
*/
static void processObjEvent(UAVObjEvent * ev)
{
UAVObjMetadata metadata;
FlightTelemetryStatsData flightStats;
int32_t retries;
int32_t success;
if (ev->obj == 0) {
updateTelemetryStats();
} else if (ev->obj == GCSTelemetryStatsHandle()) {
gcsTelemetryStatsUpdated();
} else if (ev->obj == TelemetrySettingsHandle()) {
updateSettings();
} else {
// Only process event if connected to GCS or if object FlightTelemetryStats is updated
FlightTelemetryStatsGet(&flightStats);
if (flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_CONNECTED || ev->obj == FlightTelemetryStatsHandle()) {
// Get object metadata
UAVObjGetMetadata(ev->obj, &metadata);
// Act on event
retries = 0;
success = -1;
if (ev->event == EV_UPDATED || ev->event == EV_UPDATED_MANUAL) {
// Send update to GCS (with retries)
while (retries < MAX_RETRIES && success == -1) {
success = UAVTalkSendObject(ev->obj, ev->instId, metadata.telemetryAcked, 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(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)); // linked object will be the actual object the metadata are for
}
}
}
}
/**
* Telemetry transmit task, regular priority
*
* Logic: We need to double buffer the DMA transfers. Pack the buffer until either
* 1) it is full (and then we should record the number of missed events then)
* 2) the current transaction is done (we should immediately schedule since we are slave)
* when done packing the buffer we should call PIOS_SPI_TransferBlock, change the active buffer
* and then take the semaphrore
*/
static void overoSyncTask(void *parameters)
{
UAVObjEvent ev;
// Kick off SPI transfers (once one is completed another will automatically transmit)
overosync->transaction_done = true;
overosync->sent_objects = 0;
overosync->failed_objects = 0;
overosync->received_objects = 0;
uint32_t lastUpdateTime = PIOS_Thread_Systime();
uint32_t updateTime;
fid = fopen("sim_log.opl", "w");
// Loop forever
while (1) {
// Wait for queue message
if (PIOS_Queue_Receive(queue, &ev, PIOS_QUEUE_TIMEOUT_MAX) == true) {
// Check it will fit before packetizing
if ((overosync->write_pointer + UAVObjGetNumBytes(ev.obj) + 12) >=
sizeof(overosync->transactions[overosync->loading_transaction_id].tx_buffer)) {
overosync->failed_objects ++;
} else {
// Process event. This calls transmitData
UAVTalkSendObject(uavTalkCon, ev.obj, ev.instId, false, 0);
}
updateTime = PIOS_Thread_Systime();
if(((uint32_t) (updateTime - lastUpdateTime)) > 1000) {
// Update stats. This will trigger a local send event too
OveroSyncStatsData syncStats;
syncStats.Send = overosync->sent_bytes;
syncStats.Received = 0;
syncStats.Connected = syncStats.Send > 500 ? OVEROSYNCSTATS_CONNECTED_TRUE : OVEROSYNCSTATS_CONNECTED_FALSE;
syncStats.DroppedUpdates = overosync->failed_objects;
OveroSyncStatsSet(&syncStats);
overosync->failed_objects = 0;
overosync->sent_bytes = 0;
lastUpdateTime = updateTime;
}
}
}
}
