Telemetry refactoring - Fixes #1634
This commit is contained in:
+83
-165
@@ -76,6 +76,13 @@ uint8_t telemetryProtocol = 255;
|
||||
#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)
|
||||
@@ -125,21 +132,16 @@ void FrskyValueWithMinMax::set(uint8_t value, uint8_t unit)
|
||||
}
|
||||
}
|
||||
|
||||
uint16_t getChannelRatio(uint8_t channel)
|
||||
#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(uint8_t channel, lcdint_t val)
|
||||
lcdint_t applyChannelRatio(source_t channel, lcdint_t val)
|
||||
{
|
||||
return ((int32_t)val+g_model.frsky.channels[channel].offset) * getChannelRatio(channel) * 2 / 51;
|
||||
}
|
||||
|
||||
#if defined(CPUARM)
|
||||
inline bool alarmRaised(uint8_t channel, uint8_t idx)
|
||||
{
|
||||
return g_model.frsky.channels[channel].ratio > 0 && g_model.frsky.channels[channel].alarms_value[idx] > 0 && frskyData.analog[channel].value < g_model.frsky.channels[channel].alarms_value[idx];
|
||||
}
|
||||
#endif
|
||||
|
||||
#if defined(TELEMETREZ)
|
||||
@@ -258,22 +260,12 @@ NOINLINE void processSerialData(uint8_t data)
|
||||
|
||||
#if defined(FRSKY_SPORT)
|
||||
if (IS_FRSKY_SPORT_PROTOCOL() && frskyRxBufferCount >= FRSKY_SPORT_PACKET_SIZE) {
|
||||
frskySportProcessPacket(frskyRxBuffer);
|
||||
processSportPacket(frskyRxBuffer);
|
||||
dataState = STATE_DATA_IDLE;
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
enum AlarmsCheckSteps {
|
||||
ALARM_SWR_STEP,
|
||||
ALARM_RSSI_STEP,
|
||||
ALARM_A1_STEP,
|
||||
ALARM_A2_STEP,
|
||||
ALARM_A3_STEP,
|
||||
ALARM_A4_STEP,
|
||||
ALARM_STEPS_COUNT
|
||||
};
|
||||
|
||||
void telemetryWakeup()
|
||||
{
|
||||
#if defined(CPUARM)
|
||||
@@ -331,6 +323,14 @@ void telemetryWakeup()
|
||||
}
|
||||
#endif
|
||||
|
||||
#if defined(CPUARM)
|
||||
for (int i=0; i<TELEM_VALUES_MAX; 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()) {
|
||||
@@ -340,76 +340,38 @@ void telemetryWakeup()
|
||||
|
||||
#if defined(CPUARM)
|
||||
static tmr10ms_t alarmsCheckTime = 0;
|
||||
static uint8_t alarmsCheckStep = 0;
|
||||
|
||||
#define SCHEDULE_NEXT_ALARMS_CHECK(seconds) alarmsCheckTime = get_tmr10ms() + (100*(seconds))
|
||||
if (int32_t(get_tmr10ms() - alarmsCheckTime) > 0) {
|
||||
|
||||
alarmsCheckTime = get_tmr10ms() + 100; /* next check in 1 second */
|
||||
SCHEDULE_NEXT_ALARMS_CHECK(1/*second*/);
|
||||
|
||||
uint8_t now = TelemetryItem::now();
|
||||
for (int i=0; i<TELEM_VALUES_MAX; i++) {
|
||||
if (isTelemetryFieldAvailable(i)) {
|
||||
uint8_t lastReceived = telemetryItems[i].lastReceived;
|
||||
if (lastReceived < TELEMETRY_VALUE_TIMER_CYCLE && uint8_t(now - lastReceived) > TELEMETRY_VALUE_OLD_THRESHOLD) {
|
||||
telemetryItems[i].lastReceived = TELEMETRY_VALUE_OLD;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (alarmsCheckStep == ALARM_SWR_STEP) {
|
||||
#if defined(PCBTARANIS) && defined(SWR)
|
||||
if (IS_FRSKY_SPORT_PROTOCOL() && frskyData.swr.value > 0x33) {
|
||||
AUDIO_SWR_RED();
|
||||
POPUP_WARNING(STR_ANTENNAPROBLEM);
|
||||
alarmsCheckTime = get_tmr10ms() + 300; /* next check in 3 seconds */
|
||||
}
|
||||
if (IS_FRSKY_SPORT_PROTOCOL() && frskyData.swr.value > 0x33) {
|
||||
AUDIO_SWR_RED();
|
||||
POPUP_WARNING(STR_ANTENNAPROBLEM);
|
||||
SCHEDULE_NEXT_ALARMS_CHECK(10/*seconds*/);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
else if (TELEMETRY_STREAMING()) {
|
||||
if (alarmsCheckStep == ALARM_RSSI_STEP) {
|
||||
if (getRssiAlarmValue(1) && frskyData.rssi[0].value < getRssiAlarmValue(1)) {
|
||||
AUDIO_RSSI_RED();
|
||||
alarmsCheckTime = get_tmr10ms() + 300; /* next check in 3 seconds */
|
||||
}
|
||||
else if (getRssiAlarmValue(0) && frskyData.rssi[0].value < getRssiAlarmValue(0)) {
|
||||
AUDIO_RSSI_ORANGE();
|
||||
alarmsCheckTime = get_tmr10ms() + 300; /* next check in 3 seconds */
|
||||
}
|
||||
}
|
||||
else if (alarmsCheckStep == ALARM_A1_STEP) {
|
||||
if (alarmRaised(TELEM_ANA_A1, 1)) {
|
||||
AUDIO_A1_RED();
|
||||
alarmsCheckTime = get_tmr10ms() + 300; /* next check in 3 seconds */
|
||||
}
|
||||
else if (alarmRaised(TELEM_ANA_A1, 0)) {
|
||||
AUDIO_A1_ORANGE();
|
||||
alarmsCheckTime = get_tmr10ms() + 300; /* next check in 3 seconds */
|
||||
}
|
||||
}
|
||||
else if (alarmsCheckStep == ALARM_A2_STEP) {
|
||||
if (alarmRaised(TELEM_ANA_A2, 1)) {
|
||||
AUDIO_A2_RED();
|
||||
alarmsCheckTime = get_tmr10ms() + 300; /* next check in 3 seconds */
|
||||
}
|
||||
else if (alarmRaised(TELEM_ANA_A2, 0)) {
|
||||
AUDIO_A2_ORANGE();
|
||||
alarmsCheckTime = get_tmr10ms() + 300; /* next check in 3 seconds */
|
||||
}
|
||||
}
|
||||
else if (alarmsCheckStep == ALARM_A3_STEP) {
|
||||
if (alarmRaised(TELEM_ANA_A3, 1)) {
|
||||
AUDIO_A3_RED();
|
||||
alarmsCheckTime = get_tmr10ms() + 300; /* next check in 3 seconds */
|
||||
}
|
||||
else if (alarmRaised(TELEM_ANA_A3, 0)) {
|
||||
AUDIO_A3_ORANGE();
|
||||
alarmsCheckTime = get_tmr10ms() + 300; /* next check in 3 seconds */
|
||||
}
|
||||
}
|
||||
else if (alarmsCheckStep == ALARM_A4_STEP) {
|
||||
if (alarmRaised(TELEM_ANA_A4, 1)) {
|
||||
AUDIO_A4_RED();
|
||||
alarmsCheckTime = get_tmr10ms() + 300; /* next check in 3 seconds */
|
||||
}
|
||||
else if (alarmRaised(TELEM_ANA_A4, 0)) {
|
||||
AUDIO_A4_ORANGE();
|
||||
alarmsCheckTime = get_tmr10ms() + 300; /* next check in 3 seconds */
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (++alarmsCheckStep == ALARM_STEPS_COUNT) {
|
||||
alarmsCheckStep = 0;
|
||||
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*/);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -429,7 +391,6 @@ void telemetryWakeup()
|
||||
void telemetryInterrupt10ms()
|
||||
{
|
||||
#if defined(CPUARM)
|
||||
uint16_t voltage = frskyData.hub.cellsSum; /* unit: 1/10 volts */
|
||||
#elif defined(FRSKY_HUB)
|
||||
uint16_t voltage = 0; /* unit: 1/10 volts */
|
||||
for (uint8_t i=0; i<frskyData.hub.cellsCount; i++)
|
||||
@@ -443,17 +404,19 @@ void telemetryInterrupt10ms()
|
||||
|
||||
if (TELEMETRY_STREAMING()) {
|
||||
if (!TELEMETRY_OPENXSENSOR()) {
|
||||
// power calculation
|
||||
uint8_t channel = g_model.frsky.voltsSource;
|
||||
#if defined(CPUARM)
|
||||
if (channel <= FRSKY_VOLTS_SOURCE_A4) {
|
||||
voltage = applyChannelRatio(channel, frskyData.analog[channel].value) / 10;
|
||||
for (int i=0; i<TELEM_VALUES_MAX; 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;
|
||||
}
|
||||
#endif
|
||||
|
||||
#if defined(FRSKY_HUB)
|
||||
else if (channel == FRSKY_VOLTS_SOURCE_FAS) {
|
||||
@@ -472,22 +435,21 @@ void telemetryInterrupt10ms()
|
||||
current = applyChannelRatio(channel, frskyData.analog[channel].value) / 10;
|
||||
}
|
||||
|
||||
#if defined(CPUARM)
|
||||
frskyData.hub.power = (current * voltage) / 100;
|
||||
#else
|
||||
frskyData.hub.power = ((current>>1) * (voltage>>1)) / 25;
|
||||
#endif
|
||||
|
||||
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)
|
||||
@@ -501,8 +463,12 @@ void telemetryInterrupt10ms()
|
||||
}
|
||||
else {
|
||||
#if !defined(SIMU)
|
||||
#if defined(CPUARM)
|
||||
frskyData.rssi.reset();
|
||||
#else
|
||||
frskyData.rssi[0].set(0);
|
||||
frskyData.rssi[1].set(0);
|
||||
#endif
|
||||
#endif
|
||||
}
|
||||
}
|
||||
@@ -511,6 +477,12 @@ void telemetryReset()
|
||||
{
|
||||
memclear(&frskyData, sizeof(frskyData));
|
||||
|
||||
#if defined(CPUARM)
|
||||
for (int index=0; index<TELEM_VALUES_MAX; index++) {
|
||||
telemetryItems[index].clear();
|
||||
}
|
||||
#endif
|
||||
|
||||
frskyStreaming = 0; // reset counter only if valid frsky packets are being detected
|
||||
link_counter = 0;
|
||||
|
||||
@@ -518,7 +490,7 @@ void telemetryReset()
|
||||
telemetryState = TELEMETRY_INIT;
|
||||
#endif
|
||||
|
||||
#if defined(FRSKY_HUB)
|
||||
#if defined(FRSKY_HUB) && !defined(CPUARM)
|
||||
frskyData.hub.gpsLatitude_bp = 2;
|
||||
frskyData.hub.gpsLongitude_bp = 2;
|
||||
frskyData.hub.gpsFix = -1;
|
||||
@@ -527,16 +499,18 @@ void telemetryReset()
|
||||
#if defined(SIMU)
|
||||
|
||||
#if defined(CPUARM)
|
||||
#if defined(SWR)
|
||||
frskyData.swr.value = 30;
|
||||
#endif
|
||||
#endif
|
||||
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);
|
||||
#if defined(CPUARM)
|
||||
frskyData.analog[TELEM_ANA_A3].set(100, UNIT_VOLTS);
|
||||
frskyData.analog[TELEM_ANA_A4].set(200, UNIT_VOLTS);
|
||||
#endif
|
||||
|
||||
#if !defined(CPUARM)
|
||||
frskyData.hub.fuelLevel = 75;
|
||||
frskyData.hub.rpm = 12000;
|
||||
frskyData.hub.vfas = 100;
|
||||
@@ -577,9 +551,6 @@ void telemetryReset()
|
||||
frskyData.hub.baroAltitude_bp = 50;
|
||||
frskyData.hub.minAltitude = 10;
|
||||
frskyData.hub.maxAltitude = 500;
|
||||
#if defined(CPUARM)
|
||||
frskyData.hub.baroAltitude = 340*100; //in cm
|
||||
#endif
|
||||
|
||||
frskyData.hub.accelY = 100;
|
||||
frskyData.hub.temperature1 = -30;
|
||||
@@ -588,6 +559,17 @@ void telemetryReset()
|
||||
frskyData.hub.current = 55;
|
||||
frskyData.hub.maxCurrent = 65;
|
||||
#endif
|
||||
#endif
|
||||
|
||||
#if 0
|
||||
setTelemetryValue(TELEM_PROTO_FRSKY_SPORT, RSSI_ID, 0, 75, UNIT_RAW, 0);
|
||||
setTelemetryValue(TELEM_PROTO_FRSKY_SPORT, T1_FIRST_ID, 0, 100, UNIT_CELSIUS, 0);
|
||||
setTelemetryValue(TELEM_PROTO_FRSKY_SPORT, T1_FIRST_ID, 1, 200, UNIT_CELSIUS, 0);
|
||||
setTelemetryValue(TELEM_PROTO_FRSKY_SPORT, ALT_FIRST_ID, 1, 1000, UNIT_METERS, 2);
|
||||
setTelemetryValue(TELEM_PROTO_FRSKY_SPORT, CELLS_FIRST_ID, 1, 0x10010020, UNIT_CELLS, 0);
|
||||
setTelemetryValue(TELEM_PROTO_FRSKY_SPORT, CELLS_FIRST_ID, 1, 0x10010020, UNIT_CELLS, 0);
|
||||
setTelemetryValue(TELEM_PROTO_FRSKY_SPORT, CURR_FIRST_ID, 1, 100, UNIT_AMPS, 2);
|
||||
#endif
|
||||
}
|
||||
|
||||
void telemetryInit(void)
|
||||
@@ -611,70 +593,6 @@ void telemetryInit(void)
|
||||
}
|
||||
|
||||
#if defined(CPUARM)
|
||||
void frskySetCellsCount(uint8_t cellscount)
|
||||
{
|
||||
if (cellscount <= DIM(frskyData.hub.cellVolts)) {
|
||||
frskyData.hub.cellsCount = cellscount;
|
||||
frskyData.hub.cellsState = 0;
|
||||
frskyData.hub.minCells = 0;
|
||||
frskyData.hub.minCell = 0;
|
||||
}
|
||||
}
|
||||
|
||||
void frskySetCellVoltage(uint8_t battnumber, frskyCellVoltage_t cellVolts)
|
||||
{
|
||||
// TRACE("frskySetCellVoltage() %d, %d", battnumber, cellVolts);
|
||||
|
||||
if (battnumber < frskyData.hub.cellsCount) {
|
||||
// set cell voltage
|
||||
if (cellVolts > 50) // Filter out bogus cell values apparently sent by the FLVSS in some cases
|
||||
frskyData.hub.cellVolts[battnumber] = cellVolts;
|
||||
|
||||
if (cellVolts != 0) {
|
||||
frskyData.hub.cellsState |= (1 << battnumber);
|
||||
if (frskyData.hub.cellsState == (1<<frskyData.hub.cellsCount)-1) {
|
||||
// we received voltage of all cells
|
||||
frskyData.hub.cellsState = 0;
|
||||
|
||||
// calculate Cells, Cells-, Cell and Cell-
|
||||
uint16_t cellsSum = 0; /* unit: 1/10 volts */
|
||||
frskyCellVoltage_t minCellVolts = -1;
|
||||
for (uint8_t i=0; i<frskyData.hub.cellsCount; i++) {
|
||||
frskyCellVoltage_t tmpCellVolts = frskyData.hub.cellVolts[i];
|
||||
cellsSum += tmpCellVolts;
|
||||
if (tmpCellVolts < minCellVolts) {
|
||||
// update minimum cell voltage (Cell)
|
||||
minCellVolts = tmpCellVolts;
|
||||
}
|
||||
}
|
||||
|
||||
frskyData.hub.minCellVolts = minCellVolts;
|
||||
frskyData.hub.cellsSum = cellsSum / (10 / TELEMETRY_CELL_VOLTAGE_MUTLIPLIER);
|
||||
|
||||
// update cells sum minimum (Cells-)
|
||||
if (!frskyData.hub.minCells || frskyData.hub.cellsSum < frskyData.hub.minCells) {
|
||||
frskyData.hub.minCells = frskyData.hub.cellsSum;
|
||||
}
|
||||
|
||||
// update minimum cell voltage (Cell-)
|
||||
if (!frskyData.hub.minCell || frskyData.hub.minCellVolts < frskyData.hub.minCell) {
|
||||
frskyData.hub.minCell = frskyData.hub.minCellVolts;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void frskyUpdateCells(void)
|
||||
{
|
||||
// Voltage => Cell number + Cell voltage
|
||||
uint8_t battnumber = ((frskyData.hub.volts & 0x00F0) >> 4);
|
||||
if (battnumber >= frskyData.hub.cellsCount) {
|
||||
frskySetCellsCount(battnumber+1);
|
||||
}
|
||||
frskyCellVoltage_t cellVolts = (frskyCellVoltage_t) (((((frskyData.hub.volts & 0xFF00) >> 8) + ((frskyData.hub.volts & 0x000F) << 8))) / (5*TELEMETRY_CELL_VOLTAGE_MUTLIPLIER));
|
||||
frskySetCellVoltage(battnumber, cellVolts);
|
||||
}
|
||||
#elif defined(FRSKY_HUB)
|
||||
void frskyUpdateCells(void)
|
||||
{
|
||||
|
||||
+116
-98
@@ -50,6 +50,103 @@
|
||||
#define FRSKY_SPORT_AVERAGING 4
|
||||
#define FRSKY_D_AVERAGING 8
|
||||
|
||||
|
||||
// FrSky PRIM IDs (1 byte)
|
||||
#define DATA_FRAME 0x10
|
||||
|
||||
// FrSky old DATA IDs (1 byte)
|
||||
#define GPS_ALT_BP_ID 0x01
|
||||
#define TEMP1_ID 0x02
|
||||
#define RPM_ID 0x03
|
||||
#define FUEL_ID 0x04
|
||||
#define TEMP2_ID 0x05
|
||||
#define VOLTS_ID 0x06
|
||||
#define GPS_ALT_AP_ID 0x09
|
||||
#define BARO_ALT_BP_ID 0x10
|
||||
#define GPS_SPEED_BP_ID 0x11
|
||||
#define GPS_LONG_BP_ID 0x12
|
||||
#define GPS_LAT_BP_ID 0x13
|
||||
#define GPS_COURS_BP_ID 0x14
|
||||
#define GPS_DAY_MONTH_ID 0x15
|
||||
#define GPS_YEAR_ID 0x16
|
||||
#define GPS_HOUR_MIN_ID 0x17
|
||||
#define GPS_SEC_ID 0x18
|
||||
#define GPS_SPEED_AP_ID 0x19
|
||||
#define GPS_LONG_AP_ID 0x1A
|
||||
#define GPS_LAT_AP_ID 0x1B
|
||||
#define GPS_COURS_AP_ID 0x1C
|
||||
#define BARO_ALT_AP_ID 0x21
|
||||
#define GPS_LONG_EW_ID 0x22
|
||||
#define GPS_LAT_NS_ID 0x23
|
||||
#define ACCEL_X_ID 0x24
|
||||
#define ACCEL_Y_ID 0x25
|
||||
#define ACCEL_Z_ID 0x26
|
||||
#define CURRENT_ID 0x28
|
||||
#define VARIO_ID 0x30
|
||||
#define VFAS_ID 0x39
|
||||
#define VOLTS_BP_ID 0x3A
|
||||
#define VOLTS_AP_ID 0x3B
|
||||
#define FRSKY_LAST_ID 0x3F
|
||||
#define D_RSSI_ID 0xF0
|
||||
#define D_A1_ID 0xF1
|
||||
#define D_A2_ID 0xF2
|
||||
|
||||
// FrSky new DATA IDs (2 bytes)
|
||||
#define ALT_FIRST_ID 0x0100
|
||||
#define ALT_LAST_ID 0x010f
|
||||
#define VARIO_FIRST_ID 0x0110
|
||||
#define VARIO_LAST_ID 0x011f
|
||||
#define CURR_FIRST_ID 0x0200
|
||||
#define CURR_LAST_ID 0x020f
|
||||
#define VFAS_FIRST_ID 0x0210
|
||||
#define VFAS_LAST_ID 0x021f
|
||||
#define CELLS_FIRST_ID 0x0300
|
||||
#define CELLS_LAST_ID 0x030f
|
||||
#define T1_FIRST_ID 0x0400
|
||||
#define T1_LAST_ID 0x040f
|
||||
#define T2_FIRST_ID 0x0410
|
||||
#define T2_LAST_ID 0x041f
|
||||
#define RPM_FIRST_ID 0x0500
|
||||
#define RPM_LAST_ID 0x050f
|
||||
#define FUEL_FIRST_ID 0x0600
|
||||
#define FUEL_LAST_ID 0x060f
|
||||
#define ACCX_FIRST_ID 0x0700
|
||||
#define ACCX_LAST_ID 0x070f
|
||||
#define ACCY_FIRST_ID 0x0710
|
||||
#define ACCY_LAST_ID 0x071f
|
||||
#define ACCZ_FIRST_ID 0x0720
|
||||
#define ACCZ_LAST_ID 0x072f
|
||||
#define GPS_LONG_LATI_FIRST_ID 0x0800
|
||||
#define GPS_LONG_LATI_LAST_ID 0x080f
|
||||
#define GPS_ALT_FIRST_ID 0x0820
|
||||
#define GPS_ALT_LAST_ID 0x082f
|
||||
#define GPS_SPEED_FIRST_ID 0x0830
|
||||
#define GPS_SPEED_LAST_ID 0x083f
|
||||
#define GPS_COURS_FIRST_ID 0x0840
|
||||
#define GPS_COURS_LAST_ID 0x084f
|
||||
#define GPS_TIME_DATE_FIRST_ID 0x0850
|
||||
#define GPS_TIME_DATE_LAST_ID 0x085f
|
||||
#define A3_FIRST_ID 0x0900
|
||||
#define A3_LAST_ID 0x090f
|
||||
#define A4_FIRST_ID 0x0910
|
||||
#define A4_LAST_ID 0x091f
|
||||
#define AIR_SPEED_FIRST_ID 0x0a00
|
||||
#define AIR_SPEED_LAST_ID 0x0a0f
|
||||
#define RSSI_ID 0xf101
|
||||
#define ADC1_ID 0xf102
|
||||
#define ADC2_ID 0xf103
|
||||
#define BATT_ID 0xf104
|
||||
#define SWR_ID 0xf105
|
||||
|
||||
// Default sensor data IDs (Physical IDs + CRC)
|
||||
#define DATA_ID_VARIO 0x00 // 0
|
||||
#define DATA_ID_FLVSS 0xA1 // 1
|
||||
#define DATA_ID_FAS 0x22 // 2
|
||||
#define DATA_ID_GPS 0x83 // 3
|
||||
#define DATA_ID_RPM 0xE4 // 4
|
||||
#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
|
||||
|
||||
@@ -70,14 +167,15 @@ extern uint8_t telemetryState;
|
||||
#endif
|
||||
|
||||
#if defined(CPUARM)
|
||||
#define TELEMETRY_AVERAGE_COUNT 3 //we actually average one more reading!
|
||||
#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 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
|
||||
@@ -97,97 +195,9 @@ class FrskyValueWithMinMax: public FrskyValueWithMin {
|
||||
};
|
||||
|
||||
#if defined(CPUARM)
|
||||
PACK(struct FrskySerialData {
|
||||
int16_t spare1;
|
||||
int16_t gpsAltitude_bp; // 0x01 before punct
|
||||
int16_t temperature1; // 0x02 -20 .. 250 deg. celcius
|
||||
uint16_t rpm; // 0x03 0..60,000 revs. per minute
|
||||
uint16_t fuelLevel; // 0x04 0, 25, 50, 75, 100 percent
|
||||
int16_t temperature2; // 0x05 -20 .. 250 deg. celcius
|
||||
uint16_t volts; // 0x06 1/500V increments (0..4.2V)
|
||||
uint32_t distFromEarthAxis;// 2 spares reused
|
||||
int16_t gpsAltitude_ap; // 0x01+8 after punct
|
||||
uint16_t spare2[6];
|
||||
int16_t baroAltitude_bp; // 0x10 0..9,999 meters
|
||||
uint16_t gpsSpeed_bp; // 0x11 before punct
|
||||
uint16_t gpsLongitude_bp; // 0x12 before punct
|
||||
uint16_t gpsLatitude_bp; // 0x13 before punct
|
||||
uint16_t gpsCourse_bp; // 0x14 before punct (0..359.99 deg. -- seemingly 2-decimal precision)
|
||||
uint8_t day; // 0x15
|
||||
uint8_t month; // 0x15
|
||||
uint16_t year; // 0x16
|
||||
uint8_t hour; // 0x17
|
||||
uint8_t min; // 0x17
|
||||
uint16_t sec; // 0x18
|
||||
uint16_t gpsSpeed_ap; // 0x11+8
|
||||
uint16_t gpsLongitude_ap; // 0x12+8
|
||||
uint16_t gpsLatitude_ap; // 0x13+8
|
||||
uint16_t gpsCourse_ap; // 0x14+8
|
||||
uint32_t pilotLatitude; // 2 spares reused
|
||||
uint32_t pilotLongitude; // 2 spares reused
|
||||
uint16_t baroAltitude_ap; // 0x21 after punct
|
||||
uint16_t gpsLongitudeEW; // 0x1A+8 East/West
|
||||
uint16_t gpsLatitudeNS; // 0x1B+8 North/South
|
||||
int16_t accelX; // 0x24 1/256th gram (-8g ~ +8g)
|
||||
int16_t accelY; // 0x25 1/256th gram (-8g ~ +8g)
|
||||
int16_t accelZ; // 0x26 1/256th gram (-8g ~ +8g)
|
||||
int16_t spare3;
|
||||
uint16_t current; // 0x28 Current
|
||||
int16_t spare4[7];
|
||||
int16_t varioSpeed; // 0x30 Vertical speed in cm/s
|
||||
|
||||
int32_t baroAltitudeOffset;
|
||||
int32_t baroAltitude;
|
||||
int32_t gpsAltitudeOffset;
|
||||
uint32_t gpsDistance;
|
||||
|
||||
uint16_t vfas; // 0x39 Added to FrSky protocol for home made sensors with a better precision
|
||||
uint16_t volts_bp; // 0x3A
|
||||
uint16_t volts_ap; // 0x3B
|
||||
uint16_t spare5[4];
|
||||
// end of FrSky Hub data
|
||||
|
||||
/* these fields must keep this order! */
|
||||
int16_t minAltitude;
|
||||
int16_t maxAltitude;
|
||||
uint16_t maxRpm;
|
||||
int16_t maxTemperature1;
|
||||
int16_t maxTemperature2;
|
||||
uint16_t maxGpsSpeed;
|
||||
uint16_t maxGpsDistance;
|
||||
uint16_t maxAirSpeed;
|
||||
int16_t minCell;
|
||||
int16_t minCells;
|
||||
int16_t minVfas;
|
||||
uint16_t maxCurrent;
|
||||
uint16_t maxPower;
|
||||
/* end */
|
||||
|
||||
uint8_t gpsDistNeeded:1; // 1bits out of 16bits spare reused
|
||||
int8_t gpsFix:2; // 2bits out of 16bits spare reused: -1=never fixed, 0=not fixed now, 1=fixed
|
||||
uint8_t openXsensor:1; // 1bits out of 16bits spare reused: we receive data from the openXsensor
|
||||
uint8_t varioHighPrecision:1;
|
||||
uint8_t spare6:3;
|
||||
|
||||
int32_t gpsAltitude;
|
||||
|
||||
uint16_t currentConsumption;
|
||||
uint16_t currentPrescale;
|
||||
uint16_t power;
|
||||
|
||||
uint8_t cellsCount;
|
||||
uint8_t sensorCellsCount[2];
|
||||
uint16_t cellVolts[12];
|
||||
int16_t cellsSum;
|
||||
uint16_t cellsState;
|
||||
uint16_t minCellVolts;
|
||||
|
||||
uint16_t airSpeed;
|
||||
uint16_t dTE;
|
||||
});
|
||||
|
||||
#elif defined(FRSKY_HUB)
|
||||
|
||||
PACK(struct FrskySerialData {
|
||||
int16_t baroAltitudeOffset;// spare reused
|
||||
int16_t gpsAltitude_bp; // 0x01 before punct
|
||||
@@ -306,14 +316,20 @@ enum TelemAnas {
|
||||
TELEM_ANA_COUNT
|
||||
};
|
||||
|
||||
#if defined(CPUARM)
|
||||
struct FrskyData {
|
||||
#if defined(SWR)
|
||||
FrskyValueWithMin swr; // TODO Min not needed
|
||||
#endif
|
||||
FrskyValueWithMin rssi; // TODO Min not needed
|
||||
};
|
||||
#else
|
||||
struct FrskyData {
|
||||
FrskyValueWithMinMax analog[TELEM_ANA_COUNT];
|
||||
FrskyValueWithMin rssi[2];
|
||||
#if defined(CPUARM)
|
||||
FrskyValueWithMin swr;
|
||||
#endif
|
||||
FrskySerialData hub;
|
||||
};
|
||||
#endif
|
||||
|
||||
enum AlarmLevel {
|
||||
alarm_off = 0,
|
||||
@@ -326,13 +342,12 @@ 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[0].value > 0)
|
||||
#define TELEMETRY_STREAMING() (frskyData.rssi.value > 0)
|
||||
#define TELEMETRY_RSSI() (frskyData.rssi.value)
|
||||
#define TELEMETRY_RSSI_MIN() (frskyData.rssi.min)
|
||||
|
||||
#define TELEMETRY_CELL_VOLTAGE_MUTLIPLIER 1
|
||||
|
||||
#define TELEMETRY_BARO_ALT_AVAILABLE() SWITCH_SIMU(true, frskyData.hub.baroAltitudeOffset)
|
||||
#define TELEMETRY_BARO_ALT_UNIT (IS_IMPERIAL_ENABLE() ? LENGTH_UNIT_IMP : LENGTH_UNIT_METR)
|
||||
|
||||
#define TELEMETRY_GPS_SPEED_BP frskyData.hub.gpsSpeed_bp
|
||||
#define TELEMETRY_GPS_SPEED_AP frskyData.hub.gpsSpeed_ap
|
||||
|
||||
@@ -371,6 +386,8 @@ enum AlarmLevel {
|
||||
#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_CELL_VOLTAGE_MUTLIPLIER 2
|
||||
|
||||
@@ -453,11 +470,12 @@ enum FrSkyDataState {
|
||||
#endif
|
||||
|
||||
// FrSky D Protocol
|
||||
void processHubPacket(uint8_t id, int16_t value);
|
||||
void frskyDSendNextAlarm(void);
|
||||
void frskyDProcessPacket(uint8_t *packet);
|
||||
|
||||
// FrSky S.PORT Protocol
|
||||
void frskySportProcessPacket(uint8_t *packet);
|
||||
void processSportPacket(uint8_t *packet);
|
||||
|
||||
void telemetryWakeup(void);
|
||||
void telemetryReset();
|
||||
|
||||
@@ -48,7 +48,7 @@
|
||||
#define RSSI2PKT 0xf6
|
||||
#define RSSI_REQUEST 0xf1
|
||||
|
||||
#if defined(FRSKY_HUB) || defined(WS_HOW_HIGH)
|
||||
#if !defined(CPUARM) && (defined(FRSKY_HUB) || defined(WS_HOW_HIGH))
|
||||
void checkMinMaxAltitude()
|
||||
{
|
||||
if (TELEMETRY_RELATIVE_BARO_ALT_BP > frskyData.hub.maxAltitude)
|
||||
@@ -93,7 +93,11 @@ void parseTelemHubByte(uint8_t byte)
|
||||
state = TS_IDLE;
|
||||
}
|
||||
else {
|
||||
structPos = byte*2;
|
||||
#if defined(CPUARM)
|
||||
structPos = byte;
|
||||
#else
|
||||
structPos = byte * 2;
|
||||
#endif
|
||||
state = TS_DATA_LOW;
|
||||
}
|
||||
return;
|
||||
@@ -106,6 +110,10 @@ void parseTelemHubByte(uint8_t byte)
|
||||
|
||||
state = TS_IDLE;
|
||||
|
||||
#if defined(CPUARM)
|
||||
processHubPacket(structPos, (byte << 8) + lowByte);
|
||||
#else
|
||||
|
||||
#if defined(GPS)
|
||||
if ((uint8_t)structPos == offsetof(FrskySerialData, gpsLatitude_bp)) {
|
||||
if (lowByte || byte)
|
||||
@@ -256,10 +264,11 @@ void parseTelemHubByte(uint8_t byte)
|
||||
break;
|
||||
#endif
|
||||
}
|
||||
#endif
|
||||
}
|
||||
#endif
|
||||
|
||||
#if defined(WS_HOW_HIGH)
|
||||
#if defined(WS_HOW_HIGH) && !defined(CPUARM)
|
||||
void parseTelemWSHowHighByte(uint8_t byte)
|
||||
{
|
||||
if (frskyUsrStreaming < (WSHH_TIMEOUT10ms - 10)) {
|
||||
@@ -275,6 +284,31 @@ void parseTelemWSHowHighByte(uint8_t byte)
|
||||
}
|
||||
#endif
|
||||
|
||||
#if defined(CPUARM)
|
||||
void frskyDProcessPacket(uint8_t *packet)
|
||||
{
|
||||
// What type of packet?
|
||||
switch (packet[0])
|
||||
{
|
||||
case LINKPKT: // A1/A2/RSSI values
|
||||
{
|
||||
setTelemetryValue(TELEM_PROTO_FRSKY_D, D_A1_ID, 0, packet[1], UNIT_VOLTS, 0);
|
||||
setTelemetryValue(TELEM_PROTO_FRSKY_D, D_A2_ID, 0, packet[2], UNIT_VOLTS, 0);
|
||||
setTelemetryValue(TELEM_PROTO_FRSKY_D, D_RSSI_ID, 0, packet[3], UNIT_RAW, 0);
|
||||
frskyData.rssi.set(packet[3]);
|
||||
frskyStreaming = FRSKY_TIMEOUT10ms; // reset counter only if valid frsky packets are being detected
|
||||
break;
|
||||
}
|
||||
|
||||
case USRPKT: // User Data packet
|
||||
uint8_t numBytes = 3 + (packet[1] & 0x07); // sanitize in case of data corruption leading to buffer overflow
|
||||
for (uint8_t i=3; i<numBytes; i++) {
|
||||
parseTelemHubByte(packet[i]);
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
#else
|
||||
void frskyDProcessPacket(uint8_t *packet)
|
||||
{
|
||||
// What type of packet?
|
||||
@@ -315,6 +349,7 @@ void frskyDProcessPacket(uint8_t *packet)
|
||||
#endif
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
#if !defined(PCBTARANIS)
|
||||
|
||||
@@ -347,6 +382,7 @@ void frskyDSendNextAlarm(void)
|
||||
}
|
||||
#endif
|
||||
|
||||
#if 0
|
||||
uint8_t *ptr = &frskyTxBuffer[0];
|
||||
|
||||
*ptr++ = START_STOP; // Start of packet
|
||||
@@ -376,6 +412,9 @@ void frskyDSendNextAlarm(void)
|
||||
*ptr++ = START_STOP; // End of packet
|
||||
|
||||
telemetryTransmitBuffer(frskyTxBuffer, ptr - &frskyTxBuffer[0]);
|
||||
#elif !defined(WIN32)
|
||||
#warning "FrSky module alarms removed!"
|
||||
#endif
|
||||
}
|
||||
|
||||
#else
|
||||
@@ -460,4 +499,160 @@ bool isFrskyAlarmRaised(uint8_t idx)
|
||||
}
|
||||
#endif
|
||||
|
||||
#if defined(CPUARM)
|
||||
struct FrSkyDSensor {
|
||||
const uint8_t id;
|
||||
const char * name;
|
||||
const TelemetryUnit unit;
|
||||
const uint8_t prec;
|
||||
};
|
||||
|
||||
const FrSkyDSensor frskyDSensors[] = {
|
||||
{ D_RSSI_ID, ZSTR_RSSI, UNIT_RAW, 0 },
|
||||
{ D_A1_ID, ZSTR_A1, UNIT_VOLTS, 0 },
|
||||
{ D_A2_ID, ZSTR_A2, UNIT_VOLTS, 0 },
|
||||
{ RPM_ID, ZSTR_RPM, UNIT_RAW, 0 },
|
||||
{ FUEL_ID, ZSTR_FUEL, UNIT_PERCENT, 0 },
|
||||
{ TEMP1_ID, ZSTR_TEMP, UNIT_CELSIUS, 0 },
|
||||
{ TEMP2_ID, ZSTR_TEMP, UNIT_CELSIUS, 0 },
|
||||
{ CURRENT_ID, ZSTR_CURR, UNIT_AMPS, 1 },
|
||||
{ ACCEL_X_ID, ZSTR_ACCX, UNIT_G, 3 },
|
||||
{ ACCEL_Y_ID, ZSTR_ACCY, UNIT_G, 3 },
|
||||
{ ACCEL_Z_ID, ZSTR_ACCZ, UNIT_G, 3 },
|
||||
{ VARIO_ID, ZSTR_VSPD, UNIT_METERS_PER_SECOND, 2 },
|
||||
{ VFAS_ID, ZSTR_VFAS, UNIT_VOLTS, 2 },
|
||||
{ BARO_ALT_AP_ID, ZSTR_ALT, UNIT_METERS, 2 },
|
||||
{ GPS_SPEED_BP_ID, ZSTR_GSPD, UNIT_KTS, 0 },
|
||||
{ GPS_COURS_BP_ID, ZSTR_HDG, UNIT_DEGREE, 0 },
|
||||
{ VOLTS_ID, ZSTR_CELLS, UNIT_CELLS, 2 },
|
||||
{ GPS_ALT_BP_ID, ZSTR_GPSALT, UNIT_METERS, 0 },
|
||||
{ GPS_HOUR_MIN_ID, ZSTR_GPSDATETIME, UNIT_DATETIME, 0 },
|
||||
{ GPS_LAT_AP_ID, ZSTR_GPS, UNIT_GPS, 0 },
|
||||
{ 0, NULL, UNIT_RAW, 0 } // sentinel
|
||||
};
|
||||
|
||||
const FrSkyDSensor * getFrSkyDSensor(uint8_t id)
|
||||
{
|
||||
const FrSkyDSensor * result = NULL;
|
||||
for (const FrSkyDSensor * sensor = frskyDSensors; sensor->id; sensor++) {
|
||||
if (id == sensor->id) {
|
||||
result = sensor;
|
||||
break;
|
||||
}
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
void processHubPacket(uint8_t id, int16_t value)
|
||||
{
|
||||
static uint8_t lastId = 0;
|
||||
static uint16_t lastValue = 0;
|
||||
TelemetryUnit unit = UNIT_RAW;
|
||||
uint8_t precision = 0;
|
||||
int32_t data = value;
|
||||
|
||||
if (id > FRSKY_LAST_ID || id == GPS_SPEED_AP_ID || id == GPS_ALT_AP_ID || id == GPS_COURS_AP_ID) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (id == GPS_LAT_BP_ID || id == GPS_LONG_BP_ID || id == GPS_ALT_BP_ID || id == BARO_ALT_BP_ID || id == VOLTS_BP_ID) {
|
||||
lastId = id;
|
||||
lastValue = value;
|
||||
return;
|
||||
}
|
||||
|
||||
if (id == GPS_LAT_AP_ID) {
|
||||
if (lastId == GPS_LAT_BP_ID) {
|
||||
data += lastValue << 16;
|
||||
unit = UNIT_GPS_LATITUDE;
|
||||
}
|
||||
else {
|
||||
return;
|
||||
}
|
||||
}
|
||||
else if (id == GPS_LONG_AP_ID) {
|
||||
if (lastId == GPS_LONG_BP_ID) {
|
||||
data += lastValue << 16;
|
||||
id = GPS_LAT_AP_ID;
|
||||
unit = UNIT_GPS_LONGITUDE;
|
||||
}
|
||||
else {
|
||||
return;
|
||||
}
|
||||
}
|
||||
else if (id == GPS_LAT_NS_ID) {
|
||||
id = GPS_LAT_AP_ID;
|
||||
unit = UNIT_GPS_LATITUDE_NS;
|
||||
}
|
||||
else if (id == GPS_LONG_EW_ID) {
|
||||
id = GPS_LAT_AP_ID;
|
||||
unit = UNIT_GPS_LONGITUDE_EW;
|
||||
}
|
||||
else if (id == BARO_ALT_AP_ID) {
|
||||
if (lastId == BARO_ALT_BP_ID) {
|
||||
data += lastValue * 100;
|
||||
unit = UNIT_METERS;
|
||||
precision = 2;
|
||||
}
|
||||
else {
|
||||
return;
|
||||
}
|
||||
}
|
||||
else if (id == VOLTS_ID) {
|
||||
unit = UNIT_CELLS;
|
||||
}
|
||||
else if (id == GPS_DAY_MONTH_ID) {
|
||||
id = GPS_HOUR_MIN_ID;
|
||||
unit = UNIT_DATETIME_DAY_MONTH;
|
||||
}
|
||||
else if (id == GPS_HOUR_MIN_ID) {
|
||||
unit = UNIT_DATETIME_HOUR_MIN;
|
||||
}
|
||||
else if (id == GPS_SEC_ID) {
|
||||
id = GPS_HOUR_MIN_ID;
|
||||
unit = UNIT_DATETIME_SEC;
|
||||
}
|
||||
else if (id == GPS_YEAR_ID) {
|
||||
id = GPS_HOUR_MIN_ID;
|
||||
unit = UNIT_DATETIME_YEAR;
|
||||
}
|
||||
else {
|
||||
const FrSkyDSensor * sensor = getFrSkyDSensor(id);
|
||||
if (sensor) {
|
||||
unit = sensor->unit;
|
||||
precision = sensor->prec;
|
||||
}
|
||||
}
|
||||
setTelemetryValue(TELEM_PROTO_FRSKY_D, id, 0, data, unit, precision);
|
||||
}
|
||||
|
||||
void frskyDSetDefault(int index, uint16_t id)
|
||||
{
|
||||
TelemetrySensor & telemetrySensor = g_model.telemetrySensors[index];
|
||||
|
||||
telemetrySensor.id = id;
|
||||
telemetrySensor.instance = 0;
|
||||
|
||||
const FrSkyDSensor * sensor = getFrSkyDSensor(id);
|
||||
if (sensor) {
|
||||
TelemetryUnit unit = sensor->unit;
|
||||
if (unit == UNIT_CELLS)
|
||||
unit = UNIT_VOLTS;
|
||||
uint8_t prec = min<uint8_t>(2, sensor->prec);
|
||||
telemetrySensor.init(sensor->name, unit, prec);
|
||||
if (id >= D_A1_ID && id <= D_A2_ID) {
|
||||
telemetrySensor.prec = 1;
|
||||
telemetrySensor.custom.ratio = 132;
|
||||
telemetrySensor.inputFlags = TELEM_INPUT_FLAGS_FILTERING;
|
||||
}
|
||||
else if (id == D_RSSI_ID) {
|
||||
telemetrySensor.inputFlags = TELEM_INPUT_FLAGS_FILTERING;
|
||||
}
|
||||
}
|
||||
else {
|
||||
telemetrySensor.init(id);
|
||||
}
|
||||
|
||||
eeDirty(EE_MODEL);
|
||||
}
|
||||
#endif
|
||||
|
||||
+101
-417
@@ -36,229 +36,48 @@
|
||||
|
||||
#include "../opentx.h"
|
||||
|
||||
// FrSky PRIM IDs (1 byte)
|
||||
#define DATA_FRAME 0x10
|
||||
struct FrSkySportSensor {
|
||||
const uint16_t firstId;
|
||||
const uint16_t lastId;
|
||||
const char * name;
|
||||
const TelemetryUnit unit;
|
||||
const uint8_t prec;
|
||||
};
|
||||
|
||||
// FrSky old DATA IDs (1 byte)
|
||||
#define GPS_ALT_BP_ID 0x01
|
||||
#define TEMP1_ID 0x02
|
||||
#define RPM_ID 0x03
|
||||
#define FUEL_ID 0x04
|
||||
#define TEMP2_ID 0x05
|
||||
#define VOLTS_ID 0x06
|
||||
#define GPS_ALT_AP_ID 0x09
|
||||
#define BARO_ALT_BP_ID 0x10
|
||||
#define GPS_SPEED_BP_ID 0x11
|
||||
#define GPS_LONG_BP_ID 0x12
|
||||
#define GPS_LAT_BP_ID 0x13
|
||||
#define GPS_COURS_BP_ID 0x14
|
||||
#define GPS_DAY_MONTH_ID 0x15
|
||||
#define GPS_YEAR_ID 0x16
|
||||
#define GPS_HOUR_MIN_ID 0x17
|
||||
#define GPS_SEC_ID 0x18
|
||||
#define GPS_SPEED_AP_ID 0x19
|
||||
#define GPS_LONG_AP_ID 0x1A
|
||||
#define GPS_LAT_AP_ID 0x1B
|
||||
#define GPS_COURS_AP_ID 0x1C
|
||||
#define BARO_ALT_AP_ID 0x21
|
||||
#define GPS_LONG_EW_ID 0x22
|
||||
#define GPS_LAT_NS_ID 0x23
|
||||
#define ACCEL_X_ID 0x24
|
||||
#define ACCEL_Y_ID 0x25
|
||||
#define ACCEL_Z_ID 0x26
|
||||
#define CURRENT_ID 0x28
|
||||
#define VARIO_ID 0x30
|
||||
#define VFAS_ID 0x39
|
||||
#define VOLTS_BP_ID 0x3A
|
||||
#define VOLTS_AP_ID 0x3B
|
||||
#define FRSKY_LAST_ID 0x3F
|
||||
const FrSkySportSensor sportSensors[] = {
|
||||
{ RSSI_ID, RSSI_ID, ZSTR_RSSI, UNIT_RAW, 0 },
|
||||
{ ADC1_ID, ADC1_ID, ZSTR_A1, UNIT_VOLTS, 0 },
|
||||
{ ADC2_ID, ADC2_ID, ZSTR_A2, UNIT_VOLTS, 0 },
|
||||
{ BATT_ID, BATT_ID, ZSTR_BATT, UNIT_VOLTS, 0 },
|
||||
{ T1_FIRST_ID, T2_LAST_ID, ZSTR_TEMP, UNIT_CELSIUS, 0 },
|
||||
{ RPM_FIRST_ID, RPM_LAST_ID, ZSTR_RPM, UNIT_RPMS, 0 },
|
||||
{ FUEL_FIRST_ID, FUEL_LAST_ID, ZSTR_FUEL, UNIT_PERCENT, 0 },
|
||||
{ ALT_FIRST_ID, ALT_LAST_ID, ZSTR_ALT, UNIT_METERS, 2 },
|
||||
{ VARIO_FIRST_ID, VARIO_LAST_ID, ZSTR_VSPD, UNIT_METERS_PER_SECOND, 2 },
|
||||
{ ACCX_FIRST_ID, ACCX_LAST_ID, ZSTR_ACCX, UNIT_G, 2 },
|
||||
{ ACCY_FIRST_ID, ACCY_LAST_ID, ZSTR_ACCY, UNIT_G, 2 },
|
||||
{ ACCZ_FIRST_ID, ACCZ_LAST_ID, ZSTR_ACCZ, UNIT_G, 2 },
|
||||
{ CURR_FIRST_ID, CURR_LAST_ID, ZSTR_CURR, UNIT_AMPS, 1 },
|
||||
{ VFAS_FIRST_ID, VFAS_LAST_ID, ZSTR_VFAS, UNIT_VOLTS, 2 },
|
||||
{ AIR_SPEED_FIRST_ID, AIR_SPEED_LAST_ID, ZSTR_ASPD, UNIT_METERS_PER_SECOND, 1 },
|
||||
{ GPS_SPEED_FIRST_ID, GPS_SPEED_LAST_ID, ZSTR_GSPD, UNIT_KTS, 3 },
|
||||
{ CELLS_FIRST_ID, CELLS_LAST_ID, ZSTR_CELLS, UNIT_CELLS, 2 },
|
||||
{ GPS_ALT_FIRST_ID, GPS_ALT_LAST_ID, ZSTR_GPSALT, UNIT_METERS, 2 },
|
||||
{ GPS_TIME_DATE_FIRST_ID, GPS_TIME_DATE_LAST_ID, ZSTR_GPSDATETIME, UNIT_DATETIME, 0 },
|
||||
{ GPS_LONG_LATI_FIRST_ID, GPS_LONG_LATI_LAST_ID, ZSTR_GPS, UNIT_GPS, 0 },
|
||||
{ 0, 0, NULL, UNIT_RAW, 0 } // sentinel
|
||||
};
|
||||
|
||||
// FrSky new DATA IDs (2 bytes)
|
||||
#define ALT_FIRST_ID 0x0100
|
||||
#define ALT_LAST_ID 0x010f
|
||||
#define VARIO_FIRST_ID 0x0110
|
||||
#define VARIO_LAST_ID 0x011f
|
||||
#define CURR_FIRST_ID 0x0200
|
||||
#define CURR_LAST_ID 0x020f
|
||||
#define VFAS_FIRST_ID 0x0210
|
||||
#define VFAS_LAST_ID 0x021f
|
||||
#define CELLS_FIRST_ID 0x0300
|
||||
#define CELLS_LAST_ID 0x030f
|
||||
#define T1_FIRST_ID 0x0400
|
||||
#define T1_LAST_ID 0x040f
|
||||
#define T2_FIRST_ID 0x0410
|
||||
#define T2_LAST_ID 0x041f
|
||||
#define RPM_FIRST_ID 0x0500
|
||||
#define RPM_LAST_ID 0x050f
|
||||
#define FUEL_FIRST_ID 0x0600
|
||||
#define FUEL_LAST_ID 0x060f
|
||||
#define ACCX_FIRST_ID 0x0700
|
||||
#define ACCX_LAST_ID 0x070f
|
||||
#define ACCY_FIRST_ID 0x0710
|
||||
#define ACCY_LAST_ID 0x071f
|
||||
#define ACCZ_FIRST_ID 0x0720
|
||||
#define ACCZ_LAST_ID 0x072f
|
||||
#define GPS_LONG_LATI_FIRST_ID 0x0800
|
||||
#define GPS_LONG_LATI_LAST_ID 0x080f
|
||||
#define GPS_ALT_FIRST_ID 0x0820
|
||||
#define GPS_ALT_LAST_ID 0x082f
|
||||
#define GPS_SPEED_FIRST_ID 0x0830
|
||||
#define GPS_SPEED_LAST_ID 0x083f
|
||||
#define GPS_COURS_FIRST_ID 0x0840
|
||||
#define GPS_COURS_LAST_ID 0x084f
|
||||
#define GPS_TIME_DATE_FIRST_ID 0x0850
|
||||
#define GPS_TIME_DATE_LAST_ID 0x085f
|
||||
#define A3_FIRST_ID 0x0900
|
||||
#define A3_LAST_ID 0x090f
|
||||
#define A4_FIRST_ID 0x0910
|
||||
#define A4_LAST_ID 0x091f
|
||||
#define AIR_SPEED_FIRST_ID 0x0a00
|
||||
#define AIR_SPEED_LAST_ID 0x0a0f
|
||||
#define RSSI_ID 0xf101
|
||||
#define ADC1_ID 0xf102
|
||||
#define ADC2_ID 0xf103
|
||||
#define BATT_ID 0xf104
|
||||
#define SWR_ID 0xf105
|
||||
|
||||
// Default sensor data IDs (Physical IDs + CRC)
|
||||
#define DATA_ID_VARIO 0x00 // 0
|
||||
#define DATA_ID_FLVSS 0xA1 // 1
|
||||
#define DATA_ID_FAS 0x22 // 2
|
||||
#define DATA_ID_GPS 0x83 // 3
|
||||
#define DATA_ID_RPM 0xE4 // 4
|
||||
#define DATA_ID_SP2UH 0x45 // 5
|
||||
#define DATA_ID_SP2UR 0xC6 // 6
|
||||
|
||||
void setBaroAltitude(int32_t baroAltitude)
|
||||
const FrSkySportSensor * getFrSkySportSensor(uint16_t id)
|
||||
{
|
||||
// First received barometer altitude => Altitude offset
|
||||
if (!frskyData.hub.baroAltitudeOffset)
|
||||
frskyData.hub.baroAltitudeOffset = -baroAltitude;
|
||||
|
||||
baroAltitude += frskyData.hub.baroAltitudeOffset;
|
||||
frskyData.hub.baroAltitude = baroAltitude;
|
||||
|
||||
baroAltitude /= 100;
|
||||
if (baroAltitude > frskyData.hub.maxAltitude)
|
||||
frskyData.hub.maxAltitude = baroAltitude;
|
||||
if (baroAltitude < frskyData.hub.minAltitude)
|
||||
frskyData.hub.minAltitude = baroAltitude;
|
||||
}
|
||||
|
||||
void processHubPacket(uint8_t id, uint16_t value)
|
||||
{
|
||||
if (id > FRSKY_LAST_ID)
|
||||
return;
|
||||
|
||||
if (id == GPS_LAT_BP_ID) {
|
||||
if (value)
|
||||
frskyData.hub.gpsFix = 1;
|
||||
else if (frskyData.hub.gpsFix > 0 && frskyData.hub.gpsLatitude_bp > 1)
|
||||
frskyData.hub.gpsFix = 0;
|
||||
}
|
||||
else if (id == GPS_LONG_BP_ID) {
|
||||
if (value)
|
||||
frskyData.hub.gpsFix = 1;
|
||||
else if (frskyData.hub.gpsFix > 0 && frskyData.hub.gpsLongitude_bp > 1)
|
||||
frskyData.hub.gpsFix = 0;
|
||||
}
|
||||
|
||||
if (id == GPS_ALT_BP_ID || (id >= GPS_ALT_AP_ID && id <= GPS_LAT_NS_ID && id != BARO_ALT_BP_ID && id != BARO_ALT_AP_ID)) {
|
||||
// if we don't have a fix, we may discard the value
|
||||
if (frskyData.hub.gpsFix <= 0)
|
||||
return;
|
||||
}
|
||||
|
||||
((uint16_t*)&frskyData.hub)[id] = value;
|
||||
|
||||
switch (id) {
|
||||
|
||||
case RPM_ID:
|
||||
frskyData.hub.rpm *= (uint8_t)60/(g_model.frsky.blades+2);
|
||||
if (frskyData.hub.rpm > frskyData.hub.maxRpm)
|
||||
frskyData.hub.maxRpm = frskyData.hub.rpm;
|
||||
break;
|
||||
|
||||
case TEMP1_ID:
|
||||
if (frskyData.hub.temperature1 > frskyData.hub.maxTemperature1)
|
||||
frskyData.hub.maxTemperature1 = frskyData.hub.temperature1;
|
||||
break;
|
||||
|
||||
case TEMP2_ID:
|
||||
if (frskyData.hub.temperature2 > frskyData.hub.maxTemperature2)
|
||||
frskyData.hub.maxTemperature2 = frskyData.hub.temperature2;
|
||||
break;
|
||||
|
||||
case CURRENT_ID:
|
||||
if ((int16_t)frskyData.hub.current > 0 && ((int16_t)frskyData.hub.current + g_model.frsky.fasOffset) > 0)
|
||||
frskyData.hub.current += g_model.frsky.fasOffset;
|
||||
else
|
||||
frskyData.hub.current = 0;
|
||||
if (frskyData.hub.current > frskyData.hub.maxCurrent)
|
||||
frskyData.hub.maxCurrent = frskyData.hub.current;
|
||||
break;
|
||||
|
||||
case VOLTS_AP_ID:
|
||||
#if defined(FAS_BSS)
|
||||
frskyData.hub.vfas = (frskyData.hub.volts_bp * 10 + frskyData.hub.volts_ap);
|
||||
#else
|
||||
frskyData.hub.vfas = ((frskyData.hub.volts_bp * 100 + frskyData.hub.volts_ap * 10) * 21) / 110;
|
||||
#endif
|
||||
if (!frskyData.hub.minVfas || frskyData.hub.vfas < frskyData.hub.minVfas)
|
||||
frskyData.hub.minVfas = frskyData.hub.vfas;
|
||||
break;
|
||||
|
||||
case BARO_ALT_AP_ID:
|
||||
if (frskyData.hub.baroAltitude_ap > 9)
|
||||
frskyData.hub.varioHighPrecision = true;
|
||||
if (!frskyData.hub.varioHighPrecision)
|
||||
frskyData.hub.baroAltitude_ap *= 10;
|
||||
setBaroAltitude((int32_t)100 * frskyData.hub.baroAltitude_bp + (frskyData.hub.baroAltitude_bp >= 0 ? frskyData.hub.baroAltitude_ap : -frskyData.hub.baroAltitude_ap));
|
||||
break;
|
||||
|
||||
case GPS_ALT_AP_ID:
|
||||
{
|
||||
frskyData.hub.gpsAltitude = (frskyData.hub.gpsAltitude_bp * 100) + frskyData.hub.gpsAltitude_ap;
|
||||
if (!frskyData.hub.gpsAltitudeOffset)
|
||||
frskyData.hub.gpsAltitudeOffset = -frskyData.hub.gpsAltitude;
|
||||
if (!frskyData.hub.baroAltitudeOffset) {
|
||||
int altitude = TELEMETRY_RELATIVE_GPS_ALT_BP;
|
||||
if (altitude > frskyData.hub.maxAltitude)
|
||||
frskyData.hub.maxAltitude = altitude;
|
||||
if (altitude < frskyData.hub.minAltitude)
|
||||
frskyData.hub.minAltitude = altitude;
|
||||
}
|
||||
if (!frskyData.hub.pilotLatitude && !frskyData.hub.pilotLongitude) {
|
||||
// First received GPS position => Pilot GPS position
|
||||
getGpsPilotPosition();
|
||||
}
|
||||
else if (frskyData.hub.gpsDistNeeded || g_menuStack[g_menuStackPtr] == menuTelemetryFrsky) {
|
||||
getGpsDistance();
|
||||
}
|
||||
const FrSkySportSensor * result = NULL;
|
||||
for (const FrSkySportSensor * sensor = sportSensors; sensor->firstId; sensor++) {
|
||||
if (id >= sensor->firstId && id <= sensor->lastId) {
|
||||
result = sensor;
|
||||
break;
|
||||
}
|
||||
|
||||
case GPS_SPEED_BP_ID:
|
||||
// Speed => Max speed
|
||||
if (frskyData.hub.gpsSpeed_bp > frskyData.hub.maxGpsSpeed)
|
||||
frskyData.hub.maxGpsSpeed = frskyData.hub.gpsSpeed_bp;
|
||||
break;
|
||||
|
||||
case VOLTS_ID:
|
||||
frskyUpdateCells();
|
||||
break;
|
||||
|
||||
case GPS_HOUR_MIN_ID:
|
||||
frskyData.hub.hour = ((uint8_t)(frskyData.hub.hour + g_eeGeneral.timezone + 24)) % 24;
|
||||
break;
|
||||
|
||||
case ACCEL_X_ID:
|
||||
case ACCEL_Y_ID:
|
||||
case ACCEL_Z_ID:
|
||||
((int16_t*)(&frskyData.hub))[id] /= 10;
|
||||
break;
|
||||
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
bool checkSportPacket(uint8_t *packet)
|
||||
@@ -279,7 +98,7 @@ bool checkSportPacket(uint8_t *packet)
|
||||
#define SPORT_DATA_U32(packet) (*((uint32_t *)(packet+4)))
|
||||
#define HUB_DATA_U16(packet) (*((uint16_t *)(packet+4)))
|
||||
|
||||
void frskySportProcessPacket(uint8_t *packet)
|
||||
void processSportPacket(uint8_t *packet)
|
||||
{
|
||||
uint8_t dataId = packet[0];
|
||||
uint8_t prim = packet[1];
|
||||
@@ -289,207 +108,72 @@ void frskySportProcessPacket(uint8_t *packet)
|
||||
return;
|
||||
}
|
||||
|
||||
switch (prim)
|
||||
{
|
||||
case DATA_FRAME:
|
||||
if (prim == DATA_FRAME) {
|
||||
uint32_t data = SPORT_DATA_S32(packet);
|
||||
|
||||
if (appId == RSSI_ID) {
|
||||
frskyStreaming = FRSKY_TIMEOUT10ms; // reset counter only if valid frsky packets are being detected
|
||||
link_counter += 256 / FRSKY_SPORT_AVERAGING;
|
||||
frskyData.rssi[0].set(SPORT_DATA_U8(packet));
|
||||
if (appId == RSSI_ID) {
|
||||
frskyStreaming = FRSKY_TIMEOUT10ms; // reset counter only if valid frsky packets are being detected
|
||||
data = SPORT_DATA_U8(packet);
|
||||
frskyData.rssi.set(data);
|
||||
}
|
||||
|
||||
else if (appId == SWR_ID) {
|
||||
#if defined(SWR)
|
||||
data = SPORT_DATA_U8(packet);
|
||||
frskyData.swr.set(data);
|
||||
#else
|
||||
// SWR skipped (not reliable on Taranis+)
|
||||
return;
|
||||
#endif
|
||||
}
|
||||
|
||||
if (TELEMETRY_STREAMING()/* because when Rx is OFF it happens that some old A1/A2 values are sent from the XJT module*/) {
|
||||
if ((appId >> 8) == 0) {
|
||||
// The old FrSky IDs
|
||||
uint8_t id = (uint8_t)appId;
|
||||
uint16_t value = HUB_DATA_U16(packet);
|
||||
processHubPacket(id, value);
|
||||
}
|
||||
if (appId == SWR_ID) {
|
||||
frskyData.swr.set(SPORT_DATA_U8(packet));
|
||||
else {
|
||||
if (appId == ADC1_ID || appId == ADC2_ID || appId == BATT_ID) {
|
||||
data = SPORT_DATA_U8(packet);
|
||||
}
|
||||
const FrSkySportSensor * sensor = getFrSkySportSensor(appId);
|
||||
TelemetryUnit unit = UNIT_RAW;
|
||||
uint8_t precision = 0;
|
||||
if (sensor) {
|
||||
unit = sensor->unit;
|
||||
precision = sensor->prec;
|
||||
}
|
||||
setTelemetryValue(TELEM_PROTO_FRSKY_SPORT, appId, dataId, data, unit, precision);
|
||||
}
|
||||
else if (frskyData.rssi[0].value > 0) {
|
||||
if (appId == ADC1_ID || appId == ADC2_ID) {
|
||||
// A1/A2 of DxR receivers
|
||||
uint8_t idx = appId - ADC1_ID;
|
||||
frskyData.analog[idx].set(SPORT_DATA_U8(packet), g_model.frsky.channels[idx].type);
|
||||
#if defined(VARIO)
|
||||
uint8_t varioSource = g_model.frsky.varioSource - VARIO_SOURCE_A1;
|
||||
if (varioSource == appId-ADC1_ID) {
|
||||
frskyData.hub.varioSpeed = applyChannelRatio(varioSource, frskyData.analog[varioSource].value);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
else if (appId == BATT_ID) {
|
||||
frskyData.analog[TELEM_ANA_A1].set(SPORT_DATA_U8(packet), UNIT_VOLTS);
|
||||
}
|
||||
else if ((appId >> 8) == 0) {
|
||||
// The old FrSky IDs
|
||||
uint8_t id = (uint8_t)appId;
|
||||
uint16_t value = HUB_DATA_U16(packet);
|
||||
processHubPacket(id, value);
|
||||
}
|
||||
else if (appId >= T1_FIRST_ID && appId <= T1_LAST_ID) {
|
||||
frskyData.hub.temperature1 = SPORT_DATA_S32(packet);
|
||||
if (frskyData.hub.temperature1 > frskyData.hub.maxTemperature1)
|
||||
frskyData.hub.maxTemperature1 = frskyData.hub.temperature1;
|
||||
}
|
||||
else if (appId >= T2_FIRST_ID && appId <= T2_LAST_ID) {
|
||||
frskyData.hub.temperature2 = SPORT_DATA_S32(packet);
|
||||
if (frskyData.hub.temperature2 > frskyData.hub.maxTemperature2)
|
||||
frskyData.hub.maxTemperature2 = frskyData.hub.temperature2;
|
||||
}
|
||||
else if (appId >= RPM_FIRST_ID && appId <= RPM_LAST_ID) {
|
||||
frskyData.hub.rpm = SPORT_DATA_U32(packet) / (g_model.frsky.blades+2);
|
||||
if (frskyData.hub.rpm > frskyData.hub.maxRpm)
|
||||
frskyData.hub.maxRpm = frskyData.hub.rpm;
|
||||
}
|
||||
else if (appId >= FUEL_FIRST_ID && appId <= FUEL_LAST_ID) {
|
||||
frskyData.hub.fuelLevel = SPORT_DATA_U32(packet);
|
||||
}
|
||||
else if (appId >= ALT_FIRST_ID && appId <= ALT_LAST_ID) {
|
||||
setBaroAltitude(SPORT_DATA_S32(packet));
|
||||
}
|
||||
else if (appId >= VARIO_FIRST_ID && appId <= VARIO_LAST_ID) {
|
||||
frskyData.hub.varioSpeed = SPORT_DATA_S32(packet);
|
||||
}
|
||||
else if (appId >= ACCX_FIRST_ID && appId <= ACCX_LAST_ID) {
|
||||
frskyData.hub.accelX = SPORT_DATA_S32(packet);
|
||||
}
|
||||
else if (appId >= ACCY_FIRST_ID && appId <= ACCY_LAST_ID) {
|
||||
frskyData.hub.accelY = SPORT_DATA_S32(packet);
|
||||
}
|
||||
else if (appId >= ACCZ_FIRST_ID && appId <= ACCZ_LAST_ID) {
|
||||
frskyData.hub.accelZ = SPORT_DATA_S32(packet);
|
||||
}
|
||||
else if (appId >= CURR_FIRST_ID && appId <= CURR_LAST_ID) {
|
||||
frskyData.hub.current = SPORT_DATA_U32(packet);
|
||||
if (((int16_t)frskyData.hub.current + g_model.frsky.fasOffset)>0)
|
||||
frskyData.hub.current += g_model.frsky.fasOffset;
|
||||
else
|
||||
frskyData.hub.current = 0;
|
||||
if (frskyData.hub.current > frskyData.hub.maxCurrent)
|
||||
frskyData.hub.maxCurrent = frskyData.hub.current;
|
||||
}
|
||||
else if (appId >= VFAS_FIRST_ID && appId <= VFAS_LAST_ID) {
|
||||
frskyData.hub.vfas = SPORT_DATA_U32(packet)/10; //TODO: remove /10 and display with PREC2 when using SPORT
|
||||
if (!frskyData.hub.minVfas || frskyData.hub.vfas < frskyData.hub.minVfas)
|
||||
frskyData.hub.minVfas = frskyData.hub.vfas;
|
||||
}
|
||||
else if (appId >= AIR_SPEED_FIRST_ID && appId <= AIR_SPEED_LAST_ID) {
|
||||
frskyData.hub.airSpeed = SPORT_DATA_U32(packet);
|
||||
if (frskyData.hub.airSpeed > frskyData.hub.maxAirSpeed)
|
||||
frskyData.hub.maxAirSpeed = frskyData.hub.airSpeed;
|
||||
}
|
||||
else if (appId >= GPS_SPEED_FIRST_ID && appId <= GPS_SPEED_LAST_ID) {
|
||||
frskyData.hub.gpsSpeed_bp = (uint16_t) (SPORT_DATA_U32(packet)/1000);
|
||||
if (frskyData.hub.gpsSpeed_bp > frskyData.hub.maxGpsSpeed)
|
||||
frskyData.hub.maxGpsSpeed = frskyData.hub.gpsSpeed_bp;
|
||||
}
|
||||
else if (appId >= GPS_TIME_DATE_FIRST_ID && appId <= GPS_TIME_DATE_LAST_ID) {
|
||||
uint32_t gps_time_date = SPORT_DATA_U32(packet);
|
||||
if (gps_time_date & 0x000000ff) {
|
||||
frskyData.hub.year = (uint16_t) ((gps_time_date & 0xff000000) >> 24);
|
||||
frskyData.hub.month = (uint8_t) ((gps_time_date & 0x00ff0000) >> 16);
|
||||
frskyData.hub.day = (uint8_t) ((gps_time_date & 0x0000ff00) >> 8);
|
||||
}
|
||||
else {
|
||||
frskyData.hub.hour = (uint8_t) ((gps_time_date & 0xff000000) >> 24);
|
||||
frskyData.hub.min = (uint8_t) ((gps_time_date & 0x00ff0000) >> 16);
|
||||
frskyData.hub.sec = (uint16_t) ((gps_time_date & 0x0000ff00) >> 8);
|
||||
frskyData.hub.hour = ((uint8_t) (frskyData.hub.hour + g_eeGeneral.timezone + 24)) % 24;
|
||||
}
|
||||
}
|
||||
else if (appId >= GPS_COURS_FIRST_ID && appId <= GPS_COURS_LAST_ID) {
|
||||
uint32_t course = SPORT_DATA_U32(packet);
|
||||
frskyData.hub.gpsCourse_bp = course / 100;
|
||||
frskyData.hub.gpsCourse_ap = course % 100;
|
||||
}
|
||||
else if (appId >= GPS_ALT_FIRST_ID && appId <= GPS_ALT_LAST_ID) {
|
||||
frskyData.hub.gpsAltitude = SPORT_DATA_S32(packet);
|
||||
|
||||
if (!frskyData.hub.gpsAltitudeOffset)
|
||||
frskyData.hub.gpsAltitudeOffset = -frskyData.hub.gpsAltitude;
|
||||
|
||||
if (!frskyData.hub.baroAltitudeOffset) {
|
||||
int altitude = TELEMETRY_RELATIVE_GPS_ALT_BP;
|
||||
if (altitude > frskyData.hub.maxAltitude)
|
||||
frskyData.hub.maxAltitude = altitude;
|
||||
if (altitude < frskyData.hub.minAltitude)
|
||||
frskyData.hub.minAltitude = altitude;
|
||||
}
|
||||
|
||||
if (frskyData.hub.gpsFix > 0) {
|
||||
if (!frskyData.hub.pilotLatitude && !frskyData.hub.pilotLongitude) {
|
||||
// First received GPS position => Pilot GPS position
|
||||
getGpsPilotPosition();
|
||||
}
|
||||
else if (frskyData.hub.gpsDistNeeded || g_menuStack[g_menuStackPtr] == menuTelemetryFrsky) {
|
||||
getGpsDistance();
|
||||
}
|
||||
}
|
||||
}
|
||||
else if (appId >= GPS_LONG_LATI_FIRST_ID && appId <= GPS_LONG_LATI_LAST_ID) {
|
||||
uint32_t gps_long_lati_data = SPORT_DATA_U32(packet);
|
||||
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:
|
||||
frskyData.hub.gpsLatitude_bp = (gps_long_lati_b1w / 60 * 100) + (gps_long_lati_b1w % 60);
|
||||
frskyData.hub.gpsLatitude_ap = gps_long_lati_a1w;
|
||||
frskyData.hub.gpsLatitudeNS = 'N';
|
||||
break;
|
||||
case 1:
|
||||
frskyData.hub.gpsLatitude_bp = (gps_long_lati_b1w / 60 * 100) + (gps_long_lati_b1w % 60);
|
||||
frskyData.hub.gpsLatitude_ap = gps_long_lati_a1w;
|
||||
frskyData.hub.gpsLatitudeNS = 'S';
|
||||
break;
|
||||
case 2:
|
||||
frskyData.hub.gpsLongitude_bp = (gps_long_lati_b1w / 60 * 100) + (gps_long_lati_b1w % 60);
|
||||
frskyData.hub.gpsLongitude_ap = gps_long_lati_a1w;
|
||||
frskyData.hub.gpsLongitudeEW = 'E';
|
||||
break;
|
||||
case 3:
|
||||
frskyData.hub.gpsLongitude_bp = (gps_long_lati_b1w / 60 * 100) + (gps_long_lati_b1w % 60);
|
||||
frskyData.hub.gpsLongitude_ap = gps_long_lati_a1w;
|
||||
frskyData.hub.gpsLongitudeEW = 'W';
|
||||
break;
|
||||
}
|
||||
if (frskyData.hub.gpsLongitudeEW && frskyData.hub.gpsLatitudeNS) {
|
||||
frskyData.hub.gpsFix = 1;
|
||||
}
|
||||
else if (frskyData.hub.gpsFix > 0) {
|
||||
frskyData.hub.gpsFix = 0;
|
||||
}
|
||||
}
|
||||
else if (appId >= A3_FIRST_ID && appId <= A3_LAST_ID) {
|
||||
frskyData.analog[TELEM_ANA_A3].set((SPORT_DATA_U32(packet)*255+165)/330, UNIT_VOLTS);
|
||||
}
|
||||
else if (appId >= A4_FIRST_ID && appId <= A4_LAST_ID) {
|
||||
frskyData.analog[TELEM_ANA_A4].set((SPORT_DATA_U32(packet)*255+165)/330, UNIT_VOLTS);
|
||||
}
|
||||
else if (appId >= CELLS_FIRST_ID && appId <= CELLS_LAST_ID) {
|
||||
uint32_t data = SPORT_DATA_U32(packet);
|
||||
uint8_t battnumber = data & 0xF;
|
||||
uint8_t cells = (data & 0xF0) >> 4;
|
||||
bool useSecondCell = (battnumber+1 < cells);
|
||||
|
||||
if (dataId == DATA_ID_FLVSS) {
|
||||
// first sensor, remember its cell count
|
||||
frskyData.hub.sensorCellsCount[0] = cells;
|
||||
cells += frskyData.hub.sensorCellsCount[1];
|
||||
}
|
||||
else {
|
||||
// second sensor connected
|
||||
frskyData.hub.sensorCellsCount[1] = cells;
|
||||
cells += frskyData.hub.sensorCellsCount[0];
|
||||
battnumber += frskyData.hub.sensorCellsCount[0];
|
||||
}
|
||||
|
||||
if (cells != frskyData.hub.cellsCount) {
|
||||
frskySetCellsCount(cells);
|
||||
}
|
||||
|
||||
frskySetCellVoltage(battnumber, (frskyCellVoltage_t) ((data & 0x000FFF00) >> 8) / 5);
|
||||
if (useSecondCell) {
|
||||
frskySetCellVoltage(battnumber+1, (frskyCellVoltage_t) ((data & 0xFFF00000) >> 20) / 5);
|
||||
}
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void frskySportSetDefault(int index, uint16_t id, uint8_t instance)
|
||||
{
|
||||
TelemetrySensor & telemetrySensor = g_model.telemetrySensors[index];
|
||||
|
||||
telemetrySensor.id = id;
|
||||
telemetrySensor.instance = instance;
|
||||
|
||||
const FrSkySportSensor * sensor = getFrSkySportSensor(id);
|
||||
if (sensor) {
|
||||
TelemetryUnit unit = sensor->unit;
|
||||
if (unit == UNIT_CELLS)
|
||||
unit = UNIT_VOLTS;
|
||||
uint8_t prec = min<uint8_t>(2, sensor->prec);
|
||||
telemetrySensor.init(sensor->name, unit, prec);
|
||||
if (id >= ADC1_ID && id <= BATT_ID) {
|
||||
telemetrySensor.prec = 1;
|
||||
telemetrySensor.custom.ratio = 132;
|
||||
telemetrySensor.inputFlags = TELEM_INPUT_FLAGS_FILTERING;
|
||||
}
|
||||
}
|
||||
else {
|
||||
telemetrySensor.init(id);
|
||||
}
|
||||
|
||||
eeDirty(EE_MODEL);
|
||||
}
|
||||
|
||||
Executable
+523
@@ -0,0 +1,523 @@
|
||||
#include "../opentx.h"
|
||||
|
||||
TelemetryItem telemetryItems[TELEM_VALUES_MAX];
|
||||
|
||||
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 newVal, uint32_t unit, uint32_t prec)
|
||||
{
|
||||
if (unit == UNIT_CELLS) {
|
||||
uint32_t data = uint32_t(newVal);
|
||||
uint8_t cellIndex = data & 0xF;
|
||||
uint8_t count = (data & 0xF0) >> 4;
|
||||
if (count != cells.count) {
|
||||
clear();
|
||||
cells.count = count;
|
||||
}
|
||||
cells.values[cellIndex].set(((data & 0x000FFF00) >> 8) / 5);
|
||||
if (cellIndex+1 < cells.count) {
|
||||
cells.values[cellIndex+1].set(((data & 0xFFF00000) >> 20) / 5);
|
||||
}
|
||||
if (cellIndex+2 >= cells.count) {
|
||||
newVal = 0;
|
||||
for (int i=0; i<count; 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, prec);
|
||||
}
|
||||
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);
|
||||
datetime.datestate = 1;
|
||||
}
|
||||
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);
|
||||
datetime.timestate = 1;
|
||||
}
|
||||
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 {
|
||||
newVal = sensor.getValue(newVal, unit, prec);
|
||||
if (sensor.inputFlags == TELEM_INPUT_FLAGS_AUTO_OFFSET) {
|
||||
if (!isAvailable()) {
|
||||
offsetAuto = -newVal;
|
||||
}
|
||||
newVal += offsetAuto;
|
||||
}
|
||||
else if (sensor.inputFlags == TELEM_INPUT_FLAGS_FILTERING) {
|
||||
if (!isAvailable()) {
|
||||
for (int i=0; i<TELEMETRY_AVERAGE_COUNT; i++) {
|
||||
filterValues[i] = newVal;
|
||||
}
|
||||
}
|
||||
else {
|
||||
// calculate the average from values[] and value
|
||||
// also shift readings in values [] array
|
||||
unsigned int sum = filterValues[0];
|
||||
for (int i=0; i<TELEMETRY_AVERAGE_COUNT-1; i++) {
|
||||
int32_t tmp = filterValues[i+1];
|
||||
filterValues[i] = tmp;
|
||||
sum += tmp;
|
||||
}
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
}
|
||||
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 {
|
||||
uint8_t index = sensor.cell.index;
|
||||
if (index == 0 || index == 7) {
|
||||
bool lowest = (index == 0);
|
||||
index = 0;
|
||||
for (int i=0; i<cellsItem.cells.count; i++) {
|
||||
if (cellsItem.cells.values[i].state) {
|
||||
if (index == 0 || (lowest && cellsItem.cells.values[i].value < cellsItem.cells.values[index-1].value) || (!lowest && cellsItem.cells.values[i].value > cellsItem.cells.values[index-1].value)) {
|
||||
index = i+1;
|
||||
}
|
||||
}
|
||||
else {
|
||||
index = 0;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
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);
|
||||
uint8_t prec = g_model.telemetrySensors[sensor.dist.alt-1].prec;
|
||||
if (prec > 0)
|
||||
dist /= (prec==2 ? 100 : 10);
|
||||
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++) {
|
||||
uint8_t source = sensor.calc.sources[i];
|
||||
if (source) {
|
||||
TelemetrySensor & telemetrySensor = g_model.telemetrySensors[source-1];
|
||||
TelemetryItem & telemetryItem = telemetryItems[source-1];
|
||||
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;
|
||||
}
|
||||
}
|
||||
count += 1;
|
||||
if (sensor.formula == TELEM_FORMULA_MULTIPLY) {
|
||||
mulprec += telemetrySensor.prec;
|
||||
value *= convertTelemetryValue(telemetryItem.value, telemetrySensor.unit, 0, sensor.unit, 0);
|
||||
}
|
||||
else {
|
||||
int32_t sensorValue = convertTelemetryValue(telemetryItem.value, 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;
|
||||
}
|
||||
}
|
||||
|
||||
int getTelemetryIndex(TelemetryProtocol protocol, uint16_t id, uint8_t instance)
|
||||
{
|
||||
int available = -1;
|
||||
|
||||
for (int index=0; index<TELEM_VALUES_MAX; index++) {
|
||||
TelemetrySensor & telemetrySensor = g_model.telemetrySensors[index];
|
||||
if (telemetrySensor.id == id && telemetrySensor.instance == instance) {
|
||||
return index;
|
||||
}
|
||||
else if (available < 0 && telemetrySensor.id == 0) {
|
||||
available = index;
|
||||
}
|
||||
}
|
||||
|
||||
if (available >= 0) {
|
||||
switch (protocol) {
|
||||
#if defined(FRSKY_SPORT)
|
||||
case TELEM_PROTO_FRSKY_SPORT:
|
||||
frskySportSetDefault(available, id, instance);
|
||||
break;
|
||||
#endif
|
||||
case TELEM_PROTO_FRSKY_D:
|
||||
frskyDSetDefault(available, id);
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
return available;
|
||||
}
|
||||
|
||||
void delTelemetryIndex(uint8_t index)
|
||||
{
|
||||
memclear(&g_model.telemetrySensors[index], sizeof(TelemetrySensor));
|
||||
telemetryItems[index].clear();
|
||||
eeDirty(EE_MODEL);
|
||||
}
|
||||
|
||||
int availableTelemetryIndex()
|
||||
{
|
||||
for (int index=0; index<TELEM_VALUES_MAX; index++) {
|
||||
TelemetrySensor & telemetrySensor = g_model.telemetrySensors[index];
|
||||
if (telemetrySensor.id == 0 && telemetrySensor.type == 0) {
|
||||
return index;
|
||||
}
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
void setTelemetryValue(TelemetryProtocol protocol, uint16_t id, uint8_t instance, int32_t value, uint32_t unit, uint32_t prec)
|
||||
{
|
||||
int index = getTelemetryIndex(protocol, id, instance);
|
||||
|
||||
if (index >= 0) {
|
||||
telemetryItems[index].setValue(g_model.telemetrySensors[index], value, unit, prec);
|
||||
}
|
||||
else {
|
||||
// TODO error too many sensors
|
||||
}
|
||||
}
|
||||
|
||||
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;
|
||||
this->prec = prec;
|
||||
// this->inputFlags = inputFlags;
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
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_METERS) {
|
||||
if (destUnit == UNIT_FEET) {
|
||||
// m to ft *105/32
|
||||
value = (value * 105) / 32;
|
||||
}
|
||||
}
|
||||
else if (unit == UNIT_KTS) {
|
||||
if (destUnit == UNIT_KMH) {
|
||||
// kts to km/h
|
||||
value = (value * 1852) / 1000;
|
||||
}
|
||||
else if (destUnit == UNIT_MPH) {
|
||||
// kts to mph
|
||||
value = (value * 23) / 20;
|
||||
}
|
||||
}
|
||||
else if (unit == UNIT_CELSIUS) {
|
||||
if (destUnit == UNIT_FAHRENHEIT) {
|
||||
// T(°F) = T(°C)×1,8 + 32
|
||||
value = 32 + (value*18)/10;
|
||||
}
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
return value;
|
||||
}
|
||||
|
||||
Executable
+182
@@ -0,0 +1,182 @@
|
||||
/*
|
||||
* Authors (alphabetical order)
|
||||
* - Andre Bernet <bernet.andre@gmail.com>
|
||||
* - Andreas Weitl
|
||||
* - Bertrand Songis <bsongis@gmail.com>
|
||||
* - Bryan J. Rentoul (Gruvin) <gruvin@gmail.com>
|
||||
* - Cameron Weeks <th9xer@gmail.com>
|
||||
* - Erez Raviv
|
||||
* - Gabriel Birkus
|
||||
* - Jean-Pierre Parisy
|
||||
* - Karl Szmutny
|
||||
* - Michael Blandford
|
||||
* - Michal Hlavinka
|
||||
* - Pat Mackenzie
|
||||
* - Philip Moss
|
||||
* - Rob Thomson
|
||||
* - Romolo Manfredini <romolo.manfredini@gmail.com>
|
||||
* - Thomas Husterer
|
||||
*
|
||||
* opentx is based on code named
|
||||
* gruvin9x by Bryan J. Rentoul: http://code.google.com/p/gruvin9x/,
|
||||
* er9x by Erez Raviv: http://code.google.com/p/er9x/,
|
||||
* and the original (and ongoing) project by
|
||||
* Thomas Husterer, th9x: http://code.google.com/p/th9x/
|
||||
*
|
||||
* 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_H
|
||||
#define TELEMETRY_H
|
||||
|
||||
enum TelemetryProtocol
|
||||
{
|
||||
TELEM_PROTO_FRSKY_D,
|
||||
TELEM_PROTO_FRSKY_SPORT,
|
||||
};
|
||||
|
||||
#define TELEMETRY_VALUE_TIMER_CYCLE 200 /*20 seconds*/
|
||||
#define TELEMETRY_VALUE_OLD_THRESHOLD 150 /*15 seconds*/
|
||||
#define TELEMETRY_VALUE_UNAVAILABLE 255
|
||||
#define TELEMETRY_VALUE_OLD 254
|
||||
|
||||
#define TELEMETRY_AVERAGE_COUNT 3
|
||||
|
||||
PACK(struct CellValue
|
||||
{
|
||||
uint16_t value:15;
|
||||
uint16_t state:1;
|
||||
|
||||
void set(uint16_t value)
|
||||
{
|
||||
if (value > 50) {
|
||||
this->value = value;
|
||||
this->state = 1;
|
||||
}
|
||||
}
|
||||
});
|
||||
|
||||
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 {
|
||||
int32_t offsetAuto;
|
||||
int32_t filterValues[TELEMETRY_AVERAGE_COUNT];
|
||||
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[TELEM_VALUES_MAX];
|
||||
|
||||
inline bool isTelemetryFieldAvailable(int index)
|
||||
{
|
||||
TelemetrySensor & sensor = g_model.telemetrySensors[index];
|
||||
return sensor.type == TELEM_TYPE_CALCULATED || sensor.id != 0;
|
||||
}
|
||||
|
||||
inline bool isTelemetryFieldComparisonAvailable(int index)
|
||||
{
|
||||
TelemetrySensor & sensor = g_model.telemetrySensors[index];
|
||||
if (sensor.type == TELEM_TYPE_CALCULATED)
|
||||
return true;
|
||||
if (sensor.unit == UNIT_GPS)
|
||||
return false;
|
||||
if (sensor.unit == UNIT_DATETIME)
|
||||
return false;
|
||||
return (sensor.id != 0);
|
||||
}
|
||||
|
||||
void setTelemetryValue(TelemetryProtocol protocol, uint16_t id, uint8_t instance, int32_t value, uint32_t unit, uint32_t prec);
|
||||
void delTelemetryIndex(uint8_t index);
|
||||
int availableTelemetryIndex();
|
||||
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 type, uint8_t instance);
|
||||
void frskyDSetDefault(int index, uint16_t id);
|
||||
|
||||
#endif
|
||||
Reference in New Issue
Block a user