From 8b40fb771ac8f4005d94083f950ed0ac9a593c02 Mon Sep 17 00:00:00 2001 From: Xy2019 <46518305+Xy2019@users.noreply.github.com> Date: Wed, 19 Apr 2023 18:12:39 +0800 Subject: [PATCH] fix(AFHDS3): Some sensor values displayed incorrectly (#3386) * 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 --- radio/src/pulses/afhds3.cpp | 38 +-- radio/src/telemetry/flysky_ibus.cpp | 390 +++++++++++++++++++++------- radio/src/telemetry/flysky_nv14.cpp | 2 +- 3 files changed, 309 insertions(+), 121 deletions(-) diff --git a/radio/src/pulses/afhds3.cpp b/radio/src/pulses/afhds3.cpp index ebdaa7615..dee612f47 100644 --- a/radio/src/pulses/afhds3.cpp +++ b/radio/src/pulses/afhds3.cpp @@ -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; } } } diff --git a/radio/src/telemetry/flysky_ibus.cpp b/radio/src/telemetry/flysky_ibus.cpp index ab49d43e5..cf885e1c0 100644 --- a/radio/src/telemetry/flysky_ibus.cpp +++ b/radio/src/telemetry/flysky_ibus.cpp @@ -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 \ No newline at end of file diff --git a/radio/src/telemetry/flysky_nv14.cpp b/radio/src/telemetry/flysky_nv14.cpp index ce2f7942f..d65550423 100644 --- a/radio/src/telemetry/flysky_nv14.cpp +++ b/radio/src/telemetry/flysky_nv14.cpp @@ -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;