piotr/edgetx-gx12
1
0
Fork 0
Code Issues 2 Pull Requests Actions Packages Projects Releases Wiki Activity
Files
d22628d1d6438f9803ba0ce0822c1ac02b3fbd29
edgetx-gx12/radio/src/telemetry/flysky_ibus.cpp
T

642 lines
26 KiB
C++
Raw Blame History

/*
* Copyright (C) EdgeTX
*
* Based on code named
* opentx - https://github.com/opentx/opentx
* th9x - http://code.google.com/p/th9x
* er9x - http://code.google.com/p/er9x
* gruvin9x - http://code.google.com/p/gruvin9x
*
* License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License version 2 as
* published by the Free Software Foundation.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*/
#include "edgetx.h"
/*
* TXID + RXID are already skipped in MULTI module to save memory+transmission time, format from Multi is:
* AA or AC | TX_RSSI | sensor ...
*/
#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_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 type;
const TelemetryUnit unit;
const uint8_t precision;
const char * name;
};
// 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_RF_MODULE_POWER = 0x3056, // virtual
// SENSOR_TYPE_RF_MODULE_RAW = 0x4056, // 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
{
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
AFHDS3_FRM_TEMP = 0x57, //virtual
AFHDS3_FRM_EXT_V = 0x58, //virtual
AFHDS2A_ID_TX_V = 0x7F, // TX Voltage
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_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,
};
// clang-format off
#define FS(type,name,unit,precision) {type,unit,precision,name}
const FlySkySensor flySkySensors[] = {
// flysky start
FS( SENSOR_TYPE_RX_VOL, STR_SENSOR_A1, UNIT_VOLTS, 2 ), // RX Voltage (remapped, really 0x0)
FS( SENSOR_TYPE_TEMPERATURE, STR_SENSOR_TEMP1, UNIT_CELSIUS, 1 ), // Temperature
FS( SENSOR_TYPE_MOT, STR_SENSOR_RPM, UNIT_RAW, 0 ), // RPM
FS( SENSOR_TYPE_EXT_VOL, STR_SENSOR_A3, UNIT_VOLTS, 2 ), // External voltage
FS( SENSOR_TYPE_BVD, "BVD", UNIT_VOLTS, 2 ), // BVD
FS( SENSOR_TYPE_GYROSCOPE_1_AXIS, STR_SENSOR_CELLS, UNIT_DEGREE, 1 ), //
FS( SENSOR_TYPE_PRES, STR_SENSOR_PRES, UNIT_RAW, 2 ), // 4 bytes In fact Temperature + Pressure -> Altitude
FS( SENSOR_TYPE_ALT, STR_SENSOR_ALT, UNIT_METERS, 2 ),
FS( SENSOR_TYPE_RF_MODULE_TEMP, STR_SENSOR_TEMP2, UNIT_CELSIUS, 0 ), // 1 bytes temperature
FS( SENSOR_TYPE_RF_MODULE_VOL, STR_SENSOR_TXV, UNIT_VOLTS, 2 ), // 2 bytes voltage
FS( SENSOR_TYPE_RF_MODULE_POWER, STR_SENSOR_TX_POWER, UNIT_DBM, 0 ), // 2 bytes DBM
// FS( SENSOR_TYPE_RF_MODULE_RAW, STR_SENSOR_TX_POWER, UNIT_RAW, 0 ), // 2 bytes DBM
FS( SENSOR_TYPE_TX_V, STR_SENSOR_TXV, UNIT_VOLTS, 2 ), // TX Voltage
FS( SENSOR_TYPE_RX_SNR, STR_SENSOR_RX_SNR, UNIT_DB, 0 ), // RX SNR
FS( SENSOR_TYPE_RX_NOISE, STR_SENSOR_RX_NOISE, UNIT_DBM, 0 ), // RX Noise
FS( SENSOR_TYPE_RX_RSSI, STR_SENSOR_RSSI, UNIT_DBM, 0 ), // RX RSSI (0xfc)
FS( SENSOR_TYPE_RX_ERR_RATE, STR_SENSOR_RX_QUALITY, UNIT_PERCENT, 0 ), // RX error rate
////////////////////////////////////////////////////////////////
FS( AFHDS2A_ID_BAT_CURR, STR_SENSOR_CURR, UNIT_AMPS, 2 ), // battery current A * 100
FS( AFHDS2A_ID_FUEL, STR_SENSOR_CAPACITY, UNIT_RAW, 0 ), // remaining battery percentage / mah drawn otherwise or fuel level no unit!
FS( AFHDS2A_ID_RPM, STR_SENSOR_RPM, UNIT_RAW, 0 ), // throttle value / battery capacity
FS( AFHDS2A_ID_CMP_HEAD, STR_SENSOR_HDG, UNIT_DEGREE, 0 ), // Heading 0..360 deg, 0=north 2bytes
FS( AFHDS2A_ID_CLIMB_RATE, STR_SENSOR_VSPD, UNIT_METERS_PER_SECOND, 2 ), // 2 bytes m/s *100
FS( 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
FS( AFHDS2A_ID_GPS_STATUS, STR_SENSOR_SATELLITES, UNIT_RAW, 0 ), // 2 bytes
FS( AFHDS2A_ID_ACC_X, STR_SENSOR_ACCX, UNIT_METERS_PER_SECOND, 2 ), // 2 bytes m/s *100 signed
FS( AFHDS2A_ID_ACC_Y, STR_SENSOR_ACCY, UNIT_METERS_PER_SECOND, 2 ), // 2 bytes m/s *100 signed
FS( AFHDS2A_ID_ACC_Z, STR_SENSOR_ACCZ, UNIT_METERS_PER_SECOND, 2 ), // 2 bytes m/s *100 signed
FS( AFHDS2A_ID_ROLL, STR_SENSOR_ROLL, UNIT_DEGREE, 2 ), // 2 bytes deg *100 signed
FS( AFHDS2A_ID_PITCH, STR_SENSOR_PITCH, UNIT_DEGREE, 2 ), // 2 bytes deg *100 signed
FS( AFHDS2A_ID_YAW, STR_SENSOR_YAW, UNIT_DEGREE, 2 ), // 2 bytes deg *100 signed
FS( AFHDS2A_ID_VERTICAL_SPEED, STR_SENSOR_VSPD, UNIT_METERS_PER_SECOND, 2 ), // 2 bytes m/s *100
FS( AFHDS2A_ID_GROUND_SPEED, STR_SENSOR_GSPD, UNIT_METERS_PER_SECOND, 2 ), // 2 bytes m/s *100 different unit than build-in sensor
FS( AFHDS2A_ID_GPS_DIST, STR_SENSOR_DIST, UNIT_METERS, 0 ), // 2 bytes dist from home m unsigned
FS( AFHDS2A_ID_ARMED, STR_SENSOR_ARM, UNIT_RAW, 0 ), // 2 bytes
FS( AFHDS2A_ID_FLIGHT_MODE, STR_SENSOR_FLIGHT_MODE, UNIT_RAW, 0 ), // 2 bytes index
FS( SENSOR_TYPE_PRES | 0x100, STR_SENSOR_TEMP2, UNIT_CELSIUS, 1 ), // 2 bytes Temperature
FS( AFHDS2A_ID_ODO1, STR_SENSOR_ODO1, UNIT_METERS, 2 ), // 2 bytes Odometer1 -- some magic with 330 needed
FS( AFHDS2A_ID_ODO2, STR_SENSOR_ODO2, UNIT_METERS, 2 ), // 2 bytes Odometer2 -- some magic with 330 needed
FS( AFHDS2A_ID_SPE, STR_SENSOR_ASPD, UNIT_KMH, 2 ), // 2 bytes Speed km/h -- some magic with 330 needed
FS( AFHDS2A_ID_GPS_LAT, STR_SENSOR_GPS, UNIT_GPS, 0 ), // 4 bytes signed WGS84 in degrees * 1E7
//FS( AFHDS2A_ID_GPS_LON, STR_SENSOR_GPS, UNIT_GPS, 0 ), // 4 bytes signed WGS84 in degrees * 1E7
FS( AFHDS2A_ID_GPS_ALT, STR_SENSOR_GPSALT, UNIT_METERS, 2 ), // 4 bytes signed GPS alt m*100
FS( AFHDS2A_ID_ALT, STR_SENSOR_ALT, UNIT_METERS, 2 ), // 4 bytes signed Alt m*100
FS( AFHDS2A_ID_RX_SIG_AFHDS3, STR_SENSOR_RX_QUALITY, UNIT_PERCENT, 0 ), // RX error rate
FS( AFHDS2A_ID_RX_SNR_AFHDS3, STR_SENSOR_RX_SNR, UNIT_DB, 1 ), // RX SNR
FS( AFHDS2A_ID_TX_RSSI, STR_SENSOR_TX_RSSI, UNIT_DBM, 0 ), // Pseudo sensor for TRSSI
FS( 0x00, NULL, UNIT_RAW, 0 ), // sentinel
};
// clang-format on
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] };
uint8_t data3[] = { (uint8_t)(SENSOR_TYPE_RF_MODULE_POWER>>8), (uint8_t)SENSOR_TYPE_RF_MODULE_POWER, id, packet[8], packet[9] };
// uint8_t data4[] = { (uint8_t)(SENSOR_TYPE_RF_MODULE_RAW>>8), (uint8_t)SENSOR_TYPE_RF_MODULE_RAW, id, packet[6] & 0x07};
processFlySkyAFHDS3Sensor(data1, 1 );
processFlySkyAFHDS3Sensor(data2, 2 );
processFlySkyAFHDS3Sensor(data3, 2 );
// processFlySkyAFHDS3Sensor(data4, 1 );
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 || SENSOR_TYPE_RF_MODULE_POWER == 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)
{
uint8_t buffer[8];
uint16_t id = packet[0];
const uint8_t instance = packet[1];
int32_t value;
//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];
id = id ? id : SENSOR_TYPE_RX_VOL; // Remapped
if (id == AFHDS2A_ID_RX_NOISE || id == AFHDS2A_ID_RX_RSSI) {
value = 135 - value;
}
else if (id == AFHDS2A_ID_RX_ERR_RATE) {
value = 100 - value;
telemetryData.rssi.set(value);
if (value > 0) telemetryStreaming = TELEMETRY_TIMEOUT10ms;
}
else if(id == AFHDS2A_ID_RX_SIG_AFHDS3) {
telemetryData.rssi.set(value);
if(value>0) telemetryStreaming = TELEMETRY_TIMEOUT10ms;
}
else if (id == SENSOR_TYPE_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 || id == AFHDS2A_ID_ALT_FLYSKY) {
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_GPS_LAT) {
uint8_t instance2 = 0; // Assume one instance, RX would only have one GPS
value = value / 10;
setTelemetryValue(PROTOCOL_TELEMETRY_FLYSKY_IBUS, AFHDS2A_ID_GPS_LAT, 0,
instance2, value, UNIT_GPS_LATITUDE, 0);
return;
} else if (id == AFHDS2A_ID_GPS_LON) { // Remapped to single GPS sensor:
// AFHDS2A_ID_GPS_LAT
uint8_t instance2 = 0;
value = value / 10;
setTelemetryValue(PROTOCOL_TELEMETRY_FLYSKY_IBUS, AFHDS2A_ID_GPS_LAT, 0,
instance2, value, UNIT_GPS_LONGITUDE, 0);
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->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);
return;
}
//unknown
setTelemetryValue(PROTOCOL_TELEMETRY_FLYSKY_IBUS, id, 0, instance, value, UNIT_RAW, 0);
}
void processFlySkyPacket(const uint8_t * packet)
{
// Set TX RSSI Value, reverse MULTIs scaling
setFlyskyTelemetryValue(AFHDS2A_ID_TX_RSSI, 0, packet[0], UNIT_RAW, 0);
const uint8_t * buffer = packet + 1;
int sensor = 0;
while (sensor++ < 7) {
if (*buffer == SENSOR_TYPE_END) break;
processFlySkySensor(buffer, 0xAA);
buffer += 4;
}
}
void processFlySkyPacketAC(const uint8_t * packet)
{
// Set TX RSSI Value, reverse MULTIs scaling
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 == SENSOR_TYPE_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 == 2 || data == 4) {
TRACE("[IBUS] Packet 0x%02X", data);
}
else {
TRACE("[IBUS] invalid start byte 0x%02X", data);
rxBufferCount = 0;
return;
}
if (rxBufferCount < TELEMETRY_RX_PACKET_SIZE) {
rxBuffer[rxBufferCount++] = data;
}
else {
TRACE("[IBUS] array size %d error", rxBufferCount);
rxBufferCount = 0;
}
if (rxBufferCount >= FLYSKY_TELEMETRY_LENGTH) {
// debug print the content of the packets
#if 0
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(CRLF);
#endif
if (data == 0xAA) processFlySkyPacket(rxBuffer + 1);
else if (data == 0xAC) processFlySkyPacketAC(rxBuffer + 1);
rxBufferCount = 0;
}
}
const FlySkySensor * getFlySkySensor(uint16_t id)
{
for (const FlySkySensor * sensor = flySkySensors; sensor->type; sensor++) {
if (id == sensor->type)
return sensor;
}
return nullptr;
}
void flySkySetDefault(int index, uint16_t id, uint8_t subId, uint8_t instance)
{
TelemetrySensor &telemetrySensor = g_model.telemetrySensors[index];
telemetrySensor.id = id;
telemetrySensor.subId = subId;
telemetrySensor.instance = instance;
const FlySkySensor * sensor = getFlySkySensor(id);
if (sensor) {
TelemetryUnit unit = sensor->unit;
uint8_t prec = min<uint8_t>(2, sensor->precision);
telemetrySensor.init(sensor->name, unit, prec);
if (unit == UNIT_RPMS) {
telemetrySensor.custom.ratio = 1;
telemetrySensor.custom.offset = 1;
}
}
else {
telemetrySensor.init(id);
}
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;
}
#if 0
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;
}
#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
Reference in New Issue View Git Blame Copy Permalink