Bsongis/rssi from 2 different modules (#6711)
RSSI from 2 different modules
This commit is contained in:
@@ -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();
|
||||
|
||||
@@ -214,7 +214,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,16 @@ void sportProcessTelemetryPacketWithoutCrc(uint8_t origin, const uint8_t * packe
|
||||
#endif
|
||||
|
||||
if (primId == DATA_FRAME) {
|
||||
uint8_t originMask = (origin == TELEMETRY_ENDPOINT_SPORT ? 0x02 : origin >> 2);
|
||||
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;
|
||||
}
|
||||
if (g_model.rssiSource) {
|
||||
TelemetrySensor * sensor = &g_model.telemetrySensors[g_model.rssiSource - 1];
|
||||
if (sensor->isSameInstance(PROTOCOL_TELEMETRY_FRSKY_SPORT, instance)) {
|
||||
@@ -194,7 +198,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 +208,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))
|
||||
|
||||
@@ -148,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);
|
||||
}
|
||||
|
||||
@@ -205,22 +206,26 @@ 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 (telemetryStreaming > 0) {
|
||||
telemetryStreaming--;
|
||||
}
|
||||
else {
|
||||
#if !defined(SIMU)
|
||||
telemetryData.rssi.reset();
|
||||
#endif
|
||||
for (int i=0; i<MAX_TELEMETRY_SENSORS; i++) {
|
||||
telemetryItems[i].setOld();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -228,8 +233,8 @@ void telemetryReset()
|
||||
{
|
||||
memclear(&telemetryData, sizeof(telemetryData));
|
||||
|
||||
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
|
||||
|
||||
@@ -31,6 +31,11 @@
|
||||
|
||||
extern uint8_t telemetryStreaming; // >0 (true) == data is streaming in. 0 = no data detected for some time
|
||||
|
||||
inline bool TELEMETRY_STREAMING()
|
||||
{
|
||||
return telemetryStreaming > 0;
|
||||
}
|
||||
|
||||
enum TelemetryStates {
|
||||
TELEMETRY_INIT,
|
||||
TELEMETRY_OK,
|
||||
@@ -38,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
|
||||
@@ -139,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;
|
||||
}
|
||||
|
||||
|
||||
@@ -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,28 @@ class TelemetryItem
|
||||
|
||||
inline bool isAvailable()
|
||||
{
|
||||
return (lastReceived != TELEMETRY_VALUE_UNAVAILABLE);
|
||||
return (timeout != TELEMETRY_SENSOR_TIMEOUT_UNAVAILABLE);
|
||||
}
|
||||
|
||||
inline bool isOld()
|
||||
{
|
||||
return (lastReceived == TELEMETRY_VALUE_OLD);
|
||||
}
|
||||
|
||||
inline bool hasReceiveTime()
|
||||
{
|
||||
return (lastReceived < TELEMETRY_VALUE_TIMER_CYCLE);
|
||||
}
|
||||
|
||||
inline uint8_t getDelaySinceLastValue()
|
||||
{
|
||||
// assumes lastReceived is not a special value (OLD / UNAVAILABLE)
|
||||
return (now() - lastReceived) & (TELEMETRY_VALUE_TIMER_CYCLE - 1);
|
||||
return (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