// Clear all telem blocks waiting to be sent
int clearOutboundTelemetryQueue(void) {
int cleared = 0;
for (cleared = 0; cleared < OUTBOUND_QUEUE_SIZE; cleared++) {
if (outBuffer[cleared] != 0) {
destroyTelemetryBlock(outBuffer[cleared]);
outBuffer[cleared] = 0;
}
}
return cleared;
}
// Do buffer maintenance
void outboundBufferMaintenance(void) {
if ( stagingBuffer.sendIndex >= PACKET_LENGTH ) {
destroyTelemetryBlock(stagingBuffer.telemetry.asStruct);
if ( getOutboundQueueLength() ) {
stageTelemetryBlock(popOutboundTelemetryQueue());
}
} else if ( stagingBuffer.telemetry.asStruct == 0 && getOutboundQueueLength() ) {
stageTelemetryBlock(popOutboundTelemetryQueue());
}
}
int writeDatalink(float frequency){
if (time - lastTime > frequency) {
lastTime = time;
struct telem_block* statusData = getDebugTelemetryBlock();//createTelemetryBlock();
if (statusData == 0){
return 0;
}
statusData->lat = getLatitude();
statusData->lon = getLongitude();
statusData->millis = getUTCTime();
statusData->groundSpeed = getSpeed();
statusData->heading = getHeading();
statusData->lastCommandSent = lastCommandSentCode;
statusData->errorCodes = getErrorCodes();
statusData->gpsStatus = (char)(getSatellites() + (isGPSLocked() << 4));
statusData->steeringSetpoint = getPWM(1);
statusData->throttleSetpoint = getPWM(2);
statusData->steeringOutput = sData;
statusData->throttleOutput = tData;
statusData->debugChar = debugChar;
statusData->debugInt = debugInt;
statusData->debugFloat = debugFloat;
if (BLOCKING_MODE) {
sendTelemetryBlock(statusData);
destroyTelemetryBlock(statusData);
} else {
return pushOutboundTelemetryQueue(statusData);
}
}
else if (time < lastTime){
lastTime = time;
return 0;
}
return 0;
}
