Merge against latest 2.3
This commit is contained in:
@@ -68,6 +68,9 @@ const CrossfireSensor & getCrossfireSensor(uint8_t id, uint8_t subId)
|
||||
|
||||
void processCrossfireTelemetryValue(uint8_t index, int32_t value)
|
||||
{
|
||||
if(!TELEMETRY_STREAMING())
|
||||
return;
|
||||
|
||||
const CrossfireSensor & sensor = crossfireSensors[index];
|
||||
setTelemetryValue(PROTOCOL_TELEMETRY_CROSSFIRE, sensor.id, 0, sensor.subId, value, sensor.unit, sensor.precision);
|
||||
}
|
||||
@@ -134,8 +137,14 @@ void processCrossfireTelemetryFrame()
|
||||
}
|
||||
processCrossfireTelemetryValue(i, value);
|
||||
if (i == RX_QUALITY_INDEX) {
|
||||
telemetryData.rssi.set(value);
|
||||
telemetryStreaming = TELEMETRY_TIMEOUT10ms;
|
||||
if (value) {
|
||||
telemetryData.rssi.set(value);
|
||||
telemetryStreaming = TELEMETRY_TIMEOUT10ms;
|
||||
}
|
||||
else {
|
||||
telemetryData.rssi.reset();
|
||||
telemetryStreaming = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -212,6 +221,11 @@ void processCrossfireTelemetryData(uint8_t data)
|
||||
if (telemetryRxBufferCount > 4) {
|
||||
uint8_t length = telemetryRxBuffer[1];
|
||||
if (length + 2 == telemetryRxBufferCount) {
|
||||
#if defined(BLUETOOTH)
|
||||
if (g_eeGeneral.bluetoothMode == BLUETOOTH_TELEMETRY && bluetooth.state == BLUETOOTH_STATE_CONNECTED) {
|
||||
bluetooth.write(telemetryRxBuffer, telemetryRxBufferCount);
|
||||
}
|
||||
#endif
|
||||
processCrossfireTelemetryFrame();
|
||||
telemetryRxBufferCount = 0;
|
||||
}
|
||||
|
||||
@@ -209,7 +209,9 @@ class TelemetryData {
|
||||
TelemetryExpiringDecorator<TelemetryValue> swrExternal;
|
||||
TelemetryFilterDecorator<TelemetryValue> rssi;
|
||||
uint16_t xjtVersion;
|
||||
bool varioHighPrecision;
|
||||
uint8_t varioHighPrecision:1;
|
||||
uint8_t telemetryValid:3;
|
||||
uint8_t spare:4;
|
||||
|
||||
void setSwr(uint8_t module, uint8_t value)
|
||||
{
|
||||
@@ -227,11 +229,6 @@ class TelemetryData {
|
||||
|
||||
extern TelemetryData telemetryData;
|
||||
|
||||
inline bool TELEMETRY_STREAMING()
|
||||
{
|
||||
return telemetryData.rssi.value() > 0;
|
||||
}
|
||||
|
||||
inline uint8_t TELEMETRY_RSSI()
|
||||
{
|
||||
return telemetryData.rssi.value();
|
||||
|
||||
@@ -216,7 +216,9 @@ void processResetFrame(uint8_t module, uint8_t * frame)
|
||||
void processTelemetryFrame(uint8_t module, uint8_t * frame)
|
||||
{
|
||||
uint8_t origin = (module << 2) + (frame[3] & 0x03);
|
||||
sportProcessTelemetryPacketWithoutCrc(origin, &frame[4]);
|
||||
if (origin != TELEMETRY_ENDPOINT_SPORT) {
|
||||
sportProcessTelemetryPacketWithoutCrc(origin, &frame[4]);
|
||||
}
|
||||
}
|
||||
|
||||
void processSpectrumAnalyserFrame(uint8_t module, uint8_t * frame)
|
||||
|
||||
@@ -85,14 +85,12 @@ const FrSkySportSensor sportSensors[] = {
|
||||
|
||||
const FrSkySportSensor * getFrSkySportSensor(uint16_t id, uint8_t subId=0)
|
||||
{
|
||||
const FrSkySportSensor * result = NULL;
|
||||
for (const FrSkySportSensor * sensor = sportSensors; sensor->firstId; sensor++) {
|
||||
if (id >= sensor->firstId && id <= sensor->lastId && subId == sensor->subId) {
|
||||
result = sensor;
|
||||
break;
|
||||
return sensor;
|
||||
}
|
||||
}
|
||||
return result;
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
bool checkSportPacket(const uint8_t * packet)
|
||||
@@ -164,10 +162,27 @@ void sportProcessTelemetryPacketWithoutCrc(uint8_t origin, const uint8_t * packe
|
||||
#endif
|
||||
|
||||
if (primId == DATA_FRAME) {
|
||||
uint8_t originMask;
|
||||
if (origin == TELEMETRY_ENDPOINT_SPORT) {
|
||||
originMask = 0x04;
|
||||
}
|
||||
else {
|
||||
uint8_t moduleIndex = (origin >> 2);
|
||||
originMask = 0x01 << moduleIndex;
|
||||
}
|
||||
uint8_t instance = physicalId + (origin << 5);
|
||||
if (dataId == RSSI_ID) {
|
||||
telemetryStreaming = TELEMETRY_TIMEOUT10ms; // reset counter only if valid packets are being detected
|
||||
data = SPORT_DATA_U8(packet);
|
||||
if (data > 0) {
|
||||
telemetryStreaming = TELEMETRY_TIMEOUT10ms; // reset counter only if valid packets are being detected
|
||||
telemetryData.telemetryValid |= originMask;
|
||||
}
|
||||
else {
|
||||
telemetryData.telemetryValid &= ~originMask;
|
||||
// one module may send RSSI(0) while the other is still streaming
|
||||
// in this case we don't want to update telemetryData.rssi
|
||||
return;
|
||||
}
|
||||
if (g_model.rssiSource) {
|
||||
TelemetrySensor * sensor = &g_model.telemetrySensors[g_model.rssiSource - 1];
|
||||
if (sensor->isSameInstance(PROTOCOL_TELEMETRY_FRSKY_SPORT, instance)) {
|
||||
@@ -194,7 +209,8 @@ void sportProcessTelemetryPacketWithoutCrc(uint8_t origin, const uint8_t * packe
|
||||
}
|
||||
}
|
||||
|
||||
if (TELEMETRY_STREAMING()/* because when Rx is OFF it happens that some old A1/A2 values are sent from the XJT module*/) {
|
||||
// here we discard the frame if it comes from an origin which has RSSI = 0 (RxBt and RSSI are sent in a loop by the module in some situations)
|
||||
if (TELEMETRY_STREAMING() && (telemetryData.telemetryValid & originMask)/* because when Rx is OFF it happens that some old A1/A2 values are sent from the XJT module*/) {
|
||||
if ((dataId >> 8) == 0) {
|
||||
// The old FrSky IDs
|
||||
processHubPacket(dataId, HUB_DATA_U16(packet));
|
||||
@@ -203,6 +219,7 @@ void sportProcessTelemetryPacketWithoutCrc(uint8_t origin, const uint8_t * packe
|
||||
if (dataId == ADC1_ID || dataId == ADC2_ID || dataId == BATT_ID || dataId == RAS_ID) {
|
||||
data = SPORT_DATA_U8(packet);
|
||||
}
|
||||
|
||||
if (dataId >= GPS_LONG_LATI_FIRST_ID && dataId <= GPS_LONG_LATI_LAST_ID) {
|
||||
int32_t value = (data & 0x3fffffff);
|
||||
if (data & (1 << 30))
|
||||
|
||||
@@ -21,14 +21,9 @@
|
||||
#include "opentx.h"
|
||||
|
||||
uint8_t telemetryStreaming = 0;
|
||||
uint8_t R9ModuleStreaming = 0;
|
||||
uint8_t telemetryRxBuffer[TELEMETRY_RX_PACKET_SIZE]; // Receive buffer. 9 bytes (full packet), worst case 18 bytes with byte-stuffing (+1)
|
||||
uint8_t telemetryRxBufferCount = 0;
|
||||
|
||||
#if defined(WS_HOW_HIGH)
|
||||
uint8_t wshhStreaming = 0;
|
||||
#endif
|
||||
|
||||
uint8_t telemetryState = TELEMETRY_INIT;
|
||||
|
||||
TelemetryData telemetryData;
|
||||
@@ -153,20 +148,21 @@ void telemetryWakeup()
|
||||
|
||||
SCHEDULE_NEXT_ALARMS_CHECK(1/*second*/);
|
||||
|
||||
bool sensor_lost = false;
|
||||
bool sensorLost = false;
|
||||
for (int i=0; i<MAX_TELEMETRY_SENSORS; i++) {
|
||||
if (isTelemetryFieldAvailable(i)) {
|
||||
TelemetryItem & item = telemetryItems[i];
|
||||
if (item.hasReceiveTime() && item.getDelaySinceLastValue() > TELEMETRY_VALUE_OLD_THRESHOLD) {
|
||||
if (item.timeout == 0) {
|
||||
TelemetrySensor * sensor = & g_model.telemetrySensors[i];
|
||||
if (sensor->unit != UNIT_DATETIME) {
|
||||
item.setOld();
|
||||
sensor_lost = true;
|
||||
sensorLost = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
if (sensor_lost && TELEMETRY_STREAMING() && !g_model.rssiAlarms.disabled) {
|
||||
|
||||
if (sensorLost && TELEMETRY_STREAMING() && !g_model.rssiAlarms.disabled) {
|
||||
audioEvent(AU_SENSOR_LOST);
|
||||
}
|
||||
|
||||
@@ -210,43 +206,40 @@ void telemetryWakeup()
|
||||
|
||||
void telemetryInterrupt10ms()
|
||||
{
|
||||
|
||||
if (TELEMETRY_STREAMING()) {
|
||||
if (telemetryStreaming > 0) {
|
||||
bool tick160ms = (telemetryStreaming & 0x0F) == 0;
|
||||
for (int i=0; i<MAX_TELEMETRY_SENSORS; i++) {
|
||||
const TelemetrySensor & sensor = g_model.telemetrySensors[i];
|
||||
if (sensor.type == TELEM_TYPE_CALCULATED) {
|
||||
telemetryItems[i].per10ms(sensor);
|
||||
}
|
||||
if (tick160ms && telemetryItems[i].timeout > 0) {
|
||||
telemetryItems[i].timeout--;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#if defined(WS_HOW_HIGH)
|
||||
if (wshhStreaming > 0) {
|
||||
wshhStreaming--;
|
||||
}
|
||||
#endif
|
||||
if (R9ModuleStreaming > 0) {
|
||||
R9ModuleStreaming--;
|
||||
}
|
||||
if (telemetryStreaming > 0) {
|
||||
telemetryStreaming--;
|
||||
}
|
||||
else {
|
||||
#if !defined(SIMU)
|
||||
telemetryData.rssi.reset();
|
||||
#endif
|
||||
for (auto & telemetryItem: telemetryItems) {
|
||||
if (telemetryItem.isAvailable()) {
|
||||
telemetryItem.setOld();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void telemetryReset()
|
||||
{
|
||||
memclear(&telemetryData, sizeof(telemetryData));
|
||||
telemetryData.clear();
|
||||
|
||||
for (int index=0; index<MAX_TELEMETRY_SENSORS; index++) {
|
||||
telemetryItems[index].clear();
|
||||
for (auto & telemetryItem : telemetryItems) {
|
||||
telemetryItem.clear();
|
||||
}
|
||||
|
||||
telemetryStreaming = 0; // reset counter only if valid frsky packets are being detected
|
||||
telemetryStreaming = 0; // reset counter only if valid telemetry packets are being detected
|
||||
|
||||
telemetryState = TELEMETRY_INIT;
|
||||
}
|
||||
|
||||
@@ -30,11 +30,11 @@
|
||||
#endif
|
||||
|
||||
extern uint8_t telemetryStreaming; // >0 (true) == data is streaming in. 0 = no data detected for some time
|
||||
extern uint8_t R9ModuleStreaming; // >0 (true) == R9 module is connected and sending data 0 = no data detected for some time
|
||||
|
||||
#if defined(WS_HOW_HIGH)
|
||||
extern uint8_t wshhStreaming;
|
||||
#endif
|
||||
inline bool TELEMETRY_STREAMING()
|
||||
{
|
||||
return telemetryStreaming > 0;
|
||||
}
|
||||
|
||||
enum TelemetryStates {
|
||||
TELEMETRY_INIT,
|
||||
@@ -43,7 +43,7 @@ enum TelemetryStates {
|
||||
};
|
||||
extern uint8_t telemetryState;
|
||||
|
||||
#define TELEMETRY_TIMEOUT10ms 100 // 1 second
|
||||
constexpr uint8_t TELEMETRY_TIMEOUT10ms = 100; // 1 second
|
||||
|
||||
#define TELEMETRY_SERIAL_DEFAULT 0
|
||||
#define TELEMETRY_SERIAL_8E2 1
|
||||
@@ -144,9 +144,6 @@ void logTelemetryWriteByte(uint8_t data);
|
||||
#define LOG_TELEMETRY_WRITE_BYTE(data)
|
||||
#endif
|
||||
|
||||
#define TELEMETRY_ENDPOINT_NONE 0xFF
|
||||
#define TELEMETRY_ENDPOINT_SPORT 0x07
|
||||
|
||||
class OutputTelemetryBuffer {
|
||||
public:
|
||||
OutputTelemetryBuffer()
|
||||
|
||||
@@ -120,7 +120,7 @@ void TelemetryItem::setValue(const TelemetrySensor & sensor, int32_t val, uint32
|
||||
}
|
||||
}
|
||||
else if (unit == UNIT_DATETIME) {
|
||||
uint32_t data = uint32_t(newVal);
|
||||
auto data = uint32_t(newVal);
|
||||
if (data & 0x000000ff) {
|
||||
datetime.year = (uint16_t) ((data & 0xff000000) >> 24) + 2000; // SPORT GPS year is only two digits
|
||||
datetime.month = (uint8_t) ((data & 0x00ff0000) >> 16);
|
||||
@@ -150,7 +150,7 @@ void TelemetryItem::setValue(const TelemetrySensor & sensor, int32_t val, uint32
|
||||
distFromEarthAxis = getDistFromEarthAxis(newVal);
|
||||
}
|
||||
gps.latitude = newVal;
|
||||
lastReceived = now();
|
||||
setFresh();
|
||||
return;
|
||||
}
|
||||
else if (unit == UNIT_GPS_LONGITUDE) {
|
||||
@@ -163,7 +163,7 @@ void TelemetryItem::setValue(const TelemetrySensor & sensor, int32_t val, uint32
|
||||
pilotLongitude = newVal;
|
||||
}
|
||||
gps.longitude = newVal;
|
||||
lastReceived = now();
|
||||
setFresh();
|
||||
return;
|
||||
}
|
||||
else if (unit == UNIT_DATETIME_YEAR) {
|
||||
@@ -171,13 +171,13 @@ void TelemetryItem::setValue(const TelemetrySensor & sensor, int32_t val, uint32
|
||||
return;
|
||||
}
|
||||
else if (unit == UNIT_DATETIME_DAY_MONTH) {
|
||||
uint32_t data = uint32_t(newVal);
|
||||
auto data = uint32_t(newVal);
|
||||
datetime.month = data >> 8;
|
||||
datetime.day = data & 0xFF;
|
||||
return;
|
||||
}
|
||||
else if (unit == UNIT_DATETIME_HOUR_MIN) {
|
||||
uint32_t data = uint32_t(newVal);
|
||||
auto data = uint32_t(newVal);
|
||||
datetime.hour = (data & 0xFF);
|
||||
datetime.min = data >> 8;
|
||||
return;
|
||||
@@ -193,7 +193,7 @@ void TelemetryItem::setValue(const TelemetrySensor & sensor, int32_t val, uint32
|
||||
}
|
||||
else if (unit == UNIT_TEXT) {
|
||||
*((uint32_t*)&text[prec]) = newVal;
|
||||
lastReceived = now();
|
||||
setFresh();
|
||||
return;
|
||||
}
|
||||
else {
|
||||
@@ -252,7 +252,7 @@ void TelemetryItem::setValue(const TelemetrySensor & sensor, int32_t val, uint32
|
||||
}
|
||||
|
||||
value = newVal;
|
||||
lastReceived = now();
|
||||
setFresh();
|
||||
}
|
||||
|
||||
void TelemetryItem::per10ms(const TelemetrySensor & sensor)
|
||||
@@ -266,7 +266,7 @@ void TelemetryItem::per10ms(const TelemetrySensor & sensor)
|
||||
return;
|
||||
}
|
||||
else if (currentItem.isOld()) {
|
||||
lastReceived = TELEMETRY_VALUE_OLD;
|
||||
setOld();
|
||||
return;
|
||||
}
|
||||
int32_t current = convertTelemetryValue(currentItem.value, currentSensor.unit, currentSensor.prec, UNIT_AMPS, 1);
|
||||
@@ -275,7 +275,7 @@ void TelemetryItem::per10ms(const TelemetrySensor & sensor)
|
||||
currentItem.consumption.prescale -= 3600;
|
||||
setValue(sensor, value+1, sensor.unit, sensor.prec);
|
||||
}
|
||||
lastReceived = now();
|
||||
setFresh();
|
||||
}
|
||||
break;
|
||||
|
||||
@@ -291,7 +291,7 @@ void TelemetryItem::eval(const TelemetrySensor & sensor)
|
||||
if (sensor.cell.source) {
|
||||
TelemetryItem & cellsItem = telemetryItems[sensor.cell.source-1];
|
||||
if (cellsItem.isOld()) {
|
||||
lastReceived = TELEMETRY_VALUE_OLD;
|
||||
setOld();
|
||||
}
|
||||
else {
|
||||
unsigned int index = sensor.cell.index;
|
||||
@@ -340,7 +340,7 @@ void TelemetryItem::eval(const TelemetrySensor & sensor)
|
||||
return;
|
||||
}
|
||||
else if (gpsItem.isOld()) {
|
||||
lastReceived = TELEMETRY_VALUE_OLD;
|
||||
setOld();
|
||||
return;
|
||||
}
|
||||
if (sensor.dist.alt) {
|
||||
@@ -349,7 +349,7 @@ void TelemetryItem::eval(const TelemetrySensor & sensor)
|
||||
return;
|
||||
}
|
||||
else if (altItem->isOld()) {
|
||||
lastReceived = TELEMETRY_VALUE_OLD;
|
||||
setOld();
|
||||
return;
|
||||
}
|
||||
}
|
||||
@@ -413,7 +413,7 @@ void TelemetryItem::eval(const TelemetrySensor & sensor)
|
||||
return;
|
||||
}
|
||||
else if (telemetryItem.isOld()) {
|
||||
lastReceived = TELEMETRY_VALUE_OLD;
|
||||
setOld();
|
||||
return;
|
||||
}
|
||||
}
|
||||
@@ -439,7 +439,7 @@ void TelemetryItem::eval(const TelemetrySensor & sensor)
|
||||
if (sensor.formula == TELEM_FORMULA_AVERAGE) {
|
||||
if (count == 0) {
|
||||
if (available)
|
||||
lastReceived = TELEMETRY_VALUE_OLD;
|
||||
setOld();
|
||||
return;
|
||||
}
|
||||
else {
|
||||
@@ -491,18 +491,18 @@ int lastUsedTelemetryIndex()
|
||||
|
||||
int setTelemetryValue(TelemetryProtocol protocol, uint16_t id, uint8_t subId, uint8_t instance, int32_t value, uint32_t unit, uint32_t prec)
|
||||
{
|
||||
bool available = false;
|
||||
bool sensorFound = false;
|
||||
|
||||
for (int index=0; index<MAX_TELEMETRY_SENSORS; index++) {
|
||||
TelemetrySensor & telemetrySensor = g_model.telemetrySensors[index];
|
||||
if (telemetrySensor.type == TELEM_TYPE_CUSTOM && telemetrySensor.id == id && telemetrySensor.subId == subId && (telemetrySensor.isSameInstance(protocol, instance) || g_model.ignoreSensorIds)) {
|
||||
telemetryItems[index].setValue(telemetrySensor, value, unit, prec);
|
||||
available = true;
|
||||
sensorFound = true;
|
||||
// we continue search here, because sensors can share the same id and instance
|
||||
}
|
||||
}
|
||||
|
||||
if (available || !allowNewSensors) {
|
||||
if (sensorFound || !allowNewSensors) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
@@ -585,20 +585,24 @@ PACK(typedef struct {
|
||||
|
||||
const UnitConversionRule unitConversionTable[] = {
|
||||
/* unitFrom unitTo multiplier divisor */
|
||||
{ UNIT_METERS, UNIT_FEET, 105, 32},
|
||||
{ UNIT_METERS_PER_SECOND, UNIT_FEET_PER_SECOND, 105, 32},
|
||||
{ UNIT_METERS, UNIT_FEET, 105, 32},
|
||||
{ UNIT_METERS_PER_SECOND, UNIT_FEET_PER_SECOND, 105, 32},
|
||||
|
||||
{ UNIT_KTS, UNIT_KMH, 1852, 1000}, // 1 knot = 1.85200 kilometers per hour
|
||||
{ UNIT_KTS, UNIT_MPH, 1151, 1000}, // 1 knot = 1.15077945 miles per hour
|
||||
{ UNIT_KTS, UNIT_METERS_PER_SECOND, 1000, 1944}, // 1 knot = 0.514444444 meters / second (divide with 1.94384449)
|
||||
{ UNIT_KTS, UNIT_FEET_PER_SECOND, 1688, 1000}, // 1 knot = 1.68780986 feet per second
|
||||
{ UNIT_KTS, UNIT_KMH, 1852, 1000}, // 1 knot = 1.85200 kilometers per hour
|
||||
{ UNIT_KTS, UNIT_MPH, 1151, 1000}, // 1 knot = 1.15077945 miles per hour
|
||||
{ UNIT_KTS, UNIT_METERS_PER_SECOND, 1000, 1944}, // 1 knot = 0.514444444 meters / second (divide with 1.94384449)
|
||||
{ UNIT_KTS, UNIT_FEET_PER_SECOND, 1688, 1000}, // 1 knot = 1.68780986 feet per second
|
||||
|
||||
{ UNIT_KMH, UNIT_KTS, 1000, 1852}, // 1 km/h = 0.539956803 knots (divide with 1.85200)
|
||||
{ UNIT_KMH, UNIT_MPH, 1000, 1609}, // 1 km/h = 0.621371192 miles per hour (divide with 1.60934400)
|
||||
{ UNIT_KMH, UNIT_METERS_PER_SECOND, 10, 36}, // 1 km/h = 0.277777778 meters / second (divide with 3.6)
|
||||
{ UNIT_KMH, UNIT_FEET_PER_SECOND, 911, 1000}, // 1 km/h = 0.911344415 feet per second
|
||||
{ UNIT_KMH, UNIT_KTS, 1000, 1852}, // 1 km/h = 0.539956803 knots (divide with 1.85200)
|
||||
{ UNIT_KMH, UNIT_MPH, 1000, 1609}, // 1 km/h = 0.621371192 miles per hour (divide with 1.60934400)
|
||||
{ UNIT_KMH, UNIT_METERS_PER_SECOND, 10, 36}, // 1 km/h = 0.277777778 meters / second (divide with 3.6)
|
||||
{ UNIT_KMH, UNIT_FEET_PER_SECOND, 911, 1000}, // 1 km/h = 0.911344415 feet per second
|
||||
|
||||
{ UNIT_MILLILITERS, UNIT_FLOZ, 100, 2957},
|
||||
{ UNIT_MILLILITERS, UNIT_FLOZ, 100, 2957},
|
||||
|
||||
{ UNIT_RADIANS, UNIT_DEGREE, 10000, 175}, // 1 rad = 57.29578 deg
|
||||
{ UNIT_DEGREE, UNIT_RADIANS, 175, 10000}, // 1 deg = 0,0174533 rad
|
||||
|
||||
{ 0, 0, 0, 0} // termination
|
||||
};
|
||||
|
||||
|
||||
@@ -23,10 +23,9 @@
|
||||
|
||||
#include "telemetry.h"
|
||||
|
||||
#define TELEMETRY_VALUE_TIMER_CYCLE 128 /* x160ms ~= 20.5s ; must be multiple of 2 to avoid the modulo */
|
||||
#define TELEMETRY_VALUE_OLD_THRESHOLD 62 /* x160ms ~= 10s */
|
||||
#define TELEMETRY_VALUE_UNAVAILABLE 255
|
||||
#define TELEMETRY_VALUE_OLD 254
|
||||
constexpr int8_t TELEMETRY_SENSOR_TIMEOUT_UNAVAILABLE = -2;
|
||||
constexpr int8_t TELEMETRY_SENSOR_TIMEOUT_OLD = -1;
|
||||
constexpr int8_t TELEMETRY_SENSOR_TIMEOUT_START = 125; // * 160ms = 20s
|
||||
|
||||
class TelemetryItem
|
||||
{
|
||||
@@ -46,7 +45,7 @@ class TelemetryItem
|
||||
int32_t pilotLatitude;
|
||||
};
|
||||
|
||||
uint8_t lastReceived; // for detection of sensor loss
|
||||
int8_t timeout; // for detection of sensor loss
|
||||
|
||||
union {
|
||||
struct {
|
||||
@@ -78,12 +77,6 @@ class TelemetryItem
|
||||
char text[16];
|
||||
};
|
||||
|
||||
static uint8_t now()
|
||||
{
|
||||
// 160ms granularity
|
||||
return (get_tmr10ms() >> 4) & (TELEMETRY_VALUE_TIMER_CYCLE - 1);
|
||||
}
|
||||
|
||||
TelemetryItem()
|
||||
{
|
||||
clear();
|
||||
@@ -92,7 +85,7 @@ class TelemetryItem
|
||||
void clear()
|
||||
{
|
||||
memset(reinterpret_cast<void*>(this), 0, sizeof(TelemetryItem));
|
||||
lastReceived = TELEMETRY_VALUE_UNAVAILABLE;
|
||||
timeout = TELEMETRY_SENSOR_TIMEOUT_UNAVAILABLE;
|
||||
}
|
||||
|
||||
void eval(const TelemetrySensor & sensor);
|
||||
@@ -102,36 +95,38 @@ class TelemetryItem
|
||||
|
||||
inline bool isAvailable()
|
||||
{
|
||||
return (lastReceived != TELEMETRY_VALUE_UNAVAILABLE);
|
||||
return (timeout != TELEMETRY_SENSOR_TIMEOUT_UNAVAILABLE);
|
||||
}
|
||||
|
||||
inline bool isOld()
|
||||
{
|
||||
return (lastReceived == TELEMETRY_VALUE_OLD);
|
||||
return (timeout == TELEMETRY_SENSOR_TIMEOUT_OLD);
|
||||
}
|
||||
|
||||
inline bool hasReceiveTime()
|
||||
{
|
||||
return (lastReceived < TELEMETRY_VALUE_TIMER_CYCLE);
|
||||
return timeout >= 0;
|
||||
}
|
||||
|
||||
inline uint8_t getDelaySinceLastValue()
|
||||
inline int8_t getDelaySinceLastValue()
|
||||
{
|
||||
// assumes lastReceived is not a special value (OLD / UNAVAILABLE)
|
||||
return (now() - lastReceived) & (TELEMETRY_VALUE_TIMER_CYCLE - 1);
|
||||
return hasReceiveTime() ? TELEMETRY_SENSOR_TIMEOUT_START - timeout : TELEMETRY_SENSOR_TIMEOUT_OLD;
|
||||
}
|
||||
|
||||
inline bool isFresh()
|
||||
{
|
||||
return (hasReceiveTime() && getDelaySinceLastValue() <= 1);
|
||||
return TELEMETRY_SENSOR_TIMEOUT_START - timeout <= 1; // 2 * 160ms
|
||||
}
|
||||
|
||||
inline void setFresh()
|
||||
{
|
||||
timeout = TELEMETRY_SENSOR_TIMEOUT_START;
|
||||
}
|
||||
|
||||
inline void setOld()
|
||||
{
|
||||
lastReceived = TELEMETRY_VALUE_OLD;
|
||||
timeout = TELEMETRY_SENSOR_TIMEOUT_OLD;
|
||||
}
|
||||
|
||||
void gpsReceived(); // TODO seems not used
|
||||
};
|
||||
|
||||
extern TelemetryItem telemetryItems[MAX_TELEMETRY_SENSORS];
|
||||
|
||||
Reference in New Issue
Block a user