Telemetry refactoring - Fixes #1634

This commit is contained in:
bsongis
2014-11-08 12:06:42 +01:00
parent 7d8ca6931b
commit 88d9691d16
105 changed files with 5789 additions and 3449 deletions
+83 -165
View File
@@ -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
View File
@@ -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();
+198 -3
View File
@@ -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
View File
@@ -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);
}
+523
View File
@@ -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;
}
+182
View File
@@ -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