Multi telemetry : add Hitec and improve FlySky (#6835)
* Add Hitec and update Flysky telemetry * Travis (and sky9x) doesn't like cheating ! * Cosmetics
This commit is contained in:
@@ -16,127 +16,253 @@
|
||||
#include "opentx.h"
|
||||
|
||||
/*
|
||||
* Telemetry format from goebish/Deviation/flysky_afhds2a_a7105.c
|
||||
*
|
||||
* format from RX:
|
||||
* AA | TXID | RXID | sensor id | sensor # | value 16 bit big endian | sensor id ......
|
||||
* max 7 sensors per packet
|
||||
*
|
||||
* TXID + RXID are already skipped in MULTI module to save memory+transmission time, format from Multi is:
|
||||
* AA | TX_RSSI | sensor ...
|
||||
*
|
||||
* OpenTX Mapping
|
||||
*
|
||||
* instance = sensor id
|
||||
*
|
||||
* Additional sensors from https://github.com/cleanflight/cleanflight/blob/master/src/main/telemetry/ibus.c
|
||||
*
|
||||
* AA or AC | TX_RSSI | sensor ...
|
||||
*/
|
||||
|
||||
|
||||
#define FLYSKY_TELEMETRY_LENGTH (2+7*4)
|
||||
#define FLYSKY_TELEMETRY_LENGTH (2+7*4) // Should it be 2+7*6???
|
||||
#define ALT_PRECISION 15
|
||||
#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 FlySkySensor {
|
||||
struct FlySkySensor
|
||||
{
|
||||
const uint16_t id;
|
||||
const char *name;
|
||||
const char * name;
|
||||
const TelemetryUnit unit;
|
||||
const uint8_t precision;
|
||||
};
|
||||
|
||||
#define TX_RSSI_ID 300 // Pseudo id outside 1 byte range of FlySky sensors
|
||||
#define FS_ID_TEMP 0x01
|
||||
#define FS_ID_SNR 0xFA
|
||||
#define FS_ID_NOISE 0xFB
|
||||
#define FS_ID_RSSI 0xFC
|
||||
#define FS_ID_ERR 0xFE
|
||||
// telemetry sensors ID
|
||||
enum
|
||||
{
|
||||
AFHDS2A_ID_VOLTAGE = 0x00, // Internal Voltage
|
||||
AFHDS2A_ID_TEMPERATURE = 0x01, // Temperature
|
||||
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
|
||||
|
||||
const FlySkySensor flySkySensors[] = {
|
||||
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
|
||||
|
||||
// RX Voltage (remapped, really 0x0)
|
||||
{0x100, ZSTR_A1, UNIT_VOLTS, 2},
|
||||
// Temperature
|
||||
{FS_ID_TEMP, ZSTR_TEMP1, UNIT_CELSIUS, 1},
|
||||
// RPM
|
||||
{0x02, ZSTR_RPM, UNIT_RAW, 0},
|
||||
// External voltage
|
||||
{0x03, ZSTR_A3, UNIT_VOLTS, 2},
|
||||
// RX SNR
|
||||
{FS_ID_SNR, ZSTR_RX_SNR, UNIT_DB, 0},
|
||||
// RX Noise
|
||||
{FS_ID_NOISE, ZSTR_RX_NOISE, UNIT_DB, 0},
|
||||
// RX RSSI (0xfc)
|
||||
{FS_ID_RSSI, ZSTR_RSSI, UNIT_DB, 0},
|
||||
// RX error rate
|
||||
{FS_ID_ERR, ZSTR_RX_QUALITY, UNIT_RAW, 0},
|
||||
// 0xff is an unused sensor slot
|
||||
// Pseudo sensor for TRSSI
|
||||
{TX_RSSI_ID, ZSTR_TX_RSSI, UNIT_RAW, 0},
|
||||
// sentinel
|
||||
{0x00, NULL, UNIT_RAW, 0},
|
||||
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,
|
||||
AFHDS2A_ID_S87 = 0x87,
|
||||
AFHDS2A_ID_S88 = 0x88,
|
||||
AFHDS2A_ID_S89 = 0x89,
|
||||
AFHDS2A_ID_S8a = 0x8A,
|
||||
|
||||
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,
|
||||
TX_RSSI_ID = 0x200, // Pseudo id outside 1 byte range of FlySky sensors
|
||||
};
|
||||
|
||||
static void processFlySkySensor(const uint8_t *packet)
|
||||
const FlySkySensor flySkySensors[] = {
|
||||
{AFHDS2A_ID_VOLTAGE | 0x100, ZSTR_A1, UNIT_VOLTS, 2}, // RX Voltage (remapped, really 0x0)
|
||||
{AFHDS2A_ID_TEMPERATURE, ZSTR_TEMP1, UNIT_CELSIUS, 1}, // Temperature
|
||||
{AFHDS2A_ID_MOT, ZSTR_RPM, UNIT_RAW, 0}, // RPM
|
||||
{AFHDS2A_ID_EXTV, ZSTR_A3, UNIT_VOLTS, 2}, // External voltage
|
||||
{AFHDS2A_ID_CELL_VOLTAGE, ZSTR_CELLS, UNIT_VOLTS, 2}, // Avg Cell voltage
|
||||
{AFHDS2A_ID_BAT_CURR, ZSTR_CURR, UNIT_AMPS, 2}, // battery current A * 100
|
||||
{AFHDS2A_ID_FUEL, ZSTR_CAPACITY, UNIT_RAW, 0}, // remaining battery percentage / mah drawn otherwise or fuel level no unit!
|
||||
{AFHDS2A_ID_RPM, ZSTR_RPM, UNIT_RAW, 0}, // throttle value / battery capacity
|
||||
{AFHDS2A_ID_CMP_HEAD, ZSTR_HDG, UNIT_DEGREE, 0}, // Heading 0..360 deg, 0=north 2bytes
|
||||
{AFHDS2A_ID_CLIMB_RATE, ZSTR_VSPD, UNIT_METERS_PER_SECOND, 2}, // 2 bytes m/s *100
|
||||
{AFHDS2A_ID_COG, ZSTR_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, ZSTR_SATELLITES, UNIT_RAW, 0}, // 2 bytes
|
||||
{AFHDS2A_ID_ACC_X, ZSTR_ACCX, UNIT_METERS_PER_SECOND, 2}, // 2 bytes m/s *100 signed
|
||||
{AFHDS2A_ID_ACC_Y, ZSTR_ACCY, UNIT_METERS_PER_SECOND, 2}, // 2 bytes m/s *100 signed
|
||||
{AFHDS2A_ID_ACC_Z, ZSTR_ACCZ, UNIT_METERS_PER_SECOND, 2}, // 2 bytes m/s *100 signed
|
||||
{AFHDS2A_ID_ROLL, ZSTR_ROLL, UNIT_DEGREE, 2}, // 2 bytes deg *100 signed
|
||||
{AFHDS2A_ID_PITCH, ZSTR_PITCH, UNIT_DEGREE, 2}, // 2 bytes deg *100 signed
|
||||
{AFHDS2A_ID_YAW, ZSTR_YAW, UNIT_DEGREE, 2}, // 2 bytes deg *100 signed
|
||||
{AFHDS2A_ID_VERTICAL_SPEED, ZSTR_VSPD, UNIT_METERS_PER_SECOND, 2}, // 2 bytes m/s *100
|
||||
{AFHDS2A_ID_GROUND_SPEED, ZSTR_GSPD, UNIT_METERS_PER_SECOND, 2}, // 2 bytes m/s *100 different unit than build-in sensor
|
||||
{AFHDS2A_ID_GPS_DIST, ZSTR_DIST, UNIT_METERS, 0}, // 2 bytes dist from home m unsigned
|
||||
{AFHDS2A_ID_ARMED, ZSTR_ARM, UNIT_RAW, 0}, // 2 bytes
|
||||
{AFHDS2A_ID_FLIGHT_MODE, ZSTR_FLIGHT_MODE, UNIT_RAW, 0}, // 2 bytes index
|
||||
{AFHDS2A_ID_PRES, ZSTR_PRES, UNIT_RAW, 2}, // 4 bytes In fact Temperature + Pressure -> Altitude
|
||||
{AFHDS2A_ID_PRES | 0x100, ZSTR_TEMP2, UNIT_CELSIUS, 1}, // 2 bytes Temperature
|
||||
{AFHDS2A_ID_ODO1, ZSTR_ODO1, UNIT_METERS, 2}, // 2 bytes Odometer1 -- some magic with 330 needed
|
||||
{AFHDS2A_ID_ODO2, ZSTR_ODO2, UNIT_METERS, 2}, // 2 bytes Odometer2 -- some magic with 330 needed
|
||||
{AFHDS2A_ID_SPE, ZSTR_ASPD, UNIT_KMH, 2}, // 2 bytes Speed km/h -- some magic with 330 needed
|
||||
{AFHDS2A_ID_TX_V, ZSTR_TXV, UNIT_VOLTS, 2}, // TX Voltage
|
||||
{AFHDS2A_ID_GPS_LAT, ZSTR_GPS, UNIT_RAW, 7}, // 4 bytes signed WGS84 in degrees * 1E7
|
||||
{AFHDS2A_ID_GPS_LON, ZSTR_GPS, UNIT_RAW, 7}, // 4 bytes signed WGS84 in degrees * 1E7
|
||||
{AFHDS2A_ID_GPS_ALT, ZSTR_GPSALT, UNIT_METERS, 2}, // 4 bytes signed GPS alt m*100
|
||||
{AFHDS2A_ID_ALT, ZSTR_ALT, UNIT_METERS, 2}, // 4 bytes signed Alt m*100
|
||||
|
||||
{AFHDS2A_ID_RX_SNR, ZSTR_RX_SNR, UNIT_DB, 0}, // RX SNR
|
||||
{AFHDS2A_ID_RX_NOISE, ZSTR_RX_NOISE, UNIT_DB, 0}, // RX Noise
|
||||
{AFHDS2A_ID_RX_RSSI, ZSTR_RSSI, UNIT_DB, 0}, // RX RSSI (0xfc)
|
||||
{AFHDS2A_ID_RX_ERR_RATE, ZSTR_RX_QUALITY, UNIT_RAW, 0}, // RX error rate
|
||||
{TX_RSSI_ID, ZSTR_TX_RSSI, UNIT_RAW, 0}, // Pseudo sensor for TRSSI
|
||||
|
||||
{0x00, NULL, UNIT_RAW, 0}, // sentinel
|
||||
};
|
||||
|
||||
int32_t getALT(uint32_t value);
|
||||
|
||||
static void processFlySkySensor(const uint8_t * packet, uint8_t type)
|
||||
{
|
||||
uint8_t buffer[8];
|
||||
uint16_t id = packet[0];
|
||||
const uint8_t instance = packet[1];
|
||||
int32_t value = (packet[3] << 8) + packet[2];
|
||||
int32_t value;
|
||||
|
||||
if (id == 0xff) {
|
||||
// No sensor
|
||||
return;
|
||||
//Load most likely value
|
||||
if (type == 0xAA)
|
||||
value = (packet[3] << 8) | packet[2];
|
||||
else
|
||||
value = (packet[6] << 24) | (packet[5] << 16) | (packet[4] << 8) | packet[3];
|
||||
|
||||
if (id == 0) id = 0x100; // Some part of OpenTX does not like sensor with id and instance 0, remap to 0x100
|
||||
|
||||
if (id == AFHDS2A_ID_RX_NOISE || id == AFHDS2A_ID_RX_RSSI) {
|
||||
value = 135 - value;
|
||||
}
|
||||
|
||||
if (id == 0) {
|
||||
// Some part of OpenTX does not like sensor with id and instance 0, remap to 0x100
|
||||
id = 0x100;
|
||||
}
|
||||
|
||||
if (id == FS_ID_ERR) { // ERR RATE, displayed RQLy and used as RSSI
|
||||
else if (id == AFHDS2A_ID_RX_ERR_RATE) {
|
||||
value = 100 - value;
|
||||
telemetryData.rssi.set(value);
|
||||
if (value > 0)
|
||||
telemetryStreaming = TELEMETRY_TIMEOUT10ms;
|
||||
if (value > 0) telemetryStreaming = TELEMETRY_TIMEOUT10ms;
|
||||
}
|
||||
|
||||
for (const FlySkySensor * sensor = flySkySensors; sensor->id; sensor++) {
|
||||
// Extract value, skip header
|
||||
if (sensor->id == id) {
|
||||
// The Noise and Signal sensors that are specified in dB send the absolute value
|
||||
if (id == FS_ID_NOISE || id == FS_ID_RSSI)
|
||||
value = 135 - value;
|
||||
else if (id == FS_ID_SNR) {
|
||||
if (value > 0) {
|
||||
value += 20;
|
||||
}
|
||||
}
|
||||
else if (id == FS_ID_TEMP)
|
||||
// Temperature sensors have 40 degree offset
|
||||
value -= 400;
|
||||
else if (sensor->unit == UNIT_VOLTS)
|
||||
// Voltage types are signed 16bit integers
|
||||
value = (int16_t)value;
|
||||
setTelemetryValue(PROTOCOL_TELEMETRY_FLYSKY_IBUS, id, 0, instance, value, sensor->unit, sensor->precision);
|
||||
return;
|
||||
else if (id == AFHDS2A_ID_PRES && value) {
|
||||
// Extract temperature to a new sensor
|
||||
setTelemetryValue(PROTOCOL_TELEMETRY_FLYSKY_IBUS, id | 0x100, 0, instance, ((value >> 19) - 400), UNIT_CELSIUS, 1);
|
||||
// Extract alt to a new sensor
|
||||
setTelemetryValue(PROTOCOL_TELEMETRY_FLYSKY_IBUS, AFHDS2A_ID_ALT, 0, instance, getALT(value), UNIT_METERS, 2);
|
||||
value &= PRESSURE_MASK;
|
||||
}
|
||||
else if ((id >= AFHDS2A_ID_ACC_X && id <= AFHDS2A_ID_VERTICAL_SPEED) || id == AFHDS2A_ID_CLIMB_RATE) {
|
||||
value = (int16_t) value; // Signed value
|
||||
}
|
||||
else if (id == AFHDS2A_ID_GPS_STATUS) {
|
||||
value = value >> 8;
|
||||
}
|
||||
else if (id == AFHDS2A_ID_GPS_FULL) {
|
||||
//(AC FRAME)[ID][inst][size][fix][sats][LAT]x4[LON]x4[ALT]x4
|
||||
setTelemetryValue(PROTOCOL_TELEMETRY_FLYSKY_IBUS, AFHDS2A_ID_GPS_STATUS, 0, instance, packet[4], UNIT_RAW, 0);
|
||||
for (uint8_t sensorID = AFHDS2A_ID_GPS_LAT; sensorID <= AFHDS2A_ID_GPS_ALT; sensorID++) {
|
||||
int index = 5 + (sensorID - AFHDS2A_ID_GPS_LAT) * 4;
|
||||
buffer[0] = sensorID;
|
||||
buffer[1] = instance;
|
||||
buffer[2] = 4;
|
||||
memcpy(buffer + 3, packet + index, 4);
|
||||
processFlySkySensor(buffer, 0xAC);
|
||||
}
|
||||
return;
|
||||
}
|
||||
else if (id == AFHDS2A_ID_VOLT_FULL) {
|
||||
//(AC FRAME)[ID][inst][size][ACC_X]x2[ACC_Y]x2[ACC_Z]x2[ROLL]x2[PITCH]x2[YAW]x2
|
||||
for (uint8_t sensorID = AFHDS2A_ID_EXTV; sensorID <= AFHDS2A_ID_RPM; sensorID++) {
|
||||
int index = 3 + (sensorID - AFHDS2A_ID_EXTV) * 2;
|
||||
buffer[0] = sensorID;
|
||||
buffer[1] = instance;
|
||||
buffer[2] = packet[index];
|
||||
buffer[3] = packet[index + 1];
|
||||
processFlySkySensor(buffer, 0xAA);
|
||||
}
|
||||
return;
|
||||
}
|
||||
else if (id == AFHDS2A_ID_ACC_FULL) {
|
||||
//(AC FRAME)[ID][inst][size]
|
||||
for (uint8_t sensorID = AFHDS2A_ID_ACC_X; sensorID <= AFHDS2A_ID_YAW; sensorID++) {
|
||||
int index = 3 + (sensorID - AFHDS2A_ID_ACC_X) * 2;
|
||||
buffer[0] = sensorID;
|
||||
buffer[1] = instance;
|
||||
buffer[2] = packet[index];
|
||||
buffer[3] = packet[index + 1];
|
||||
processFlySkySensor(buffer, 0xAA);
|
||||
}
|
||||
return;
|
||||
}
|
||||
for (const FlySkySensor * sensor = flySkySensors; sensor->id; sensor++) {
|
||||
if (sensor->id != id) continue;
|
||||
if (sensor->unit == UNIT_CELSIUS) value -= 400; // Temperature sensors have 40 degree offset
|
||||
else if (sensor->unit == UNIT_VOLTS) value = (uint16_t) value; // Voltage types are unsigned 16bit integers
|
||||
setTelemetryValue(PROTOCOL_TELEMETRY_FLYSKY_IBUS, id, 0, instance, value, sensor->unit, sensor->precision);
|
||||
return;
|
||||
}
|
||||
//unknown
|
||||
setTelemetryValue(PROTOCOL_TELEMETRY_FLYSKY_IBUS, id, 0, instance, value, UNIT_RAW, 0);
|
||||
}
|
||||
|
||||
void processFlySkyPacket(const uint8_t *packet)
|
||||
void processFlySkyPacket(const uint8_t * packet)
|
||||
{
|
||||
// Set TX RSSI Value, reverse MULTIs scaling
|
||||
setTelemetryValue(PROTOCOL_TELEMETRY_FLYSKY_IBUS, TX_RSSI_ID, 0, 0, packet[0], UNIT_RAW, 0);
|
||||
|
||||
for (int sensor = 0; sensor < 7; sensor++) {
|
||||
int index = 1 + (4 * sensor);
|
||||
processFlySkySensor(packet+index);
|
||||
const uint8_t * buffer = packet + 1;
|
||||
int sesnor = 0;
|
||||
while (sesnor++ < 7) {
|
||||
if (*buffer == AFHDS2A_ID_END) break;
|
||||
processFlySkySensor(buffer, 0xAA);
|
||||
buffer += 4;
|
||||
}
|
||||
}
|
||||
|
||||
void processFlySkyTelemetryData(uint8_t data, uint8_t* rxBuffer, uint8_t& rxBufferCount)
|
||||
void processFlySkyPacketAC(const uint8_t * packet)
|
||||
{
|
||||
if (rxBufferCount == 0 && data != 0xAA) {
|
||||
// Set TX RSSI Value, reverse MULTIs scaling
|
||||
setTelemetryValue(PROTOCOL_TELEMETRY_FLYSKY_IBUS, TX_RSSI_ID, 0, 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;
|
||||
uint8_t size = buffer[2];
|
||||
processFlySkySensor(buffer, 0xAC);
|
||||
buffer += size + 3;
|
||||
}
|
||||
}
|
||||
|
||||
void processFlySkyTelemetryData(uint8_t data, uint8_t * rxBuffer, uint8_t &rxBufferCount)
|
||||
{
|
||||
if (rxBufferCount == 0)
|
||||
return;
|
||||
|
||||
if (data == 0xAA || data == 0xAC) {
|
||||
TRACE("[IBUS] Packet 0x%02X", data);
|
||||
}
|
||||
else {
|
||||
TRACE("[IBUS] invalid start byte 0x%02X", data);
|
||||
rxBufferCount = 0;
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -148,24 +274,23 @@ void processFlySkyTelemetryData(uint8_t data, uint8_t* rxBuffer, uint8_t& rxBuff
|
||||
rxBufferCount = 0;
|
||||
}
|
||||
|
||||
|
||||
if (rxBufferCount >= FLYSKY_TELEMETRY_LENGTH) {
|
||||
// debug print the content of the packets
|
||||
#if 0
|
||||
debugPrintf("[IBUS] Packet 0x%02X rssi 0x%02X: ",
|
||||
rxBuffer[0], rxBuffer[1]);
|
||||
debugPrintf(", rssi 0x%02X: ", rxBuffer[1]);
|
||||
for (int i=0; i<7; i++) {
|
||||
debugPrintf("[%02X %02X %02X%02X] ", rxBuffer[i*4+2], rxBuffer[i*4 + 3],
|
||||
rxBuffer[i*4 + 4], rxBuffer[i*4 + 5]);
|
||||
}
|
||||
debugPrintf("\r\n");
|
||||
#endif
|
||||
processFlySkyPacket(rxBuffer+1);
|
||||
if (data == 0xAA) processFlySkyPacket(rxBuffer + 1);
|
||||
else if (data == 0xAC) processFlySkyPacketAC(rxBuffer + 1);
|
||||
rxBufferCount = 0;
|
||||
}
|
||||
}
|
||||
|
||||
const FlySkySensor *getFlySkySensor(uint16_t id)
|
||||
const FlySkySensor * getFlySkySensor(uint16_t id)
|
||||
{
|
||||
for (const FlySkySensor * sensor = flySkySensors; sensor->id; sensor++) {
|
||||
if (id == sensor->id)
|
||||
@@ -181,7 +306,7 @@ void flySkySetDefault(int index, uint16_t id, uint8_t subId, uint8_t instance)
|
||||
telemetrySensor.subId = subId;
|
||||
telemetrySensor.instance = instance;
|
||||
|
||||
const FlySkySensor *sensor = getFlySkySensor(id);
|
||||
const FlySkySensor * sensor = getFlySkySensor(id);
|
||||
if (sensor) {
|
||||
TelemetryUnit unit = sensor->unit;
|
||||
uint8_t prec = min<uint8_t>(2, sensor->precision);
|
||||
@@ -198,3 +323,71 @@ void flySkySetDefault(int index, uint16_t id, uint8_t subId, uint8_t instance)
|
||||
|
||||
storageDirty(EE_MODEL);
|
||||
}
|
||||
|
||||
uint16_t ibusTempToK(int16_t tempertureIbus)
|
||||
{
|
||||
return (uint16_t) (tempertureIbus - 400) + 2731;
|
||||
}
|
||||
|
||||
int32_t log2fix(uint32_t x)
|
||||
{
|
||||
int32_t b = 1U << (ALT_PRECISION - 1);
|
||||
int32_t y = 0;
|
||||
while (x < 1U << ALT_PRECISION) {
|
||||
x <<= 1;
|
||||
y -= 1U << ALT_PRECISION;
|
||||
}
|
||||
|
||||
while (x >= 2U << ALT_PRECISION) {
|
||||
x >>= 1;
|
||||
y += 1U << ALT_PRECISION;
|
||||
}
|
||||
|
||||
uint64_t z = x;
|
||||
for (size_t i = 0; i < ALT_PRECISION; i++) {
|
||||
z = (z * z) >> ALT_PRECISION;
|
||||
if (z >= 2U << ALT_PRECISION) {
|
||||
z >>= 1;
|
||||
y += b;
|
||||
}
|
||||
b >>= 1;
|
||||
}
|
||||
return y;
|
||||
}
|
||||
|
||||
int32_t getALT(uint32_t value)
|
||||
{
|
||||
uint32_t pressurePa = value & PRESSURE_MASK;
|
||||
if (pressurePa == 0) return 0;
|
||||
uint16_t temperatureK = ibusTempToK((uint16_t) (value >> 19));
|
||||
static uint32_t initPressure = 0;
|
||||
static uint16_t initTemperature = 0;
|
||||
if (initPressure <= 0) // use current pressure for ground altitude -> 0
|
||||
{
|
||||
initPressure = pressurePa;
|
||||
initTemperature = temperatureK;
|
||||
}
|
||||
int temperature = (initTemperature + temperatureK) >> 1; //div 2
|
||||
bool tempNegative = temperature < 0;
|
||||
if (tempNegative) temperature = temperature * -1;
|
||||
uint64_t helper = R_DIV_G_MUL_10_Q15;
|
||||
helper = helper * (uint64_t) temperature;
|
||||
helper = helper >> ALT_PRECISION;
|
||||
|
||||
uint32_t po_to_p = (uint32_t)(initPressure << (ALT_PRECISION - 1));
|
||||
po_to_p = po_to_p / pressurePa;
|
||||
//shift missing bit
|
||||
po_to_p = po_to_p << 1;
|
||||
if (po_to_p == 0) return 0;
|
||||
uint64_t t = log2fix(po_to_p) * INV_LOG2_E_Q1DOT31;
|
||||
int32_t ln = t >> 31;
|
||||
|
||||
bool neg = ln < 0;
|
||||
if (neg) ln = ln * -1;
|
||||
helper = helper * (uint64_t) ln;
|
||||
helper = helper >> ALT_PRECISION;
|
||||
int result = (int) helper;
|
||||
|
||||
if (neg ^ tempNegative) result = result * -1;
|
||||
return result;
|
||||
}
|
||||
Reference in New Issue
Block a user