Mirror/opentx
1
0
Fork 0
mirror of https://github.com/opentx/opentx.git synced 2025-07-14 11:59:50 +03:00
Code Issues Wiki Activity

Telemetry files split

Browse source
This commit is contained in:
Bertrand Songis 2016-03-28 11:24:35 +02:00
parent 485d30f65b
commit 746dc450fa
25 changed files with 1713 additions and 1609 deletions
Download patch file Download diff file Expand all files Collapse all files

2
companion/src/simulation/simulatorimport.h
View file

@ -23,7 +23,7 @@
#ifdef INIT_IMPORT
#undef INIT_IMPORT
#ifdef FRSKY
frskyStreaming = 20;
telemetryStreaming = 20;
#endif
#endif

4
radio/src/CMakeLists.txt
View file

@ -512,6 +512,8 @@ if(ARCH STREQUAL ARM)
tasks_arm.cpp
audio_arm.cpp
telemetry/telemetry.cpp
telemetry/telemetry_holders.cpp
telemetry/telemetry_sensors.cpp
telemetry/frsky.cpp
telemetry/frsky_d_arm.cpp
telemetry/frsky_sport.cpp
@ -637,7 +639,7 @@ else()
option(GPS "GPS support" ON)
option(VARIO "Vario support" ON)
add_definitions(-DFRSKY)
set(SRC ${SRC} telemetry/frsky.cpp telemetry/frsky_d.cpp)
set(SRC ${SRC} telemetry/telemetry.cpp telemetry/telemetry_holders.cpp telemetry/frsky.cpp telemetry/frsky_d.cpp)
set(FIRMWARE_SRC ${FIRMWARE_SRC} targets/common_avr/telemetry_driver.cpp)
set(GUI_SRC ${GUI_SRC} view_telemetry.cpp)
if(FRSKY_HUB)

4
radio/src/gui/9x/menu_model_telemetry.cpp
View file

@ -609,7 +609,7 @@ void menuModelTelemetry(uint8_t event)
case ITEM_TELEMETRY_A2_LABEL:
lcd_putsLeft(y, STR_ACHANNEL);
lcdDrawNumber(2*FW, y, ch+1, 0);
putsTelemetryChannelValue(TELEM_COL2+6*FW, y, dest, frskyData.analog[ch].value, LEFT);
putsTelemetryChannelValue(TELEM_COL2+6*FW, y, dest, telemetryData.analog[ch].value, LEFT);
break;
case ITEM_TELEMETRY_A1_RANGE:
@ -739,7 +739,7 @@ void menuModelTelemetry(uint8_t event)
case ITEM_TELEMETRY_FAS_OFFSET:
lcd_putsLeft(y, STR_FAS_OFFSET);
lcdDrawNumber(TELEM_COL2, y, g_model.frsky.fasOffset, attr|LEFT|PREC1);
lcdDrawNumber(TELEM_COL2+6*FW, y, frskyData.hub.current, LEFT|PREC1);
lcdDrawNumber(TELEM_COL2+6*FW, y, telemetryData.hub.current, LEFT|PREC1);
lcdDrawChar(TELEM_COL2+8*FW, y, 'A');
if (attr) g_model.frsky.fasOffset = checkIncDec(event, g_model.frsky.fasOffset, -120, 120, EE_MODEL);
break;

46
radio/src/gui/9x/view_telemetry.cpp
View file

@ -38,7 +38,7 @@ void displayRssiLine()
lcdDrawSolidHorizontalLine(0, 55, 128, 0); // separator
uint8_t rssi;
#if !defined(CPUARM)
rssi = min((uint8_t)99, frskyData.rssi[1].value);
rssi = min((uint8_t)99, telemetryData.rssi[1].value);
lcd_putsLeft(STATUS_BAR_Y, STR_TX); lcdDrawNumber(4*FW+1, STATUS_BAR_Y, rssi, LEADING0, 2);
lcdDrawRect(BAR_LEFT+1, 57, 38, 7);
lcdDrawFilledRect(BAR_LEFT+1, 58, 4*rssi/11, 5, (rssi < getRssiAlarmValue(0)) ? DOTTED : SOLID);
@ -59,17 +59,17 @@ void displayRssiLine()
void displayGpsTime()
{
uint8_t att = (TELEMETRY_STREAMING() ? LEFT|LEADING0 : LEFT|LEADING0|BLINK);
lcdDrawNumber(CENTER_OFS+6*FW+7, STATUS_BAR_Y, frskyData.hub.hour, att, 2);
lcdDrawNumber(CENTER_OFS+6*FW+7, STATUS_BAR_Y, telemetryData.hub.hour, att, 2);
lcdDrawChar(CENTER_OFS+8*FW+4, STATUS_BAR_Y, ':', att);
lcdDrawNumber(CENTER_OFS+9*FW+2, STATUS_BAR_Y, frskyData.hub.min, att, 2);
lcdDrawNumber(CENTER_OFS+9*FW+2, STATUS_BAR_Y, telemetryData.hub.min, att, 2);
lcdDrawChar(CENTER_OFS+11*FW-1, STATUS_BAR_Y, ':', att);
lcdDrawNumber(CENTER_OFS+12*FW-3, STATUS_BAR_Y, frskyData.hub.sec, att, 2);
lcdDrawNumber(CENTER_OFS+12*FW-3, STATUS_BAR_Y, telemetryData.hub.sec, att, 2);
lcdInvertLastLine();
}
void displayGpsCoord(uint8_t y, char direction, int16_t bp, int16_t ap)
{
if (frskyData.hub.gpsFix >= 0) {
if (telemetryData.hub.gpsFix >= 0) {
if (!direction) direction = '-';
lcdDrawNumber(TELEM_2ND_COLUMN, y, bp / 100, LEFT); // ddd before '.'
lcdDrawChar(lcdLastPos, y, '@');
@ -107,9 +107,9 @@ void displayVoltageScreenLine(uint8_t y, uint8_t index)
{
drawStringWithIndex(0, y, STR_A, index+1, 0);
if (TELEMETRY_STREAMING()) {
putsTelemetryChannelValue(3*FW+6*FW+4, y-FH, index+TELEM_A1-1, frskyData.analog[index].value, DBLSIZE);
lcdDrawChar(12*FW-1, y-FH, '<'); putsTelemetryChannelValue(17*FW, y-FH, index+TELEM_A1-1, frskyData.analog[index].min, NO_UNIT);
lcdDrawChar(12*FW, y, '>'); putsTelemetryChannelValue(17*FW, y, index+TELEM_A1-1, frskyData.analog[index].max, NO_UNIT);
putsTelemetryChannelValue(3*FW+6*FW+4, y-FH, index+TELEM_A1-1, telemetryData.analog[index].value, DBLSIZE);
lcdDrawChar(12*FW-1, y-FH, '<'); putsTelemetryChannelValue(17*FW, y-FH, index+TELEM_A1-1, telemetryData.analog[index].min, NO_UNIT);
lcdDrawChar(12*FW, y, '>'); putsTelemetryChannelValue(17*FW, y, index+TELEM_A1-1, telemetryData.analog[index].max, NO_UNIT);
}
}
#endif
@ -142,10 +142,10 @@ void displayVoltagesScreen()
break;
#if defined(FRSKY_HUB)
case FRSKY_VOLTS_SOURCE_FAS:
putsTelemetryChannelValue(3*FW+6*FW+4, FH, TELEM_VFAS-1, frskyData.hub.vfas, DBLSIZE);
putsTelemetryChannelValue(3*FW+6*FW+4, FH, TELEM_VFAS-1, telemetryData.hub.vfas, DBLSIZE);
break;
case FRSKY_VOLTS_SOURCE_CELLS:
putsTelemetryChannelValue(3*FW+6*FW+4, FH, TELEM_CELLS_SUM-1, frskyData.hub.cellsSum, DBLSIZE);
putsTelemetryChannelValue(3*FW+6*FW+4, FH, TELEM_CELLS_SUM-1, telemetryData.hub.cellsSum, DBLSIZE);
break;
#endif
}
@ -159,13 +159,13 @@ void displayVoltagesScreen()
break;
#if defined(FRSKY_HUB)
case FRSKY_CURRENT_SOURCE_FAS:
putsTelemetryChannelValue(3*FW+6*FW+4, 3*FH, TELEM_CURRENT-1, frskyData.hub.current, DBLSIZE);
putsTelemetryChannelValue(3*FW+6*FW+4, 3*FH, TELEM_CURRENT-1, telemetryData.hub.current, DBLSIZE);
break;
#endif
}
putsTelemetryChannelValue(4, 5*FH, TELEM_POWER-1, frskyData.hub.power, LEFT|DBLSIZE);
putsTelemetryChannelValue(3*FW+4+4*FW+6*FW+FW, 5*FH, TELEM_CONSUMPTION-1, frskyData.hub.currentConsumption, DBLSIZE);
putsTelemetryChannelValue(4, 5*FH, TELEM_POWER-1, telemetryData.hub.power, LEFT|DBLSIZE);
putsTelemetryChannelValue(3*FW+4+4*FW+6*FW+FW, 5*FH, TELEM_CONSUMPTION-1, telemetryData.hub.currentConsumption, DBLSIZE);
}
else {
displayVoltageScreenLine(analog > 0 ? 5*FH : 4*FH, analog ? 2-analog : 0);
@ -174,11 +174,11 @@ void displayVoltagesScreen()
#if defined(FRSKY_HUB)
// Cells voltage
if (frskyData.hub.cellsCount > 0) {
if (telemetryData.hub.cellsCount > 0) {
uint8_t y = 1*FH;
for (uint8_t k=0; k<frskyData.hub.cellsCount && k<6; k++) {
for (uint8_t k=0; k<telemetryData.hub.cellsCount && k<6; k++) {
#if defined(GAUGES)
uint8_t attr = (barsThresholds[THLD_CELL] && frskyData.hub.cellVolts[k] < barsThresholds[THLD_CELL]) ? BLINK|PREC2 : PREC2;
uint8_t attr = (barsThresholds[THLD_CELL] && telemetryData.hub.cellVolts[k] < barsThresholds[THLD_CELL]) ? BLINK|PREC2 : PREC2;
#else
uint8_t attr = PREC2;
#endif
@ -198,20 +198,20 @@ void displayAfterFlightScreen()
if (IS_GPS_AVAILABLE()) {
// Latitude
lcd_putsLeft(line, STR_LATITUDE);
displayGpsCoord(line, frskyData.hub.gpsLatitudeNS, frskyData.hub.gpsLatitude_bp, frskyData.hub.gpsLatitude_ap);
displayGpsCoord(line, telemetryData.hub.gpsLatitudeNS, telemetryData.hub.gpsLatitude_bp, telemetryData.hub.gpsLatitude_ap);
// Longitude
line+=1*FH+1;
lcd_putsLeft(line, STR_LONGITUDE);
displayGpsCoord(line, frskyData.hub.gpsLongitudeEW, frskyData.hub.gpsLongitude_bp, frskyData.hub.gpsLongitude_ap);
displayGpsCoord(line, telemetryData.hub.gpsLongitudeEW, telemetryData.hub.gpsLongitude_bp, telemetryData.hub.gpsLongitude_ap);
displayGpsTime();
line+=1*FH+1;
}
// Rssi
lcd_putsLeft(line, STR_MINRSSI);
lcdDrawText(TELEM_2ND_COLUMN, line, STR_TX);
lcdDrawNumber(TELEM_2ND_COLUMN+3*FW, line, frskyData.rssi[1].min, LEFT|LEADING0, 2);
lcdDrawNumber(TELEM_2ND_COLUMN+3*FW, line, telemetryData.rssi[1].min, LEFT|LEADING0, 2);
lcdDrawText(TELEM_2ND_COLUMN+6*FW, line, STR_RX);
lcdDrawNumber(TELEM_2ND_COLUMN+9*FW, line, frskyData.rssi[0].min, LEFT|LEADING0, 2);
lcdDrawNumber(TELEM_2ND_COLUMN+9*FW, line, telemetryData.rssi[0].min, LEFT|LEADING0, 2);
}
#endif
@ -372,9 +372,9 @@ bool displayNumbersTelemetryScreen(FrSkyScreenData & screen)
#if defined(FRSKY_HUB)
if (field == TELEM_ACC) {
lcd_putsLeft(STATUS_BAR_Y, STR_ACCEL);
lcdDrawNumber(4*FW, STATUS_BAR_Y, frskyData.hub.accelX, LEFT|PREC2);
lcdDrawNumber(10*FW, STATUS_BAR_Y, frskyData.hub.accelY, LEFT|PREC2);
lcdDrawNumber(16*FW, STATUS_BAR_Y, frskyData.hub.accelZ, LEFT|PREC2);
lcdDrawNumber(4*FW, STATUS_BAR_Y, telemetryData.hub.accelX, LEFT|PREC2);
lcdDrawNumber(10*FW, STATUS_BAR_Y, telemetryData.hub.accelY, LEFT|PREC2);
lcdDrawNumber(16*FW, STATUS_BAR_Y, telemetryData.hub.accelZ, LEFT|PREC2);
break;
}
#endif

48
radio/src/logs.cpp
View file

@ -234,9 +234,9 @@ void writeLogs()
#if defined(FRSKY)
#if !defined(CPUARM)
f_printf(&g_oLogFile, "%d,%d,%d,", frskyStreaming, RAW_FRSKY_MINMAX(frskyData.rssi[0]), RAW_FRSKY_MINMAX(frskyData.rssi[1]));
f_printf(&g_oLogFile, "%d,%d,%d,", telemetryStreaming, RAW_FRSKY_MINMAX(telemetryData.rssi[0]), RAW_FRSKY_MINMAX(telemetryData.rssi[1]));
for (uint8_t i=0; i<MAX_FRSKY_A_CHANNELS; i++) {
int16_t converted_value = applyChannelRatio(i, RAW_FRSKY_MINMAX(frskyData.analog[i]));
int16_t converted_value = applyChannelRatio(i, RAW_FRSKY_MINMAX(telemetryData.analog[i]));
f_printf(&g_oLogFile, "%d.%02d,", converted_value/100, converted_value%100);
}
@ -245,36 +245,36 @@ void writeLogs()
if (IS_USR_PROTO_FRSKY_HUB()) {
f_printf(&g_oLogFile, "%4d-%02d-%02d,%02d:%02d:%02d,%03d.%04d%c,%03d.%04d%c,%03d.%02d," TELEMETRY_GPS_SPEED_FORMAT TELEMETRY_GPS_ALT_FORMAT TELEMETRY_BARO_ALT_FORMAT TELEMETRY_VSPEED_FORMAT TELEMETRY_ASPEED_FORMAT "%d,%d,%d,%d," TELEMETRY_CELLS_FORMAT TELEMETRY_CURRENT_FORMAT "%d," TELEMETRY_VFAS_FORMAT "%d,%d,%d,",
frskyData.hub.year+2000,
frskyData.hub.month,
frskyData.hub.day,
frskyData.hub.hour,
frskyData.hub.min,
frskyData.hub.sec,
frskyData.hub.gpsLongitude_bp,
frskyData.hub.gpsLongitude_ap,
frskyData.hub.gpsLongitudeEW ? frskyData.hub.gpsLongitudeEW : '-',
frskyData.hub.gpsLatitude_bp,
frskyData.hub.gpsLatitude_ap,
frskyData.hub.gpsLatitudeNS ? frskyData.hub.gpsLatitudeNS : '-',
frskyData.hub.gpsCourse_bp,
frskyData.hub.gpsCourse_ap,
telemetryData.hub.year+2000,
telemetryData.hub.month,
telemetryData.hub.day,
telemetryData.hub.hour,
telemetryData.hub.min,
telemetryData.hub.sec,
telemetryData.hub.gpsLongitude_bp,
telemetryData.hub.gpsLongitude_ap,
telemetryData.hub.gpsLongitudeEW ? telemetryData.hub.gpsLongitudeEW : '-',
telemetryData.hub.gpsLatitude_bp,
telemetryData.hub.gpsLatitude_ap,
telemetryData.hub.gpsLatitudeNS ? telemetryData.hub.gpsLatitudeNS : '-',
telemetryData.hub.gpsCourse_bp,
telemetryData.hub.gpsCourse_ap,
TELEMETRY_GPS_SPEED_ARGS
TELEMETRY_GPS_ALT_ARGS
TELEMETRY_BARO_ALT_ARGS
TELEMETRY_VSPEED_ARGS
TELEMETRY_ASPEED_ARGS
frskyData.hub.temperature1,
frskyData.hub.temperature2,
frskyData.hub.rpm,
frskyData.hub.fuelLevel,
telemetryData.hub.temperature1,
telemetryData.hub.temperature2,
telemetryData.hub.rpm,
telemetryData.hub.fuelLevel,
TELEMETRY_CELLS_ARGS
TELEMETRY_CURRENT_ARGS
frskyData.hub.currentConsumption,
telemetryData.hub.currentConsumption,
TELEMETRY_VFAS_ARGS
frskyData.hub.accelX,
frskyData.hub.accelY,
frskyData.hub.accelZ);
telemetryData.hub.accelX,
telemetryData.hub.accelY,
telemetryData.hub.accelZ);
}
#endif

30
radio/src/maths.cpp
View file

@ -143,21 +143,21 @@ double gpsToDouble(bool neg, int16_t bp, int16_t ap)
#if defined(FRSKY_HUB) && !defined(CPUARM)
void extractLatitudeLongitude(uint32_t * latitude, uint32_t * longitude)
{
div_t qr = div(frskyData.hub.gpsLatitude_bp, 100);
*latitude = ((uint32_t)(qr.quot) * 1000000) + (((uint32_t)(qr.rem) * 10000 + frskyData.hub.gpsLatitude_ap) * 5) / 3;
div_t qr = div(telemetryData.hub.gpsLatitude_bp, 100);
*latitude = ((uint32_t)(qr.quot) * 1000000) + (((uint32_t)(qr.rem) * 10000 + telemetryData.hub.gpsLatitude_ap) * 5) / 3;
qr = div(frskyData.hub.gpsLongitude_bp, 100);
*longitude = ((uint32_t)(qr.quot) * 1000000) + (((uint32_t)(qr.rem) * 10000 + frskyData.hub.gpsLongitude_ap) * 5) / 3;
qr = div(telemetryData.hub.gpsLongitude_bp, 100);
*longitude = ((uint32_t)(qr.quot) * 1000000) + (((uint32_t)(qr.rem) * 10000 + telemetryData.hub.gpsLongitude_ap) * 5) / 3;
}
void getGpsPilotPosition()
{
extractLatitudeLongitude(&frskyData.hub.pilotLatitude, &frskyData.hub.pilotLongitude);
uint32_t lat = frskyData.hub.pilotLatitude / 10000;
extractLatitudeLongitude(&telemetryData.hub.pilotLatitude, &telemetryData.hub.pilotLongitude);
uint32_t lat = telemetryData.hub.pilotLatitude / 10000;
uint32_t angle2 = (lat*lat) / 10000;
uint32_t angle4 = angle2 * angle2;
frskyData.hub.distFromEarthAxis = 139*(((uint32_t)10000000-((angle2*(uint32_t)123370)/81)+(angle4/25))/12500);
// TRACE("frskyData.hub.distFromEarthAxis=%d", frskyData.hub.distFromEarthAxis);
telemetryData.hub.distFromEarthAxis = 139*(((uint32_t)10000000-((angle2*(uint32_t)123370)/81)+(angle4/25))/12500);
// TRACE("telemetryData.hub.distFromEarthAxis=%d", telemetryData.hub.distFromEarthAxis);
}
void getGpsDistance()
@ -166,21 +166,21 @@ void getGpsDistance()
extractLatitudeLongitude(&lat, &lng);
// printf("lat=%d (%d), long=%d (%d)\n", lat, abs(lat - frskyData.hub.pilotLatitude), lng, abs(lng - frskyData.hub.pilotLongitude));
// printf("lat=%d (%d), long=%d (%d)\n", lat, abs(lat - telemetryData.hub.pilotLatitude), lng, abs(lng - telemetryData.hub.pilotLongitude));
uint32_t angle = (lat > frskyData.hub.pilotLatitude) ? lat - frskyData.hub.pilotLatitude : frskyData.hub.pilotLatitude - lat;
uint32_t angle = (lat > telemetryData.hub.pilotLatitude) ? lat - telemetryData.hub.pilotLatitude : telemetryData.hub.pilotLatitude - lat;
uint32_t dist = EARTH_RADIUS * angle / 1000000;
uint32_t result = dist*dist;
angle = (lng > frskyData.hub.pilotLongitude) ? lng - frskyData.hub.pilotLongitude : frskyData.hub.pilotLongitude - lng;
dist = frskyData.hub.distFromEarthAxis * angle / 1000000;
angle = (lng > telemetryData.hub.pilotLongitude) ? lng - telemetryData.hub.pilotLongitude : telemetryData.hub.pilotLongitude - lng;
dist = telemetryData.hub.distFromEarthAxis * angle / 1000000;
result += dist*dist;
dist = abs(TELEMETRY_BARO_ALT_AVAILABLE() ? TELEMETRY_RELATIVE_BARO_ALT_BP : TELEMETRY_RELATIVE_GPS_ALT_BP);
result += dist*dist;
frskyData.hub.gpsDistance = isqrt32(result);
if (frskyData.hub.gpsDistance > frskyData.hub.maxGpsDistance)
frskyData.hub.maxGpsDistance = frskyData.hub.gpsDistance;
telemetryData.hub.gpsDistance = isqrt32(result);
if (telemetryData.hub.gpsDistance > telemetryData.hub.maxGpsDistance)
telemetryData.hub.maxGpsDistance = telemetryData.hub.gpsDistance;
}
#endif

50
radio/src/mixer.cpp
View file

@ -370,39 +370,39 @@ getvalue_t getValue(mixsrc_t i)
}
}
#elif defined(FRSKY)
else if (i==MIXSRC_FIRST_TELEM-1+TELEM_RSSI_TX) return frskyData.rssi[1].value;
else if (i==MIXSRC_FIRST_TELEM-1+TELEM_RSSI_RX) return frskyData.rssi[0].value;
else if (i==MIXSRC_FIRST_TELEM-1+TELEM_A1) return frskyData.analog[TELEM_ANA_A1].value;
else if (i==MIXSRC_FIRST_TELEM-1+TELEM_A2) return frskyData.analog[TELEM_ANA_A2].value;
else if (i==MIXSRC_FIRST_TELEM-1+TELEM_RSSI_TX) return telemetryData.rssi[1].value;
else if (i==MIXSRC_FIRST_TELEM-1+TELEM_RSSI_RX) return telemetryData.rssi[0].value;
else if (i==MIXSRC_FIRST_TELEM-1+TELEM_A1) return telemetryData.analog[TELEM_ANA_A1].value;
else if (i==MIXSRC_FIRST_TELEM-1+TELEM_A2) return telemetryData.analog[TELEM_ANA_A2].value;
#if defined(FRSKY_SPORT)
else if (i==MIXSRC_FIRST_TELEM-1+TELEM_ALT) return frskyData.hub.baroAltitude;
else if (i==MIXSRC_FIRST_TELEM-1+TELEM_ALT) return telemetryData.hub.baroAltitude;
#elif defined(FRSKY_HUB) || defined(WS_HOW_HIGH)
else if (i==MIXSRC_FIRST_TELEM-1+TELEM_ALT) return TELEMETRY_RELATIVE_BARO_ALT_BP;
#endif
#if defined(FRSKY_HUB)
else if (i==MIXSRC_FIRST_TELEM-1+TELEM_RPM) return frskyData.hub.rpm;
else if (i==MIXSRC_FIRST_TELEM-1+TELEM_FUEL) return frskyData.hub.fuelLevel;
else if (i==MIXSRC_FIRST_TELEM-1+TELEM_T1) return frskyData.hub.temperature1;
else if (i==MIXSRC_FIRST_TELEM-1+TELEM_T2) return frskyData.hub.temperature2;
else if (i==MIXSRC_FIRST_TELEM-1+TELEM_RPM) return telemetryData.hub.rpm;
else if (i==MIXSRC_FIRST_TELEM-1+TELEM_FUEL) return telemetryData.hub.fuelLevel;
else if (i==MIXSRC_FIRST_TELEM-1+TELEM_T1) return telemetryData.hub.temperature1;
else if (i==MIXSRC_FIRST_TELEM-1+TELEM_T2) return telemetryData.hub.temperature2;
else if (i==MIXSRC_FIRST_TELEM-1+TELEM_SPEED) return TELEMETRY_GPS_SPEED_BP;
else if (i==MIXSRC_FIRST_TELEM-1+TELEM_DIST) return frskyData.hub.gpsDistance;
else if (i==MIXSRC_FIRST_TELEM-1+TELEM_DIST) return telemetryData.hub.gpsDistance;
else if (i==MIXSRC_FIRST_TELEM-1+TELEM_GPSALT) return TELEMETRY_RELATIVE_GPS_ALT_BP;
else if (i==MIXSRC_FIRST_TELEM-1+TELEM_CELL) return (int16_t)TELEMETRY_MIN_CELL_VOLTAGE;
else if (i==MIXSRC_FIRST_TELEM-1+TELEM_CELLS_SUM) return (int16_t)frskyData.hub.cellsSum;
else if (i==MIXSRC_FIRST_TELEM-1+TELEM_VFAS) return (int16_t)frskyData.hub.vfas;
else if (i==MIXSRC_FIRST_TELEM-1+TELEM_CURRENT) return (int16_t)frskyData.hub.current;
else if (i==MIXSRC_FIRST_TELEM-1+TELEM_CONSUMPTION) return frskyData.hub.currentConsumption;
else if (i==MIXSRC_FIRST_TELEM-1+TELEM_POWER) return frskyData.hub.power;
else if (i==MIXSRC_FIRST_TELEM-1+TELEM_ACCx) return frskyData.hub.accelX;
else if (i==MIXSRC_FIRST_TELEM-1+TELEM_ACCy) return frskyData.hub.accelY;
else if (i==MIXSRC_FIRST_TELEM-1+TELEM_ACCz) return frskyData.hub.accelZ;
else if (i==MIXSRC_FIRST_TELEM-1+TELEM_HDG) return frskyData.hub.gpsCourse_bp;
else if (i==MIXSRC_FIRST_TELEM-1+TELEM_VSPEED) return frskyData.hub.varioSpeed;
else if (i==MIXSRC_FIRST_TELEM-1+TELEM_ASPEED) return frskyData.hub.airSpeed;
else if (i==MIXSRC_FIRST_TELEM-1+TELEM_DTE) return frskyData.hub.dTE;
else if (i<=MIXSRC_FIRST_TELEM-1+TELEM_MIN_A1) return frskyData.analog[TELEM_ANA_A1].min;
else if (i==MIXSRC_FIRST_TELEM-1+TELEM_MIN_A2) return frskyData.analog[TELEM_ANA_A2].min;
else if (i<=MIXSRC_FIRST_TELEM-1+TELEM_CSW_MAX) return *(((int16_t*)(&frskyData.hub.minAltitude))+i-(MIXSRC_FIRST_TELEM-1+TELEM_MIN_ALT));
else if (i==MIXSRC_FIRST_TELEM-1+TELEM_CELLS_SUM) return (int16_t)telemetryData.hub.cellsSum;
else if (i==MIXSRC_FIRST_TELEM-1+TELEM_VFAS) return (int16_t)telemetryData.hub.vfas;
else if (i==MIXSRC_FIRST_TELEM-1+TELEM_CURRENT) return (int16_t)telemetryData.hub.current;
else if (i==MIXSRC_FIRST_TELEM-1+TELEM_CONSUMPTION) return telemetryData.hub.currentConsumption;
else if (i==MIXSRC_FIRST_TELEM-1+TELEM_POWER) return telemetryData.hub.power;
else if (i==MIXSRC_FIRST_TELEM-1+TELEM_ACCx) return telemetryData.hub.accelX;
else if (i==MIXSRC_FIRST_TELEM-1+TELEM_ACCy) return telemetryData.hub.accelY;
else if (i==MIXSRC_FIRST_TELEM-1+TELEM_ACCz) return telemetryData.hub.accelZ;
else if (i==MIXSRC_FIRST_TELEM-1+TELEM_HDG) return telemetryData.hub.gpsCourse_bp;
else if (i==MIXSRC_FIRST_TELEM-1+TELEM_VSPEED) return telemetryData.hub.varioSpeed;
else if (i==MIXSRC_FIRST_TELEM-1+TELEM_ASPEED) return telemetryData.hub.airSpeed;
else if (i==MIXSRC_FIRST_TELEM-1+TELEM_DTE) return telemetryData.hub.dTE;
else if (i<=MIXSRC_FIRST_TELEM-1+TELEM_MIN_A1) return telemetryData.analog[TELEM_ANA_A1].min;
else if (i==MIXSRC_FIRST_TELEM-1+TELEM_MIN_A2) return telemetryData.analog[TELEM_ANA_A2].min;
else if (i<=MIXSRC_FIRST_TELEM-1+TELEM_CSW_MAX) return *(((int16_t*)(&telemetryData.hub.minAltitude))+i-(MIXSRC_FIRST_TELEM-1+TELEM_MIN_ALT));
#endif
#endif
else return 0;

