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
}
}
}