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 }