21
radio/src/opentx.h
View file

@ -1301,26 +1301,7 @@ void evalFunctions();
extern volatile rotenc_t g_rotenc[1];
#endif
#if defined(CPUARM)
#include "telemetry/telemetry.h"
#endif
#if defined (FRSKY)
// FrSky Telemetry
#include "telemetry/frsky.h"
#elif defined(JETI)
// Jeti-DUPLEX Telemetry
#include "telemetry/jeti.h"
#elif defined(ARDUPILOT)
// ArduPilot Telemetry
#include "telemetry/ardupilot.h"
#elif defined(NMEA)
// NMEA Telemetry
#include "telemetry/nmea.h"
#elif defined(MAVLINK)
// Mavlink Telemetry
#include "telemetry/mavlink.h"
#endif
#include "telemetry/telemetry.h"
#if defined(CPUARM)
uint16_t crc16(uint8_t * buf, uint32_t len);

2
radio/src/simu.cpp
View file

@ -524,7 +524,7 @@ int main(int argc,char **argv)
#endif
#if defined(FRSKY) && !defined(FRSKY_SPORT)
frskyStreaming = 1;
telemetryStreaming = 1;
#endif
printf("Model size = %d\n", (int)sizeof(g_model));

4
radio/src/targets/common_avr/telemetry_driver.cpp
View file

@ -33,7 +33,7 @@ void telemetryEnableRx(void)
UCSRB_N(TLM_USART) |= (1 << RXCIE_N(TLM_USART)); // enable Interrupt
}
void processSerialData(uint8_t data);
void processFrskyTelemetryData(uint8_t data);
extern uint8_t frskyRxBufferCount; // TODO not driver, change name
ISR(USART_RX_vect_N(TLM_USART))
@ -82,7 +82,7 @@ ISR(USART_RX_vect_N(TLM_USART))
frskyRxBufferCount = 0;
}
else {
processSerialData(data);
processFrskyTelemetryData(data);
}
cli() ;

519
radio/src/telemetry/frsky.cpp
View file

