void sendTelemetry(void)
{
if (millis() - lastCycleTime >= CYCLETIME) {
lastCycleTime = millis();
cycleNum++;
// Frame 1: sent every 200ms
sendAccel();
sendBaro();
sendTemperature1();
sendTelemetryTail();
if ((cycleNum % 5) == 0) { // Frame 2: Sent every 1s
if (sensors(SENSOR_GPS)) {
sendGPS();
sendTelemetryTail();
}
}
if (cycleNum == 25) { //Frame 3: Sent every 5s
cycleNum = 0;
sendTime();
sendTelemetryTail();
}
}
}
void handleFrSkyTelemetry(void)
{
if (!canSendFrSkyTelemetry()) {
return;
}
uint32_t now = millis();
if (!hasEnoughTimeLapsedSinceLastTelemetryTransmission(now)) {
return;
}
lastCycleTime = now;
cycleNum++;
// Sent every 125ms
sendAccel();
sendVario();
sendTelemetryTail();
if ((cycleNum % 4) == 0) { // Sent every 500ms
sendBaro();
sendHeading();
sendTelemetryTail();
}
if ((cycleNum % 8) == 0) { // Sent every 1s
sendTemperature1();
if (feature(FEATURE_VBAT)) {
sendVoltage();
sendVoltageAmp();
sendAmperage();
sendFuelLevel();
}
#ifdef GPS
if (sensors(SENSOR_GPS))
sendGPS();
#endif
sendTelemetryTail();
}
if (cycleNum == 40) { //Frame 3: Sent every 5s
cycleNum = 0;
sendTime();
sendTelemetryTail();
}
}
void sendFRSKYTelemetry(void)
{
static uint32_t lastCycleTime = 0;
static uint8_t cycleNum = 0;
if (currentTimeMS - lastCycleTime >= CYCLETIME)
{
lastCycleTime = currentTimeMS;
cycleNum++;
// Sent every 125ms
sendAccel();
sendTelemetryTail();
if ((cycleNum % 4) == 0) // Sent every 500ms
{
sendBaro();
sendHeading();
sendTelemetryTail();
}
if ((cycleNum % 8) == 0) // Sent every 1s
{
sendTemperature1();
if (feature(FEATURE_VBAT))
{
sendVoltage();
sendVoltageAmp();
}
if (sensors(SENSOR_GPS))
sendGPS();
sendTelemetryTail();
}
if (cycleNum == 40) //Frame 3: Sent every 5s
{
cycleNum = 0;
sendTime();
sendTelemetryTail();
}
}
}
void sendTelemetry(void)
{
if (mcfg.telemetry_softserial == TELEMETRY_UART && !f.ARMED)
return;
if (serialTotalBytesWaiting(core.telemport) != 0)
return;
if (millis() - lastCycleTime >= CYCLETIME) {
lastCycleTime = millis();
cycleNum++;
// Sent every 125ms
sendAccel();
sendTelemetryTail();
if ((cycleNum % 4) == 0) { // Sent every 500ms
sendBaro();
sendHeading();
sendTelemetryTail();
}
if ((cycleNum % 8) == 0) { // Sent every 1s
sendTemperature1();
if (feature(FEATURE_VBAT)) {
sendVoltage();
sendVoltageAmp();
}
if (sensors(SENSOR_GPS))
sendGPS();
sendTelemetryTail();
}
if (cycleNum == 40) { //Frame 3: Sent every 5s
cycleNum = 0;
sendTime();
sendTelemetryTail();
}
}
}
void handleFrSkyTelemetry(void)
{
if (!canSendFrSkyTelemetry()) {
return;
}
uint32_t now = millis();
if (!hasEnoughTimeLapsedSinceLastTelemetryTransmission(now)) {
return;
}
lastCycleTime = now;
cycleNum++;
// Sent every 125ms
sendAccel();
sendVario();
sendTelemetryTail();
if ((cycleNum % 4) == 0) { // Sent every 500ms
if (lastCycleTime > DELAY_FOR_BARO_INITIALISATION) { //Allow 5s to boot correctly
sendBaro();
}
sendHeading();
sendTelemetryTail();
}
if ((cycleNum % 8) == 0) { // Sent every 1s
sendTemperature1();
sendThrottleOrBatterySizeAsRpm();
if (feature(FEATURE_VBAT)) {
sendVoltage();
sendVoltageAmp();
sendAmperage();
sendFuelLevel();
}
#ifdef GPS
if (sensors(SENSOR_GPS)) {
sendSpeed();
sendGpsAltitude();
sendSatalliteSignalQualityAsTemperature2();
}
#endif
// Send GPS information to display compass information
if (sensors(SENSOR_GPS) || (telemetryConfig->gpsNoFixLatitude != 0 && telemetryConfig->gpsNoFixLongitude != 0)) {
sendGPS();
}
sendTelemetryTail();
}
if (cycleNum == 40) { //Frame 3: Sent every 5s
cycleNum = 0;
sendTime();
sendTelemetryTail();
}
}
void Telemetry::main(const State& state, Config& config)
{
// Re-initialize variables
cmd_ = 0;
checksum_ = 0;
// Check whether we have a request available
if ((n_bytes_available_ = Serial_Telemetry.available()) > 0)
{
// Read all data to tmp buffer. It is possible that not all the data has arrived yet
Serial_Telemetry.readBytes(&(rx_data_buffer_[ptr_]), n_bytes_available_);
ptr_ += n_bytes_available_;
}
else // No more data to receive
{
if(ptr_ > 0) // We have data to process
{
// Check for the magic word
if (rx_data_buffer_[0] == magic_word_[0] && rx_data_buffer_[1] == magic_word_[1])
{
cmd_ = rx_data_buffer_[2];
}
if (cmd_ == TELEMETRY_CMD_IN_CONFIG)
{
Telemetry::receiveConfig(config);
}
else
{
// Send Magic Word
write8((uint8_t)magic_word_[0]);
write8((uint8_t)magic_word_[1]);
// Send cmd
write8(cmd_);
// Perform desired operation
switch (cmd_)
{
case TELEMETRY_CMD_OUT_STATUS:
sendStatus(state);
break;
case TELEMETRY_CMD_OUT_IMU:
sendIMU(state);
break;
case TELEMETRY_CMD_OUT_MAG:
sendMagnetometer(state);
break;
case TELEMETRY_CMD_OUT_BARO:
sendBarometer(state);
break;
case TELEMETRY_CMD_OUT_TEMP:
sendTemperature(state);
break;
case TELEMETRY_CMD_OUT_RC:
sendRC(state);
break;
case TELEMETRY_CMD_OUT_GPS:
sendGPS(state);
break;
case TELEMETRY_CMD_OUT_SONAR:
sendSonar(state);
break;
case TELEMETRY_CMD_OUT_ATTITUDE:
sendAttitude(state);
break;
case TELEMETRY_CMD_OUT_CONTROL:
sendControl(state);
break;
case TELEMETRY_CMD_OUT_CONFIG:
sendConfig(config);
break;
} // switch cmd
sendCheckSum();
} // config_in
ptr_ = 0; // Prepare for next package
} // ptr_ > 0
} // n_bytes > 0
}
