/**
* Update telemetry statistics and handle connection handshake
*/
static void updateTelemetryStats()
{
UAVTalkStats utalkStats;
FlightTelemetryStatsData flightStats;
GCSTelemetryStatsData gcsStats;
uint8_t forceUpdate;
uint8_t connectionTimeout;
uint32_t timeNow;
// Get stats
UAVTalkGetStats(&utalkStats);
UAVTalkResetStats();
// Get object data
FlightTelemetryStatsGet(&flightStats);
GCSTelemetryStatsGet(&gcsStats);
// Update stats object
if (flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) {
flightStats.RxDataRate = (float)utalkStats.rxBytes / ((float)STATS_UPDATE_PERIOD_MS / 1000.0);
flightStats.TxDataRate = (float)utalkStats.txBytes / ((float)STATS_UPDATE_PERIOD_MS / 1000.0);
flightStats.RxFailures += utalkStats.rxErrors;
flightStats.TxFailures += txErrors;
flightStats.TxRetries += txRetries;
txErrors = 0;
txRetries = 0;
} else {
flightStats.RxDataRate = 0;
flightStats.TxDataRate = 0;
flightStats.RxFailures = 0;
flightStats.TxFailures = 0;
flightStats.TxRetries = 0;
txErrors = 0;
txRetries = 0;
}
// Check for connection timeout
timeNow = xTaskGetTickCount() * portTICK_RATE_MS;
if (utalkStats.rxObjects > 0) {
timeOfLastObjectUpdate = timeNow;
}
if ((timeNow - timeOfLastObjectUpdate) > CONNECTION_TIMEOUT_MS) {
connectionTimeout = 1;
} else {
connectionTimeout = 0;
}
// Update connection state
forceUpdate = 1;
if (flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_DISCONNECTED) {
// Wait for connection request
if (gcsStats.Status == GCSTELEMETRYSTATS_STATUS_HANDSHAKEREQ) {
flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_HANDSHAKEACK;
}
} else if (flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_HANDSHAKEACK) {
// Wait for connection
if (gcsStats.Status == GCSTELEMETRYSTATS_STATUS_CONNECTED) {
flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_CONNECTED;
} else if (gcsStats.Status == GCSTELEMETRYSTATS_STATUS_DISCONNECTED) {
flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_DISCONNECTED;
}
} else if (flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) {
if (gcsStats.Status != GCSTELEMETRYSTATS_STATUS_CONNECTED || connectionTimeout) {
flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_DISCONNECTED;
} else {
forceUpdate = 0;
}
} else {
flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_DISCONNECTED;
}
// Update the telemetry alarm
if (flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) {
AlarmsClear(SYSTEMALARMS_ALARM_TELEMETRY);
} else {
AlarmsSet(SYSTEMALARMS_ALARM_TELEMETRY, SYSTEMALARMS_ALARM_ERROR);
}
// Update object
FlightTelemetryStatsSet(&flightStats);
// Force telemetry update if not connected
if (forceUpdate) {
FlightTelemetryStatsUpdated();
}
}
/**
* Update telemetry statistics and handle connection handshake
*/
static void updateTelemetryStats()
{
// flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_CONNECTED;
// gcsStats.Status = GCSTELEMETRYSTATS_STATUS_CONNECTED;
FlightTelemetryStatsData flightStats;
GCSTelemetryStatsData gcsStats;
uint8_t forceUpdate;
uint8_t connectionTimeout;
uint32_t timeNow;
// Get object data
FlightTelemetryStatsGet(&flightStats);
GCSTelemetryStatsGet(&gcsStats);
// Update stats object
if (flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) {
//flightStats.RxDataRate = (float)utalkStats.rxBytes / ((float)STATS_UPDATE_PERIOD_MS / 1000.0);
//flightStats.TxDataRate = (float)utalkStats.txBytes / ((float)STATS_UPDATE_PERIOD_MS / 1000.0);
//flightStats.RxFailures += utalkStats.rxErrors;
flightStats.TxFailures += txErrors;
flightStats.TxRetries += txRetries;
txErrors = 0;
txRetries = 0;
} else {
flightStats.RxDataRate = 0;
flightStats.TxDataRate = 0;
flightStats.RxFailures = 0;
flightStats.TxFailures = 0;
flightStats.TxRetries = 0;
txErrors = 0;
txRetries = 0;
}
// Check for connection timeout
timeNow = xTaskGetTickCount() * portTICK_RATE_MS;
if ((timeNow - lastOperatorHeartbeat) > CONNECTION_TIMEOUT_MS) {
connectionTimeout = 1;
} else {
connectionTimeout = 0;
}
// Update connection state
forceUpdate = 1;
// FIXME Hardcode value for now
// connectionTimeout = 0;
// If the GCS heartbeat has been received in the last five seconds
// set as connected
if (flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_DISCONNECTED) {
if (connectionTimeout == 0) {
// Switching from disconnected to connected
flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_CONNECTED;
gcsStats.Status = GCSTELEMETRYSTATS_STATUS_CONNECTED;
} else {
// Not switching, no update needed
forceUpdate = 0;
}
} else {
// We were connected, checking if we're still connected
if (connectionTimeout == 1) {
flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_DISCONNECTED;
gcsStats.Status = GCSTELEMETRYSTATS_STATUS_DISCONNECTED;
} else {
// We were connected and still are, no update needed
forceUpdate = 0;
}
}
// FIXME HARDCODED VALUES
flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_CONNECTED;
gcsStats.Status = GCSTELEMETRYSTATS_STATUS_CONNECTED;
forceUpdate = 1;
// if (flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_DISCONNECTED) {
// // Wait for connection request
// if (gcsStats.Status == GCSTELEMETRYSTATS_STATUS_HANDSHAKEREQ) {
// flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_HANDSHAKEACK;
// }
// } else if (flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_HANDSHAKEACK) {
// // Wait for connection
// if (gcsStats.Status == GCSTELEMETRYSTATS_STATUS_CONNECTED) {
// flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_CONNECTED;
// } else if (gcsStats.Status == GCSTELEMETRYSTATS_STATUS_DISCONNECTED) {
// flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_DISCONNECTED;
// }
// } else if (flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) {
// if (gcsStats.Status != GCSTELEMETRYSTATS_STATUS_CONNECTED || connectionTimeout) {
// flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_DISCONNECTED;
// } else {
// forceUpdate = 0;
// }
// } else {
// flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_DISCONNECTED;
// }
// Update the telemetry alarm
if (flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) {
AlarmsClear(SYSTEMALARMS_ALARM_TELEMETRY);
} else {
AlarmsSet(SYSTEMALARMS_ALARM_TELEMETRY, SYSTEMALARMS_ALARM_ERROR);
}
// Update object
FlightTelemetryStatsSet(&flightStats);
// Force telemetry update if not connected
if (forceUpdate) {
FlightTelemetryStatsUpdated();
}
}
