piotr/edgetx-gx12
1
0
Fork 0
Code Issues 2 Pull Requests Actions Packages Projects Releases Wiki Activity

fix(AFHDS3): Some sensor values displayed incorrectly (#3386)

Browse Source 
* Fix the bug that some sensor values are displayed incorrectly

* Fix typos

* Fixed some units problem.

* Unit of SNR should be db and not dbm.

* Fixed Alt scaling in NV14.

* Fixed the temperature and voltage error of the FRM303 module.

* Fixed temperature data error.

---------

Co-authored-by: Richard Li <richard.li@ces.hk>
This commit is contained in:
Xy2019
2023-04-19 20:12:39 +10:00
committed by GitHub
co-authored by Richard Li
parent 7e9ac82497
commit 8b40fb771a
3 changed files with 309 additions and 121 deletions
Download Patch File Download Diff File Expand all files Collapse all files
radio/src/pulses/afhds3.cpp
+8 -30
View File
@@ -40,7 +40,7 @@
#define FAILSAFE_NOPULSES_VALUE 0x8001
//get channel value outside of afhds3 namespace
int32_t getChannelValue(uint8_t channel);
void processFlySkyAFHDS3Sensor(const uint8_t * packet, uint8_t type);
void processFlySkySensor(const uint8_t * packet, uint8_t type);
namespace afhds3
@@ -656,37 +656,15 @@ void ProtoState::parseData(uint8_t* rxBuffer, uint8_t rxBufferCount)
if (telemetry[0] == 0x22) {
telemetry++;
while (telemetry < rxBuffer + rxBufferCount) {
uint8_t length = telemetry[0];
uint8_t id = telemetry[1];
if (id == 0xFE) {
id = 0xF7; //use new id because format is different
}
if (length == 0 || telemetry + length > rxBuffer + rxBufferCount) {
uint8_t len = telemetry[0];
if (len < 4 || telemetry + len > rxBuffer + rxBufferCount)
{
break;
}
if (length == 4) {
//one byte value fill missing byte
uint8_t data[] = { id, telemetry[2], telemetry[3], 0 };
::processFlySkySensor(data, 0xAA);
}
if (length == 5) {
if (id == 0xFA) {
telemetry[1] = 0xF8; //remap to afhds3 snr
}
::processFlySkySensor(telemetry + 1, 0xAA);
}
else if (length == 6 && id == FRM302_STATUS) {
//convert to ibus
uint16_t t = (uint16_t) (((int16_t) telemetry[3] * 10) + 400);
uint8_t dataTemp[] = { ++id, telemetry[2], (uint8_t) (t & 0xFF), (uint8_t) (t >> 8) };
::processFlySkySensor(dataTemp, 0xAA);
uint8_t dataVoltage[] = { ++id, telemetry[2], telemetry[4], telemetry[5] };
::processFlySkySensor(dataVoltage, 0xAA);
}
else if (length == 7) {
::processFlySkySensor(telemetry + 1, 0xAC);
}
telemetry += length;
telemetry[0] = 0;
::processFlySkyAFHDS3Sensor( telemetry, len-3 );
telemetry += len;
}
}
}
radio/src/telemetry/flysky_ibus.cpp
+300 -90
View File
@@ -32,15 +32,87 @@
#define R_DIV_G_MUL_10_Q15 (uint64_t)9591506
#define INV_LOG2_E_Q1DOT31 (uint64_t)0x58b90bfc // Inverse log base 2 of e
#define PRESSURE_MASK 0x7FFFF
#define REMAP_CONST 0x1000 // Some part of OpenTX does not like sensor with id and instance 0, remap to 0x1000
struct FlySkySensor
{
const uint16_t id;
const uint16_t type;
const char * name;
const TelemetryUnit unit;
const uint8_t precision;
};
// telemetry sensors type
enum
{
SENSOR_TYPE_RX_VOL= 0x00 | REMAP_CONST, // RX supply voltage, remapped
SENSOR_TYPE_TEMPERATURE = 0x01, // Temperature
SENSOR_TYPE_MOT = 0x02, // RPM
SENSOR_TYPE_EXT_VOL = 0x03, // Sensor voltage
SENSOR_TYPE_BVD = 0x103, // built-in voltage measurement of the receiver (the highest bit of ID is 1), which distinguishes independent voltage sensors.
SENSOR_TYPE_GYROSCOPE_1_AXIS = 0x04, // signed short in 0.1 degrees per second, 0x8000 if unknown
SENSOR_TYPE_GPS = 0x40, // GPS
SENSOR_TYPE_PRES = 0x41, // Pressure
SENSOR_TYPE_ALT = 0x1041, // virtual
SENSOR_TYPE_RF_MODULE = 0x56, // Ext flysky rf module sensor(voltage and temperature)
SENSOR_TYPE_RF_MODULE_TEMP = 0x1056, // virtual
SENSOR_TYPE_RF_MODULE_VOL = 0x2056, // virtual
SENSOR_TYPE_TX_V = 0x7F, // TX Voltage
SENSOR_TYPE_ANALOG_SERVO =0xF0, // IBUS2 device
SENSOR_TYPE_DIGITAL_SERVO =0xF1, // IBUS2 device
SENSOR_TYPE_RX_SNR = 0xFA, // SNR
SENSOR_TYPE_RX_NOISE = 0xFB, // Noise
SENSOR_TYPE_RX_RSSI = 0xFC, // RSSI
SENSOR_TYPE_RX_ERR_RATE = 0xFE, // Error rate
SENSOR_TYPE_SIGNAL_STRENGTH = 0xFE, // Error rate
SENSOR_TYPE_END = 0xFF,
/////////////////////////////////////////////////////////////////////////////
AFHDS2A_ID_BAT_CURR = 0x05, // battery current A * 100
AFHDS2A_ID_FUEL = 0x06, // remaining battery percentage / mah drawn otherwise or fuel level no unit!
AFHDS2A_ID_RPM = 0x07, // throttle value / battery capacity
AFHDS2A_ID_CMP_HEAD = 0x08, // Heading 0..360 deg, 0=north 2bytes
AFHDS2A_ID_CLIMB_RATE = 0x09, // 2 bytes m/s *100 signed
AFHDS2A_ID_COG = 0x0A, // 2 bytes Course over ground(NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. unknown max uint
AFHDS2A_ID_GPS_STATUS = 0x0B, // 2 bytes
AFHDS2A_ID_ACC_X = 0x0C, // 2 bytes m/s *100 signed
AFHDS2A_ID_ACC_Y = 0x0D, // 2 bytes m/s *100 signed
AFHDS2A_ID_ACC_Z = 0x0E, // 2 bytes m/s *100 signed
AFHDS2A_ID_ROLL = 0x0F, // 2 bytes deg *100 signed
AFHDS2A_ID_PITCH = 0x10, // 2 bytes deg *100 signed
AFHDS2A_ID_YAW = 0x11, // 2 bytes deg *100 signed
AFHDS2A_ID_VERTICAL_SPEED = 0x12, // 2 bytes m/s *100 signed
AFHDS2A_ID_GROUND_SPEED = 0x13, // 2 bytes m/s *100 different unit than build-in sensor
AFHDS2A_ID_GPS_DIST = 0x14, // 2 bytes distance from home m unsigned
AFHDS2A_ID_ARMED = 0x15, // 2 bytes
AFHDS2A_ID_FLIGHT_MODE = 0x16, // 2 bytes
AFHDS2A_ID_ODO1 = 0x7C, // Odometer1
AFHDS2A_ID_ODO2 = 0x7D, // Odometer2
AFHDS2A_ID_SPE = 0x7E, // Speed 2 bytes km/h
AFHDS2A_ID_GPS_LAT = 0x80, // 4bytes signed WGS84 in degrees * 1E7
AFHDS2A_ID_GPS_LON = 0x81, // 4bytes signed WGS84 in degrees * 1E7
AFHDS2A_ID_GPS_ALT = 0x82, // 4bytes signed!!! GPS alt m*100
AFHDS2A_ID_ALT = 0x83, // 4bytes signed!!! Alt m*100
AFHDS2A_ID_RX_SIG_AFHDS3 = 0xF7, // SIG
AFHDS2A_ID_RX_SNR_AFHDS3 = 0xF8, // SNR
AFHDS2A_ID_ALT_FLYSKY = 0xF9, // Altitude 2 bytes signed in m - used in FlySky native TX
// AC type telemetry with multiple values in one packet
AFHDS2A_ID_GPS_FULL = 0xFD,
AFHDS2A_ID_VOLT_FULL = 0xF0,
AFHDS2A_ID_ACC_FULL = 0xEF,
AFHDS2A_ID_TX_RSSI = 0x200, // Pseudo id outside 1 byte range of FlySky sensors
};
// telemetry sensors ID
enum
{
@@ -49,36 +121,13 @@ enum
AFHDS2A_ID_MOT = 0x02, // RPM
AFHDS2A_ID_EXTV = 0x03, // External Voltage
AFHDS2A_ID_CELL_VOLTAGE = 0x04, // Avg Cell voltage
AFHDS2A_ID_BAT_CURR = 0x05, // battery current A * 100
AFHDS2A_ID_FUEL = 0x06, // remaining battery percentage / mah drawn otherwise or fuel level no unit!
AFHDS2A_ID_RPM = 0x07, // throttle value / battery capacity
AFHDS2A_ID_CMP_HEAD = 0x08, // Heading 0..360 deg, 0=north 2bytes
AFHDS2A_ID_CLIMB_RATE = 0x09, // 2 bytes m/s *100 signed
AFHDS2A_ID_COG = 0x0A, // 2 bytes Course over ground(NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. unknown max uint
AFHDS2A_ID_GPS_STATUS = 0x0B, // 2 bytes
AFHDS2A_ID_ACC_X = 0x0C, // 2 bytes m/s *100 signed
AFHDS2A_ID_ACC_Y = 0x0D, // 2 bytes m/s *100 signed
AFHDS2A_ID_ACC_Z = 0x0E, // 2 bytes m/s *100 signed
AFHDS2A_ID_ROLL = 0x0F, // 2 bytes deg *100 signed
AFHDS2A_ID_PITCH = 0x10, // 2 bytes deg *100 signed
AFHDS2A_ID_YAW = 0x11, // 2 bytes deg *100 signed
AFHDS2A_ID_VERTICAL_SPEED = 0x12, // 2 bytes m/s *100 signed
AFHDS2A_ID_GROUND_SPEED = 0x13, // 2 bytes m/s *100 different unit than build-in sensor
AFHDS2A_ID_GPS_DIST = 0x14, // 2 bytes distance from home m unsigned
AFHDS2A_ID_ARMED = 0x15, // 2 bytes
AFHDS2A_ID_FLIGHT_MODE = 0x16, // 2 bytes
AFHDS3_FRM_TEMP = 0x57, //virtual
AFHDS3_FRM_EXT_V = 0x58, //virtual
AFHDS2A_ID_PRES = 0x41, // Pressure
AFHDS2A_ID_ODO1 = 0x7C, // Odometer1
AFHDS2A_ID_ODO2 = 0x7D, // Odometer2
AFHDS2A_ID_SPE = 0x7E, // Speed 2 bytes km/h
AFHDS2A_ID_TX_V = 0x7F, // TX Voltage
AFHDS2A_ID_GPS_LAT = 0x80, // 4bytes signed WGS84 in degrees * 1E7
AFHDS2A_ID_GPS_LON = 0x81, // 4bytes signed WGS84 in degrees * 1E7
AFHDS2A_ID_GPS_ALT = 0x82, // 4bytes signed!!! GPS alt m*100
AFHDS2A_ID_ALT = 0x83, // 4bytes signed!!! Alt m*100
AFHDS2A_ID_S84 = 0x84,
AFHDS2A_ID_S85 = 0x85,
AFHDS2A_ID_S86 = 0x86,
@@ -86,70 +135,165 @@ enum
AFHDS2A_ID_S88 = 0x88,
AFHDS2A_ID_S89 = 0x89,
AFHDS2A_ID_S8a = 0x8A,
AFHDS2A_ID_RX_SIG_AFHDS3 = 0xF7, // SIG
AFHDS2A_ID_RX_SNR_AFHDS3 = 0xF8, // SNR
AFHDS2A_ID_ALT_FLYSKY = 0xF9, // Altitude 2 bytes signed in m - used in FlySky native TX
AFHDS2A_ID_RX_SNR = 0xFA, // SNR
AFHDS2A_ID_RX_NOISE = 0xFB, // Noise
AFHDS2A_ID_RX_RSSI = 0xFC, // RSSI
AFHDS2A_ID_RX_ERR_RATE = 0xFE, // Error rate
AFHDS2A_ID_END = 0xFF,
// AC type telemetry with multiple values in one packet
AFHDS2A_ID_GPS_FULL = 0xFD,
AFHDS2A_ID_VOLT_FULL = 0xF0,
AFHDS2A_ID_ACC_FULL = 0xEF,
AFHDS2A_ID_TX_RSSI = 0x200, // Pseudo id outside 1 byte range of FlySky sensors
};
const FlySkySensor flySkySensors[] = {
{AFHDS2A_ID_VOLTAGE | 0x100, STR_SENSOR_A1, UNIT_VOLTS, 2}, // RX Voltage (remapped, really 0x0)
{AFHDS2A_ID_TEMPERATURE, STR_SENSOR_TEMP1, UNIT_CELSIUS, 1}, // Temperature
{AFHDS2A_ID_MOT, STR_SENSOR_RPM, UNIT_RAW, 0}, // RPM
{AFHDS2A_ID_EXTV, STR_SENSOR_A3, UNIT_VOLTS, 2}, // External voltage
{AFHDS2A_ID_CELL_VOLTAGE, STR_SENSOR_CELLS, UNIT_VOLTS, 2}, // Avg Cell voltage
{AFHDS2A_ID_BAT_CURR, STR_SENSOR_CURR, UNIT_AMPS, 2}, // battery current A * 100
{AFHDS2A_ID_FUEL, STR_SENSOR_CAPACITY, UNIT_RAW, 0}, // remaining battery percentage / mah drawn otherwise or fuel level no unit!
{AFHDS2A_ID_RPM, STR_SENSOR_RPM, UNIT_RAW, 0}, // throttle value / battery capacity
{AFHDS2A_ID_CMP_HEAD, STR_SENSOR_HDG, UNIT_DEGREE, 0}, // Heading 0..360 deg, 0=north 2bytes
{AFHDS2A_ID_CLIMB_RATE, STR_SENSOR_VSPD, UNIT_METERS_PER_SECOND, 2}, // 2 bytes m/s *100
{AFHDS2A_ID_COG, STR_SENSOR_HDG, UNIT_DEGREE, 2}, // 2 bytes Course over ground(NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. unknown max uint
{AFHDS2A_ID_GPS_STATUS, STR_SENSOR_SATELLITES, UNIT_RAW, 0}, // 2 bytes
{AFHDS2A_ID_ACC_X, STR_SENSOR_ACCX, UNIT_METERS_PER_SECOND, 2}, // 2 bytes m/s *100 signed
{AFHDS2A_ID_ACC_Y, STR_SENSOR_ACCY, UNIT_METERS_PER_SECOND, 2}, // 2 bytes m/s *100 signed
{AFHDS2A_ID_ACC_Z, STR_SENSOR_ACCZ, UNIT_METERS_PER_SECOND, 2}, // 2 bytes m/s *100 signed
{AFHDS2A_ID_ROLL, STR_SENSOR_ROLL, UNIT_DEGREE, 2}, // 2 bytes deg *100 signed
{AFHDS2A_ID_PITCH, STR_SENSOR_PITCH, UNIT_DEGREE, 2}, // 2 bytes deg *100 signed
{AFHDS2A_ID_YAW, STR_SENSOR_YAW, UNIT_DEGREE, 2}, // 2 bytes deg *100 signed
{AFHDS2A_ID_VERTICAL_SPEED, STR_SENSOR_VSPD, UNIT_METERS_PER_SECOND, 2}, // 2 bytes m/s *100
{AFHDS2A_ID_GROUND_SPEED, STR_SENSOR_GSPD, UNIT_METERS_PER_SECOND, 2}, // 2 bytes m/s *100 different unit than build-in sensor
{AFHDS2A_ID_GPS_DIST, STR_SENSOR_DIST, UNIT_METERS, 0}, // 2 bytes dist from home m unsigned
{AFHDS2A_ID_ARMED, STR_SENSOR_ARM, UNIT_RAW, 0}, // 2 bytes
{AFHDS2A_ID_FLIGHT_MODE, STR_SENSOR_FLIGHT_MODE, UNIT_RAW, 0}, // 2 bytes index
{AFHDS2A_ID_PRES, STR_SENSOR_PRES, UNIT_RAW, 2}, // 4 bytes In fact Temperature + Pressure -> Altitude
{AFHDS2A_ID_PRES | 0x100, STR_SENSOR_TEMP2, UNIT_CELSIUS, 1}, // 2 bytes Temperature
{AFHDS3_FRM_TEMP, STR_SENSOR_TEMP2, UNIT_CELSIUS, 1}, // 2 bytes temperature
{AFHDS3_FRM_EXT_V, STR_SENSOR_TXV, UNIT_VOLTS, 2}, // 2 bytes voltage
{AFHDS2A_ID_ODO1, STR_SENSOR_ODO1, UNIT_METERS, 2}, // 2 bytes Odometer1 -- some magic with 330 needed
{AFHDS2A_ID_ODO2, STR_SENSOR_ODO2, UNIT_METERS, 2}, // 2 bytes Odometer2 -- some magic with 330 needed
{AFHDS2A_ID_SPE, STR_SENSOR_ASPD, UNIT_KMH, 2}, // 2 bytes Speed km/h -- some magic with 330 needed
{AFHDS2A_ID_TX_V, STR_SENSOR_TXV, UNIT_VOLTS, 2}, // TX Voltage
{AFHDS2A_ID_GPS_LAT, STR_SENSOR_GPS, UNIT_RAW, 7}, // 4 bytes signed WGS84 in degrees * 1E7
{AFHDS2A_ID_GPS_LON, STR_SENSOR_GPS, UNIT_RAW, 7}, // 4 bytes signed WGS84 in degrees * 1E7
{AFHDS2A_ID_GPS_ALT, STR_SENSOR_GPSALT, UNIT_METERS, 2}, // 4 bytes signed GPS alt m*100
{AFHDS2A_ID_ALT, STR_SENSOR_ALT, UNIT_METERS, 2}, // 4 bytes signed Alt m*100
{AFHDS2A_ID_RX_SIG_AFHDS3, STR_SENSOR_RX_QUALITY, UNIT_RAW, 0}, // RX error rate
{AFHDS2A_ID_RX_SNR_AFHDS3, STR_SENSOR_RX_SNR, UNIT_DB, 1}, // RX SNR
{AFHDS2A_ID_RX_SNR, STR_SENSOR_RX_SNR, UNIT_DB, 0}, // RX SNR
{AFHDS2A_ID_RX_NOISE, STR_SENSOR_RX_NOISE, UNIT_DB, 0}, // RX Noise
{AFHDS2A_ID_RX_RSSI, STR_SENSOR_RSSI, UNIT_DB, 0}, // RX RSSI (0xfc)
{AFHDS2A_ID_RX_ERR_RATE, STR_SENSOR_RX_QUALITY, UNIT_RAW, 0}, // RX error rate
{AFHDS2A_ID_TX_RSSI, STR_SENSOR_TX_RSSI, UNIT_RAW, 0}, // Pseudo sensor for TRSSI
{0x00, NULL, UNIT_RAW, 0}, // sentinel
const FlySkySensor flySkySensors[] = {
// flysky start
{ SENSOR_TYPE_RX_VOL, STR_SENSOR_A1, UNIT_VOLTS, 2 }, // RX Voltage (remapped, really 0x0)
{ SENSOR_TYPE_TEMPERATURE, STR_SENSOR_TEMP1, UNIT_CELSIUS, 1 }, // Temperature
{ SENSOR_TYPE_MOT, STR_SENSOR_RPM, UNIT_RAW, 0 }, // RPM
{ SENSOR_TYPE_EXT_VOL, STR_SENSOR_A3, UNIT_VOLTS, 2 }, // External voltage
{ SENSOR_TYPE_BVD, "BVD", UNIT_VOLTS, 2 }, // BVD
{ SENSOR_TYPE_GYROSCOPE_1_AXIS, STR_SENSOR_CELLS, UNIT_DEGREE, 1 }, //
{ SENSOR_TYPE_PRES, STR_SENSOR_PRES, UNIT_RAW, 2 }, // 4 bytes In fact Temperature + Pressure -> Altitude
{ SENSOR_TYPE_ALT, STR_SENSOR_ALT, UNIT_METERS, 2 },
{ SENSOR_TYPE_RF_MODULE_TEMP, STR_SENSOR_TEMP2, UNIT_CELSIUS, 0 }, // 1 bytes temperature
{ SENSOR_TYPE_RF_MODULE_VOL, STR_SENSOR_TXV, UNIT_VOLTS, 2 }, // 2 bytes voltage
{ SENSOR_TYPE_TX_V, STR_SENSOR_TXV, UNIT_VOLTS, 2 }, // TX Voltage
{ SENSOR_TYPE_RX_SNR, STR_SENSOR_RX_SNR, UNIT_DB, 0 }, // RX SNR
{ SENSOR_TYPE_RX_NOISE, STR_SENSOR_RX_NOISE, UNIT_DBM, 0 }, // RX Noise
{ SENSOR_TYPE_RX_RSSI, STR_SENSOR_RSSI, UNIT_DBM, 0 }, // RX RSSI (0xfc)
{ SENSOR_TYPE_RX_ERR_RATE, STR_SENSOR_RX_QUALITY, UNIT_PERCENT, 0 }, // RX error rate
////////////////////////////////////////////////////////////////
{ AFHDS2A_ID_BAT_CURR, STR_SENSOR_CURR, UNIT_AMPS, 2 }, // battery current A * 100
{ AFHDS2A_ID_FUEL, STR_SENSOR_CAPACITY, UNIT_RAW, 0 }, // remaining battery percentage / mah drawn otherwise or fuel level no unit!
{ AFHDS2A_ID_RPM, STR_SENSOR_RPM, UNIT_RAW, 0 }, // throttle value / battery capacity
{ AFHDS2A_ID_CMP_HEAD, STR_SENSOR_HDG, UNIT_DEGREE, 0 }, // Heading 0..360 deg, 0=north 2bytes
{ AFHDS2A_ID_CLIMB_RATE, STR_SENSOR_VSPD, UNIT_METERS_PER_SECOND, 2 }, // 2 bytes m/s *100
{ AFHDS2A_ID_COG, STR_SENSOR_HDG, UNIT_DEGREE, 2 }, // 2 bytes Course over ground(NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. unknown max uint
{ AFHDS2A_ID_GPS_STATUS, STR_SENSOR_SATELLITES, UNIT_RAW, 0 }, // 2 bytes
{ AFHDS2A_ID_ACC_X, STR_SENSOR_ACCX, UNIT_METERS_PER_SECOND, 2 }, // 2 bytes m/s *100 signed
{ AFHDS2A_ID_ACC_Y, STR_SENSOR_ACCY, UNIT_METERS_PER_SECOND, 2 }, // 2 bytes m/s *100 signed
{ AFHDS2A_ID_ACC_Z, STR_SENSOR_ACCZ, UNIT_METERS_PER_SECOND, 2 }, // 2 bytes m/s *100 signed
{ AFHDS2A_ID_ROLL, STR_SENSOR_ROLL, UNIT_DEGREE, 2 }, // 2 bytes deg *100 signed
{ AFHDS2A_ID_PITCH, STR_SENSOR_PITCH, UNIT_DEGREE, 2 }, // 2 bytes deg *100 signed
{ AFHDS2A_ID_YAW, STR_SENSOR_YAW, UNIT_DEGREE, 2 }, // 2 bytes deg *100 signed
{ AFHDS2A_ID_VERTICAL_SPEED, STR_SENSOR_VSPD, UNIT_METERS_PER_SECOND, 2 }, // 2 bytes m/s *100
{ AFHDS2A_ID_GROUND_SPEED, STR_SENSOR_GSPD, UNIT_METERS_PER_SECOND, 2 }, // 2 bytes m/s *100 different unit than build-in sensor
{ AFHDS2A_ID_GPS_DIST, STR_SENSOR_DIST, UNIT_METERS, 0 }, // 2 bytes dist from home m unsigned
{ AFHDS2A_ID_ARMED, STR_SENSOR_ARM, UNIT_RAW, 0 }, // 2 bytes
{ AFHDS2A_ID_FLIGHT_MODE, STR_SENSOR_FLIGHT_MODE, UNIT_RAW, 0 }, // 2 bytes index
{ SENSOR_TYPE_PRES | 0x100, STR_SENSOR_TEMP2, UNIT_CELSIUS, 1 }, // 2 bytes Temperature
{ AFHDS2A_ID_ODO1, STR_SENSOR_ODO1, UNIT_METERS, 2 }, // 2 bytes Odometer1 -- some magic with 330 needed
{ AFHDS2A_ID_ODO2, STR_SENSOR_ODO2, UNIT_METERS, 2 }, // 2 bytes Odometer2 -- some magic with 330 needed
{ AFHDS2A_ID_SPE, STR_SENSOR_ASPD, UNIT_KMH, 2 }, // 2 bytes Speed km/h -- some magic with 330 needed
{ AFHDS2A_ID_GPS_LAT, STR_SENSOR_GPS, UNIT_RAW, 7 }, // 4 bytes signed WGS84 in degrees * 1E7
{ AFHDS2A_ID_GPS_LON, STR_SENSOR_GPS, UNIT_RAW, 7 }, // 4 bytes signed WGS84 in degrees * 1E7
{ AFHDS2A_ID_GPS_ALT, STR_SENSOR_GPSALT, UNIT_METERS, 2 }, // 4 bytes signed GPS alt m*100
{ AFHDS2A_ID_ALT, STR_SENSOR_ALT, UNIT_METERS, 2 }, // 4 bytes signed Alt m*100
{ AFHDS2A_ID_RX_SIG_AFHDS3, STR_SENSOR_RX_QUALITY, UNIT_RAW, 0 }, // RX error rate
{ AFHDS2A_ID_RX_SNR_AFHDS3, STR_SENSOR_RX_SNR, UNIT_DB, 1 }, // RX SNR
{ AFHDS2A_ID_TX_RSSI, STR_SENSOR_TX_RSSI, UNIT_RAW, 0 }, // Pseudo sensor for TRSSI
{ 0x00, NULL, UNIT_RAW, 0 }, // sentinel
};
int32_t getALT(uint32_t value);
inline int setFlyskyTelemetryValue( int16_t type, uint8_t instance, int32_t value, uint32_t unit, uint32_t prec)
{
return setTelemetryValue(PROTOCOL_TELEMETRY_FLYSKY_IBUS, type, 0, instance, value, unit, prec );
}
void processFlySkyAFHDS3Sensor(const uint8_t * packet, uint8_t len )
{
uint16_t type = (packet[0] << 8) | packet[1];
type = type ? type : SENSOR_TYPE_RX_VOL; // Remapped
uint8_t id = packet[2];
int32_t value=0;
if(len == 1)
{
value = packet[3];
}
else if (len == 2)
{
value = (packet[4] << 8) | packet[3];
}
else if(len == 4)
{
value = (packet[6] << 24) | (packet[5] << 16) | (packet[4] << 8) | packet[3];
}
else
{
if( SENSOR_TYPE_GPS == type )
{
//INRM301 does not support GPS temporarily.
return;
}
else if( SENSOR_TYPE_RF_MODULE == type )
{
uint8_t data1[] = { (uint8_t)(SENSOR_TYPE_RF_MODULE_TEMP>>8), (uint8_t)SENSOR_TYPE_RF_MODULE_TEMP, id, packet[3] };
uint8_t data2[] = { (uint8_t)(SENSOR_TYPE_RF_MODULE_VOL>>8), (uint8_t)SENSOR_TYPE_RF_MODULE_VOL, id, packet[4], packet[5] };
processFlySkyAFHDS3Sensor(data1, 1 );
processFlySkyAFHDS3Sensor(data2, 2 );
return;
}
else
return;
}
if (SENSOR_TYPE_RX_NOISE == type || SENSOR_TYPE_RX_RSSI == type)
{
value = -value;
}
if ( (SENSOR_TYPE_EXT_VOL == type) )
{
if ( id&0x80 )
{
type = SENSOR_TYPE_BVD;
}
}
else if(SENSOR_TYPE_RX_RSSI == type || SENSOR_TYPE_RX_NOISE == type || SENSOR_TYPE_RX_SNR == type)
{
if( value>=0 )
value = (value+2)/4;
else
value = (value-2)/4;
}
else if (SENSOR_TYPE_RX_ERR_RATE == type)
{
telemetryData.rssi.set( value );
if (value > 0) telemetryStreaming = TELEMETRY_TIMEOUT10ms;
}
else if (SENSOR_TYPE_PRES == type)
{
int32_t alt = getALT(value);
int16_t temp = (value >> 19);
uint8_t data1[] = { (uint8_t)(SENSOR_TYPE_ALT>>8), (uint8_t)(SENSOR_TYPE_ALT&0xff), id, (uint8_t)alt, (uint8_t)(alt>>8), (uint8_t)(alt>>16), (uint8_t)(alt>>24) };
uint8_t data2[] = { (uint8_t)(SENSOR_TYPE_TEMPERATURE>>8), (uint8_t)(SENSOR_TYPE_TEMPERATURE&0xff), id, (uint8_t)temp, (uint8_t)(temp>>8) };
processFlySkyAFHDS3Sensor(data1, 4 );
processFlySkyAFHDS3Sensor(data2, 2 );
value &= PRESSURE_MASK;
}
if(SENSOR_TYPE_TEMPERATURE == type)
{
value -= 400;// Temperature sensors have 40 degree offset
}
for (const FlySkySensor * sensor = flySkySensors; sensor->type; sensor++)
{
if (sensor->type != type) continue;
if (sensor->unit == UNIT_VOLTS) value = (int16_t) value; // Voltage types are unsigned 16bit integers
setFlyskyTelemetryValue(type, id, value, sensor->unit, sensor->precision);
return;
}
//unknown
setFlyskyTelemetryValue(type, id, value, UNIT_RAW, 0);
}
void processFlySkySensor(const uint8_t * packet, uint8_t type)
{
@@ -162,9 +306,9 @@ void processFlySkySensor(const uint8_t * packet, uint8_t type)
if (type == 0xAA)
value = (packet[3] << 8) | packet[2];
else
value = (packet[6] << 24) | (packet[5] << 16) | (packet[4] << 8) | packet[3];
value = (packet[5] << 24) | (packet[4] << 16) | (packet[3] << 8) | packet[2];
if (id == 0) id = 0x100; // Some part of OpenTX does not like sensor with id and instance 0, remap to 0x100
id = id ? id : SENSOR_TYPE_RX_VOL; // Remapped
if (id == AFHDS2A_ID_RX_NOISE || id == AFHDS2A_ID_RX_RSSI) {
value = 135 - value;
@@ -228,8 +372,8 @@ void processFlySkySensor(const uint8_t * packet, uint8_t type)
}
return;
}
for (const FlySkySensor * sensor = flySkySensors; sensor->id; sensor++) {
if (sensor->id != id) continue;
for (const FlySkySensor * sensor = flySkySensors; sensor->type; sensor++) {
if (sensor->type != id) continue;
if (sensor->unit == UNIT_CELSIUS) value -= 400; // Temperature sensors have 40 degree offset
else if (sensor->unit == UNIT_VOLTS) value = (int16_t) value; // Voltage types are unsigned 16bit integers
setTelemetryValue(PROTOCOL_TELEMETRY_FLYSKY_IBUS, id, 0, instance, value, sensor->unit, sensor->precision);
@@ -242,12 +386,12 @@ void processFlySkySensor(const uint8_t * packet, uint8_t type)
void processFlySkyPacket(const uint8_t * packet)
{
// Set TX RSSI Value, reverse MULTIs scaling
setTelemetryValue(PROTOCOL_TELEMETRY_FLYSKY_IBUS, AFHDS2A_ID_TX_RSSI, 0, 0, packet[0], UNIT_RAW, 0);
setFlyskyTelemetryValue(AFHDS2A_ID_TX_RSSI, 0, packet[0], UNIT_RAW, 0);
const uint8_t * buffer = packet + 1;
int sesnor = 0;
while (sesnor++ < 7) {
if (*buffer == AFHDS2A_ID_END) break;
if (*buffer == SENSOR_TYPE_END) break;
processFlySkySensor(buffer, 0xAA);
buffer += 4;
}
@@ -256,11 +400,11 @@ void processFlySkyPacket(const uint8_t * packet)
void processFlySkyPacketAC(const uint8_t * packet)
{
// Set TX RSSI Value, reverse MULTIs scaling
setTelemetryValue(PROTOCOL_TELEMETRY_FLYSKY_IBUS, AFHDS2A_ID_TX_RSSI, 0, 0, packet[0], UNIT_RAW, 0);
setFlyskyTelemetryValue(AFHDS2A_ID_TX_RSSI, 0, packet[0], UNIT_RAW, 0);
const uint8_t * buffer = packet + 1;
while (buffer - packet < 26) //28 + 1(multi TX rssi) - 3(ac header)
{
if (*buffer == AFHDS2A_ID_END) break;
if (*buffer == SENSOR_TYPE_END) break;
uint8_t size = buffer[2];
processFlySkySensor(buffer, 0xAC);
buffer += size + 3;
@@ -272,7 +416,7 @@ void processFlySkyTelemetryData(uint8_t data, uint8_t * rxBuffer, uint8_t &rxBuf
if (rxBufferCount == 0)
return;
if (data == 0xAA || data == 0xAC) {
if (data == 2 || data == 4) {
TRACE("[IBUS] Packet 0x%02X", data);
}
else {
@@ -307,8 +451,8 @@ void processFlySkyTelemetryData(uint8_t data, uint8_t * rxBuffer, uint8_t &rxBuf
const FlySkySensor * getFlySkySensor(uint16_t id)
{
for (const FlySkySensor * sensor = flySkySensors; sensor->id; sensor++) {
if (id == sensor->id)
for (const FlySkySensor * sensor = flySkySensors; sensor->type; sensor++) {
if (id == sensor->type)
return sensor;
}
return nullptr;
@@ -370,6 +514,7 @@ int32_t log2fix(uint32_t x)
return y;
}
#if 0
int32_t getALT(uint32_t value)
{
uint32_t pressurePa = value & PRESSURE_MASK;
@@ -406,3 +551,68 @@ int32_t getALT(uint32_t value)
if (neg ^ tempNegative) result = result * -1;
return result;
}
#else
/*==================================================================================================
table:
Function:
==================================================================================================*/
const int16_t tAltitude[225]=
{ // In half meter unit
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
};
/*==================================================================================================
Name:
Function:
Input:
Output:
==================================================================================================*/
int32_t getALT(uint32_t Pressure)//CalculateAltitude( uint32_t Pressure, uint32_t SeaLevelPressure )
{
uint32_t Index;
int32_t Altitude1;
int32_t Altitude2;
uint32_t Decimal;
uint64_t Ratio;
uint32_t SeaLevelPressure=101320;
Pressure = Pressure & PRESSURE_MASK;
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;
}
Ratio -= ( 1 << 16 ) * 250 / 1000; // from 0.000 (inclusive) to 0.875 (non-inclusive)
Index = Ratio >> 8;
Decimal = Ratio & ( ( 1 << 8 ) - 1 );
Altitude1 = tAltitude[Index];
Altitude2 = Altitude1 - tAltitude[Index + 1];
Altitude1 = Altitude1 - ( Altitude2 * Decimal + ( 1 << 7 ) ) / ( 1 << 8 );
Altitude1 *= 100;
if( Altitude1 >= 0 )
{
return( ( Altitude1 + 1 ) / 2 );
}
else
{
return( ( Altitude1 - 1 ) / 2 );
}
}
#endif
radio/src/telemetry/flysky_nv14.cpp
+1 -1
View File
@@ -172,7 +172,7 @@ int32_t GetSensorValueFlySkyNv14(const FlyskyNv14Sensor* sensor,
value = value & PRESSURE_MASK;
break;
case 1:
value = CalculateAltitude(value);
value = CalculateAltitude(10 * value);
{
tmr10ms_t currTimer = getTicks();
int32_t currAlt = value;