void handleSmartPortTelemetry(void) { uint32_t smartPortLastServiceTime = millis(); if (!smartPortTelemetryEnabled) { return; } if (!canSendSmartPortTelemetry()) { return; } while (serialRxBytesWaiting(smartPortSerialPort) > 0) { uint8_t c = serialRead(smartPortSerialPort); smartPortDataReceive(c); } uint32_t now = millis(); // if timed out, reconfigure the UART back to normal so the GUI or CLI works if ((now - smartPortLastRequestTime) > SMARTPORT_NOT_CONNECTED_TIMEOUT_MS) { smartPortState = SPSTATE_TIMEDOUT; return; } while (smartPortHasRequest) { // Ensure we won't get stuck in the loop if there happens to be nothing available to send in a timely manner - dump the slot if we loop in there for too long. if ((millis() - smartPortLastServiceTime) > SMARTPORT_SERVICE_TIMEOUT_MS) { smartPortHasRequest = 0; return; } // we can send back any data we want, our table keeps track of the order and frequency of each data type we send uint16_t id = frSkyDataIdTable[smartPortIdCnt]; if (id == 0) { // end of table reached, loop back smartPortIdCnt = 0; id = frSkyDataIdTable[smartPortIdCnt]; } smartPortIdCnt++; int32_t tmpi; static uint8_t t1Cnt = 0; switch(id) { #ifdef GPS case FSSP_DATAID_SPEED : if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) { uint32_t tmpui = (GPS_speed * 36 + 36 / 2) / 100; smartPortSendPackage(id, tmpui); // given in 0.1 m/s, provide in KM/H smartPortHasRequest = 0; } break; #endif case FSSP_DATAID_VFAS : if (feature(FEATURE_VBAT)) { uint16_t vfasVoltage; if (telemetryConfig->frsky_vfas_cell_voltage) { vfasVoltage = vbat / batteryCellCount; } else { vfasVoltage = vbat; } smartPortSendPackage(id, vfasVoltage * 10); // given in 0.1V, convert to volts smartPortHasRequest = 0; } break; case FSSP_DATAID_CURRENT : if (feature(FEATURE_CURRENT_METER)) { smartPortSendPackage(id, amperage / 10); // given in 10mA steps, unknown requested unit smartPortHasRequest = 0; } break; //case FSSP_DATAID_RPM : case FSSP_DATAID_ALTITUDE : if (sensors(SENSOR_BARO)) { smartPortSendPackage(id, BaroAlt); // unknown given unit, requested 100 = 1 meter smartPortHasRequest = 0; } break; case FSSP_DATAID_FUEL : if (feature(FEATURE_CURRENT_METER)) { smartPortSendPackage(id, mAhDrawn); // given in mAh, unknown requested unit smartPortHasRequest = 0; } break; //case FSSP_DATAID_ADC1 : //case FSSP_DATAID_ADC2 : #ifdef GPS case FSSP_DATAID_LATLONG : if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) { uint32_t tmpui = 0; // the same ID is sent twice, one for longitude, one for latitude // the MSB of the sent uint32_t helps FrSky keep track // the even/odd bit of our counter helps us keep track if (smartPortIdCnt & 1) { tmpui = abs(GPS_coord[LON]); // now we have unsigned value and one bit to spare tmpui = (tmpui + tmpui / 2) / 25 | 0x80000000; // 6/100 = 1.5/25, division by power of 2 is fast if (GPS_coord[LON] < 0) tmpui |= 0x40000000; } else { tmpui = abs(GPS_coord[LAT]); // now we have unsigned value and one bit to spare tmpui = (tmpui + tmpui / 2) / 25; // 6/100 = 1.5/25, division by power of 2 is fast if (GPS_coord[LAT] < 0) tmpui |= 0x40000000; } smartPortSendPackage(id, tmpui); smartPortHasRequest = 0; } break; #endif //case FSSP_DATAID_CAP_USED : case FSSP_DATAID_VARIO : if (sensors(SENSOR_BARO)) { smartPortSendPackage(id, vario); // unknown given unit but requested in 100 = 1m/s smartPortHasRequest = 0; } break; case FSSP_DATAID_HEADING : smartPortSendPackage(id, attitude.values.yaw * 10); // given in 10*deg, requested in 10000 = 100 deg smartPortHasRequest = 0; break; case FSSP_DATAID_ACCX : smartPortSendPackage(id, accSmooth[X] / 44); // unknown input and unknown output unit // we can only show 00.00 format, another digit won't display right on Taranis // dividing by roughly 44 will give acceleration in G units smartPortHasRequest = 0; break; case FSSP_DATAID_ACCY : smartPortSendPackage(id, accSmooth[Y] / 44); smartPortHasRequest = 0; break; case FSSP_DATAID_ACCZ : smartPortSendPackage(id, accSmooth[Z] / 44); smartPortHasRequest = 0; break; case FSSP_DATAID_T1 : // we send all the flags as decimal digits for easy reading // the t1Cnt simply allows the telemetry view to show at least some changes t1Cnt++; if (t1Cnt >= 4) { t1Cnt = 1; } tmpi = t1Cnt * 10000; // start off with at least one digit so the most significant 0 won't be cut off // the Taranis seems to be able to fit 5 digits on the screen // the Taranis seems to consider this number a signed 16 bit integer if (ARMING_FLAG(OK_TO_ARM)) tmpi += 1; if (ARMING_FLAG(PREVENT_ARMING)) tmpi += 2; if (ARMING_FLAG(ARMED)) tmpi += 4; if (FLIGHT_MODE(ANGLE_MODE)) tmpi += 10; if (FLIGHT_MODE(HORIZON_MODE)) tmpi += 20; if (FLIGHT_MODE(UNUSED_MODE)) tmpi += 40; if (FLIGHT_MODE(PASSTHRU_MODE)) tmpi += 40; if (FLIGHT_MODE(MAG_MODE)) tmpi += 100; if (FLIGHT_MODE(BARO_MODE)) tmpi += 200; if (FLIGHT_MODE(SONAR_MODE)) tmpi += 400; if (FLIGHT_MODE(GPS_HOLD_MODE)) tmpi += 1000; if (FLIGHT_MODE(GPS_HOME_MODE)) tmpi += 2000; if (FLIGHT_MODE(HEADFREE_MODE)) tmpi += 4000; smartPortSendPackage(id, (uint32_t)tmpi); smartPortHasRequest = 0; break; case FSSP_DATAID_T2 : if (sensors(SENSOR_GPS)) { #ifdef GPS // provide GPS lock status smartPortSendPackage(id, (STATE(GPS_FIX) ? 1000 : 0) + (STATE(GPS_FIX_HOME) ? 2000 : 0) + GPS_numSat); smartPortHasRequest = 0; #endif } else if (feature(FEATURE_GPS)) { smartPortSendPackage(id, 0); smartPortHasRequest = 0; } break; #ifdef GPS case FSSP_DATAID_GPS_ALT : if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) { smartPortSendPackage(id, GPS_altitude * 100); // given in 0.1m , requested in 10 = 1m (should be in mm, probably a bug in opentx, tested on 2.0.1.7) smartPortHasRequest = 0; } break; #endif case FSSP_DATAID_A4 : if (feature(FEATURE_VBAT)) { smartPortSendPackage(id, vbat * 10 / batteryCellCount ); // given in 0.1V, convert to volts smartPortHasRequest = 0; } break; default: break; // if nothing is sent, smartPortHasRequest isn't cleared, we already incremented the counter, just loop back to the start } } }
void handleSmartPortTelemetry(void) { uint32_t smartPortLastServiceTime = millis(); if (!smartPortTelemetryEnabled) { return; } if (!canSendSmartPortTelemetry()) { return; } while (serialRxBytesWaiting(smartPortSerialPort) > 0) { uint8_t c = serialRead(smartPortSerialPort); smartPortDataReceive(c); } uint32_t now = millis(); // if timed out, reconfigure the UART back to normal so the GUI or CLI works if ((now - smartPortLastRequestTime) > SMARTPORT_NOT_CONNECTED_TIMEOUT_MS) { smartPortState = SPSTATE_TIMEDOUT; return; } while (smartPortHasRequest) { // Ensure we won't get stuck in the loop if there happens to be nothing available to send in a timely manner - dump the slot if we loop in there for too long. if ((millis() - smartPortLastServiceTime) > SMARTPORT_SERVICE_TIMEOUT_MS) { smartPortHasRequest = 0; return; } // we can send back any data we want, our table keeps track of the order and frequency of each data type we send uint16_t id = frSkyDataIdTable[smartPortIdCnt]; if (id == 0) { // end of table reached, loop back smartPortIdCnt = 0; id = frSkyDataIdTable[smartPortIdCnt]; } smartPortIdCnt++; int32_t tmpi; switch(id) { #ifdef GPS case FSSP_DATAID_SPEED : if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) { uint32_t tmpui = (GPS_speed * 36 + 36 / 2) / 100; smartPortSendPackage(id, tmpui); // given in 0.1 m/s, provide in KM/H smartPortHasRequest = 0; } break; #endif case FSSP_DATAID_VFAS : if (feature(FEATURE_VBAT)) { smartPortSendPackage(id, vbat * 10); // given in 0.1V, convert to volts smartPortHasRequest = 0; } break; case FSSP_DATAID_CELLS : if (feature(FEATURE_VBAT) && telemetryConfig()->telemetry_send_cells) { /* * A cell packet is formated this way: https://github.com/jcheger/frsky-arduino/blob/master/FrskySP/FrskySP.cpp * content | length * ---------- | ------ * volt[id] | 12-bits * celltotal | 4 bits * cellid | 4 bits */ static uint8_t currentCell = 0; // Track current cell index number // Cells Data Payload uint32_t payload = 0; payload |= ((uint16_t)(vbat * 100 + batteryCellCount) / (batteryCellCount * 2)) & 0x0FFF; // Cell Voltage formatted for payload, modified code from cleanflight Frsky.c, TESTING NOTE: (uint16_t)(4.2 * 500.0) & 0x0FFF; payload <<= 4; payload |= (uint8_t)batteryCellCount & 0x0F; // Cell Total Count formatted for payload payload <<= 4; payload |= (uint8_t)currentCell & 0x0F; // Current Cell Index Number formatted for payload // Send Payload smartPortSendPackage(id, payload); smartPortHasRequest = 0; // Incremental Counter currentCell++; currentCell %= batteryCellCount; // Reset counter @ max index } break; case FSSP_DATAID_CURRENT : if (feature(FEATURE_AMPERAGE_METER)) { amperageMeter_t *state = getAmperageMeter(batteryConfig()->amperageMeterSource); smartPortSendPackage(id, state->amperage / 10); // given in 10mA steps, unknown requested unit smartPortHasRequest = 0; } break; //case FSSP_DATAID_RPM : case FSSP_DATAID_ALTITUDE : if (sensors(SENSOR_BARO)) { smartPortSendPackage(id, BaroAlt); // unknown given unit, requested 100 = 1 meter smartPortHasRequest = 0; } break; case FSSP_DATAID_FUEL : if (feature(FEATURE_AMPERAGE_METER)) { amperageMeter_t *state = getAmperageMeter(batteryConfig()->amperageMeterSource); smartPortSendPackage(id, state->mAhDrawn); // given in mAh, unknown requested unit smartPortHasRequest = 0; } break; //case FSSP_DATAID_ADC1 : //case FSSP_DATAID_ADC2 : #ifdef GPS case FSSP_DATAID_LATLONG : if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) { uint32_t tmpui = 0; // the same ID is sent twice, one for longitude, one for latitude // the MSB of the sent uint32_t helps FrSky keep track // the even/odd bit of our counter helps us keep track if (smartPortIdCnt & 1) { tmpui = abs(GPS_coord[LON]); // now we have unsigned value and one bit to spare tmpui = (tmpui + tmpui / 2) / 25 | 0x80000000; // 6/100 = 1.5/25, division by power of 2 is fast if (GPS_coord[LON] < 0) tmpui |= 0x40000000; } else { tmpui = abs(GPS_coord[LAT]); // now we have unsigned value and one bit to spare tmpui = (tmpui + tmpui / 2) / 25; // 6/100 = 1.5/25, division by power of 2 is fast if (GPS_coord[LAT] < 0) tmpui |= 0x40000000; } smartPortSendPackage(id, tmpui); smartPortHasRequest = 0; } break; #endif //case FSSP_DATAID_CAP_USED : case FSSP_DATAID_VARIO : if (sensors(SENSOR_BARO)) { smartPortSendPackage(id, vario); // unknown given unit but requested in 100 = 1m/s smartPortHasRequest = 0; } break; case FSSP_DATAID_HEADING : smartPortSendPackage(id, attitude.values.yaw * 10); // given in 10*deg, requested in 10000 = 100 deg smartPortHasRequest = 0; break; case FSSP_DATAID_ACCX : smartPortSendPackage(id, accSmooth[X] / 44); // unknown input and unknown output unit // we can only show 00.00 format, another digit won't display right on Taranis // dividing by roughly 44 will give acceleration in G units smartPortHasRequest = 0; break; case FSSP_DATAID_ACCY : smartPortSendPackage(id, accSmooth[Y] / 44); smartPortHasRequest = 0; break; case FSSP_DATAID_ACCZ : smartPortSendPackage(id, accSmooth[Z] / 44); smartPortHasRequest = 0; break; case FSSP_DATAID_T1 : // we send all the flags as decimal digits for easy reading tmpi = 10000; // start off with at least one digit so the most significant 0 won't be cut off // the Taranis seems to be able to fit 5 digits on the screen // the Taranis seems to consider this number a signed 16 bit integer if (ARMING_FLAG(OK_TO_ARM)) tmpi += 1; if (ARMING_FLAG(PREVENT_ARMING)) tmpi += 2; if (ARMING_FLAG(ARMED)) tmpi += 4; if (FLIGHT_MODE(ANGLE_MODE)) tmpi += 10; if (FLIGHT_MODE(HORIZON_MODE)) tmpi += 20; if (FLIGHT_MODE(GTUNE_MODE) || FLIGHT_MODE(PASSTHRU_MODE)) tmpi += 40; if (FLIGHT_MODE(MAG_MODE)) tmpi += 100; if (FLIGHT_MODE(BARO_MODE)) tmpi += 200; if (FLIGHT_MODE(SONAR_MODE)) tmpi += 400; if (FLIGHT_MODE(GPS_HOLD_MODE)) tmpi += 1000; if (FLIGHT_MODE(GPS_HOME_MODE)) tmpi += 2000; if (FLIGHT_MODE(HEADFREE_MODE)) tmpi += 4000; smartPortSendPackage(id, (uint32_t)tmpi); smartPortHasRequest = 0; break; case FSSP_DATAID_T2 : if (sensors(SENSOR_GPS)) { #ifdef GPS // provide GPS lock status smartPortSendPackage(id, (STATE(GPS_FIX) ? 1000 : 0) + (STATE(GPS_FIX_HOME) ? 2000 : 0) + GPS_numSat); smartPortHasRequest = 0; #endif } else if (feature(FEATURE_GPS)) { smartPortSendPackage(id, 0); smartPortHasRequest = 0; } break; #ifdef GPS case FSSP_DATAID_GPS_ALT : if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) { smartPortSendPackage(id, GPS_altitude * 100); // given in 0.1m , requested in 10 = 1m (should be in mm, probably a bug in opentx, tested on 2.0.1.7) smartPortHasRequest = 0; } break; #endif case FSSP_DATAID_A4 : if (feature(FEATURE_VBAT)) { smartPortSendPackage(id, vbat * 10 / batteryCellCount ); //sending calculated average cell value with 0.01 precision smartPortHasRequest = 0; } break; default: break; // if nothing is sent, smartPortHasRequest isn't cleared, we already incremented the counter, just loop back to the start } } }