[NV14] Telemetry fixes, use FlySky RX defaults (#856)

* Port from NV14 repository

* Fix altimeter

* Change AFHDS2A default mode to PWM and iBUS

* Fix datacopy.cpp generation for NV14 Windows build

* Cleanup

* Format

* typo fix

* Sngl for alarm and temperature sensor

* remove temperature sensor
This commit is contained in:
eshifri
2021-10-15 10:18:03 +11:00
committed by GitHub
parent a62545a674
commit b1a41438a3
8 changed files with 213 additions and 146 deletions
@@ -166,7 +166,7 @@ inline int MAX_MOUSE_ANALOG_SOURCES(Board::Type board, int version)
#define MAX_CURVES(board, version) ((version >= 219 || HAS_LARGE_LCD(board)) ? 32 : 16)
#define MAX_GVARS(board, version) 9
#define MAX_SCRIPTS(board) (IS_FAMILY_HORUS_OR_T16(board) ? 9 : 7)
#define MAX_TELEMETRY_SENSORS(board, version) (version <= 218 ? 32 : ((IS_FAMILY_HORUS_OR_T16(board) || IS_TARANIS_X9(board)) ? 60 : 40))
#define MAX_TELEMETRY_SENSORS(board, version) (version <= 218 ? 32 : ((IS_FAMILY_HORUS_OR_T16(board) || IS_TARANIS_X9(board) || IS_FLYSKY_NV14(board)) ? 60 : 40))
#define NUM_PPM_INPUTS(board, version) 16
#define ROTENC_COUNT(board, version) ((IS_STM32(board) && version >= 218) ? 0 : 1)
#define MAX_AUX_TRIMS(board) ((IS_FAMILY_HORUS_OR_T16(board) && !IS_FLYSKY_NV14(board)) ? 2 : 0)
+1 -1
View File
@@ -471,7 +471,7 @@ PACK(struct ModuleData {
void setDefault() {
rx_id[0] = rx_id[1] = rx_id[2] = rx_id[3] = 0;
mode = 0;
mode = 3;
rfPower = 0;
rx_freq[0] = 50;
rx_freq[1] = 0;
+1 -2
View File
@@ -73,7 +73,6 @@ set(FIRMWARE_DEPENDENCIES datacopy)
set(HARDWARE_TOUCH ON)
set(SOFTWARE_KEYBOARD ON)
set(FIRMWARE_DEPENDENCIES datacopy)
add_definitions(-DSTM32F429_439xx -DSDRAM -DCOLORLCD -DLIBOPENUI
-DHARDWARE_TOUCH -DHARDWARE_KEYS -DSOFTWARE_KEYBOARD)
@@ -197,7 +196,7 @@ MACRO(GET_GCC_INCLUDE_PATH gcc_include_path)
# Remove the leading and trailing empty characters
string(REPLACE "\r" "" _line ${_line})
string(SUBSTRING "${_line}" 1 -1 _line)
set(_include_path "${_include_path}" "-I${_line}")
set(_include_path "${_include_path}" "-I\"${_line}\"")
endif()
if(${_line} STREQUAL "#include <...> search starts here:")
+129 -130
View File
@@ -21,45 +21,9 @@
#include "opentx.h"
#include "flysky_nv14.h"
#include "flysky_ibus.h"
#include "dataconstants.h"
enum FlySkySensorType_E {
FLYSKY_SENSOR_RX_VOLTAGE=0,
FLYSKY_SENSOR_RX_SIGNAL,
FLYSKY_SENSOR_RX_RSSI,
FLYSKY_SENSOR_RX_NOISE,
FLYSKY_SENSOR_RX_SNR,
FLYSKY_SENSOR_TEMP,
FLYSKY_SENSOR_EXT_VOLTAGE,
FLYSKY_SENSOR_MOTO_RPM,
FLYSKY_SENSOR_PRESSURE,
FLYSKY_SENSOR_GPS
};
typedef struct FLYSKY_GPS_INFO_S {
uint8_t position_fix;
uint8_t satell_cnt;
uint8_t latitude[4];
uint8_t longtitude[4];
uint8_t altitude[4];
uint8_t g_speed[2];
uint8_t direction[2];
} gps_info_t;
typedef struct FLYSKY_SENSOR_DATA_S {
uint8_t sensor_type;
uint8_t sensor_id;
uint8_t voltage[2];
uint8_t signal;
uint8_t rssi[2];
uint8_t noise[2];
uint8_t snr[2];
uint8_t temp[2];
uint8_t ext_voltage[2];
uint8_t moto_rpm[2];
uint8_t pressure_value[2];
gps_info_t gps_info;
} rx_sensor_t;
#define FLYSKY_FIXED_RX_VOLTAGE (uint8_t)(FLYSKY_SENSOR_RX_VOLTAGE + (uint8_t)0xA0)
@@ -72,6 +36,7 @@ typedef struct FLYSKY_SENSOR_DATA_S {
#define DECIMAL(val) (val >> FIXED_PRECISION)
#define R_DIV_G_MUL_10_Q15 UINT64_C(9591506)
#define INV_LOG2_E_Q1DOT31 UINT64_C(0x58b90bfc) // Inverse log base 2 of e
#define PRESSURE_MASK 0x7FFFF
struct FlyskyNv14Sensor {
const uint16_t id;
@@ -91,6 +56,8 @@ union nv14SensorData {
uint32_t UINT32;
};
FlyskyNv14Sensor defaultNv14Sensor = {0, 0, "UNKNOWN", UNIT_RAW, 0, 0, 2, false};
const FlyskyNv14Sensor Nv14Sensor[]=
{
{FLYSKY_FIXED_RX_VOLTAGE, 0, STR_SENSOR_A1, UNIT_VOLTS, 2, 0, 2, false},
@@ -103,79 +70,34 @@ const FlyskyNv14Sensor Nv14Sensor[]=
{FLYSKY_SENSOR_EXT_VOLTAGE,0, STR_SENSOR_A3, UNIT_VOLTS, 2, 0, 2, false},
{FLYSKY_SENSOR_MOTO_RPM, 0, STR_SENSOR_RPM, UNIT_RPMS, 0, 0, 2, false},
{FLYSKY_SENSOR_PRESSURE, 0, STR_SENSOR_PRES, UNIT_RAW, 1, 0, 2, false},
{FLYSKY_SENSOR_PRESSURE, 1, STR_SENSOR_ALT, UNIT_METERS, 0, 0, 2, true},
{FLYSKY_SENSOR_PRESSURE, 1, STR_SENSOR_ALT, UNIT_METERS, 2, 0, 2, true},
// {FLYSKY_SENSOR_PRESSURE, 2, STR_SENSOR_TEMP2, UNIT_CELSIUS, 1, 0, 4, true},
{FLYSKY_SENSOR_GPS, 1, STR_SENSOR_SATELLITES, UNIT_RAW, 0, 0, 1, false},
{FLYSKY_SENSOR_GPS, 2, STR_SENSOR_GPS, UNIT_GPS_LATITUDE, 0, 1, 4, true},
{FLYSKY_SENSOR_GPS, 3, STR_SENSOR_GPS, UNIT_GPS_LONGITUDE, 0, 5, 4, true},
{FLYSKY_SENSOR_GPS, 4, STR_SENSOR_ALT, UNIT_METERS, 0, 8, 2, true},
{FLYSKY_SENSOR_GPS, 5, STR_SENSOR_GSPD, UNIT_KMH, 1, 10, 2, false},
{FLYSKY_SENSOR_GPS, 6, STR_SENSOR_HDG, UNIT_DEGREE, 3, 12, 2, false},
// {FLYSKY_SENSOR_SYNC, 0, "Sync", UNIT_RAW, 0, 0, 2, false},
defaultNv14Sensor
};
FlyskyNv14Sensor defaultNv14Sensor = {0, 0, "UNKNOWN", UNIT_RAW, 0, 0, 2, false};
const signed short tAltitude[225] = {
20558, 20357, 20158, 19962, 19768, 19576, 19387, 19200, 19015, 18831, 18650,
18471, 18294, 18119, 17946, 17774, 17604, 17436, 17269, 17105, 16941, 16780,
16619, 16461, 16304, 16148, 15993, 15841, 15689, 15539, 15390, 15242, 15096,
14950, 14806, 14664, 14522, 14381, 14242, 14104, 13966, 13830, 13695, 13561,
13428, 13296, 13165, 13035, 12906, 12777, 12650, 12524, 12398, 12273, 12150,
12027, 11904, 11783, 11663, 11543, 11424, 11306, 11189, 11072, 10956, 10841,
10726, 10613, 10500, 10387, 10276, 10165, 10054, 9945, 9836, 9727, 9620,
9512, 9406, 9300, 9195, 9090, 8986, 8882, 8779, 8677, 8575, 8474,
8373, 8273, 8173, 8074, 7975, 7877, 7779, 7682, 7585, 7489, 7394,
7298, 7204, 7109, 7015, 6922, 6829, 6737, 6645, 6553, 6462, 6371,
6281, 6191, 6102, 6012, 5924, 5836, 5748, 5660, 5573, 5487, 5400,
5314, 5229, 5144, 5059, 4974, 4890, 4807, 4723, 4640, 4557, 4475,
4393, 4312, 4230, 4149, 4069, 3988, 3908, 3829, 3749, 3670, 3591,
3513, 3435, 3357, 3280, 3202, 3125, 3049, 2972, 2896, 2821, 2745,
2670, 2595, 2520, 2446, 2372, 2298, 2224, 2151, 2078, 2005, 1933,
1861, 1789, 1717, 1645, 1574, 1503, 1432, 1362, 1292, 1222, 1152,
1082, 1013, 944, 875, 806, 738, 670, 602, 534, 467, 399,
332, 265, 199, 132, 66, 0, -66, -131, -197, -262, -327,
-392, -456, -521, -585, -649, -713, -776, -840, -903, -966, -1029,
-1091, -1154, -1216, -1278, -1340, -1402, -1463, -1525, -1586, -1647, -1708,
-1769, -1829, -1889, -1950, -2010
};
extern uint32_t NV14internalModuleFwVersion;
signed short CalculateAltitude(unsigned int Pressure,
unsigned int SeaLevelPressure)
extern int32_t getALT(uint32_t value);
signed short CalculateAltitude(unsigned int pressure)
{
unsigned int Index;
signed int Altitude1;
signed int Altitude2;
unsigned int Decimal;
unsigned long Ratio;
Ratio = (((unsigned long long)Pressure << 16) + (SeaLevelPressure / 2)) /
SeaLevelPressure;
if (Ratio < ((1 << 16) * 250 / 1000)) { // 0.250 inclusive
Ratio = (1 << 16) * 250 / 1000;
} else if (Ratio > (1 << 16) * 1125 / 1000 - 1) { // 1.125 non-inclusive
Ratio = (1 << 16) * 1125 / 1000 - 1;
}
// from 0.000 (inclusive) to 0.875 (non-inclusive)
Ratio -= (1 << 16) * 250 / 1000;
Index = Ratio >> 8;
Decimal = Ratio & ((1 << 8) - 1);
Altitude1 = tAltitude[Index];
Altitude2 = Altitude1 - tAltitude[Index + 1];
Altitude1 = Altitude1 - (Altitude2 * Decimal + (1 << 7)) / (1 << 8);
if (Altitude1 >= 0) {
return ((Altitude1 + 1) / 2);
} else {
return ((Altitude1 - 1) / 2);
}
int32_t alt = getALT(pressure);
return alt;
}
const FlyskyNv14Sensor* getFlyskyNv14Sensor(uint16_t id, uint8_t subId)
{
for (unsigned index = 0; index < *(&Nv14Sensor + 1) - Nv14Sensor; index++) {
if (Nv14Sensor[index].id == id && Nv14Sensor[index].subId == subId) {
return &Nv14Sensor[index];
for (const FlyskyNv14Sensor* sensor = Nv14Sensor; sensor->id; sensor++) {
if (sensor->id == id && sensor->subId == subId) {
return sensor;
}
}
return &defaultNv14Sensor;
@@ -190,6 +112,10 @@ void flySkyNv14SetDefault(int index, uint8_t id, uint8_t subId,
telemetrySensor.instance = instance;
const FlyskyNv14Sensor* sensor = getFlyskyNv14Sensor(id, subId);
telemetrySensor.init(sensor->name, sensor->unit, sensor->precision);
if (sensor->unit == UNIT_RPMS) {
telemetrySensor.custom.ratio = 1;
telemetrySensor.custom.offset = 1;
}
storageDirty(EE_MODEL);
}
@@ -206,46 +132,119 @@ int32_t GetSensorValueFlySkyNv14(const FlyskyNv14Sensor* sensor,
else if (sensor->bytes == 4)
value = sensorData->UINT32;
if (sensor->id == FLYSKY_SENSOR_RX_RSSI) {
if (value < -200) value = -200;
#if defined(PCBNV14)
if (!g_model.rssiAlarms.flysky_telemetry)
#endif
{
value += 200;
value /= 2;
// For older RF module FW Sgml is in [0, 10] range
// and we need to use RSSI for alarm
if (NV14internalModuleFwVersion < 0x1000E) {
if (sensor->id == FLYSKY_SENSOR_RX_RSSI) {
if (value < -200) value = -200;
// if g_model.rssiAlarms.flysky_telemetry == 1
// RSSI will be kept within native FlySky range [-90, -60]
if (!g_model.rssiAlarms.flysky_telemetry) {
value += 200;
value /= 2;
}
telemetryData.rssi.set(value);
}
TRACE("RSSI: %d", value);
telemetryData.rssi.set(value);
}
if (sensor->id == FLYSKY_SENSOR_PRESSURE && sensor->subId != 0) {
value = CalculateAltitude(value, 101325);
value = (value + 500) / 1000;
} else if (sensor->id == FLYSKY_SENSOR_RX_SIGNAL) {
telemetryData.rssi.set(value);
}
if (sensor->id == FLYSKY_SENSOR_PRESSURE) {
switch(sensor->subId)
{
case 0:
value = value & PRESSURE_MASK;
break;
case 1:
value = CalculateAltitude(value);
break;
case 2:
// TO DO: fix temperature calculation
value = (int16_t)(value >> 19) + 150;// - 400;
break;
}
}
return value;
}
void flySkyNv14ProcessTelemetryPacket(uint8_t* ptr, uint8_t len)
// Module pulse synchronization
#define SAFE_SYNC_LAG 800 /* us */
#define SYNC_UPDATE_TIMEOUT 200 /* *10ms */
#define AFHDS2_SYNC_SAMPLES 8
#define AFHDS2_NEGATIVE_SYNC_LIMIT (AFHDS2_PERIOD - SAFE_SYNC_LAG)
int16_t syncAfhds2min = 0;
int16_t syncAfhds2max = 0;
unsigned currentSyncIndex;
void flySkyNv14Sync(int16_t delayValue)
{
uint8_t sensorType = *ptr++;
uint8_t sensorID = *ptr++;
TRACE("ID[%02X] Type[%02X] len=%d", ptr[1], ptr[0], len);
// remove unknown sensor (strange values)
if (sensorType == 0xEE) return;
if (sensorType == FLYSKY_SENSOR_RX_VOLTAGE)
sensorType = FLYSKY_FIXED_RX_VOLTAGE;
for (const FlyskyNv14Sensor sensor : Nv14Sensor) {
if (sensor.id == sensorType) {
int32_t value = GetSensorValueFlySkyNv14(&sensor, ptr);
TRACE("[%02X] id=%02X(%s) subId=%02X value=%d",
sensorID, sensor.id, sensor.name, sensor.subId, value);
setTelemetryValue(PROTOCOL_TELEMETRY_FLYSKY_NV14, sensor.id, sensor.subId,
sensorID, value, sensor.unit, sensor.precision);
}
if (delayValue > AFHDS2_NEGATIVE_SYNC_LIMIT) {
delayValue -= AFHDS2_PERIOD;
}
if (currentSyncIndex == 0) {
syncAfhds2min = AFHDS2_PERIOD;
syncAfhds2max = -SAFE_SYNC_LAG;
}
telemetryStreaming = TELEMETRY_TIMEOUT10ms;
if (delayValue > syncAfhds2max) {
syncAfhds2max = delayValue;
}
if (delayValue < syncAfhds2min) {
syncAfhds2min = delayValue;
}
if (currentSyncIndex++ == AFHDS2_SYNC_SAMPLES) {
currentSyncIndex = 0;
// check against to late delivered frames up to 800us, some frames still in
// range
if (syncAfhds2min < 0 && syncAfhds2max < SAFE_SYNC_LAG) {
getModuleSyncStatus(INTERNAL_MODULE)
.update(AFHDS2_PERIOD, (syncAfhds2min - 100) + SAFE_SYNC_LAG);
} else if (syncAfhds2max > SAFE_SYNC_LAG + 100) { // > 900us
if (syncAfhds2min > 100) { // never sync if last registred value is below
// 100us - we are to close to perfect time
getModuleSyncStatus(INTERNAL_MODULE)
.update(AFHDS2_PERIOD, (syncAfhds2min - 100) + SAFE_SYNC_LAG);
} else if (syncAfhds2min < 0) {
getModuleSyncStatus(INTERNAL_MODULE)
.update(AFHDS2_PERIOD, (syncAfhds2max - 900) + SAFE_SYNC_LAG);
}
}
}
}
void flySkyNv14ProcessTelemetryPacket(const uint8_t* ptr, uint8_t size)
{
uint8_t sensorID = ptr[0];
uint8_t instnace = ptr[1];
int sensorCount = 0;
if (sensorID != FLYSKY_SENSOR_SYNC) sensorCount++;
// native telemetry for 1.1.2
if (NV14internalModuleFwVersion >= 0x010102) {
if (sensorID == FLYSKY_SENSOR_SYNC)
flySkyNv14Sync((int16_t)(ptr[3] << 8 | ptr[2]));
uint8_t frameType = 0xAA;
if (size > 4) {
frameType = 0xAC;
} else if (size != 4) {
return;
}
processFlySkySensor(ptr, frameType);
} else {
if (sensorID == FLYSKY_SENSOR_RX_VOLTAGE)
sensorID = FLYSKY_FIXED_RX_VOLTAGE;
for (const FlyskyNv14Sensor* sensor = Nv14Sensor; sensor->id; sensor++) {
if (sensor->id == sensorID) {
int32_t value = GetSensorValueFlySkyNv14(sensor, ptr + 2);
setTelemetryValue(PROTOCOL_TELEMETRY_FLYSKY_NV14, sensor->id,
sensor->subId, instnace, value, sensor->unit,
sensor->precision);
if (sensor->id == FLYSKY_SENSOR_SYNC) flySkyNv14Sync(value);
}
}
}
if (sensorCount) {
telemetryStreaming = TELEMETRY_TIMEOUT10ms;
}
}
+44 -1
View File
@@ -22,7 +22,50 @@
#ifndef __FLYSKY_NV14_H__
#define __FLYSKY_NV14_H__
enum FlySkySensorType_E {
FLYSKY_SENSOR_RX_VOLTAGE,
FLYSKY_SENSOR_RX_SIGNAL,
FLYSKY_SENSOR_RX_RSSI,
FLYSKY_SENSOR_RX_NOISE,
FLYSKY_SENSOR_RX_SNR,
FLYSKY_SENSOR_TEMP,
FLYSKY_SENSOR_EXT_VOLTAGE,
FLYSKY_SENSOR_MOTO_RPM,
FLYSKY_SENSOR_PRESSURE,
FLYSKY_SENSOR_GPS,
FLYSKY_SENSOR_SYNC = 0xEE,
};
typedef struct FLYSKY_GPS_INFO_S {
uint8_t position_fix;
uint8_t satell_cnt;
uint8_t latitude[4];
uint8_t longtitude[4];
uint8_t altitude[4];
uint8_t g_speed[2];
uint8_t direction[2];
} gps_info_t;
typedef struct FLYSKY_SENSOR_DATA_S {
uint8_t sensor_type;
uint8_t sensor_id;
uint8_t voltage[2];
uint8_t signal;
uint8_t rssi[2];
uint8_t noise[2];
uint8_t snr[2];
uint8_t temp[2];
uint8_t ext_voltage[2];
uint8_t moto_rpm[2];
uint8_t pressure_value[2];
gps_info_t gps_info;
} rx_sensor_t;
void flySkyNv14SetDefault(int index, uint8_t id, uint8_t subId, uint8_t instance);
void flySkyNv14ProcessTelemetryPacket(uint8_t * ptr, uint8_t len );
void flySkyNv14ProcessTelemetryPacket(const uint8_t * ptr, uint8_t SensorType );
void processInternalFlySkyTelemetryData(uint8_t byte);
uint8_t intmoduleGetByte(uint8_t * byte);
extern bool syncAfhds2Module;
#endif
+23 -9
View File
@@ -75,6 +75,13 @@ void processTelemetryData(uint8_t data)
}
#endif
#if defined(AFHDS2)
if(telemetryProtocol == PROTOCOL_TELEMETRY_FLYSKY_NV14) {
processInternalFlySkyTelemetryData(data);
return;
}
#endif
#if defined(AFHDS3)
if (telemetryProtocol == PROTOCOL_TELEMETRY_AFHDS3) {
afhds3::processTelemetryData(EXTERNAL_MODULE, data, telemetryRxBuffer, telemetryRxBufferCount, TELEMETRY_RX_PACKET_SIZE);
@@ -151,6 +158,15 @@ void telemetryWakeup()
}
#endif
#if defined(STM32)
if (telemetryGetByte(&data)) {
LOG_TELEMETRY_WRITE_START();
do {
processTelemetryData(data);
LOG_TELEMETRY_WRITE_BYTE(data);
} while (telemetryGetByte(&data));
}
#if defined(PCBNV14)
if (//!moduleUpdateActive(INTERNAL_MODULE) &&
moduleState[INTERNAL_MODULE].protocol == PROTOCOL_CHANNELS_AFHDS2A &&
@@ -163,15 +179,6 @@ void telemetryWakeup()
}
#endif
#if defined(STM32)
if (telemetryGetByte(&data)) {
LOG_TELEMETRY_WRITE_START();
do {
processTelemetryData(data);
LOG_TELEMETRY_WRITE_BYTE(data);
} while (telemetryGetByte(&data));
}
#if defined(MULTI_PROTOLIST)
if (isModuleMultimodule(EXTERNAL_MODULE) &&
(moduleState[EXTERNAL_MODULE].protocol ==
@@ -369,6 +376,13 @@ void telemetryInit(uint8_t protocol)
}
#endif
#if defined(AFHDS2)
else if (protocol == PROTOCOL_TELEMETRY_FLYSKY_NV14) {
telemetryPortInit(INTMODULE_USART_AFHDS2_BAUDRATE, TELEMETRY_SERIAL_DEFAULT);
telemetryPortSetDirectionInput();
}
#endif
else {
telemetryPortInit(FRSKY_SPORT_BAUDRATE, TELEMETRY_SERIAL_WITHOUT_DMA);
#if defined(LUA)
+12
View File
@@ -133,6 +133,12 @@ inline const char* getRssiLabel()
if (telemetryProtocol == PROTOCOL_TELEMETRY_GHOST) {
return "RQly";
}
#endif
#if defined (PCBNV14)
extern uint32_t NV14internalModuleFwVersion;
if ( (telemetryProtocol == PROTOCOL_TELEMETRY_FLYSKY_NV14)
&& (NV14internalModuleFwVersion >= 0x1000E) )
return "Sgnl";
#endif
return "RSSI";
}
@@ -174,6 +180,12 @@ inline uint8_t modelTelemetryProtocol()
}
#endif
#if defined(AFHDS2)
if (isModuleAFHDS2A(INTERNAL_MODULE)) {
return PROTOCOL_TELEMETRY_FLYSKY_NV14;
}
#endif
// default choice
return PROTOCOL_TELEMETRY_FRSKY_SPORT;
}
+2 -2
View File
@@ -554,9 +554,9 @@ int setTelemetryValue(TelemetryProtocol protocol, uint16_t id, uint8_t subId, ui
break;
#endif
#if defined(MULTIMODULE) || defined(AFHDS3)
#if defined(MULTIMODULE) || defined(AFHDS3) || defined(AFHDS2)
case PROTOCOL_TELEMETRY_FLYSKY_IBUS:
flySkySetDefault(index,id, subId, instance);
flySkySetDefault(index, id, subId, instance);
break;
#endif