Bsongis/rssi from 2 different modules (#6711)

RSSI from 2 different modules
This commit is contained in:
Bertrand Songis
2019-09-05 17:24:48 +02:00
committed by GitHub
parent a2e71d43c0
commit 13dee00eed
28 changed files with 190 additions and 120 deletions
+3 -6
View File
@@ -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();
+3 -1
View File
@@ -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)
+12 -6
View File
@@ -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))
+15 -10
View File
@@ -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
+6 -4
View File
@@ -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()
+17 -17
View File
@@ -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;
}
+14 -29
View File
@@ -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];