@ -18,113 +18,14 @@
* GNU General Public License for more details.
*/
#include "../opentx.h"
#include "opentx.h"
uint8_t frskyStreaming = 0;
#if defined(WS_HOW_HIGH)
uint8_t frskyUsrStreaming = 0;
#endif
uint8_t link_counter = 0;
#define FRSKY_RX_PACKET_SIZE 19
uint8_t frskyRxBuffer[FRSKY_RX_PACKET_SIZE]; // Receive buffer. 9 bytes (full packet), worst case 18 bytes with byte-stuffing (+1)
#if !defined(CPUARM) && !defined(PCBFLAMENCO)
uint8_t frskyTxBuffer[FRSKY_TX_PACKET_SIZE];
#endif
#if !defined(CPUARM)
uint8_t frskyTxBufferCount = 0;
#endif
#if defined(CPUARM)
uint8_t telemetryState = TELEMETRY_INIT;
#endif
uint8_t frskyRxBufferCount = 0;
FrskyData frskyData;
#if defined(CPUARM)
uint8_t telemetryProtocol = 255;
#if defined(REVX)
uint8_t serialInversion = 0;
#endif
#define IS_FRSKY_D_PROTOCOL() (telemetryProtocol == PROTOCOL_FRSKY_D)
#define IS_FRSKY_SPORT_PROTOCOL() (telemetryProtocol == PROTOCOL_FRSKY_SPORT)
#else
#define IS_FRSKY_D_PROTOCOL() (true)
#define IS_FRSKY_SPORT_PROTOCOL() (false)
#endif
#if defined(CPUARM)
void FrskyValueWithMin::reset()
{
memclear(this, sizeof(*this));
}
#endif
void FrskyValueWithMin::set(uint8_t value)
{
#if defined(CPUARM)
if (this->value == 0) {
memset(values, value, TELEMETRY_AVERAGE_COUNT);
this->value = value;
}
else {
//calculate the average from values[] and value
//also shift readings in values [] array
unsigned int sum = values[0];
for (int i=0; i<TELEMETRY_AVERAGE_COUNT-1; i++) {
uint8_t tmp = values[i+1];
values[i] = tmp;
sum += tmp;
}
values[TELEMETRY_AVERAGE_COUNT-1] = value;
sum += value;
this->value = sum/(TELEMETRY_AVERAGE_COUNT+1);
}
#else
if (this->value == 0) {
this->value = value;
}
else {
sum += value;
if (link_counter == 0) {
this->value = sum / (IS_FRSKY_D_PROTOCOL() ? FRSKY_D_AVERAGING : FRSKY_SPORT_AVERAGING);
sum = 0;
}
}
#endif
if (!min || value < min) {
min = value;
}
}
void FrskyValueWithMinMax::set(uint8_t value, uint8_t unit)
{
FrskyValueWithMin::set(value);
if (unit != UNIT_VOLTS) {
this->value = value;
}
if (!max || value > max) {
max = value;
}
}
#if !defined(CPUARM)
uint16_t getChannelRatio(source_t channel)
{
return (uint16_t)g_model.frsky.channels[channel].ratio << g_model.frsky.channels[channel].multiplier;
}
lcdint_t applyChannelRatio(source_t channel, lcdint_t val)
{
return ((int32_t)val+g_model.frsky.channels[channel].offset) * getChannelRatio(channel) * 2 / 51;
}
uint8_t frskyTxBuffer[FRSKY_TX_PACKET_SIZE];
uint8_t frskyTxBufferCount = 0;
#endif
#if defined(TELEMETREZ)
@ -138,7 +39,7 @@ extern uint8_t TrotCount;
extern uint8_t TezRotary;
#endif
NOINLINE void processSerialData(uint8_t data)
NOINLINE void processFrskyTelemetryData(uint8_t data)
{
static uint8_t dataState = STATE_DATA_IDLE;
@ -166,8 +67,7 @@ NOINLINE void processSerialData(uint8_t data)
}
#endif
switch (dataState)
{
switch (dataState) {
case STATE_DATA_START:
if (data == START_STOP) {
if (IS_FRSKY_SPORT_PROTOCOL()) {
@ -262,412 +162,23 @@ NOINLINE void processSerialData(uint8_t data)
#endif
}
void telemetryWakeup()
{
#if defined(CPUARM)
uint8_t requiredTelemetryProtocol = MODEL_TELEMETRY_PROTOCOL();
#if defined(REVX)
uint8_t requiredSerialInversion = g_model.moduleData[EXTERNAL_MODULE].invertedSerial;
if (telemetryProtocol != requiredTelemetryProtocol || serialInversion != requiredSerialInversion) {
serialInversion = requiredSerialInversion;
#else
if (telemetryProtocol != requiredTelemetryProtocol) {
#endif
telemetryInit(requiredTelemetryProtocol);
telemetryProtocol = requiredTelemetryProtocol;
}
#endif
#if defined(CPUSTM32)
uint8_t data;
#if defined(LOG_TELEMETRY) && !defined(SIMU)
static tmr10ms_t lastTime = 0;
tmr10ms_t newTime = get_tmr10ms();
struct gtm utm;
gettime(&utm);
#endif
while (telemetryFifo.pop(data)) {
processSerialData(data);
#if defined(LOG_TELEMETRY) && !defined(SIMU)
extern FIL g_telemetryFile;
if (lastTime != newTime) {
f_printf(&g_telemetryFile, "\r\n%4d-%02d-%02d,%02d:%02d:%02d.%02d0: %02X", utm.tm_year+1900, utm.tm_mon+1, utm.tm_mday, utm.tm_hour, utm.tm_min, utm.tm_sec, g_ms100, data);
lastTime = newTime;
}
else {
f_printf(&g_telemetryFile, " %02X", data);
}
#endif
}
#elif defined(PCBSKY9X)
if (telemetryProtocol == PROTOCOL_FRSKY_D_SECONDARY) {
uint8_t data;
while (telemetrySecondPortReceive(data)) {
processSerialData(data);
}
}
else {
// Receive serial data here
rxPdcUsart(processSerialData);
}
#endif
#if !defined(CPUARM)
if (IS_FRSKY_D_PROTOCOL()) {
// Attempt to transmit any waiting Fr-Sky alarm set packets every 50ms (subject to packet buffer availability)
static uint8_t frskyTxDelay = 5;
if (frskyAlarmsSendState && (--frskyTxDelay == 0)) {
frskyTxDelay = 5; // 50ms
#if !defined(SIMU)
frskyDSendNextAlarm();
#endif
}
}
#endif
#if defined(CPUARM)
for (int i=0; i<MAX_SENSORS; i++) {
const TelemetrySensor & sensor = g_model.telemetrySensors[i];
if (sensor.type == TELEM_TYPE_CALCULATED) {
telemetryItems[i].eval(sensor);
}
}
#endif
#if defined(VARIO)
if (TELEMETRY_STREAMING() && !IS_FAI_ENABLED()) {
varioWakeup();
}
#endif
#if defined(PCBTARANIS) && defined(REVPLUS)
#define FRSKY_BAD_ANTENNA() (IS_VALID_XJT_VERSION() && frskyData.swr.value > 0x33)
#else
#define FRSKY_BAD_ANTENNA() (frskyData.swr.value > 0x33)
#endif
#if defined(CPUARM)
static tmr10ms_t alarmsCheckTime = 0;
#define SCHEDULE_NEXT_ALARMS_CHECK(seconds) alarmsCheckTime = get_tmr10ms() + (100*(seconds))
if (int32_t(get_tmr10ms() - alarmsCheckTime) > 0) {
SCHEDULE_NEXT_ALARMS_CHECK(1/*second*/);
uint8_t now = TelemetryItem::now();
bool sensor_lost = false;
for (int i=0; i<MAX_SENSORS; i++) {
if (isTelemetryFieldAvailable(i)) {
uint8_t lastReceived = telemetryItems[i].lastReceived;
if (lastReceived < TELEMETRY_VALUE_TIMER_CYCLE && uint8_t(now - lastReceived) > TELEMETRY_VALUE_OLD_THRESHOLD) {
sensor_lost = true;
telemetryItems[i].lastReceived = TELEMETRY_VALUE_OLD;
TelemetrySensor * sensor = & g_model.telemetrySensors[i];
if (sensor->unit == UNIT_DATETIME) {
telemetryItems[i].datetime.datestate = 0;
telemetryItems[i].datetime.timestate = 0;
}
}
}
}
if (sensor_lost && TELEMETRY_STREAMING()) {
audioEvent(AU_SENSOR_LOST);
}
#if defined(PCBTARANIS)
if ((g_model.moduleData[INTERNAL_MODULE].rfProtocol != RF_PROTO_OFF || g_model.moduleData[EXTERNAL_MODULE].type == MODULE_TYPE_XJT) && FRSKY_BAD_ANTENNA()) {
AUDIO_SWR_RED();
POPUP_WARNING(STR_ANTENNAPROBLEM);
SCHEDULE_NEXT_ALARMS_CHECK(10/*seconds*/);
}
#endif
if (TELEMETRY_STREAMING()) {
if (getRssiAlarmValue(1) && TELEMETRY_RSSI() < getRssiAlarmValue(1)) {
AUDIO_RSSI_RED();
SCHEDULE_NEXT_ALARMS_CHECK(10/*seconds*/);
}
else if (getRssiAlarmValue(0) && TELEMETRY_RSSI() < getRssiAlarmValue(0)) {
AUDIO_RSSI_ORANGE();
SCHEDULE_NEXT_ALARMS_CHECK(10/*seconds*/);
}
}
}
if (TELEMETRY_STREAMING()) {
if (telemetryState == TELEMETRY_KO) {
AUDIO_TELEMETRY_BACK();
}
telemetryState = TELEMETRY_OK;
}
else if (telemetryState == TELEMETRY_OK) {
telemetryState = TELEMETRY_KO;
AUDIO_TELEMETRY_LOST();
}
#endif
}
void telemetryInterrupt10ms()
{
#if defined(CPUARM)
#elif defined(FRSKY_HUB)
uint16_t voltage = 0; /* unit: 1/10 volts */
for (uint8_t i=0; i<frskyData.hub.cellsCount; i++)
voltage += frskyData.hub.cellVolts[i];
voltage /= (10 / TELEMETRY_CELL_VOLTAGE_MUTLIPLIER);
frskyData.hub.cellsSum = voltage;
if (frskyData.hub.cellsSum < frskyData.hub.minCells) {
frskyData.hub.minCells = frskyData.hub.cellsSum;
}
#endif
if (TELEMETRY_STREAMING()) {
if (!TELEMETRY_OPENXSENSOR()) {
#if defined(CPUARM)
for (int i=0; i<MAX_SENSORS; i++) {
const TelemetrySensor & sensor = g_model.telemetrySensors[i];
if (sensor.type == TELEM_TYPE_CALCULATED) {
telemetryItems[i].per10ms(sensor);
}
}
#else
// power calculation
uint8_t channel = g_model.frsky.voltsSource;
if (channel <= FRSKY_VOLTS_SOURCE_A2) {
voltage = applyChannelRatio(channel, frskyData.analog[channel].value) / 10;
}
#if defined(FRSKY_HUB)
else if (channel == FRSKY_VOLTS_SOURCE_FAS) {
voltage = frskyData.hub.vfas;
}
#endif
#if defined(FRSKY_HUB)
uint16_t current = frskyData.hub.current; /* unit: 1/10 amps */
#else
uint16_t current = 0;
#endif
channel = g_model.frsky.currentSource - FRSKY_CURRENT_SOURCE_A1;
if (channel < MAX_FRSKY_A_CHANNELS) {
current = applyChannelRatio(channel, frskyData.analog[channel].value) / 10;
}
frskyData.hub.power = ((current>>1) * (voltage>>1)) / 25;
frskyData.hub.currentPrescale += current;
if (frskyData.hub.currentPrescale >= 3600) {
frskyData.hub.currentConsumption += 1;
frskyData.hub.currentPrescale -= 3600;
}
#endif
}
#if !defined(CPUARM)
if (frskyData.hub.power > frskyData.hub.maxPower) {
frskyData.hub.maxPower = frskyData.hub.power;
}
#endif
}
#if defined(WS_HOW_HIGH)
if (frskyUsrStreaming > 0) {
frskyUsrStreaming--;
}
#endif
if (frskyStreaming > 0) {
frskyStreaming--;
}
else {
#if !defined(SIMU)
#if defined(CPUARM)
frskyData.rssi.reset();
#else
frskyData.rssi[0].set(0);
frskyData.rssi[1].set(0);
#endif
#endif
}
}
void telemetryReset()
{
memclear(&frskyData, sizeof(frskyData));
#if defined(CPUARM)
for (int index=0; index<MAX_SENSORS; index++) {
telemetryItems[index].clear();
}
#endif
frskyStreaming = 0; // reset counter only if valid frsky packets are being detected
link_counter = 0;
#if defined(CPUARM)
telemetryState = TELEMETRY_INIT;
#endif
#if defined(FRSKY_HUB) && !defined(CPUARM)
frskyData.hub.gpsLatitude_bp = 2;
frskyData.hub.gpsLongitude_bp = 2;
frskyData.hub.gpsFix = -1;
#endif
#if defined(SIMU)
#if defined(CPUARM)
frskyData.swr.value = 30;
frskyData.rssi.value = 75;
#else
frskyData.rssi[0].value = 75;
frskyData.rssi[1].value = 75;
frskyData.analog[TELEM_ANA_A1].set(120, UNIT_VOLTS);
frskyData.analog[TELEM_ANA_A2].set(240, UNIT_VOLTS);
#endif
#if !defined(CPUARM)
frskyData.hub.fuelLevel = 75;
frskyData.hub.rpm = 12000;
frskyData.hub.vfas = 100;
frskyData.hub.minVfas = 90;
#if defined(GPS)
frskyData.hub.gpsFix = 1;
frskyData.hub.gpsLatitude_bp = 4401;
frskyData.hub.gpsLatitude_ap = 7710;
frskyData.hub.gpsLongitude_bp = 1006;
frskyData.hub.gpsLongitude_ap = 8872;
frskyData.hub.gpsSpeed_bp = 200; //in knots
frskyData.hub.gpsSpeed_ap = 0;
getGpsPilotPosition();
frskyData.hub.gpsLatitude_bp = 4401;
frskyData.hub.gpsLatitude_ap = 7455;
frskyData.hub.gpsLongitude_bp = 1006;
frskyData.hub.gpsLongitude_ap = 9533;
getGpsDistance();
#endif
frskyData.hub.airSpeed = 1000; // 185.1 km/h
frskyData.hub.cellsCount = 6;
frskyData.hub.cellVolts[0] = 410/TELEMETRY_CELL_VOLTAGE_MUTLIPLIER;
frskyData.hub.cellVolts[1] = 420/TELEMETRY_CELL_VOLTAGE_MUTLIPLIER;
frskyData.hub.cellVolts[2] = 430/TELEMETRY_CELL_VOLTAGE_MUTLIPLIER;
frskyData.hub.cellVolts[3] = 440/TELEMETRY_CELL_VOLTAGE_MUTLIPLIER;
frskyData.hub.cellVolts[4] = 450/TELEMETRY_CELL_VOLTAGE_MUTLIPLIER;
frskyData.hub.cellVolts[5] = 460/TELEMETRY_CELL_VOLTAGE_MUTLIPLIER;
frskyData.hub.minCellVolts = 250/TELEMETRY_CELL_VOLTAGE_MUTLIPLIER;
frskyData.hub.minCell = 300; //unit 10mV
frskyData.hub.minCells = 220; //unit 100mV
//frskyData.hub.cellsSum = 261; //calculated from cellVolts[]
frskyData.hub.gpsAltitude_bp = 50;
frskyData.hub.baroAltitude_bp = 50;
frskyData.hub.minAltitude = 10;
frskyData.hub.maxAltitude = 500;
frskyData.hub.accelY = 100;
frskyData.hub.temperature1 = -30;
frskyData.hub.maxTemperature1 = 100;
frskyData.hub.current = 55;
frskyData.hub.maxCurrent = 65;
#endif
#endif
/*Add some default sensor values to the simulator*/
#if defined(CPUARM) && defined(SIMU)
for (int i=0; i<MAX_SENSORS; i++) {
const TelemetrySensor & sensor = g_model.telemetrySensors[i];
switch (sensor.id)
{
case RSSI_ID:
setTelemetryValue(TELEM_PROTO_FRSKY_SPORT, RSSI_ID, 0, sensor.instance , 75, UNIT_RAW, 0);
break;
case ADC1_ID:
setTelemetryValue(TELEM_PROTO_FRSKY_SPORT, ADC1_ID, 0, sensor.instance, 100, UNIT_RAW, 0);
break;
case ADC2_ID:
setTelemetryValue(TELEM_PROTO_FRSKY_SPORT, ADC2_ID, 0, sensor.instance, 245, UNIT_RAW, 0);
break;
case SWR_ID:
setTelemetryValue(TELEM_PROTO_FRSKY_SPORT, SWR_ID, 0, sensor.instance, 30, UNIT_RAW, 0);
break;
case BATT_ID:
setTelemetryValue(TELEM_PROTO_FRSKY_SPORT, BATT_ID, 0, sensor.instance, 100, UNIT_RAW, 0);
break;
}
}
#endif
}
#if defined(CPUARM)
// we don't reset the telemetry here as we would also reset the consumption after model load
void telemetryInit(uint8_t protocol)
{
if (protocol == PROTOCOL_FRSKY_D) {
telemetryPortInit(FRSKY_D_BAUDRATE);
}
#if defined(CROSSFIRE)
else if (protocol == PROTOCOL_PULSES_CROSSFIRE) {
telemetryPortInit(CROSSFIRE_BAUDRATE);
telemetryPortSetDirectionOutput();
}
#endif
else if (protocol == PROTOCOL_FRSKY_D_SECONDARY) {
telemetryPortInit(0);
serial2TelemetryInit(PROTOCOL_FRSKY_D_SECONDARY);
}
else {
telemetryPortInit(FRSKY_SPORT_BAUDRATE);
}
#if defined(REVX) && !defined(SIMU)
if (serialInversion) {
setMFP();
}
else {
clearMFP();
}
#endif
}
#else
void telemetryInit()
{
telemetryPortInit();
}
#endif
#if defined(CPUARM)
#elif defined(FRSKY_HUB)
void frskyUpdateCells(void)
{
// Voltage => Cell number + Cell voltage
uint8_t battnumber = ((frskyData.hub.volts & 0x00F0) >> 4);
uint8_t battnumber = ((telemetryData.hub.volts & 0x00F0) >> 4);
if (battnumber < 12) {
if (frskyData.hub.cellsCount < battnumber+1) {
frskyData.hub.cellsCount = battnumber+1;
if (telemetryData.hub.cellsCount < battnumber+1) {
telemetryData.hub.cellsCount = battnumber+1;
}
uint8_t cellVolts = (uint8_t)(((((frskyData.hub.volts & 0xFF00) >> 8) + ((frskyData.hub.volts & 0x000F) << 8))) / 10);
frskyData.hub.cellVolts[battnumber] = cellVolts;
if (!frskyData.hub.minCellVolts || cellVolts<frskyData.hub.minCellVolts || battnumber==frskyData.hub.minCellIdx) {
frskyData.hub.minCellIdx = battnumber;
frskyData.hub.minCellVolts = cellVolts;
if (!frskyData.hub.minCell || frskyData.hub.minCellVolts<frskyData.hub.minCell)
frskyData.hub.minCell = frskyData.hub.minCellVolts;
uint8_t cellVolts = (uint8_t)(((((telemetryData.hub.volts & 0xFF00) >> 8) + ((telemetryData.hub.volts & 0x000F) << 8))) / 10);
telemetryData.hub.cellVolts[battnumber] = cellVolts;
if (!telemetryData.hub.minCellVolts || cellVolts<telemetryData.hub.minCellVolts || battnumber==telemetryData.hub.minCellIdx) {
telemetryData.hub.minCellIdx = battnumber;
telemetryData.hub.minCellVolts = cellVolts;
if (!telemetryData.hub.minCell || telemetryData.hub.minCellVolts<telemetryData.hub.minCell)
telemetryData.hub.minCell = telemetryData.hub.minCellVolts;
}
}
}
#endif
NOINLINE uint8_t getRssiAlarmValue(uint8_t alarm)
{
return (45 - 3*alarm + g_model.frsky.rssiAlarms[alarm].value);
}
#if defined(LUA)
Fifo<LuaTelemetryValue, 16> * luaInputTelemetryFifo = NULL;
Fifo<LuaTelemetryValue, 16> * luaOutputTelemetryFifo = NULL;
#endif

209
radio/src/telemetry/frsky.h
View file

@ -21,7 +21,7 @@
#ifndef _FRSKY_H_
#define _FRSKY_H_
#include <inttypes.h>
#include "telemetry_holders.h"
#define FRSKY_SPORT_BAUDRATE 57600
#define FRSKY_D_BAUDRATE 9600
@ -161,59 +161,9 @@
#define DATA_ID_SP2UH 0x45 // 5
#define DATA_ID_SP2UR 0xC6 // 6
// Global Fr-Sky telemetry data variables
extern uint8_t frskyStreaming; // >0 (true) == data is streaming in. 0 = nodata detected for some time
#if defined(WS_HOW_HIGH)
extern uint8_t frskyUsrStreaming;
#endif
extern uint8_t link_counter;
#if defined(CPUARM)
enum TelemetryStates {
TELEMETRY_INIT,
TELEMETRY_OK,
TELEMETRY_KO
};
extern uint8_t telemetryState;
#endif
#if defined(CPUARM)
#define TELEMETRY_AVERAGE_COUNT 3 // we actually average one more reading!
#define RAW_FRSKY_MINMAX(v) v.values[TELEMETRY_AVERAGE_COUNT-1]
class FrskyValueWithMin {
public:
uint8_t value; // fitered value (average of last TELEMETRY_AVERAGE_COUNT+1 values)
uint8_t min;
uint8_t values[TELEMETRY_AVERAGE_COUNT];
void set(uint8_t value);
void reset();
};
#else
#define RAW_FRSKY_MINMAX(v) v.value
class FrskyValueWithMin {
public:
uint8_t value;
uint8_t min;
uint16_t sum;
void set(uint8_t value);
};
#endif
class FrskyValueWithMinMax: public FrskyValueWithMin {
public:
uint8_t max;
void set(uint8_t value, uint8_t unit);
};
#if defined(CPUARM)
#elif defined(FRSKY_HUB)
PACK(struct FrskySerialData {
#if !defined(CPUARM)
#if defined(FRSKY_HUB)
PACK(struct FrskyTelemetryData {
int16_t baroAltitudeOffset; // spare reused
int16_t gpsAltitude_bp; // 0x01 before punct
int16_t temperature1; // 0x02 -20 .. 250 deg. celcius
@ -291,7 +241,7 @@ PACK(struct FrskySerialData {
int16_t dTE;
});
#elif defined(WS_HOW_HIGH)
PACK(struct FrskySerialData {
PACK(struct FrskyTelemetryData {
int16_t baroAltitude_bp; // 0..9,999 meters
int16_t baroAltitudeOffset;
int16_t minAltitude;
@ -305,7 +255,7 @@ PACK(struct FrskySerialData {
#endif
});
#elif defined(VARIO)
PACK(struct FrskySerialData {
PACK(struct FrskyTelemetryData {
int16_t varioSpeed; // Vertical speed in cm/s
uint16_t currentConsumption;
uint16_t currentPrescale;
@ -313,41 +263,17 @@ PACK(struct FrskySerialData {
uint16_t maxPower;
});
#else
PACK(struct FrskySerialData {
PACK(struct FrskyTelemetryData {
uint16_t currentConsumption;
uint16_t currentPrescale;
uint16_t power;
uint16_t maxPower;
});
#endif
enum TelemAnas {
TELEM_ANA_A1,
TELEM_ANA_A2,
#if defined(CPUARM)
TELEM_ANA_A3,
TELEM_ANA_A4,
#endif
TELEM_ANA_COUNT
};
#if defined(CPUARM)
struct FrskyData {
FrskyValueWithMin swr; // TODO Min not needed
FrskyValueWithMin rssi; // TODO Min not needed
uint16_t xjtVersion;
bool varioHighPrecision;
};
#else
struct FrskyData {
FrskyValueWithMinMax analog[TELEM_ANA_COUNT];
FrskyValueWithMin rssi[2];
FrskySerialData hub;
};
#endif
#if defined(PCBTARANIS) && defined(REVPLUS)
#define IS_VALID_XJT_VERSION() (frskyData.xjtVersion != 0 && frskyData.xjtVersion != 0xff)
#define IS_VALID_XJT_VERSION() (telemetryData.xjtVersion != 0 && telemetryData.xjtVersion != 0xff)
#else
#define IS_VALID_XJT_VERSION() (1)
#endif
@ -364,101 +290,99 @@ enum AlarmLevel {
#define ALARM_LEVEL(channel, alarm) ((g_model.frsky.channels[channel].alarms_level >> (2*alarm)) & 3)
#if defined(CPUARM)
#define TELEMETRY_STREAMING() (frskyData.rssi.value > 0)
#define TELEMETRY_RSSI() (frskyData.rssi.value)
#define TELEMETRY_RSSI_MIN() (frskyData.rssi.min)
#define TELEMETRY_STREAMING() (telemetryData.rssi.value > 0)
#define TELEMETRY_RSSI() (telemetryData.rssi.value)
#define TELEMETRY_RSSI_MIN() (telemetryData.rssi.min)
#define TELEMETRY_CELL_VOLTAGE_MUTLIPLIER 1
#define TELEMETRY_GPS_SPEED_BP frskyData.hub.gpsSpeed_bp
#define TELEMETRY_GPS_SPEED_AP frskyData.hub.gpsSpeed_ap
#define TELEMETRY_GPS_SPEED_BP telemetryData.hub.gpsSpeed_bp
#define TELEMETRY_GPS_SPEED_AP telemetryData.hub.gpsSpeed_ap
#define TELEMETRY_ABSOLUTE_GPS_ALT (frskyData.hub.gpsAltitude)
#define TELEMETRY_RELATIVE_GPS_ALT (frskyData.hub.gpsAltitude + frskyData.hub.gpsAltitudeOffset)
#define TELEMETRY_ABSOLUTE_GPS_ALT (telemetryData.hub.gpsAltitude)
#define TELEMETRY_RELATIVE_GPS_ALT (telemetryData.hub.gpsAltitude + telemetryData.hub.gpsAltitudeOffset)
#define TELEMETRY_RELATIVE_GPS_ALT_BP (TELEMETRY_RELATIVE_GPS_ALT / 100)
#define TELEMETRY_RELATIVE_BARO_ALT_BP (frskyData.hub.baroAltitude / 100)
#define TELEMETRY_RELATIVE_BARO_ALT_AP (frskyData.hub.baroAltitude % 100)
#define TELEMETRY_RELATIVE_BARO_ALT_BP (telemetryData.hub.baroAltitude / 100)
#define TELEMETRY_RELATIVE_BARO_ALT_AP (telemetryData.hub.baroAltitude % 100)
#define TELEMETRY_BARO_ALT_PREPARE() div_t baroAltitudeDivision = div(getConvertedTelemetryValue(frskyData.hub.baroAltitude, UNIT_DIST), 100)
#define TELEMETRY_BARO_ALT_PREPARE() div_t baroAltitudeDivision = div(getConvertedTelemetryValue(telemetryData.hub.baroAltitude, UNIT_DIST), 100)
#define TELEMETRY_BARO_ALT_FORMAT "%c%d.%02d,"
#define TELEMETRY_BARO_ALT_ARGS frskyData.hub.baroAltitude < 0 ? '-' : ' ', abs(baroAltitudeDivision.quot), abs(baroAltitudeDivision.rem),
#define TELEMETRY_BARO_ALT_ARGS telemetryData.hub.baroAltitude < 0 ? '-' : ' ', abs(baroAltitudeDivision.quot), abs(baroAltitudeDivision.rem),
#define TELEMETRY_GPS_ALT_FORMAT "%c%d.%02d,"
#define TELEMETRY_GPS_ALT_ARGS frskyData.hub.gpsAltitude < 0 ? '-' : ' ', abs(frskyData.hub.gpsAltitude / 100), abs(frskyData.hub.gpsAltitude % 100),
#define TELEMETRY_GPS_ALT_ARGS telemetryData.hub.gpsAltitude < 0 ? '-' : ' ', abs(telemetryData.hub.gpsAltitude / 100), abs(telemetryData.hub.gpsAltitude % 100),
#define TELEMETRY_SPEED_UNIT (IS_IMPERIAL_ENABLE() ? SPEED_UNIT_IMP : SPEED_UNIT_METR)
#define TELEMETRY_GPS_SPEED_FORMAT "%d,"
#define TELEMETRY_GPS_SPEED_ARGS frskyData.hub.gpsSpeed_bp,
#define TELEMETRY_GPS_SPEED_ARGS telemetryData.hub.gpsSpeed_bp,
#if defined(CPUARM)
#define TELEMETRY_CELLS_ARGS frskyData.hub.cellsSum / 10, frskyData.hub.cellsSum % 10, TELEMETRY_CELL_VOLTAGE(0)/100, TELEMETRY_CELL_VOLTAGE(0)%100, TELEMETRY_CELL_VOLTAGE(1)/100, TELEMETRY_CELL_VOLTAGE(1)%100, TELEMETRY_CELL_VOLTAGE(2)/100, TELEMETRY_CELL_VOLTAGE(2)%100, TELEMETRY_CELL_VOLTAGE(3)/100, TELEMETRY_CELL_VOLTAGE(3)%100, TELEMETRY_CELL_VOLTAGE(4)/100, TELEMETRY_CELL_VOLTAGE(4)%100, TELEMETRY_CELL_VOLTAGE(5)/100, TELEMETRY_CELL_VOLTAGE(5)%100, TELEMETRY_CELL_VOLTAGE(6)/100, TELEMETRY_CELL_VOLTAGE(6)%100, TELEMETRY_CELL_VOLTAGE(7)/100, TELEMETRY_CELL_VOLTAGE(7)%100, TELEMETRY_CELL_VOLTAGE(8)/100, TELEMETRY_CELL_VOLTAGE(8)%100, TELEMETRY_CELL_VOLTAGE(9)/100, TELEMETRY_CELL_VOLTAGE(9)%100, TELEMETRY_CELL_VOLTAGE(10)/100, TELEMETRY_CELL_VOLTAGE(10)%100, TELEMETRY_CELL_VOLTAGE(11)/100, TELEMETRY_CELL_VOLTAGE(11)%100,
#define TELEMETRY_CELLS_ARGS telemetryData.hub.cellsSum / 10, telemetryData.hub.cellsSum % 10, TELEMETRY_CELL_VOLTAGE(0)/100, TELEMETRY_CELL_VOLTAGE(0)%100, TELEMETRY_CELL_VOLTAGE(1)/100, TELEMETRY_CELL_VOLTAGE(1)%100, TELEMETRY_CELL_VOLTAGE(2)/100, TELEMETRY_CELL_VOLTAGE(2)%100, TELEMETRY_CELL_VOLTAGE(3)/100, TELEMETRY_CELL_VOLTAGE(3)%100, TELEMETRY_CELL_VOLTAGE(4)/100, TELEMETRY_CELL_VOLTAGE(4)%100, TELEMETRY_CELL_VOLTAGE(5)/100, TELEMETRY_CELL_VOLTAGE(5)%100, TELEMETRY_CELL_VOLTAGE(6)/100, TELEMETRY_CELL_VOLTAGE(6)%100, TELEMETRY_CELL_VOLTAGE(7)/100, TELEMETRY_CELL_VOLTAGE(7)%100, TELEMETRY_CELL_VOLTAGE(8)/100, TELEMETRY_CELL_VOLTAGE(8)%100, TELEMETRY_CELL_VOLTAGE(9)/100, TELEMETRY_CELL_VOLTAGE(9)%100, TELEMETRY_CELL_VOLTAGE(10)/100, TELEMETRY_CELL_VOLTAGE(10)%100, TELEMETRY_CELL_VOLTAGE(11)/100, TELEMETRY_CELL_VOLTAGE(11)%100,
#define TELEMETRY_CELLS_FORMAT "%d.%d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,"
#define TELEMETRY_CELLS_LABEL "Cell volts,Cell 1,Cell 2,Cell 3,Cell 4,Cell 5,Cell 6,Cell 7,Cell 8,Cell 9,Cell 10,Cell 11,Cell 12,"
#else
#define TELEMETRY_CELLS_ARGS frskyData.hub.cellsSum / 10, frskyData.hub.cellsSum % 10, TELEMETRY_CELL_VOLTAGE(0)/100, TELEMETRY_CELL_VOLTAGE(0)%100, TELEMETRY_CELL_VOLTAGE(1)/100, TELEMETRY_CELL_VOLTAGE(1)%100, TELEMETRY_CELL_VOLTAGE(2)/100, TELEMETRY_CELL_VOLTAGE(2)%100, TELEMETRY_CELL_VOLTAGE(3)/100, TELEMETRY_CELL_VOLTAGE(3)%100, TELEMETRY_CELL_VOLTAGE(4)/100, TELEMETRY_CELL_VOLTAGE(4)%100, TELEMETRY_CELL_VOLTAGE(5)/100, TELEMETRY_CELL_VOLTAGE(5)%100,
#define TELEMETRY_CELLS_ARGS telemetryData.hub.cellsSum / 10, telemetryData.hub.cellsSum % 10, TELEMETRY_CELL_VOLTAGE(0)/100, TELEMETRY_CELL_VOLTAGE(0)%100, TELEMETRY_CELL_VOLTAGE(1)/100, TELEMETRY_CELL_VOLTAGE(1)%100, TELEMETRY_CELL_VOLTAGE(2)/100, TELEMETRY_CELL_VOLTAGE(2)%100, TELEMETRY_CELL_VOLTAGE(3)/100, TELEMETRY_CELL_VOLTAGE(3)%100, TELEMETRY_CELL_VOLTAGE(4)/100, TELEMETRY_CELL_VOLTAGE(4)%100, TELEMETRY_CELL_VOLTAGE(5)/100, TELEMETRY_CELL_VOLTAGE(5)%100,
#define TELEMETRY_CELLS_FORMAT "%d.%d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,"
#define TELEMETRY_CELLS_LABEL "Cell volts,Cell 1,Cell 2,Cell 3,Cell 4,Cell 5,Cell 6,"
#endif
#define TELEMETRY_CURRENT_FORMAT "%d.%d,"
#define TELEMETRY_CURRENT_ARGS frskyData.hub.current / 10, frskyData.hub.current % 10,
#define TELEMETRY_CURRENT_ARGS telemetryData.hub.current / 10, telemetryData.hub.current % 10,
#define TELEMETRY_VFAS_FORMAT "%d.%d,"
#define TELEMETRY_VFAS_ARGS frskyData.hub.vfas / 10, frskyData.hub.vfas % 10,
#define TELEMETRY_VFAS_ARGS telemetryData.hub.vfas / 10, telemetryData.hub.vfas % 10,
#define TELEMETRY_VSPEED_FORMAT "%c%d.%02d,"
#define TELEMETRY_VSPEED_ARGS frskyData.hub.varioSpeed < 0 ? '-' : ' ', abs(frskyData.hub.varioSpeed / 100), abs(frskyData.hub.varioSpeed % 100),
#define TELEMETRY_VSPEED_ARGS telemetryData.hub.varioSpeed < 0 ? '-' : ' ', abs(telemetryData.hub.varioSpeed / 100), abs(telemetryData.hub.varioSpeed % 100),
#define TELEMETRY_ASPEED_FORMAT "%d.%d,"
#define TELEMETRY_ASPEED_ARGS frskyData.hub.airSpeed / 10, frskyData.hub.airSpeed % 10,
#define TELEMETRY_ASPEED_ARGS telemetryData.hub.airSpeed / 10, telemetryData.hub.airSpeed % 10,
#define TELEMETRY_OPENXSENSOR() (0)
#else
#define TELEMETRY_STREAMING() (frskyStreaming > 0)
#define TELEMETRY_RSSI() (frskyData.rssi[0].value)
#define TELEMETRY_RSSI_MIN() (frskyData.rssi[0].min)
#define TELEMETRY_STREAMING() (telemetryStreaming > 0)
#define TELEMETRY_RSSI() (telemetryData.rssi[0].value)
#define TELEMETRY_RSSI_MIN() (telemetryData.rssi[0].min)
#define TELEMETRY_CELL_VOLTAGE_MUTLIPLIER 2
#define TELEMETRY_BARO_ALT_AVAILABLE() (frskyData.hub.baroAltitudeOffset)
#define TELEMETRY_BARO_ALT_AVAILABLE() (telemetryData.hub.baroAltitudeOffset)
#define TELEMETRY_BARO_ALT_UNIT (IS_IMPERIAL_ENABLE() ? LENGTH_UNIT_IMP : LENGTH_UNIT_METR)
#define TELEMETRY_RELATIVE_BARO_ALT_BP frskyData.hub.baroAltitude_bp
#define TELEMETRY_RELATIVE_BARO_ALT_AP frskyData.hub.baroAltitude_ap
#define TELEMETRY_RELATIVE_GPS_ALT_BP frskyData.hub.gpsAltitude_bp
#define TELEMETRY_GPS_SPEED_BP frskyData.hub.gpsSpeed_bp
#define TELEMETRY_GPS_SPEED_AP frskyData.hub.gpsSpeed_ap
#define TELEMETRY_RELATIVE_BARO_ALT_BP telemetryData.hub.baroAltitude_bp
#define TELEMETRY_RELATIVE_BARO_ALT_AP telemetryData.hub.baroAltitude_ap
#define TELEMETRY_RELATIVE_GPS_ALT_BP telemetryData.hub.gpsAltitude_bp
#define TELEMETRY_GPS_SPEED_BP telemetryData.hub.gpsSpeed_bp
#define TELEMETRY_GPS_SPEED_AP telemetryData.hub.gpsSpeed_ap
#define TELEMETRY_BARO_ALT_PREPARE()
#define TELEMETRY_BARO_ALT_FORMAT "%d,"
#define TELEMETRY_BARO_ALT_ARGS frskyData.hub.baroAltitude_bp,
#define TELEMETRY_BARO_ALT_ARGS telemetryData.hub.baroAltitude_bp,
#define TELEMETRY_GPS_ALT_FORMAT "%d,"
#define TELEMETRY_GPS_ALT_ARGS frskyData.hub.gpsAltitude_bp,
#define TELEMETRY_GPS_ALT_ARGS telemetryData.hub.gpsAltitude_bp,
#define TELEMETRY_SPEED_UNIT (IS_IMPERIAL_ENABLE() ? SPEED_UNIT_IMP : SPEED_UNIT_METR)
#define TELEMETRY_GPS_SPEED_FORMAT "%d,"
#define TELEMETRY_GPS_SPEED_ARGS frskyData.hub.gpsSpeed_bp,
#define TELEMETRY_GPS_SPEED_ARGS telemetryData.hub.gpsSpeed_bp,
#if defined(CPUARM)
#define TELEMETRY_CELLS_ARGS frskyData.hub.cellsSum / 10, frskyData.hub.cellsSum % 10, frskyData.hub.cellVolts[0]*2/100, frskyData.hub.cellVolts[0]*2%100, frskyData.hub.cellVolts[1]*2/100, frskyData.hub.cellVolts[1]*2%100, frskyData.hub.cellVolts[2]*2/100, frskyData.hub.cellVolts[2]*2%100, frskyData.hub.cellVolts[3]*2/100, frskyData.hub.cellVolts[3]*2%100, frskyData.hub.cellVolts[4]*2/100, frskyData.hub.cellVolts[4]*2%100, frskyData.hub.cellVolts[5]*2/100, frskyData.hub.cellVolts[5]*2%100, frskyData.hub.cellVolts[6]*2/100, frskyData.hub.cellVolts[6]*2%100, frskyData.hub.cellVolts[7]*2/100, frskyData.hub.cellVolts[7]*2%100, frskyData.hub.cellVolts[8]*2/100, frskyData.hub.cellVolts[8]*2%100, frskyData.hub.cellVolts[9]*2/100, frskyData.hub.cellVolts[9]*2%100, frskyData.hub.cellVolts[10]*2/100, frskyData.hub.cellVolts[10]*2%100, frskyData.hub.cellVolts[11]*2/100, frskyData.hub.cellVolts[11]*2%100,
#define TELEMETRY_CELLS_ARGS telemetryData.hub.cellsSum / 10, telemetryData.hub.cellsSum % 10, telemetryData.hub.cellVolts[0]*2/100, telemetryData.hub.cellVolts[0]*2%100, telemetryData.hub.cellVolts[1]*2/100, telemetryData.hub.cellVolts[1]*2%100, telemetryData.hub.cellVolts[2]*2/100, telemetryData.hub.cellVolts[2]*2%100, telemetryData.hub.cellVolts[3]*2/100, telemetryData.hub.cellVolts[3]*2%100, telemetryData.hub.cellVolts[4]*2/100, telemetryData.hub.cellVolts[4]*2%100, telemetryData.hub.cellVolts[5]*2/100, telemetryData.hub.cellVolts[5]*2%100, telemetryData.hub.cellVolts[6]*2/100, telemetryData.hub.cellVolts[6]*2%100, telemetryData.hub.cellVolts[7]*2/100, telemetryData.hub.cellVolts[7]*2%100, telemetryData.hub.cellVolts[8]*2/100, telemetryData.hub.cellVolts[8]*2%100, telemetryData.hub.cellVolts[9]*2/100, telemetryData.hub.cellVolts[9]*2%100, telemetryData.hub.cellVolts[10]*2/100, telemetryData.hub.cellVolts[10]*2%100, telemetryData.hub.cellVolts[11]*2/100, telemetryData.hub.cellVolts[11]*2%100,
#define TELEMETRY_CELLS_FORMAT "%d.%d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,"
#define TELEMETRY_CELLS_LABEL "Cell volts,Cell 1,Cell 2,Cell 3,Cell 4,Cell 5,Cell 6,Cell 7,Cell 8,Cell 9,Cell 10,Cell 11,Cell 12,"
#else
#define TELEMETRY_CELLS_ARGS frskyData.hub.cellsSum / 10, frskyData.hub.cellsSum % 10, frskyData.hub.cellVolts[0]*2/100, frskyData.hub.cellVolts[0]*2%100, frskyData.hub.cellVolts[1]*2/100, frskyData.hub.cellVolts[1]*2%100, frskyData.hub.cellVolts[2]*2/100, frskyData.hub.cellVolts[2]*2%100, frskyData.hub.cellVolts[3]*2/100, frskyData.hub.cellVolts[3]*2%100, frskyData.hub.cellVolts[4]*2/100, frskyData.hub.cellVolts[4]*2%100, frskyData.hub.cellVolts[5]*2/100, frskyData.hub.cellVolts[5]*2%100,
#define TELEMETRY_CELLS_ARGS telemetryData.hub.cellsSum / 10, telemetryData.hub.cellsSum % 10, telemetryData.hub.cellVolts[0]*2/100, telemetryData.hub.cellVolts[0]*2%100, telemetryData.hub.cellVolts[1]*2/100, telemetryData.hub.cellVolts[1]*2%100, telemetryData.hub.cellVolts[2]*2/100, telemetryData.hub.cellVolts[2]*2%100, telemetryData.hub.cellVolts[3]*2/100, telemetryData.hub.cellVolts[3]*2%100, telemetryData.hub.cellVolts[4]*2/100, telemetryData.hub.cellVolts[4]*2%100, telemetryData.hub.cellVolts[5]*2/100, telemetryData.hub.cellVolts[5]*2%100,
#define TELEMETRY_CELLS_FORMAT "%d.%d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,"
#define TELEMETRY_CELLS_LABEL "Cell volts,Cell 1,Cell 2,Cell 3,Cell 4,Cell 5,Cell 6,"
#endif
#define TELEMETRY_CURRENT_FORMAT "%d.%02d,"
#define TELEMETRY_CURRENT_ARGS frskyData.hub.current / 100, frskyData.hub.current % 100,
#define TELEMETRY_CURRENT_ARGS telemetryData.hub.current / 100, telemetryData.hub.current % 100,
#define TELEMETRY_VFAS_FORMAT "%d.%d,"
#define TELEMETRY_VFAS_ARGS frskyData.hub.vfas / 10, frskyData.hub.vfas % 10,
#define TELEMETRY_VFAS_ARGS telemetryData.hub.vfas / 10, telemetryData.hub.vfas % 10,
#define TELEMETRY_VSPEED_FORMAT "%c%d.%02d,"
#define TELEMETRY_VSPEED_ARGS frskyData.hub.varioSpeed < 0 ? '-' : ' ', frskyData.hub.varioSpeed / 100, frskyData.hub.varioSpeed % 100,
#define TELEMETRY_VSPEED_ARGS telemetryData.hub.varioSpeed < 0 ? '-' : ' ', telemetryData.hub.varioSpeed / 100, telemetryData.hub.varioSpeed % 100,
#define TELEMETRY_ASPEED_FORMAT "%d.%d,"
#define TELEMETRY_ASPEED_ARGS frskyData.hub.airSpeed / 10, frskyData.hub.airSpeed % 10,
#define TELEMETRY_ASPEED_ARGS telemetryData.hub.airSpeed / 10, telemetryData.hub.airSpeed % 10,
#if defined(FRSKY_HUB)
#define TELEMETRY_OPENXSENSOR() (frskyData.hub.openXsensor)
#define TELEMETRY_OPENXSENSOR() (telemetryData.hub.openXsensor)
#else
#define TELEMETRY_OPENXSENSOR() (0)
#endif
#endif
#define TELEMETRY_CELL_VOLTAGE(k) (frskyData.hub.cellVolts[k] * TELEMETRY_CELL_VOLTAGE_MUTLIPLIER)
#define TELEMETRY_MIN_CELL_VOLTAGE (frskyData.hub.minCellVolts * TELEMETRY_CELL_VOLTAGE_MUTLIPLIER)
extern FrskyData frskyData;
#define TELEMETRY_CELL_VOLTAGE(k) (telemetryData.hub.cellVolts[k] * TELEMETRY_CELL_VOLTAGE_MUTLIPLIER)
#define TELEMETRY_MIN_CELL_VOLTAGE (telemetryData.hub.minCellVolts * TELEMETRY_CELL_VOLTAGE_MUTLIPLIER)
#define START_STOP 0x7e
#define BYTESTUFF 0x7d
@ -476,6 +400,8 @@ enum FrSkyDataState {
#endif
};
#define FRSKY_RX_PACKET_SIZE 19
#if defined(CPUARM)
#define frskySendAlarms()
#else
@ -523,6 +449,39 @@ void telemetryInit(void);
void telemetryInterrupt10ms();
enum TelemetryProtocol
{
TELEM_PROTO_FRSKY_D,
TELEM_PROTO_FRSKY_SPORT,
};
enum TelemAnas {
TELEM_ANA_A1,
TELEM_ANA_A2,
#if defined(CPUARM)
TELEM_ANA_A3,
TELEM_ANA_A4,
#endif
TELEM_ANA_COUNT
};
#if defined(CPUARM)
struct TelemetryData {
TelemetryValueWithMin swr; // TODO Min not needed
TelemetryValueWithMin rssi; // TODO Min not needed
uint16_t xjtVersion;
bool varioHighPrecision;
};
#else
struct TelemetryData {
TelemetryValueWithMinMax analog[TELEM_ANA_COUNT];
TelemetryValueWithMin rssi[2];
FrskyTelemetryData hub;
};
#endif
extern TelemetryData telemetryData;
#if defined(CPUARM)
typedef uint16_t frskyCellVoltage_t;
#elif defined(FRSKY_HUB)
@ -535,7 +494,7 @@ void frskySetCellVoltage(uint8_t battnumber, frskyCellVoltage_t cellVolts);
void frskyUpdateCells();
#endif
void processSerialData(uint8_t data);
void processFrskyTelemetryData(uint8_t data);
#if defined(PCBTARANIS)
inline uint8_t modelTelemetryProtocol()

188
radio/src/telemetry/frsky_d.cpp
View file

@ -23,10 +23,10 @@
#if (defined(FRSKY_HUB) || defined(WS_HOW_HIGH))
void checkMinMaxAltitude()
{
if (TELEMETRY_RELATIVE_BARO_ALT_BP > frskyData.hub.maxAltitude)
frskyData.hub.maxAltitude = TELEMETRY_RELATIVE_BARO_ALT_BP;
if (TELEMETRY_RELATIVE_BARO_ALT_BP < frskyData.hub.minAltitude)
frskyData.hub.minAltitude = TELEMETRY_RELATIVE_BARO_ALT_BP;
if (TELEMETRY_RELATIVE_BARO_ALT_BP > telemetryData.hub.maxAltitude)
telemetryData.hub.maxAltitude = TELEMETRY_RELATIVE_BARO_ALT_BP;
if (TELEMETRY_RELATIVE_BARO_ALT_BP < telemetryData.hub.minAltitude)
telemetryData.hub.minAltitude = TELEMETRY_RELATIVE_BARO_ALT_BP;
}
#endif
@ -71,148 +71,148 @@ void parseTelemHubByte(uint8_t byte)
state = TS_IDLE;
#if defined(GPS)
if ((uint8_t)structPos == offsetof(FrskySerialData, gpsLatitude_bp)) {
if ((uint8_t)structPos == offsetof(FrskyTelemetryData, gpsLatitude_bp)) {
if (lowByte || byte)
frskyData.hub.gpsFix = 1;
else if (frskyData.hub.gpsFix > 0 && frskyData.hub.gpsLatitude_bp > 1)
frskyData.hub.gpsFix = 0;
telemetryData.hub.gpsFix = 1;
else if (telemetryData.hub.gpsFix > 0 && telemetryData.hub.gpsLatitude_bp > 1)
telemetryData.hub.gpsFix = 0;
}
else if ((uint8_t)structPos == offsetof(FrskySerialData, gpsLongitude_bp)) {
else if ((uint8_t)structPos == offsetof(FrskyTelemetryData, gpsLongitude_bp)) {
if (lowByte || byte)
frskyData.hub.gpsFix = 1;
else if (frskyData.hub.gpsFix > 0 && frskyData.hub.gpsLongitude_bp > 1)
frskyData.hub.gpsFix = 0;
telemetryData.hub.gpsFix = 1;
else if (telemetryData.hub.gpsFix > 0 && telemetryData.hub.gpsLongitude_bp > 1)
telemetryData.hub.gpsFix = 0;
}
if ((uint8_t)structPos == offsetof(FrskySerialData, gpsAltitude_bp) ||
((uint8_t)structPos >= offsetof(FrskySerialData, gpsAltitude_ap) && (uint8_t)structPos <= offsetof(FrskySerialData, gpsLatitudeNS) && (uint8_t)structPos != offsetof(FrskySerialData, baroAltitude_bp) && (uint8_t)structPos != offsetof(FrskySerialData, baroAltitude_ap))) {
if ((uint8_t)structPos == offsetof(FrskyTelemetryData, gpsAltitude_bp) ||
((uint8_t)structPos >= offsetof(FrskyTelemetryData, gpsAltitude_ap) && (uint8_t)structPos <= offsetof(FrskyTelemetryData, gpsLatitudeNS) && (uint8_t)structPos != offsetof(FrskyTelemetryData, baroAltitude_bp) && (uint8_t)structPos != offsetof(FrskyTelemetryData, baroAltitude_ap))) {
// If we don't have a fix, we may discard the value
if (frskyData.hub.gpsFix <= 0)
if (telemetryData.hub.gpsFix <= 0)
return;
}
#endif // #if defined(GPS)
((uint8_t*)&frskyData.hub)[structPos] = lowByte;
((uint8_t*)&frskyData.hub)[structPos+1] = byte;
((uint8_t*)&telemetryData.hub)[structPos] = lowByte;
((uint8_t*)&telemetryData.hub)[structPos+1] = byte;
switch ((uint8_t)structPos) {
case offsetof(FrskySerialData, rpm):
frskyData.hub.rpm *= (uint8_t)60/(g_model.frsky.blades+2);
if (frskyData.hub.rpm > frskyData.hub.maxRpm)
frskyData.hub.maxRpm = frskyData.hub.rpm;
case offsetof(FrskyTelemetryData, rpm):
telemetryData.hub.rpm *= (uint8_t)60/(g_model.frsky.blades+2);
if (telemetryData.hub.rpm > telemetryData.hub.maxRpm)
telemetryData.hub.maxRpm = telemetryData.hub.rpm;
break;
case offsetof(FrskySerialData, temperature1):
if (frskyData.hub.temperature1 > frskyData.hub.maxTemperature1)
frskyData.hub.maxTemperature1 = frskyData.hub.temperature1;
case offsetof(FrskyTelemetryData, temperature1):
if (telemetryData.hub.temperature1 > telemetryData.hub.maxTemperature1)
telemetryData.hub.maxTemperature1 = telemetryData.hub.temperature1;
break;
case offsetof(FrskySerialData, temperature2):
if (frskyData.hub.temperature2 > frskyData.hub.maxTemperature2)
frskyData.hub.maxTemperature2 = frskyData.hub.temperature2;
case offsetof(FrskyTelemetryData, temperature2):
if (telemetryData.hub.temperature2 > telemetryData.hub.maxTemperature2)
telemetryData.hub.maxTemperature2 = telemetryData.hub.temperature2;
break;
case offsetof(FrskySerialData, current):
case offsetof(FrskyTelemetryData, current):
#if defined(FAS_OFFSET) || !defined(CPUM64)
if ((int16_t)frskyData.hub.current > 0 && ((int16_t)frskyData.hub.current + g_model.frsky.fasOffset) > 0)
frskyData.hub.current += g_model.frsky.fasOffset;
if ((int16_t)telemetryData.hub.current > 0 && ((int16_t)telemetryData.hub.current + g_model.frsky.fasOffset) > 0)
telemetryData.hub.current += g_model.frsky.fasOffset;
else
frskyData.hub.current = 0;
telemetryData.hub.current = 0;
#endif
if (frskyData.hub.current > frskyData.hub.maxCurrent)
frskyData.hub.maxCurrent = frskyData.hub.current;
if (telemetryData.hub.current > telemetryData.hub.maxCurrent)
telemetryData.hub.maxCurrent = telemetryData.hub.current;
break;
case offsetof(FrskySerialData, currentConsumption):
case offsetof(FrskyTelemetryData, currentConsumption):
// we receive data from openXsensor. stops the calculation of consumption and power
frskyData.hub.openXsensor = 1;
telemetryData.hub.openXsensor = 1;
break;
case offsetof(FrskySerialData, volts_ap):
case offsetof(FrskyTelemetryData, volts_ap):
#if defined(FAS_BSS)
frskyData.hub.vfas = (frskyData.hub.volts_bp * 10 + frskyData.hub.volts_ap);
telemetryData.hub.vfas = (telemetryData.hub.volts_bp * 10 + telemetryData.hub.volts_ap);
#else
frskyData.hub.vfas = ((frskyData.hub.volts_bp * 100 + frskyData.hub.volts_ap * 10) * 21) / 110;
telemetryData.hub.vfas = ((telemetryData.hub.volts_bp * 100 + telemetryData.hub.volts_ap * 10) * 21) / 110;
#endif
/* TODO later if (!frskyData.hub.minVfas || frskyData.hub.minVfas > frskyData.hub.vfas)
frskyData.hub.minVfas = frskyData.hub.vfas; */
/* TODO later if (!telemetryData.hub.minVfas || telemetryData.hub.minVfas > telemetryData.hub.vfas)
telemetryData.hub.minVfas = telemetryData.hub.vfas; */
break;
case offsetof(FrskySerialData, baroAltitude_bp):
case offsetof(FrskyTelemetryData, baroAltitude_bp):
// First received barometer altitude => Altitude offset
if (!frskyData.hub.baroAltitudeOffset)
frskyData.hub.baroAltitudeOffset = -frskyData.hub.baroAltitude_bp;
frskyData.hub.baroAltitude_bp += frskyData.hub.baroAltitudeOffset;
if (!telemetryData.hub.baroAltitudeOffset)
telemetryData.hub.baroAltitudeOffset = -telemetryData.hub.baroAltitude_bp;
telemetryData.hub.baroAltitude_bp += telemetryData.hub.baroAltitudeOffset;
checkMinMaxAltitude();
break;
#if defined(GPS)
case offsetof(FrskySerialData, gpsAltitude_ap):
if (!frskyData.hub.gpsAltitudeOffset)
frskyData.hub.gpsAltitudeOffset = -frskyData.hub.gpsAltitude_bp;
frskyData.hub.gpsAltitude_bp += frskyData.hub.gpsAltitudeOffset;
if (!frskyData.hub.baroAltitudeOffset) {
if (frskyData.hub.gpsAltitude_bp > frskyData.hub.maxAltitude)
frskyData.hub.maxAltitude = frskyData.hub.gpsAltitude_bp;
if (frskyData.hub.gpsAltitude_bp < frskyData.hub.minAltitude)
frskyData.hub.minAltitude = frskyData.hub.gpsAltitude_bp;
case offsetof(FrskyTelemetryData, gpsAltitude_ap):
if (!telemetryData.hub.gpsAltitudeOffset)
telemetryData.hub.gpsAltitudeOffset = -telemetryData.hub.gpsAltitude_bp;
telemetryData.hub.gpsAltitude_bp += telemetryData.hub.gpsAltitudeOffset;
if (!telemetryData.hub.baroAltitudeOffset) {
if (telemetryData.hub.gpsAltitude_bp > telemetryData.hub.maxAltitude)
telemetryData.hub.maxAltitude = telemetryData.hub.gpsAltitude_bp;
if (telemetryData.hub.gpsAltitude_bp < telemetryData.hub.minAltitude)
telemetryData.hub.minAltitude = telemetryData.hub.gpsAltitude_bp;
}
if (!frskyData.hub.pilotLatitude && !frskyData.hub.pilotLongitude) {
if (!telemetryData.hub.pilotLatitude && !telemetryData.hub.pilotLongitude) {
// First received GPS position => Pilot GPS position
getGpsPilotPosition();
}
else if (frskyData.hub.gpsDistNeeded || menuHandlers[menuLevel] == menuTelemetryFrsky) {
else if (telemetryData.hub.gpsDistNeeded || menuHandlers[menuLevel] == menuTelemetryFrsky) {
getGpsDistance();
}
break;
case offsetof(FrskySerialData, gpsSpeed_bp):
case offsetof(FrskyTelemetryData, gpsSpeed_bp):
// Speed => Max speed
if (frskyData.hub.gpsSpeed_bp > frskyData.hub.maxGpsSpeed)
frskyData.hub.maxGpsSpeed = frskyData.hub.gpsSpeed_bp;
if (telemetryData.hub.gpsSpeed_bp > telemetryData.hub.maxGpsSpeed)
telemetryData.hub.maxGpsSpeed = telemetryData.hub.gpsSpeed_bp;
break;
#endif
case offsetof(FrskySerialData, volts):
case offsetof(FrskyTelemetryData, volts):
frskyUpdateCells();
break;
#if defined(GPS)
case offsetof(FrskySerialData, hour):
frskyData.hub.hour = ((uint8_t)(frskyData.hub.hour + g_eeGeneral.timezone + 24)) % 24;
case offsetof(FrskyTelemetryData, hour):
telemetryData.hub.hour = ((uint8_t)(telemetryData.hub.hour + g_eeGeneral.timezone + 24)) % 24;
break;
#endif
case offsetof(FrskySerialData, accelX):
case offsetof(FrskySerialData, accelY):
case offsetof(FrskySerialData, accelZ):
*(int16_t*)(&((uint8_t*)&frskyData.hub)[structPos]) /= 10;
case offsetof(FrskyTelemetryData, accelX):
case offsetof(FrskyTelemetryData, accelY):
case offsetof(FrskyTelemetryData, accelZ):
*(int16_t*)(&((uint8_t*)&telemetryData.hub)[structPos]) /= 10;
break;
#if 0
case offsetof(FrskySerialData, gpsAltitude_bp):
case offsetof(FrskySerialData, fuelLevel):
case offsetof(FrskySerialData, gpsLongitude_bp):
case offsetof(FrskySerialData, gpsLatitude_bp):
case offsetof(FrskySerialData, gpsCourse_bp):
case offsetof(FrskySerialData, day):
case offsetof(FrskySerialData, year):
case offsetof(FrskySerialData, sec):
case offsetof(FrskySerialData, gpsSpeed_ap):
case offsetof(FrskySerialData, gpsLongitude_ap):
case offsetof(FrskySerialData, gpsLatitude_ap):
case offsetof(FrskySerialData, gpsCourse_ap):
case offsetof(FrskySerialData, gpsLongitudeEW):
case offsetof(FrskySerialData, gpsLatitudeNS):
case offsetof(FrskySerialData, varioSpeed):
case offsetof(FrskySerialData, power): /* because sent by openXsensor */
case offsetof(FrskySerialData, vfas):
case offsetof(FrskySerialData, volts_bp):
case offsetof(FrskyTelemetryData, gpsAltitude_bp):
case offsetof(FrskyTelemetryData, fuelLevel):
case offsetof(FrskyTelemetryData, gpsLongitude_bp):
case offsetof(FrskyTelemetryData, gpsLatitude_bp):
case offsetof(FrskyTelemetryData, gpsCourse_bp):
case offsetof(FrskyTelemetryData, day):
case offsetof(FrskyTelemetryData, year):
case offsetof(FrskyTelemetryData, sec):
case offsetof(FrskyTelemetryData, gpsSpeed_ap):
case offsetof(FrskyTelemetryData, gpsLongitude_ap):
case offsetof(FrskyTelemetryData, gpsLatitude_ap):
case offsetof(FrskyTelemetryData, gpsCourse_ap):
case offsetof(FrskyTelemetryData, gpsLongitudeEW):
case offsetof(FrskyTelemetryData, gpsLatitudeNS):
case offsetof(FrskyTelemetryData, varioSpeed):
case offsetof(FrskyTelemetryData, power): /* because sent by openXsensor */
case offsetof(FrskyTelemetryData, vfas):
case offsetof(FrskyTelemetryData, volts_bp):
break;
default:
*((uint16_t *)(((uint8_t*)&frskyData.hub) + structPos)) = previousValue;
*((uint16_t *)(((uint8_t*)&telemetryData.hub) + structPos)) = previousValue;
break;
#endif
}
@ -222,16 +222,16 @@ void parseTelemHubByte(uint8_t byte)
#if defined(WS_HOW_HIGH)
void parseTelemWSHowHighByte(uint8_t byte)
{
if (frskyUsrStreaming < (WSHH_TIMEOUT10ms - 10)) {
((uint8_t*)&frskyData.hub)[offsetof(FrskySerialData, baroAltitude_bp)] = byte;
if (wshhStreaming < (WSHH_TIMEOUT10ms - 10)) {
((uint8_t*)&telemetryData.hub)[offsetof(FrskyTelemetryData, baroAltitude_bp)] = byte;
checkMinMaxAltitude();
}
else {
// At least 100mS passed since last data received
((uint8_t*)&frskyData.hub)[offsetof(FrskySerialData, baroAltitude_bp)+1] = byte;
((uint8_t*)&telemetryData.hub)[offsetof(FrskyTelemetryData, baroAltitude_bp)+1] = byte;
}
// baroAltitude_bp unit here is feet!
frskyUsrStreaming = WSHH_TIMEOUT10ms; // reset counter
wshhStreaming = WSHH_TIMEOUT10ms; // reset counter
}
#endif
@ -242,16 +242,16 @@ void frskyDProcessPacket(uint8_t *packet)
{
case LINKPKT: // A1/A2/RSSI values
{
frskyData.analog[TELEM_ANA_A1].set(packet[1], g_model.frsky.channels[TELEM_ANA_A1].type);
frskyData.analog[TELEM_ANA_A2].set(packet[2], g_model.frsky.channels[TELEM_ANA_A2].type);
frskyData.rssi[0].set(packet[3]);
frskyData.rssi[1].set(packet[4] / 2);
frskyStreaming = FRSKY_TIMEOUT10ms; // reset counter only if valid frsky packets are being detected
telemetryData.analog[TELEM_ANA_A1].set(packet[1], g_model.frsky.channels[TELEM_ANA_A1].type);
telemetryData.analog[TELEM_ANA_A2].set(packet[2], g_model.frsky.channels[TELEM_ANA_A2].type);
telemetryData.rssi[0].set(packet[3]);
telemetryData.rssi[1].set(packet[4] / 2);
telemetryStreaming = FRSKY_TIMEOUT10ms; // reset counter only if valid frsky packets are being detected
link_counter += 256 / FRSKY_D_AVERAGING;
#if defined(VARIO)
uint8_t varioSource = g_model.frsky.varioSource - VARIO_SOURCE_A1;
if (varioSource < 2) {
frskyData.hub.varioSpeed = applyChannelRatio(varioSource, frskyData.analog[varioSource].value);
telemetryData.hub.varioSpeed = applyChannelRatio(varioSource, telemetryData.analog[varioSource].value);
}
#endif
break;

8
radio/src/telemetry/frsky_d_arm.cpp
View file

@ -73,8 +73,8 @@ void frskyDProcessPacket(uint8_t *packet)
setTelemetryValue(TELEM_PROTO_FRSKY_D, D_A1_ID, 0, 0, packet[1], UNIT_VOLTS, 0);
setTelemetryValue(TELEM_PROTO_FRSKY_D, D_A2_ID, 0, 0, packet[2], UNIT_VOLTS, 0);
setTelemetryValue(TELEM_PROTO_FRSKY_D, D_RSSI_ID, 0, 0, packet[3], UNIT_RAW, 0);
frskyData.rssi.set(packet[3]);
frskyStreaming = FRSKY_TIMEOUT10ms; // reset counter only if valid frsky packets are being detected
telemetryData.rssi.set(packet[3]);
telemetryStreaming = FRSKY_TIMEOUT10ms; // reset counter only if valid frsky packets are being detected
break;
}
@ -178,8 +178,8 @@ void processHubPacket(uint8_t id, int16_t value)
}
else if (id == BARO_ALT_AP_ID) {
if (lastId == BARO_ALT_BP_ID) {
if (data > 9 || frskyData.varioHighPrecision) {
frskyData.varioHighPrecision = true;
if (data > 9 || telemetryData.varioHighPrecision) {
telemetryData.varioHighPrecision = true;
data /= 10; // map hi precision vario into low precision. Altitude is stored in 0.1m anyways
}
data = (int16_t)lastValue * 10 + (((int16_t)lastValue < 0) ? -data : data);

20
radio/src/telemetry/frsky_sport.cpp
View file

@ -212,32 +212,32 @@ void processSportPacket(uint8_t * packet)
uint32_t data = SPORT_DATA_S32(packet);
if (id == RSSI_ID) {
frskyStreaming = FRSKY_TIMEOUT10ms; // reset counter only if valid frsky packets are being detected
telemetryStreaming = FRSKY_TIMEOUT10ms; // reset counter only if valid frsky packets are being detected
data = SPORT_DATA_U8(packet);
if (data == 0)
frskyData.rssi.reset();
telemetryData.rssi.reset();
else
frskyData.rssi.set(data);
telemetryData.rssi.set(data);
}
#if defined(PCBTARANIS) && defined(REVPLUS)
else if (id == XJT_VERSION_ID) {
frskyData.xjtVersion = HUB_DATA_U16(packet);
telemetryData.xjtVersion = HUB_DATA_U16(packet);
if (!IS_VALID_XJT_VERSION()) {
frskyData.swr.set(0x00);
telemetryData.swr.set(0x00);
}
}
else if (id == SWR_ID) {
if (IS_VALID_XJT_VERSION())
frskyData.swr.set(SPORT_DATA_U8(packet));
telemetryData.swr.set(SPORT_DATA_U8(packet));
else
frskyData.swr.set(0x00);
telemetryData.swr.set(0x00);
}
#else
else if (id == XJT_VERSION_ID) {
frskyData.xjtVersion = HUB_DATA_U16(packet);
telemetryData.xjtVersion = HUB_DATA_U16(packet);
}
else if (id == SWR_ID) {
frskyData.swr.set(SPORT_DATA_U8(packet));
telemetryData.swr.set(SPORT_DATA_U8(packet));
}
#endif
@ -339,7 +339,7 @@ bool sportWaitState(SportUpdateState state, int timeout)
for (int i=timeout/2; i>=0; i--) {
uint8_t byte ;
while (telemetryGetByte(&byte)) {
processSerialData(byte);
processFrskyTelemetryData(byte);
}
if (sportUpdateState == state) {
return true;

54
radio/src/telemetry/mavlink.h
View file

@ -1,29 +1,29 @@
/*
* Copyright (C) OpenTX
*
* Based on code named
* th9x - http://code.google.com/p/th9x
* er9x - http://code.google.com/p/er9x
* gruvin9x - http://code.google.com/p/gruvin9x
*
* License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License version 2 as
* published by the Free Software Foundation.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*/
/*
* Copyright (C) OpenTX
*
* Based on code named
* th9x - http://code.google.com/p/th9x
* er9x - http://code.google.com/p/er9x
* gruvin9x - http://code.google.com/p/gruvin9x
*
* License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License version 2 as
* published by the Free Software Foundation.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*/
/*! \file mavlink.h
* Mavlink include file
*/
#ifndef _MAVLINK_H_
#define _MAVLINK_H_
#ifndef _MAVLINK_H_
#define _MAVLINK_H_
#define MAVLINK_USE_CONVENIENCE_FUNCTIONS
#define MAVLINK_COMM_NUM_BUFFERS 1
@ -32,11 +32,11 @@
#include "targets/common_avr/serial_driver.h"
#include "opentx.h"
extern int8_t mav_heartbeat;
extern int8_t mav_heartbeat;
extern mavlink_system_t mavlink_system;
#define LEN_STATUSTEXT 20
extern char mav_statustext[LEN_STATUSTEXT];
#define LEN_STATUSTEXT 20
extern char mav_statustext[LEN_STATUSTEXT];
extern void SERIAL_start_uart_send();
extern void SERIAL_end_uart_send();
extern void SERIAL_send_uart_bytes(const uint8_t * buf, uint16_t len);
@ -321,5 +321,5 @@ inline uint8_t getPrecisMavlinParamsValue(uint8_t idx) {
void lcd_outdezFloat(uint8_t x, uint8_t y, float val, uint8_t precis, uint8_t mode);
#endif
#endif // _MAVLINK_H_
#endif // _MAVLINK_H_

989
radio/src/telemetry/telemetry.cpp
View file

File diff suppressed because it is too large Load diff

145
radio/src/telemetry/telemetry.h
View file

@ -21,12 +21,41 @@
#ifndef _TELEMETRY_H_
#define _TELEMETRY_H_
enum TelemetryProtocol
{
TELEM_PROTO_FRSKY_D,
TELEM_PROTO_FRSKY_SPORT,
};
#if defined (FRSKY)
// FrSky Telemetry
#include "frsky.h"
#elif defined(JETI)
// Jeti-DUPLEX Telemetry
#include "jeti.h"
#elif defined(ARDUPILOT)
// ArduPilot Telemetry
#include "ardupilot.h"
#elif defined(NMEA)
// NMEA Telemetry
#include "nmea.h"
#elif defined(MAVLINK)
// Mavlink Telemetry
#include "mavlink.h"
#endif
extern uint8_t telemetryStreaming; // >0 (true) == data is streaming in. 0 = no data detected for some time
#if defined(WS_HOW_HIGH)
extern uint8_t wshhStreaming;
#endif
extern uint8_t link_counter;
#if defined(CPUARM)
enum TelemetryStates {
TELEMETRY_INIT,
TELEMETRY_OK,
TELEMETRY_KO
};
extern uint8_t telemetryState;
#endif
#if defined(CPUARM)
#define TELEMETRY_VALUE_TIMER_CYCLE 200 /*20 seconds*/
#define TELEMETRY_VALUE_OLD_THRESHOLD 150 /*15 seconds*/
#define TELEMETRY_VALUE_UNAVAILABLE 255
@ -60,108 +89,32 @@ PACK(struct CellValue
}
});
class TelemetryItem
{
public:
union {
int32_t value; // value, stored as uint32_t but interpreted accordingly to type
uint32_t distFromEarthAxis;
};
union {
int32_t valueMin; // min store
uint32_t pilotLongitude;
};
union {
int32_t valueMax; // max store
uint32_t pilotLatitude;
};
uint8_t lastReceived; // for detection of sensor loss
union {
struct {
int32_t offsetAuto;
int32_t filterValues[TELEMETRY_AVERAGE_COUNT];
} std;
struct {
uint16_t prescale;
} consumption;
struct {
uint8_t count;
CellValue values[6];
} cells;
struct {
uint8_t datestate;
uint16_t year;
uint8_t month;
uint8_t day;
uint8_t timestate;
uint8_t hour;
uint8_t min;
uint8_t sec;
} datetime;
struct {
uint16_t longitude_bp;
uint16_t longitude_ap;
char longitudeEW;
uint16_t latitude_bp;
uint16_t latitude_ap;
char latitudeNS;
// pilot longitude is stored in min
// pilot latitude is stored in max
// distFromEarthAxis is stored in value
void extractLatitudeLongitude(uint32_t * latitude, uint32_t * longitude)
{
div_t qr = div(latitude_bp, 100);
*latitude = ((uint32_t)(qr.quot) * 1000000) + (((uint32_t)(qr.rem) * 10000 + latitude_ap) * 5) / 3;
qr = div(longitude_bp, 100);
*longitude = ((uint32_t)(qr.quot) * 1000000) + (((uint32_t)(qr.rem) * 10000 + longitude_ap) * 5) / 3;
}
} gps;
};
static uint8_t now()
{
return (get_tmr10ms() / 10) % TELEMETRY_VALUE_TIMER_CYCLE;
}
TelemetryItem()
{
clear();
}
void clear()
{
memset(this, 0, sizeof(*this));
lastReceived = TELEMETRY_VALUE_UNAVAILABLE;
}
void eval(const TelemetrySensor & sensor);
void per10ms(const TelemetrySensor & sensor);
void setValue(const TelemetrySensor & sensor, int32_t newVal, uint32_t unit, uint32_t prec=0);
bool isAvailable();
bool isFresh();
bool isOld();
void gpsReceived();
};
extern TelemetryItem telemetryItems[MAX_SENSORS];
extern uint8_t allowNewSensors;
void setTelemetryValue(TelemetryProtocol protocol, uint16_t id, uint8_t subId, uint8_t instance, int32_t value, uint32_t unit, uint32_t prec);
void delTelemetryIndex(uint8_t index);
int availableTelemetryIndex();
int lastUsedTelemetryIndex();
int32_t getTelemetryValue(uint8_t index, uint8_t & prec);
int32_t convertTelemetryValue(int32_t value, uint8_t unit, uint8_t prec, uint8_t destUnit, uint8_t destPrec);
void frskySportSetDefault(int index, uint16_t id, uint8_t subId, uint8_t instance);
void frskyDSetDefault(int index, uint16_t id);
#endif
#define IS_DISTANCE_UNIT(unit) ((unit) == UNIT_METERS || (unit) == UNIT_FEET)
#define IS_SPEED_UNIT(unit) ((unit) >= UNIT_KTS && (unit) <= UNIT_MPH)
#if defined(CPUARM)
extern uint8_t telemetryProtocol;
#define IS_FRSKY_D_PROTOCOL() (telemetryProtocol == PROTOCOL_FRSKY_D)
#define IS_FRSKY_SPORT_PROTOCOL() (telemetryProtocol == PROTOCOL_FRSKY_SPORT)
#else
#define IS_FRSKY_D_PROTOCOL() (true)
#define IS_FRSKY_SPORT_PROTOCOL() (false)
#endif
#if defined(CPUARM)
#include "telemetry_sensors.h"
#endif
#endif // _TELEMETRY_H_

77
radio/src/telemetry/telemetry_holders.cpp Normal file
View file

@ -0,0 +1,77 @@
/*
* Copyright (C) OpenTX
*
* Based on code named
* th9x - http://code.google.com/p/th9x
* er9x - http://code.google.com/p/er9x
* gruvin9x - http://code.google.com/p/gruvin9x
*
* License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License version 2 as
* published by the Free Software Foundation.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*/
#include "opentx.h"
#if defined(CPUARM)
void TelemetryValueWithMin::reset()
{
memclear(this, sizeof(*this));
}
#endif
void TelemetryValueWithMin::set(uint8_t value)
{
#if defined(CPUARM)
if (this->value == 0) {
memset(values, value, TELEMETRY_AVERAGE_COUNT);
this->value = value;
}
else {
//calculate the average from values[] and value
//also shift readings in values [] array
unsigned int sum = values[0];
for (int i=0; i<TELEMETRY_AVERAGE_COUNT-1; i++) {
uint8_t tmp = values[i+1];
values[i] = tmp;
sum += tmp;
}
values[TELEMETRY_AVERAGE_COUNT-1] = value;
sum += value;
this->value = sum/(TELEMETRY_AVERAGE_COUNT+1);
}
#else
if (this->value == 0) {
this->value = value;
}
else {
sum += value;
if (link_counter == 0) {
this->value = sum / (IS_FRSKY_D_PROTOCOL() ? FRSKY_D_AVERAGING : FRSKY_SPORT_AVERAGING);
sum = 0;
}
}
#endif
if (!min || value < min) {
min = value;
}
}
void TelemetryValueWithMinMax::set(uint8_t value, uint8_t unit)
{
TelemetryValueWithMin::set(value);
if (unit != UNIT_VOLTS) {
this->value = value;
}
if (!max || value > max) {
max = value;
}
}

54
radio/src/telemetry/telemetry_holders.h Normal file
View file

@ -0,0 +1,54 @@
/*
* Copyright (C) OpenTX
*
* Based on code named
* th9x - http://code.google.com/p/th9x
* er9x - http://code.google.com/p/er9x
* gruvin9x - http://code.google.com/p/gruvin9x
*
* License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License version 2 as
* published by the Free Software Foundation.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*/
#ifndef _TELEMETRY_HOLDERS_H_
#define _TELEMETRY_HOLDERS_H_
#include <inttypes.h>
#if defined(CPUARM)
#define TELEMETRY_AVERAGE_COUNT 3 // we actually average one more reading!
#define RAW_FRSKY_MINMAX(v) v.values[TELEMETRY_AVERAGE_COUNT-1]
class TelemetryValueWithMin {
public:
uint8_t value; // fitered value (average of last TELEMETRY_AVERAGE_COUNT+1 values)
uint8_t min;
uint8_t values[TELEMETRY_AVERAGE_COUNT];
void set(uint8_t value);
void reset();
};
#else
#define RAW_FRSKY_MINMAX(v) v.value
class TelemetryValueWithMin {
public:
uint8_t value;
uint8_t min;
uint16_t sum;
void set(uint8_t value);
};
#endif
class TelemetryValueWithMinMax: public TelemetryValueWithMin {
public:
uint8_t max;
void set(uint8_t value, uint8_t unit);
};
#endif // _TELEMETRY_HOLDERS_H_

693
radio/src/telemetry/telemetry_sensors.cpp Normal file
View file

@ -0,0 +1,693 @@
/*
* Copyright (C) OpenTX
*
* Based on code named
* th9x - http://code.google.com/p/th9x
* er9x - http://code.google.com/p/er9x
* gruvin9x - http://code.google.com/p/gruvin9x
*
* License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License version 2 as
* published by the Free Software Foundation.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*/
#include "opentx.h"
TelemetryItem telemetryItems[MAX_SENSORS];
uint8_t allowNewSensors;
void TelemetryItem::gpsReceived()
{
if (!distFromEarthAxis) {
gps.extractLatitudeLongitude(&pilotLatitude, &pilotLongitude);
uint32_t lat = pilotLatitude / 10000;
uint32_t angle2 = (lat*lat) / 10000;
uint32_t angle4 = angle2 * angle2;
distFromEarthAxis = 139*(((uint32_t)10000000-((angle2*(uint32_t)123370)/81)+(angle4/25))/12500);
}
lastReceived = now();
}
void TelemetryItem::setValue(const TelemetrySensor & sensor, int32_t val, uint32_t unit, uint32_t prec)
{
int32_t newVal = val;
if (unit == UNIT_CELLS) {
uint32_t data = uint32_t(newVal);
uint8_t cellsCount = (data >> 24);
uint8_t cellIndex = ((data >> 16) & 0x0F);
uint16_t cellValue = (data & 0xFFFF);
if (cellsCount == 0) {
cellsCount = (cellIndex >= cells.count ? cellIndex + 1 : cells.count);
if (cellsCount != cells.count) {
clear();
cells.count = cellsCount;
// we skip this round as we are not sure we received all cells values
return;
}
}
else if (cellsCount != cells.count) {
clear();
cells.count = cellsCount;
}
cells.values[cellIndex].set(cellValue);
if (cellIndex+1 == cells.count) {
newVal = 0;
for (int i=0; i<cellsCount; i++) {
if (cells.values[i].state) {
newVal += cells.values[i].value;
}
else {
// we didn't receive all cells values
return;
}
}
newVal = sensor.getValue(newVal, UNIT_VOLTS, 2);
}
else {
// we didn't receive all cells values
return;
}
}
else if (unit == UNIT_DATETIME) {
uint32_t data = uint32_t(newVal);
if (data & 0x000000ff) {
datetime.year = (uint16_t) ((data & 0xff000000) >> 24);
datetime.month = (uint8_t) ((data & 0x00ff0000) >> 16);
datetime.day = (uint8_t) ((data & 0x0000ff00) >> 8);
if (datetime.year != 0) {
datetime.datestate = 1;
}
#if defined(RTCLOCK)
if (g_eeGeneral.adjustRTC && (datetime.datestate == 1)) {
struct gtm t;
gettime(&t);
t.tm_year = datetime.year+4;
t.tm_mon = datetime.month-1;
t.tm_mday = datetime.day;
rtcSetTime(&t);
}
#endif
}
else {
datetime.hour = ((uint8_t) ((data & 0xff000000) >> 24) + g_eeGeneral.timezone + 24) % 24;
datetime.min = (uint8_t) ((data & 0x00ff0000) >> 16);
datetime.sec = (uint16_t) ((data & 0x0000ff00) >> 8);
if (datetime.datestate == 1) {
datetime.timestate = 1;
}
#if defined(RTCLOCK)
if (g_eeGeneral.adjustRTC && datetime.datestate == 1) {
struct gtm t;
gettime(&t);
if (abs((t.tm_hour-datetime.hour)*3600 + (t.tm_min-datetime.min)*60 + (t.tm_sec-datetime.sec)) > 20) {
// we adjust RTC only if difference is > 20 seconds
t.tm_hour = datetime.hour;
t.tm_min = datetime.min;
t.tm_sec = datetime.sec;
g_rtcTime = gmktime(&t); // update local timestamp and get wday calculated
rtcSetTime(&t);
}
}
#endif
}
if (datetime.year == 0) {
return;
}
newVal = 0;
}
else if (unit == UNIT_GPS) {
uint32_t gps_long_lati_data = uint32_t(newVal);
uint32_t gps_long_lati_b1w, gps_long_lati_a1w;
gps_long_lati_b1w = (gps_long_lati_data & 0x3fffffff) / 10000;
gps_long_lati_a1w = (gps_long_lati_data & 0x3fffffff) % 10000;
switch ((gps_long_lati_data & 0xc0000000) >> 30) {
case 0:
gps.latitude_bp = (gps_long_lati_b1w / 60 * 100) + (gps_long_lati_b1w % 60);
gps.latitude_ap = gps_long_lati_a1w;
gps.latitudeNS = 'N';
break;
case 1:
gps.latitude_bp = (gps_long_lati_b1w / 60 * 100) + (gps_long_lati_b1w % 60);
gps.latitude_ap = gps_long_lati_a1w;
gps.latitudeNS = 'S';
break;
case 2:
gps.longitude_bp = (gps_long_lati_b1w / 60 * 100) + (gps_long_lati_b1w % 60);
gps.longitude_ap = gps_long_lati_a1w;
gps.longitudeEW = 'E';
break;
case 3:
gps.longitude_bp = (gps_long_lati_b1w / 60 * 100) + (gps_long_lati_b1w % 60);
gps.longitude_ap = gps_long_lati_a1w;
gps.longitudeEW = 'W';
break;
}
if (gps.longitudeEW && gps.latitudeNS) {
gpsReceived();
}
return;
}
else if (unit >= UNIT_GPS_LONGITUDE && unit <= UNIT_GPS_LATITUDE_NS) {
uint32_t data = uint32_t(newVal);
switch (unit) {
case UNIT_GPS_LONGITUDE:
gps.longitude_bp = data >> 16;
gps.longitude_ap = data & 0xFFFF;
break;
case UNIT_GPS_LATITUDE:
gps.latitude_bp = data >> 16;
gps.latitude_ap = data & 0xFFFF;
break;
case UNIT_GPS_LONGITUDE_EW:
gps.longitudeEW = data;
break;
case UNIT_GPS_LATITUDE_NS:
gps.latitudeNS = data;
break;
}
if (gps.longitudeEW && gps.latitudeNS && gps.longitude_ap && gps.latitude_ap) {
gpsReceived();
}
return;
}
else if (unit == UNIT_DATETIME_YEAR) {
datetime.year = newVal;
return;
}
else if (unit == UNIT_DATETIME_DAY_MONTH) {
uint32_t data = uint32_t(newVal);
datetime.month = data >> 8;
datetime.day = data & 0xFF;
datetime.datestate = 1;
return;
}
else if (unit == UNIT_DATETIME_HOUR_MIN) {
uint32_t data = uint32_t(newVal);
datetime.hour = ((data & 0xFF) + g_eeGeneral.timezone + 24) % 24;
datetime.min = data >> 8;
}
else if (unit == UNIT_DATETIME_SEC) {
datetime.sec = newVal & 0xFF;
datetime.timestate = 1;
newVal = 0;
}
else if (unit == UNIT_RPMS) {
if (sensor.custom.ratio != 0) {
newVal = (newVal * sensor.custom.offset) / sensor.custom.ratio;
}
}
else {
newVal = sensor.getValue(newVal, unit, prec);
if (sensor.autoOffset) {
if (!isAvailable()) {
std.offsetAuto = -newVal;
}
newVal += std.offsetAuto;
}
else if (sensor.filter) {
if (!isAvailable()) {
for (int i=0; i<TELEMETRY_AVERAGE_COUNT; i++) {
std.filterValues[i] = newVal;
}
}
else {
// calculate the average from values[] and value
// also shift readings in values [] array
unsigned int sum = std.filterValues[0];
for (int i=0; i<TELEMETRY_AVERAGE_COUNT-1; i++) {
int32_t tmp = std.filterValues[i+1];
std.filterValues[i] = tmp;
sum += tmp;
}
std.filterValues[TELEMETRY_AVERAGE_COUNT-1] = newVal;
sum += newVal;
newVal = sum/(TELEMETRY_AVERAGE_COUNT+1);
}
}
}
if (!isAvailable()) {
valueMin = newVal;
valueMax = newVal;
}
else if (newVal < valueMin) {
valueMin = newVal;
}
else if (newVal > valueMax) {
valueMax = newVal;
if (sensor.unit == UNIT_VOLTS) {
valueMin = newVal; // the batt was changed
}
}
for (int i=0; i<MAX_SENSORS; i++) {
TelemetrySensor & it = g_model.telemetrySensors[i];
if (it.type == TELEM_TYPE_CALCULATED && it.formula == TELEM_FORMULA_TOTALIZE && &g_model.telemetrySensors[it.consumption.source-1] == &sensor) {
TelemetryItem & item = telemetryItems[i];
int32_t increment = it.getValue(val, unit, prec);
item.setValue(it, item.value+increment, it.unit, it.prec);
}
}
value = newVal;
lastReceived = now();
}
bool TelemetryItem::isAvailable()
{
return (lastReceived != TELEMETRY_VALUE_UNAVAILABLE);
}
bool TelemetryItem::isFresh()
{
return (lastReceived < TELEMETRY_VALUE_TIMER_CYCLE) && (uint8_t(now() - lastReceived) < 2);
}
bool TelemetryItem::isOld()
{
return (lastReceived == TELEMETRY_VALUE_OLD);
}
void TelemetryItem::per10ms(const TelemetrySensor & sensor)
{
switch (sensor.formula) {
case TELEM_FORMULA_CONSUMPTION:
if (sensor.consumption.source) {
TelemetrySensor & currentSensor = g_model.telemetrySensors[sensor.consumption.source-1];
TelemetryItem & currentItem = telemetryItems[sensor.consumption.source-1];
if (!currentItem.isAvailable()) {
return;
}
else if (currentItem.isOld()) {
lastReceived = TELEMETRY_VALUE_OLD;
return;
}
int32_t current = convertTelemetryValue(currentItem.value, currentSensor.unit, currentSensor.prec, UNIT_AMPS, 1);
currentItem.consumption.prescale += current;
if (currentItem.consumption.prescale >= 3600) {
currentItem.consumption.prescale -= 3600;
setValue(sensor, value+1, sensor.unit, sensor.prec);
}
lastReceived = now();
}
break;
default:
break;
}
}
void TelemetryItem::eval(const TelemetrySensor & sensor)
{
switch (sensor.formula) {
case TELEM_FORMULA_CELL:
if (sensor.cell.source) {
TelemetryItem & cellsItem = telemetryItems[sensor.cell.source-1];
if (cellsItem.isOld()) {
lastReceived = TELEMETRY_VALUE_OLD;
}
else {
unsigned int index = sensor.cell.index;
if (index == TELEM_CELL_INDEX_LOWEST || index == TELEM_CELL_INDEX_HIGHEST || index == TELEM_CELL_INDEX_DELTA) {
unsigned int lowest=0, highest=0;
for (int i=0; i<cellsItem.cells.count; i++) {
if (cellsItem.cells.values[i].state) {
if (!lowest || cellsItem.cells.values[i].value < cellsItem.cells.values[lowest-1].value)
lowest = i+1;
if (!highest || cellsItem.cells.values[i].value > cellsItem.cells.values[highest-1].value)
highest = i+1;
}
else {
lowest = highest = 0;
}
}
if (lowest) {
switch (index) {
case TELEM_CELL_INDEX_LOWEST:
setValue(sensor, cellsItem.cells.values[lowest-1].value, UNIT_VOLTS, 2);
break;
case TELEM_CELL_INDEX_HIGHEST:
setValue(sensor, cellsItem.cells.values[highest-1].value, UNIT_VOLTS, 2);
break;
case TELEM_CELL_INDEX_DELTA:
setValue(sensor, cellsItem.cells.values[highest-1].value - cellsItem.cells.values[lowest-1].value, UNIT_VOLTS, 2);
break;
}
}
}
else {
index -= 1;
if (index < cellsItem.cells.count && cellsItem.cells.values[index].state) {
setValue(sensor, cellsItem.cells.values[index].value, UNIT_VOLTS, 2);
}
}
}
}
break;
case TELEM_FORMULA_DIST:
if (sensor.dist.gps) {
TelemetryItem gpsItem = telemetryItems[sensor.dist.gps-1];
TelemetryItem * altItem = NULL;
if (!gpsItem.isAvailable()) {
return;
}
else if (gpsItem.isOld()) {
lastReceived = TELEMETRY_VALUE_OLD;
return;
}
if (sensor.dist.alt) {
altItem = &telemetryItems[sensor.dist.alt-1];
if (!altItem->isAvailable()) {
return;
}
else if (altItem->isOld()) {
lastReceived = TELEMETRY_VALUE_OLD;
return;
}
}
uint32_t latitude, longitude;
gpsItem.gps.extractLatitudeLongitude(&latitude, &longitude);
uint32_t angle = (latitude > gpsItem.pilotLatitude) ? latitude - gpsItem.pilotLatitude : gpsItem.pilotLatitude - latitude;
uint32_t dist = EARTH_RADIUS * angle / 1000000;
uint32_t result = dist*dist;
angle = (longitude > gpsItem.pilotLongitude) ? longitude - gpsItem.pilotLongitude : gpsItem.pilotLongitude - longitude;
dist = gpsItem.distFromEarthAxis * angle / 1000000;
result += dist*dist;
if (altItem) {
dist = abs(altItem->value) / g_model.telemetrySensors[sensor.dist.alt-1].getPrecDivisor();
result += dist*dist;
}
setValue(sensor, isqrt32(result), UNIT_METERS);
}
break;
case TELEM_FORMULA_ADD:
case TELEM_FORMULA_AVERAGE:
case TELEM_FORMULA_MIN:
case TELEM_FORMULA_MAX:
case TELEM_FORMULA_MULTIPLY:
{
int32_t value=0, count=0, available=0, maxitems=4, mulprec=0;
if (sensor.formula == TELEM_FORMULA_MULTIPLY) {
maxitems = 2;
value = 1;
}
for (int i=0; i<maxitems; i++) {
int8_t source = sensor.calc.sources[i];
if (source) {
unsigned int index = abs(source)-1;
TelemetrySensor & telemetrySensor = g_model.telemetrySensors[index];
TelemetryItem & telemetryItem = telemetryItems[index];
if (sensor.formula == TELEM_FORMULA_AVERAGE) {
if (telemetryItem.isAvailable())
available = 1;
else
continue;
if (telemetryItem.isOld())
continue;
}
else {
if (!telemetryItem.isAvailable()) {
return;
}
else if (telemetryItem.isOld()) {
lastReceived = TELEMETRY_VALUE_OLD;
return;
}
}
int32_t sensorValue = telemetryItem.value;
if (source < 0)
sensorValue = -sensorValue;
count += 1;
if (sensor.formula == TELEM_FORMULA_MULTIPLY) {
mulprec += telemetrySensor.prec;
value *= convertTelemetryValue(sensorValue, telemetrySensor.unit, 0, sensor.unit, 0);
}
else {
sensorValue = convertTelemetryValue(sensorValue, telemetrySensor.unit, telemetrySensor.prec, sensor.unit, sensor.prec);
if (sensor.formula == TELEM_FORMULA_MIN)
value = (count==1 ? sensorValue : min<int32_t>(value, sensorValue));
else if (sensor.formula == TELEM_FORMULA_MAX)
value = (count==1 ? sensorValue : max<int32_t>(value, sensorValue));
else
value += sensorValue;
}
}
}
if (sensor.formula == TELEM_FORMULA_AVERAGE) {
if (count == 0) {
if (available)
lastReceived = TELEMETRY_VALUE_OLD;
return;
}
else {
value = (value + count/2) / count;
}
}
else if (sensor.formula == TELEM_FORMULA_MULTIPLY) {
if (count == 0)
return;
value = convertTelemetryValue(value, sensor.unit, mulprec, sensor.unit, sensor.prec);
}
setValue(sensor, value, sensor.unit, sensor.prec);
break;
}
default:
break;
}
}
void delTelemetryIndex(uint8_t index)
{
memclear(&g_model.telemetrySensors[index], sizeof(TelemetrySensor));
telemetryItems[index].clear();
storageDirty(EE_MODEL);
}
int availableTelemetryIndex()
{
for (int index=0; index<MAX_SENSORS; index++) {
TelemetrySensor & telemetrySensor = g_model.telemetrySensors[index];
if (!telemetrySensor.isAvailable()) {
return index;
}
}
return -1;
}
int lastUsedTelemetryIndex()
{
for (int index=MAX_SENSORS-1; index>=0; index--) {
TelemetrySensor & telemetrySensor = g_model.telemetrySensors[index];
if (telemetrySensor.isAvailable()) {
return index;
}
}
return -1;
}
void setTelemetryValue(TelemetryProtocol protocol, uint16_t id, uint8_t subId, uint8_t instance, int32_t value, uint32_t unit, uint32_t prec)
{
bool available = false;
for (int index=0; index<MAX_SENSORS; index++) {
TelemetrySensor & telemetrySensor = g_model.telemetrySensors[index];
if (telemetrySensor.type == TELEM_TYPE_CUSTOM && telemetrySensor.id == id && telemetrySensor.subId == subId && (telemetrySensor.instance == instance || g_model.ignoreSensorIds)) {
telemetryItems[index].setValue(telemetrySensor, value, unit, prec);
available = true;
// we continue search here, because sensors can share the same id and instance
}
}
if (available || !allowNewSensors) {
return;
}
int index = availableTelemetryIndex();
if (index >= 0) {
switch (protocol) {
#if defined(FRSKY_SPORT)
case TELEM_PROTO_FRSKY_SPORT:
frskySportSetDefault(index, id, subId, instance);
break;
#endif
#if defined(FRSKY)
case TELEM_PROTO_FRSKY_D:
frskyDSetDefault(index, id);
break;
#endif
default:
return;
}
telemetryItems[index].setValue(g_model.telemetrySensors[index], value, unit, prec);
}
else {
POPUP_WARNING(STR_TELEMETRYFULL);
}
}
void TelemetrySensor::init(const char * label, uint8_t unit, uint8_t prec)
{
memclear(this->label, TELEM_LABEL_LEN);
strncpy(this->label, label, TELEM_LABEL_LEN);
this->unit = unit;
if (prec > 1 && (IS_DISTANCE_UNIT(unit) || IS_SPEED_UNIT(unit))) {
// 2 digits precision is not needed here
prec = 1;
}
this->prec = prec;
}
void TelemetrySensor::init(uint16_t id)
{
char label[4];
label[0] = hex2zchar((id & 0xf000) >> 12);
label[1] = hex2zchar((id & 0x0f00) >> 8);
label[2] = hex2zchar((id & 0x00f0) >> 4);
label[3] = hex2zchar((id & 0x000f) >> 0);
init(label);
}
bool TelemetrySensor::isAvailable() const
{
return ZLEN(label) > 0;
}
PACK(typedef struct {
uint8_t unitFrom;
uint8_t unitTo;
int16_t multiplier;
int16_t divisor;
}) UnitConversionRule;
const UnitConversionRule unitConversionTable[] = {
/* unitFrom unitTo multiplier divisor */
{ UNIT_METERS, UNIT_FEET, 105, 32},
{ UNIT_METERS_PER_SECOND, UNIT_FEET_PER_SECOND, 105, 32},
{ UNIT_KTS, UNIT_KMH, 1852, 1000}, // 1 knot = 1.85200 kilometers per hour
{ UNIT_KTS, UNIT_MPH, 1151, 1000}, // 1 knot = 1.15077945 miles per hour
{ UNIT_KTS, UNIT_METERS_PER_SECOND, 1000, 1944}, // 1 knot = 0.514444444 meters / second (divide with 1.94384449)
{ UNIT_KTS, UNIT_FEET_PER_SECOND, 1688, 1000}, // 1 knot = 1.68780986 feet per second
{ UNIT_KMH, UNIT_KTS, 1000, 1852}, // 1 km/h = 0.539956803 knots (divide with 1.85200)
{ UNIT_KMH, UNIT_MPH, 1000, 1609}, // 1 km/h = 0.621371192 miles per hour (divide with 1.60934400)
{ UNIT_KMH, UNIT_METERS_PER_SECOND, 10, 36}, // 1 km/h = 0.277777778 meters / second (divide with 3.6)
{ UNIT_KMH, UNIT_FEET_PER_SECOND, 911, 1000}, // 1 km/h = 0.911344415 feet per second
{ UNIT_MILLILITERS, UNIT_FLOZ, 100, 2957},
{ 0, 0, 0, 0} // termination
};
int32_t convertTelemetryValue(int32_t value, uint8_t unit, uint8_t prec, uint8_t destUnit, uint8_t destPrec)
{
for (int i=prec; i<destPrec; i++)
value *= 10;
if (unit == UNIT_CELSIUS) {
if (destUnit == UNIT_FAHRENHEIT) {
// T(°F) = T(°C)×1,8 + 32
value = 32 + (value*18) / 10;
}
}
else {
const UnitConversionRule * p = unitConversionTable;
while (p->divisor) {
if (p->unitFrom == unit && p->unitTo == destUnit) {
value = (value * (int32_t)p->multiplier) / (int32_t)p->divisor;
break;
}
++p;
}
}
for (int i=destPrec; i<prec; i++)
value /= 10;
return value;
}
int32_t TelemetrySensor::getValue(int32_t value, uint8_t unit, uint8_t prec) const
{
if (type == TELEM_TYPE_CUSTOM && custom.ratio) {
if (this->prec == 2) {
value *= 10;
prec = 2;
}
else {
prec = 1;
}
value = (custom.ratio * value + 122) / 255;
}
value = convertTelemetryValue(value, unit, prec, this->unit, this->prec);
if (type == TELEM_TYPE_CUSTOM) {
value += custom.offset;
if (value < 0 && onlyPositive) {
value = 0;
}
}
return value;
}
bool TelemetrySensor::isConfigurable() const
{
if (type == TELEM_TYPE_CALCULATED) {
if (formula >= TELEM_FORMULA_CELL) {
return false;
}
}
else {
if (unit >= UNIT_FIRST_VIRTUAL) {
return false;
}
}
return true;
}
bool TelemetrySensor::isPrecConfigurable() const
{
if (isConfigurable()) {
return true;
}
else if (unit == UNIT_CELLS) {
return true;
}
else {
return false;
}
}
int32_t TelemetrySensor::getPrecMultiplier() const
{
/*
Important: the return type must be signed, otherwise
mathematic operations with a negative telemetry value won't work
*/
if (prec == 2) return 1;
if (prec == 1) return 10;
return 100;
}
int32_t TelemetrySensor::getPrecDivisor() const
{
if (prec == 2) return 100;
if (prec == 1) return 10;
return 1;
}

115
radio/src/telemetry/telemetry_sensors.h Normal file
View file

@ -0,0 +1,115 @@
/*
* Copyright (C) OpenTX
*
* Based on code named
* th9x - http://code.google.com/p/th9x
* er9x - http://code.google.com/p/er9x
* gruvin9x - http://code.google.com/p/gruvin9x
*
* License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License version 2 as
* published by the Free Software Foundation.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*/
#ifndef _TELEMETRY_SENSORS_H_
#define _TELEMETRY_SENSORS_H_
class TelemetryItem
{
public:
union {
int32_t value; // value, stored as uint32_t but interpreted accordingly to type
uint32_t distFromEarthAxis;
};
union {
int32_t valueMin; // min store
uint32_t pilotLongitude;
};
union {
int32_t valueMax; // max store
uint32_t pilotLatitude;
};
uint8_t lastReceived; // for detection of sensor loss
union {
struct {
int32_t offsetAuto;
int32_t filterValues[TELEMETRY_AVERAGE_COUNT];
} std;
struct {
uint16_t prescale;
} consumption;
struct {
uint8_t count;
CellValue values[6];
} cells;
struct {
uint8_t datestate;
uint16_t year;
uint8_t month;
uint8_t day;
uint8_t timestate;
uint8_t hour;
uint8_t min;
uint8_t sec;
} datetime;
struct {
uint16_t longitude_bp;
uint16_t longitude_ap;
char longitudeEW;
uint16_t latitude_bp;
uint16_t latitude_ap;
char latitudeNS;
// pilot longitude is stored in min
// pilot latitude is stored in max
// distFromEarthAxis is stored in value
void extractLatitudeLongitude(uint32_t * latitude, uint32_t * longitude)
{
div_t qr = div(latitude_bp, 100);
*latitude = ((uint32_t)(qr.quot) * 1000000) + (((uint32_t)(qr.rem) * 10000 + latitude_ap) * 5) / 3;
qr = div(longitude_bp, 100);
*longitude = ((uint32_t)(qr.quot) * 1000000) + (((uint32_t)(qr.rem) * 10000 + longitude_ap) * 5) / 3;
}
} gps;
};
static uint8_t now()
{
return (get_tmr10ms() / 10) % TELEMETRY_VALUE_TIMER_CYCLE;
}
TelemetryItem()
{
clear();
}
void clear()
{
memset(this, 0, sizeof(*this));
lastReceived = TELEMETRY_VALUE_UNAVAILABLE;
}
void eval(const TelemetrySensor & sensor);
void per10ms(const TelemetrySensor & sensor);
void setValue(const TelemetrySensor & sensor, int32_t newVal, uint32_t unit, uint32_t prec=0);
bool isAvailable();
bool isFresh();
bool isOld();
void gpsReceived();
};
extern TelemetryItem telemetryItems[MAX_SENSORS];
extern uint8_t allowNewSensors;
#endif // _TELEMETRY_SENSORS_H_

34
radio/src/tests/frsky.cpp
View file

@ -34,7 +34,7 @@ void displayVoltagesScreen();
TEST(FrSky, gpsNfuel)
{
g_model.frsky.usrProto = 1;
frskyData.hub.gpsFix = 1;
telemetryData.hub.gpsFix = 1;
uint8_t pkt1[] = { 0xfd, 0x07, 0x00, 0x5e, 0x14, 0x2c, 0x00, 0x5e, 0x1c, 0x03 };
uint8_t pkt2[] = { 0xfd, 0x07, 0x00, 0x00, 0x5e, 0x13, 0x38, 0x0c, 0x5e, 0x1b };
@ -50,14 +50,14 @@ TEST(FrSky, gpsNfuel)
frskyDProcessPacket(pkt5);
frskyDProcessPacket(pkt6);
frskyDProcessPacket(pkt7);
EXPECT_EQ(frskyData.hub.gpsCourse_bp, 44);
EXPECT_EQ(frskyData.hub.gpsCourse_ap, 03);
EXPECT_EQ(frskyData.hub.gpsLongitude_bp / 100, 120);
EXPECT_EQ(frskyData.hub.gpsLongitude_bp % 100, 15);
EXPECT_EQ(frskyData.hub.gpsLongitude_ap, 0x2698);
EXPECT_EQ(frskyData.hub.gpsLatitudeNS, 'N');
EXPECT_EQ(frskyData.hub.gpsLongitudeEW, 'E');
EXPECT_EQ(frskyData.hub.fuelLevel, 100);
EXPECT_EQ(telemetryData.hub.gpsCourse_bp, 44);
EXPECT_EQ(telemetryData.hub.gpsCourse_ap, 03);
EXPECT_EQ(telemetryData.hub.gpsLongitude_bp / 100, 120);
EXPECT_EQ(telemetryData.hub.gpsLongitude_bp % 100, 15);
EXPECT_EQ(telemetryData.hub.gpsLongitude_ap, 0x2698);
EXPECT_EQ(telemetryData.hub.gpsLatitudeNS, 'N');
EXPECT_EQ(telemetryData.hub.gpsLongitudeEW, 'E');
EXPECT_EQ(telemetryData.hub.fuelLevel, 100);
}
TEST(FrSky, dateNtime)
@ -68,17 +68,17 @@ TEST(FrSky, dateNtime)
frskyDProcessPacket(pkt1);
frskyDProcessPacket(pkt2);
frskyDProcessPacket(pkt3);
EXPECT_EQ(frskyData.hub.day, 15);
EXPECT_EQ(frskyData.hub.month, 07);
EXPECT_EQ(frskyData.hub.year, 11);
EXPECT_EQ(frskyData.hub.hour, 06);
EXPECT_EQ(frskyData.hub.min, 18);
EXPECT_EQ(frskyData.hub.sec, 50);
EXPECT_EQ(telemetryData.hub.day, 15);
EXPECT_EQ(telemetryData.hub.month, 07);
EXPECT_EQ(telemetryData.hub.year, 11);
EXPECT_EQ(telemetryData.hub.hour, 06);
EXPECT_EQ(telemetryData.hub.min, 18);
EXPECT_EQ(telemetryData.hub.sec, 50);
}
#endif
#if defined(FRSKY) && defined(CPUARM)
TEST(FrSky, FrskyValueWithMinAveraging)
TEST(FrSky, TelemetryValueWithMinAveraging)
{
/*
The following expected[] array is filled
@ -90,7 +90,7 @@ TEST(FrSky, FrskyValueWithMinAveraging)
uint8_t expected[] = { 10, 12, 17, 25, 35, 45, 55, 65, 75, 85, 92, 97, 100, 100, 100, 100, 100};
int testPos = 0;
//test of averaging
FrskyValueWithMin testVal;
TelemetryValueWithMin testVal;
testVal.value = 0;
testVal.set(10);
EXPECT_EQ(RAW_FRSKY_MINMAX(testVal), 10);

2
radio/src/tests/gtests.h
View file

@ -79,7 +79,7 @@ inline void MIXER_RESET()
inline void TELEMETRY_RESET()
{
#if defined(FRSKY)
memclear(&frskyData, sizeof(frskyData));
memclear(&telemetryData, sizeof(telemetryData));
TELEMETRY_RSSI() = 100;
#endif
#if defined(CPUARM) && defined(FRSKY)

4
radio/src/vario.cpp
View file

@ -80,7 +80,7 @@ void varioWakeup()
if (isFunctionActive(FUNCTION_VARIO)) {
#if defined(AUDIO)
cli();
int16_t verticalSpeed = frskyData.hub.varioSpeed;
int16_t verticalSpeed = telemetryData.hub.varioSpeed;
sei();
#if defined(PCBSTD)
@ -126,7 +126,7 @@ void varioWakeup()
#elif defined(BUZZER) // && !defined(AUDIO)
int8_t verticalSpeed = limit((int16_t)-100, (int16_t)(frskyData.hub.varioSpeed/10), (int16_t)+100);
int8_t verticalSpeed = limit((int16_t)-100, (int16_t)(telemetryData.hub.varioSpeed/10), (int16_t)+100);
uint16_t interval;
if (verticalSpeed == 0) {