Merge with latest 2.3

This commit is contained in:
Bertrand Songis
2019-11-07 15:04:45 +01:00
286 changed files with 9956 additions and 10363 deletions
+294 -93
View File
@@ -16,148 +16,281 @@
#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_t)9591506
#define INV_LOG2_E_Q1DOT31 (uint64_t)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
// 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
{0xfe, 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 - SIGNED value
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
//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 = -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_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;
}
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_SNR) {
telemetryData.rssi.set(value);
}
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 = -value;
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_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;
}
telemetryStreaming = TELEMETRY_TIMEOUT10ms;
}
void processFlySkyTelemetryData(uint8_t data)
void processFlySkyPacketAC(const uint8_t * packet)
{
if (telemetryRxBufferCount == 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;
}
if (telemetryRxBufferCount < TELEMETRY_RX_PACKET_SIZE) {
telemetryRxBuffer[telemetryRxBufferCount++] = data;
if (rxBufferCount < TELEMETRY_RX_PACKET_SIZE) {
rxBuffer[rxBufferCount++] = data;
}
else {
TRACE("[IBUS] array size %d error", telemetryRxBufferCount);
telemetryRxBufferCount = 0;
TRACE("[IBUS] array size %d error", rxBufferCount);
rxBufferCount = 0;
}
if (telemetryRxBufferCount >= FLYSKY_TELEMETRY_LENGTH) {
if (rxBufferCount >= FLYSKY_TELEMETRY_LENGTH) {
// debug print the content of the packets
#if 0
debugPrintf("[IBUS] Packet 0x%02X rssi 0x%02X: ",
telemetryRxBuffer[0], telemetryRxBuffer[1]);
debugPrintf(", rssi 0x%02X: ", rxBuffer[1]);
for (int i=0; i<7; i++) {
debugPrintf("[%02X %02X %02X%02X] ", telemetryRxBuffer[i*4+2], telemetryRxBuffer[i*4 + 3],
telemetryRxBuffer[i*4 + 4], telemetryRxBuffer[i*4 + 5]);
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(telemetryRxBuffer+1);
telemetryRxBufferCount = 0;
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)
@@ -173,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);
@@ -190,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;
}
+5 -2
View File
@@ -21,11 +21,14 @@
#ifndef _FLYSKY_IBUS_H
#define _FLYSKY_IBUS_H
void processFlySkyTelemetryData(uint8_t data);
void processFlySkyTelemetryData(uint8_t data, uint8_t * rxBuffer, uint8_t &rxBufferCount);
void flySkySetDefault(int index, uint16_t id, uint8_t subId, uint8_t instance);
// Used by multi protocol
void processFlySkyPacket(const uint8_t *packet);
void processFlySkyPacket(const uint8_t * packet);
void processFlySkyPacketAC(const uint8_t * packet);
#endif
+29
View File
@@ -39,6 +39,8 @@ class Pxx2Telemetry
static void processPowerMeterFrame(uint8_t module, uint8_t * frame);
static void processAuthenticationFrame(uint8_t module, uint8_t * frame);
public:
};
@@ -221,6 +223,29 @@ void processTelemetryFrame(uint8_t module, uint8_t * frame)
}
}
#if defined(ACCESS_LIB)
void processAuthenticationFrame(uint8_t module, uint8_t * frame)
{
uint8_t cryptoType = frame[3];
uint8_t messageDigest[16] = {0};
if (frame[0] == 4 && PXX2_AUTH_REFUSED_FLAG == frame[4]) {
POPUP_INFORMATION(STR_AUTH_FAILURE);
return;
}
if (INTERNAL_MODULE == module && accessCRL(cryptoType, frame+4, messageDigest)) {
moduleState[module].mode = MODULE_MODE_AUTHENTICATION;
Pxx2Pulses & pxx2 = intmodulePulsesData.pxx2;
pxx2.setupAuthenticationFrame(module, cryptoType, (const uint8_t *)messageDigest);
intmoduleSendBuffer(pxx2.getData(), pxx2.getSize());
// we remain in AUTHENTICATION mode to avoid a CHANNELS frame is sent at the end of the mixing process
}
}
#else
#define processAuthenticationFrame(module, frame)
#endif
void processSpectrumAnalyserFrame(uint8_t module, uint8_t * frame)
{
if (moduleState[module].mode != MODULE_MODE_SPECTRUM_ANALYSER) {
@@ -313,6 +338,10 @@ void processModuleFrame(uint8_t module, uint8_t *frame)
processTelemetryFrame(module, frame);
break;
case PXX2_TYPE_ID_AUTHENTICATION:
processAuthenticationFrame(module, frame);
break;
case PXX2_TYPE_ID_RESET:
processResetFrame(module, frame);
break;
+433
View File
@@ -0,0 +1,433 @@
/*
* Copyright (C) OpenTX
*
* 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 "opentx.h"
/* Full telemetry
packet[0] = TX RSSI value
packet[1] = TX LQI value
packet[2] = frame number
packet[3-7] telemetry data
The frame number takes the following values: 0x00, 0x11, 0x12, ..., 0x1C. The frames can be present or not, they also do not have to follow each others.
Here is a description of the telemetry data for each frame number:
- frame 0x00
data byte 0 -> 0x00 unknown
data byte 1 -> 0x00 unknown
data byte 2 -> 0x00 unknown
data byte 3 -> RX Batt Volt_H
data byte 4 -> RX Batt Volt_L => RX Batt=(Volt_H*256+Volt_L)/28
- frame 0x11
data byte 0 -> 0xAF start of frame
data byte 1 -> 0x00 unknown
data byte 2 -> 0x2D station type 0x2D=standard station nitro or electric, 0xAC=advanced station
data byte 3 -> RX Batt Volt_H
data byte 4 -> RX Batt Volt_L => RX Batt=(Volt_H*256+Volt_L)/28
- frame 0x12
data byte 0 -> Lat_sec_H GPS : latitude second
data byte 1 -> Lat_sec_L signed int : 1/100 of second
data byte 2 -> Lat_deg_min_H GPS : latitude degree.minute
data byte 3 -> Lat_deg_min_L signed int : +=North, - = south
data byte 4 -> Time_second GPS Time
- frame 0x13
data byte 0 -> GPS Longitude second
data byte 1 -> signed int : 1/100 of second
data byte 2 -> GPS Longitude degree.minute
data byte 3 -> signed int : +=Est, - = west
data byte 4 -> Temp2 Temperature2=Temp2-40°C
- frame 0x14
data byte 0 -> Speed_H
data byte 1 -> Speed_L GPS Speed=Speed_H*256+Speed_L km/h
data byte 2 -> Alti_sea_H
data byte 3 -> Alti_sea_L GPS Altitude=Alti_sea_H*256+Alti_sea_L m
data byte 4 -> Temp1 Temperature1=Temp1-40°C
- frame 0x15
data byte 0 -> FUEL
data byte 1 -> RPM1_L
data byte 2 -> RPM1_H RPM1=RPM1_H*256+RPM1_L
data byte 3 -> RPM2_L
data byte 4 -> RPM2_H RPM2=RPM2_H*256+RPM2_L
- frame 0x16
data byte 0 -> Date_year GPS Date
data byte 1 -> Date_month
data byte 2 -> Date_day
data byte 3 -> Time_hour GPS Time
data byte 4 -> Time_min
- frame 0x17
data byte 0 -> COURSEH
data byte 1 -> COURSEL GPS heading = COURSEH*256+COURSEL in degrees
data byte 2 -> Count GPS satellites
data byte 3 -> Temp3 Temperature3=Temp2-40°C
data byte 4 -> Temp4 Temperature4=Temp3-40°C
- frame 0x18
data byte 0 -> Volt_L Volt=(Volt_H*256+Volt_L)/10 V
data byte 1 -> Volt_H
data byte 2 -> AMP_L
data byte 3 -> AMP_H Amp=(AMP1_*256+AMP_L -180)/14 in signed A
- frame 0x19 Servo sensor
data byte 0 -> AMP_Servo1 Amp=AMP_Servo1/10 in A
data byte 1 -> AMP_Servo2 Amp=AMP_Servo2/10 in A
data byte 2 -> AMP_Servo3 Amp=AMP_Servo3/10 in A
data byte 3 -> AMP_Servo4 Amp=AMP_Servo4/10 in A
- frame 0x1A
data byte 2 -> ASpeed_H Air speed=ASpeed_H*256+ASpeed_L km/h
data byte 3 -> ASpeed_L
- frame 0x1B Variometer sensor
data byte 0 -> Alti1H
data byte 1 -> Alti1L Altitude unfiltered
data byte 2 -> Alti2H
data byte 3 -> Alti2L Altitude filtered
- frame 0x1C Unknown
- frame 0x22 Unknown
*/
#define HITEC_TELEMETRY_LENGTH 8
struct HitecSensor
{
const uint16_t id;
const char * name;
const TelemetryUnit unit;
const uint8_t precision;
};
// telemetry frames
enum
{
HITEC_FRAME_00 = 0x00,
HITEC_FRAME_11 = 0x11,
HITEC_FRAME_12 = 0x12,
HITEC_FRAME_13 = 0x13,
HITEC_FRAME_14 = 0x14,
HITEC_FRAME_15 = 0x15,
HITEC_FRAME_16 = 0x16,
HITEC_FRAME_17 = 0x17,
HITEC_FRAME_18 = 0x18,
HITEC_FRAME_19 = 0x19,
HITEC_FRAME_1A = 0x1A,
HITEC_FRAME_1B = 0x1B,
HITEC_FRAME_1C = 0x1C,
HITEC_FRAME_22 = 0x22,
};
// telemetry sensors ID
enum
{
HITEC_ID_RX_VOLTAGE = 0x0003, // RX_Batt Voltage
HITEC_ID_GPS_LAT_LONG = 0x1200, // GPS latitude longitude
HITEC_ID_TEMP2 = 0x1304, // Temperature sensor 2
HITEC_ID_GPS_SPEED = 0x1400, // GPS speed
HITEC_ID_GPS_ALTITUDE = 0x1402, // GPS altitude sea level
HITEC_ID_TEMP1 = 0x1404, // Temperature sensor 1
HITEC_ID_FUEL = 0x1500, // Fuel
HITEC_ID_RPM1 = 0x1501, // RPM1
HITEC_ID_RPM2 = 0x1503, // RPM2
HITEC_ID_GPS_DATETIME = 0x1600, // GPS date time
HITEC_ID_GPS_HEADING = 0x1700, // GPS heading
HITEC_ID_GPS_COUNT = 0x1702, // GPS count
HITEC_ID_TEMP3 = 0x1703, // Temperature sensor 3
HITEC_ID_TEMP4 = 0x1704, // Temperature sensor 4
HITEC_ID_VOLTAGE = 0x1800, // Voltage sensor
HITEC_ID_AMP = 0x1802, // Amp sensor
HITEC_ID_C50 = 0x1803, // Amp sensor C50
HITEC_ID_C200 = 0x1804, // Amp sensor C200
HITEC_ID_AMP_S1 = 0x1900, // Amp servo 1 sensor
HITEC_ID_AMP_S2 = 0x1901, // Amp servo 2 sensor
HITEC_ID_AMP_S3 = 0x1902, // Amp servo 3 sensor
HITEC_ID_AMP_S4 = 0x1903, // Amp servo 4 sensor
HITEC_ID_AIR_SPEED = 0x1A02, // Air speed
HITEC_ID_VARIO = 0x1B00, // Vario
HITEC_ID_ALT = 0x1B02, // Vario
TX_RSSI_ID = 0xFF00, // Pseudo id outside 1 byte range of Hitec sensors
TX_LQI_ID = 0xFF01, // Pseudo id outside 1 byte range of Hitec sensors
};
const HitecSensor hitecSensors[] = {
//frame 00
{HITEC_ID_RX_VOLTAGE, ZSTR_BATT, UNIT_VOLTS, 2}, // RX_Batt Voltage
//frame 11
//frame 12
{HITEC_ID_GPS_LAT_LONG, ZSTR_GPS, UNIT_GPS, 0}, // GPS position
//frame 13
{HITEC_ID_TEMP2, ZSTR_TEMP2, UNIT_CELSIUS, 0}, // Temperature sensor 2
//frame 14
{HITEC_ID_GPS_SPEED, ZSTR_GSPD, UNIT_KMH, 0}, // GPS speed
{HITEC_ID_GPS_ALTITUDE, ZSTR_GPSALT, UNIT_METERS, 0}, // GPS altitude sea level
{HITEC_ID_TEMP1, ZSTR_TEMP1, UNIT_CELSIUS, 0}, // Temperature sensor 1
//frame 15
{HITEC_ID_FUEL, ZSTR_FUEL, UNIT_PERCENT, 0}, // Fuel
{HITEC_ID_RPM1, ZSTR_RPM, UNIT_RPMS, 0}, // RPM1
{HITEC_ID_RPM2, ZSTR_RPM2, UNIT_RPMS, 0}, // RPM2
//frame 16
{HITEC_ID_GPS_DATETIME, ZSTR_GPS, UNIT_DATETIME, 0}, // GPS date time
//frame 17
{HITEC_ID_GPS_HEADING, ZSTR_HDG, UNIT_DEGREE, 0}, // GPS Heading
{HITEC_ID_GPS_COUNT, ZSTR_SATELLITES, UNIT_RAW, 0}, // GPS count
{HITEC_ID_TEMP3, ZSTR_TEMP3, UNIT_CELSIUS, 0}, // Temperature sensor 3
{HITEC_ID_TEMP4, ZSTR_TEMP4, UNIT_CELSIUS, 0}, // Temperature sensor 4
//frame 18
{HITEC_ID_VOLTAGE, ZSTR_A1, UNIT_VOLTS, 1}, // Voltage sensor
{HITEC_ID_AMP, ZSTR_CURR, UNIT_AMPS, 0}, // Amp sensor
{HITEC_ID_C50, ZSTR_C50, UNIT_AMPS, 1}, // Amp sensor C50
{HITEC_ID_C200, ZSTR_C200, UNIT_AMPS, 0}, // Amp sensor C200
//frame 19
{HITEC_ID_AMP_S1, ZSTR_CURR_SERVO1, UNIT_AMPS, 1}, // Amp sensor
{HITEC_ID_AMP_S2, ZSTR_CURR_SERVO2, UNIT_AMPS, 1}, // Amp sensor
{HITEC_ID_AMP_S3, ZSTR_CURR_SERVO3, UNIT_AMPS, 1}, // Amp sensor
{HITEC_ID_AMP_S4, ZSTR_CURR_SERVO4, UNIT_AMPS, 1}, // Amp sensor
//frame 1A
{HITEC_ID_AIR_SPEED, ZSTR_ASPD, UNIT_KMH, 0}, // Air speed
//frame 1B
{HITEC_ID_VARIO, ZSTR_VSPD, UNIT_METERS_PER_SECOND, 1}, // Vario
{HITEC_ID_ALT, ZSTR_ALT, UNIT_METERS, 1}, // Altitude
{TX_RSSI_ID, ZSTR_TX_RSSI, UNIT_RAW, 0}, // Pseudo id outside 1 byte range of Hitec sensors
{TX_LQI_ID, ZSTR_TX_QUALITY, UNIT_RAW, 0}, // Pseudo id outside 1 byte range of Hitec sensors// Pseudo sensor for TLQI
{0x00, NULL, UNIT_RAW, 0}, // sentinel
};
const HitecSensor * getHitecSensor(uint16_t id)
{
for (const HitecSensor * sensor = hitecSensors; sensor->id; sensor++) {
if (id == sensor->id)
return sensor;
}
return nullptr;
}
void processHitecPacket(const uint8_t * packet)
{
static uint16_t rssi = 0, lqi = 0;
// Set TX RSSI Value, reverse MULTIs scaling
rssi = ((packet[0] * 10) + (rssi * 90)) / 100; // quick filtering
setTelemetryValue(PROTOCOL_TELEMETRY_HITEC, TX_RSSI_ID, 0, 0, rssi >> 1, UNIT_RAW, 0);
telemetryData.rssi.set(rssi >> 1);
if (packet[0] > 0) telemetryStreaming = TELEMETRY_TIMEOUT10ms;
// Set TX LQI Value, reverse MULTIs scaling
lqi = ((packet[1] * 10) + (lqi * 90)) / 100; // quick filtering
setTelemetryValue(PROTOCOL_TELEMETRY_HITEC, TX_LQI_ID, 0, 0, lqi, UNIT_RAW, 0);
const HitecSensor * sensor;
int32_t value, deg, min, sec, alt, amp;
static uint8_t second = 0;
static int32_t last_alt = 0;
static uint16_t last_ms = 0;
uint16_t current_ms;
switch (packet[2]) {
case HITEC_FRAME_00:
value = (((packet[6] << 8) | packet[7]) * 100) / 28;
sensor = getHitecSensor(HITEC_ID_RX_VOLTAGE);
setTelemetryValue(PROTOCOL_TELEMETRY_HITEC, HITEC_ID_RX_VOLTAGE, 0, 0, value, sensor->unit, sensor->precision);
return;
case HITEC_FRAME_11:
value = (((packet[6] << 8) | packet[7]) * 100) / 28;
sensor = getHitecSensor(HITEC_ID_RX_VOLTAGE);
setTelemetryValue(PROTOCOL_TELEMETRY_HITEC, HITEC_ID_RX_VOLTAGE, 0, 0, value, sensor->unit, sensor->precision);
return;
case HITEC_FRAME_12:
//value=(packet[5]<<24)|(packet[6]<<16)|(packet[3]<<8)|packet[4];
min = (int16_t) ((packet[5] << 8) | packet[6]);
deg = min / 100;
min = min - deg * 100;
sec = (int16_t) ((packet[3] << 8) | packet[4]);
value = deg * 1000000 + (min * 150000 + sec * 25) / 9;
setTelemetryValue(PROTOCOL_TELEMETRY_HITEC, HITEC_ID_GPS_LAT_LONG, 0, 0, value, UNIT_GPS_LATITUDE, 0);
second = packet[7];
return;
case HITEC_FRAME_13:
//value=(packet[5]<<24)|(packet[6]<<16)|(packet[3]<<8)|packet[4];
min = (int16_t) ((packet[5] << 8) | packet[6]);
deg = min / 100;
min = min - deg * 100;
sec = (int16_t) ((packet[3] << 8) | packet[4]);
value = deg * 1000000 + (min * 150000 + sec * 25) / 9;
setTelemetryValue(PROTOCOL_TELEMETRY_HITEC, HITEC_ID_GPS_LAT_LONG, 0, 0, value, UNIT_GPS_LONGITUDE, 0);
value = packet[7] - 40;
sensor = getHitecSensor(HITEC_ID_TEMP2);
setTelemetryValue(PROTOCOL_TELEMETRY_HITEC, HITEC_ID_TEMP2, 0, 0, value, sensor->unit, sensor->precision);
return;
case HITEC_FRAME_14:
value = (packet[3] << 8) | packet[4];
sensor = getHitecSensor(HITEC_ID_GPS_SPEED);
setTelemetryValue(PROTOCOL_TELEMETRY_HITEC, HITEC_ID_GPS_SPEED, 0, 0, value, sensor->unit, sensor->precision);
value = (packet[5] << 8) | packet[6];
sensor = getHitecSensor(HITEC_ID_GPS_ALTITUDE);
setTelemetryValue(PROTOCOL_TELEMETRY_HITEC, HITEC_ID_GPS_ALTITUDE, 0, 0, value, sensor->unit, sensor->precision);
value = packet[7] - 40;
sensor = getHitecSensor(HITEC_ID_TEMP1);
setTelemetryValue(PROTOCOL_TELEMETRY_HITEC, HITEC_ID_TEMP1, 0, 0, value, sensor->unit, sensor->precision);
return;
case HITEC_FRAME_15:
value = packet[3] * 25;
if (value > 100) value = 100;
sensor = getHitecSensor(HITEC_ID_FUEL);
setTelemetryValue(PROTOCOL_TELEMETRY_HITEC, HITEC_ID_FUEL, 0, 0, value, sensor->unit, sensor->precision);
value = (packet[5] << 8) | packet[4];
sensor = getHitecSensor(HITEC_ID_RPM1);
setTelemetryValue(PROTOCOL_TELEMETRY_HITEC, HITEC_ID_RPM1, 0, 0, value, sensor->unit, sensor->precision);
value = (packet[7] << 8) | packet[6];
sensor = getHitecSensor(HITEC_ID_RPM2);
setTelemetryValue(PROTOCOL_TELEMETRY_HITEC, HITEC_ID_RPM2, 0, 0, value, sensor->unit, sensor->precision);
return;
case HITEC_FRAME_16:
sensor = getHitecSensor(HITEC_ID_GPS_DATETIME);
value = (packet[3] << 24) | (packet[4] << 16) | (packet[5] << 8) | 0x01;
setTelemetryValue(PROTOCOL_TELEMETRY_HITEC, HITEC_ID_GPS_DATETIME, 0, 0, value, sensor->unit, sensor->precision);
value = (packet[6] << 24) | (packet[7] << 16) | (second << 8);
setTelemetryValue(PROTOCOL_TELEMETRY_HITEC, HITEC_ID_GPS_DATETIME, 0, 0, value, sensor->unit, sensor->precision);
return;
case HITEC_FRAME_17:
value = (packet[3] << 8) | packet[4];
if (value <= 359) { // Filter strange values received time to time
sensor = getHitecSensor(HITEC_ID_GPS_HEADING);
setTelemetryValue(PROTOCOL_TELEMETRY_HITEC, HITEC_ID_GPS_HEADING, 0, 0, value, sensor->unit, sensor->precision);
}
value = packet[5];
sensor = getHitecSensor(HITEC_ID_GPS_COUNT);
setTelemetryValue(PROTOCOL_TELEMETRY_HITEC, HITEC_ID_GPS_COUNT, 0, 0, value, sensor->unit, sensor->precision);
value = packet[6] - 40;
sensor = getHitecSensor(HITEC_ID_TEMP3);
setTelemetryValue(PROTOCOL_TELEMETRY_HITEC, HITEC_ID_TEMP3, 0, 0, value, sensor->unit, sensor->precision);
value = packet[7] - 40;
sensor = getHitecSensor(HITEC_ID_TEMP4);
setTelemetryValue(PROTOCOL_TELEMETRY_HITEC, HITEC_ID_TEMP4, 0, 0, value, sensor->unit, sensor->precision);
return;
case HITEC_FRAME_18:
value = (packet[4] << 8) | packet[3];
if (value) value += 2; // Measured voltage seems to be 0.2V lower than real
sensor = getHitecSensor(HITEC_ID_VOLTAGE);
setTelemetryValue(PROTOCOL_TELEMETRY_HITEC, HITEC_ID_VOLTAGE, 0, 0, value, sensor->unit, sensor->precision);
//I'm adding below 3 amp sensors but there is only one really since I don't know how to really calculate them
value = (int16_t) ((packet[6] << 8) | packet[5]);
sensor = getHitecSensor(HITEC_ID_AMP);
setTelemetryValue(PROTOCOL_TELEMETRY_HITEC, HITEC_ID_AMP, 0, 0, value, sensor->unit, sensor->precision);
amp = ((value + 114.875) * 1.441) + 0.5;
sensor = getHitecSensor(HITEC_ID_C50);
setTelemetryValue(PROTOCOL_TELEMETRY_HITEC, HITEC_ID_C50, 0, 0, amp, sensor->unit, sensor->precision);
amp = value * 3 + 165;
sensor = getHitecSensor(HITEC_ID_C200);
setTelemetryValue(PROTOCOL_TELEMETRY_HITEC, HITEC_ID_C200, 0, 0, amp, sensor->unit, sensor->precision);
return;
case HITEC_FRAME_19:
value = packet[3];
sensor = getHitecSensor(HITEC_ID_AMP_S1);
setTelemetryValue(PROTOCOL_TELEMETRY_HITEC, HITEC_ID_AMP_S1, 0, 0, value, sensor->unit, sensor->precision);
value = packet[4];
sensor = getHitecSensor(HITEC_ID_AMP_S2);
setTelemetryValue(PROTOCOL_TELEMETRY_HITEC, HITEC_ID_AMP_S2, 0, 0, value, sensor->unit, sensor->precision);
value = packet[5];
sensor = getHitecSensor(HITEC_ID_AMP_S3);
setTelemetryValue(PROTOCOL_TELEMETRY_HITEC, HITEC_ID_AMP_S3, 0, 0, value, sensor->unit, sensor->precision);
value = packet[6];
sensor = getHitecSensor(HITEC_ID_AMP_S4);
setTelemetryValue(PROTOCOL_TELEMETRY_HITEC, HITEC_ID_AMP_S4, 0, 0, value, sensor->unit, sensor->precision);
return;
case HITEC_FRAME_1A:
value = (packet[5] << 8) | packet[6];
sensor = getHitecSensor(HITEC_ID_AIR_SPEED);
setTelemetryValue(PROTOCOL_TELEMETRY_HITEC, HITEC_ID_AIR_SPEED, 0, 0, value, sensor->unit, sensor->precision);
return;
case HITEC_FRAME_1B:
alt = (int16_t) ((packet[3] << 8) | packet[4]);
sensor = getHitecSensor(HITEC_ID_ALT);
setTelemetryValue(PROTOCOL_TELEMETRY_HITEC, HITEC_ID_ALT, 0, 0, alt, sensor->unit, sensor->precision);
current_ms = RTOS_GET_MS();
sensor = getHitecSensor(HITEC_ID_VARIO);
value = (alt - last_alt) * 100;
if ((current_ms - last_ms) < 1000)
value /= (int32_t) (current_ms - last_ms);
else
value = 0;
setTelemetryValue(PROTOCOL_TELEMETRY_HITEC, HITEC_ID_VARIO, 0, 0, value, sensor->unit, sensor->precision);
last_alt = alt;
last_ms = current_ms;
return;
case HITEC_FRAME_1C:
case HITEC_FRAME_22:
return;
}
//unknown
value = (packet[6] << 24) | (packet[5] << 16) | (packet[4] << 8) | packet[3];
setTelemetryValue(PROTOCOL_TELEMETRY_HITEC, packet[2], 0, 0, value, UNIT_RAW, 0);
}
void processHitecTelemetryData(uint8_t data, uint8_t * rxBuffer, uint8_t &rxBufferCount)
{
if (rxBufferCount == 0)
return;
if (data != 0xAA) {
TRACE("[HITEC] invalid start byte 0x%02X", data);
rxBufferCount = 0;
return;
}
if (rxBuffer[3] == HITEC_FRAME_00 || (rxBuffer[3] >= HITEC_FRAME_11 && rxBuffer[3] <= HITEC_FRAME_1C) || rxBuffer[3] == HITEC_FRAME_22) {
TRACE("[HITEC] Frame 0x%02X", rxBuffer[3]);
}
else {
TRACE("[HITEC] wrong frame 0x%02X", rxBuffer[3]);
rxBufferCount = 0;
return;
}
if (rxBufferCount < TELEMETRY_RX_PACKET_SIZE) {
rxBuffer[rxBufferCount++] = data;
}
else {
TRACE("[HITEC] array size %d error", rxBufferCount);
rxBufferCount = 0;
return;
}
if (rxBufferCount >= HITEC_TELEMETRY_LENGTH) {
// debug print the content of the packets
#if 0
debugPrintf(" rssi 0x%02X lqi 0x%02X: ",
rxBuffer[1], rxBuffer[2]);
for (int i=0; i<5; i++) {
debugPrintf("%02X ", rxBuffer[4+i]);
}
debugPrintf("\r\n");
#endif
processHitecPacket(rxBuffer + 1);
rxBufferCount = 0;
}
}
void hitecSetDefault(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 HitecSensor * sensor = getHitecSensor(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);
}
+30
View File
@@ -0,0 +1,30 @@
/*
* Copyright (C) OpenTX
*
* Based on code named
* 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.
*/
#ifndef _HITEC_H
#define _HITEC_H
void processHitecTelemetryData(uint8_t data, uint8_t* rxBuffer, uint8_t& rxBufferCount);
void hitecSetDefault(int index, uint16_t id, uint8_t subId, uint8_t instance);
// Used by multi protocol
void processHitecPacket(const uint8_t *packet);
#endif
+358 -123
View File
@@ -21,24 +21,29 @@
#include "telemetry.h"
#include "multi.h"
MultiModuleStatus multiModuleStatus;
MultiModuleSyncStatus multiSyncStatus;
uint8_t multiBindStatus = MULTI_NORMAL_OPERATION;
#define MULTI_CHAN_BITS 11
extern uint8_t g_moduleIdx;
enum MultiPacketTypes : uint8_t {
enum MultiPacketTypes : uint8_t
{
MultiStatus = 1,
FrSkySportTelemtry,
FrSkySportTelemetry,
FrSkyHubTelemetry,
SpektrumTelemetry,
DSMBindPacket,
FlyskyIBusTelemetry,
ConfigCommand,
InputSync,
FrskySportPolling
FrskySportPolling,
HitecTelemetry,
SpectrumScannerPacket,
FlyskyIBusTelemetryAC,
MultiRxChannels
};
enum MultiBufferState : uint8_t {
enum MultiBufferState : uint8_t
{
NoProtocolDetected,
MultiFirstByteReceived,
ReceivingMultiProtocol,
@@ -48,71 +53,248 @@ enum MultiBufferState : uint8_t {
FrskyTelemetryFallbackFirstByte,
FrskyTelemetryFallbackNextBytes,
FlyskyTelemetryFallback,
HitecTelemetryFallback,
MultiStatusOrFrskyData
};
MultiBufferState guessProtocol()
#if defined(INTERNAL_MODULE_MULTI)
static MultiModuleStatus multiModuleStatus[NUM_MODULES] = {MultiModuleStatus(), MultiModuleStatus()};
static MultiModuleSyncStatus multiSyncStatus[NUM_MODULES] = {MultiModuleSyncStatus(), MultiModuleSyncStatus()};
static uint8_t multiBindStatus[NUM_MODULES] = {MULTI_NORMAL_OPERATION, MULTI_NORMAL_OPERATION};
static MultiBufferState multiTelemetryBufferState[NUM_MODULES];
MultiModuleStatus &getMultiModuleStatus(uint8_t module)
{
if (g_model.moduleData[EXTERNAL_MODULE].getMultiProtocol(false) == MODULE_SUBTYPE_MULTI_DSM2)
return multiModuleStatus[module];
}
MultiModuleSyncStatus &getMultiSyncStatus(uint8_t module)
{
return multiSyncStatus[module];
}
uint8_t getMultiBindStatus(uint8_t module)
{
return multiBindStatus[module];
}
void setMultiBindStatus(uint8_t module, uint8_t bindStatus)
{
multiBindStatus[module] = bindStatus;
}
MultiBufferState getMultiTelemetryBufferState(uint8_t module)
{
return multiTelemetryBufferState[module];
}
void setMultiTelemetryBufferState(uint8_t module, MultiBufferState state)
{
multiTelemetryBufferState[module] = state;
}
// Use additional telemetry buffer
uint8_t intTelemetryRxBuffer[TELEMETRY_RX_PACKET_SIZE];
uint8_t intTelemetryRxBufferCount;
#else // !INTERNAL_MODULE_MULTI
static MultiModuleStatus multiModuleStatus;
static MultiModuleSyncStatus multiSyncStatus;
static uint8_t multiBindStatus = MULTI_NORMAL_OPERATION;
static MultiBufferState multiTelemetryBufferState;
MultiModuleStatus& getMultiModuleStatus(uint8_t)
{
return multiModuleStatus;
}
MultiModuleSyncStatus& getMultiSyncStatus(uint8_t)
{
return multiSyncStatus;
}
uint8_t getMultiBindStatus(uint8_t)
{
return multiBindStatus;
}
void setMultiBindStatus(uint8_t, uint8_t bindStatus)
{
multiBindStatus = bindStatus;
}
MultiBufferState getMultiTelemetryBufferState(uint8_t)
{
return multiTelemetryBufferState;
}
void setMultiTelemetryBufferState(uint8_t, MultiBufferState state)
{
multiTelemetryBufferState = state;
}
#endif // INTERNAL_MODULE_MULTI
static MultiBufferState guessProtocol(uint8_t module)
{
uint32_t moduleIdx = EXTERNAL_MODULE;
#if defined(INTERNAL_MODULE_MULTI)
if (isModuleMultimodule(INTERNAL_MODULE)) {
moduleIdx = INTERNAL_MODULE;
}
#endif
if (g_model.moduleData[moduleIdx].getMultiProtocol(false) == MODULE_SUBTYPE_MULTI_DSM2)
return SpektrumTelemetryFallback;
else if (g_model.moduleData[EXTERNAL_MODULE].getMultiProtocol(false) == MODULE_SUBTYPE_MULTI_FS_AFHDS2A)
else if (g_model.moduleData[module].getMultiProtocol(false) == MODULE_SUBTYPE_MULTI_FS_AFHDS2A)
return FlyskyTelemetryFallback;
else
return FrskyTelemetryFallback;
}
static void processMultiStatusPacket(const uint8_t *data)
static void processMultiScannerPacket(const uint8_t *data)
{
// At least two status packets without bind flag
bool wasBinding = (multiModuleStatus.isBinding());
uint8_t cur_channel = data[0];
if (moduleState[g_moduleIdx].mode == MODULE_MODE_SPECTRUM_ANALYSER) {
for (uint8_t channel = 0; channel <5; channel++) {
uint8_t power = max<int>(0,(data[channel+1] - 34) >> 1); // remove everything below -120dB
multiModuleStatus.flags = data[0];
multiModuleStatus.major = data[1];
multiModuleStatus.minor = data[2];
multiModuleStatus.revision = data[3];
multiModuleStatus.patch = data[4];
multiModuleStatus.lastUpdate = get_tmr10ms();
if (wasBinding && !multiModuleStatus.isBinding() && multiBindStatus == MULTI_BIND_INITIATED)
multiBindStatus = MULTI_BIND_FINISHED;
#if LCD_W == 480
coord_t x = cur_channel*2;
if (x < LCD_W) {
reusableBuffer.spectrumAnalyser.bars[x] = power;
reusableBuffer.spectrumAnalyser.bars[x+1] = power;
if (power > reusableBuffer.spectrumAnalyser.max[x]) {
reusableBuffer.spectrumAnalyser.max[x] = power;
reusableBuffer.spectrumAnalyser.max[x+1] = power;
}
#elif LCD_W == 212
coord_t x = cur_channel;
if (x <= LCD_W) {
reusableBuffer.spectrumAnalyser.bars[x] = power;
if (power > reusableBuffer.spectrumAnalyser.max[x]) {
reusableBuffer.spectrumAnalyser.max[x] = power;
}
#else
coord_t x = cur_channel/2 + 1;
if (x <= LCD_W) {
reusableBuffer.spectrumAnalyser.bars[x] = power;
if (power > reusableBuffer.spectrumAnalyser.max[x]) {
reusableBuffer.spectrumAnalyser.max[x] = power;
}
#endif
}
if (++cur_channel > MULTI_SCANNER_MAX_CHANNEL)
cur_channel = 0;
}
}
}
static void processMultiSyncPacket(const uint8_t *data)
static void processMultiStatusPacket(const uint8_t * data, uint8_t module)
{
multiSyncStatus.lastUpdate = get_tmr10ms();
multiSyncStatus.interval = data[4];
multiSyncStatus.target = data[5];
MultiModuleStatus &status = getMultiModuleStatus(module);
// At least two status packets without bind flag
bool wasBinding = status.isBinding();
status.flags = data[0];
status.major = data[1];
status.minor = data[2];
status.revision = data[3];
status.patch = data[4];
status.lastUpdate = get_tmr10ms();
if (getMultiModuleStatus(module).requiresFailsafeCheck) {
getMultiModuleStatus(module).requiresFailsafeCheck = false;
if (getMultiModuleStatus(module).supportsFailsafe() && g_model.moduleData[module].failsafeMode == FAILSAFE_NOT_SET)
POPUP_WARNING(STR_NO_FAILSAFE);
}
if (wasBinding && !status.isBinding() && getMultiBindStatus(module) == MULTI_BIND_INITIATED)
setMultiBindStatus(module, MULTI_BIND_FINISHED);
}
static void processMultiSyncPacket(const uint8_t * data, uint8_t module)
{
MultiModuleSyncStatus &status = getMultiSyncStatus(module);
status.lastUpdate = get_tmr10ms();
status.interval = data[4];
status.target = data[5];
#if !defined(PPM_PIN_SERIAL)
auto oldlag = multiSyncStatus.inputLag;
auto oldlag = status.inputLag;
(void) oldlag;
#endif
multiSyncStatus.calcAdjustedRefreshRate(data[0] << 8 | data[1], data[2] << 8 | data[3]);
status.calcAdjustedRefreshRate(data[0] << 8 | data[1], data[2] << 8 | data[3]);
#if !defined(PPM_PIN_SERIAL)
TRACE("MP ADJ: rest: %d, lag %04d, diff: %04d target: %d, interval: %d, Refresh: %d, intAdjRefresh: %d, adjRefresh %d\r\n", extmodulePulsesData.dsm2.rest,
multiSyncStatus.inputLag, oldlag-multiSyncStatus.inputLag, multiSyncStatus.target, multiSyncStatus.interval, multiSyncStatus.refreshRate, multiSyncStatus.adjustedRefreshRate/50,
multiSyncStatus.getAdjustedRefreshRate());
TRACE("MP ADJ: rest: %d, lag %04d, diff: %04d target: %d, interval: %d, Refresh: %d, intAdjRefresh: %d, adjRefresh %d\r\n",
module == EXTERNAL_MODULE ? extmodulePulsesData.dsm2.rest : 0,
status.inputLag, oldlag - status.inputLag, status.target, status.interval, status.refreshRate, status.adjustedRefreshRate / 50,
status.getAdjustedRefreshRate());
#endif
}
#if defined(PCBTARANIS) || defined(PCBHORUS)
static void processMultiRxChannels(const uint8_t * data, uint8_t len)
{
if (g_model.trainerData.mode != TRAINER_MODE_MULTI)
return;
static void processMultiTelemetryPaket(const uint8_t *packet)
//uint8_t pps = data[0];
//uint8_t rssi = data[1];
int ch = max(data[2], (uint8_t)0);
int maxCh = min(ch + data[3], MAX_TRAINER_CHANNELS);
uint32_t bits = 0;
uint8_t bitsavailable = 0;
uint8_t byteIdx = 4;
while (ch < maxCh) {
while (bitsavailable < MULTI_CHAN_BITS && byteIdx < len) {
bits |= (uint32_t)(data[byteIdx++]) << (uint32_t)bitsavailable;
bitsavailable += 8;
}
int value = bits & ((1 << MULTI_CHAN_BITS) - 1);
bitsavailable -= MULTI_CHAN_BITS;
bits >>= MULTI_CHAN_BITS;
ppmInput[ch] = (value - 1024) * 500 / 800;
ch++;
if (byteIdx >= len)
break;
}
if (ch == maxCh)
ppmInputValidityTimer = PPM_IN_VALID_TIMEOUT;
}
#endif
static void processMultiTelemetryPaket(const uint8_t * packet, uint8_t module)
{
uint8_t type = packet[0];
uint8_t len = packet[1];
const uint8_t *data = packet + 2;
const uint8_t * data = packet + 2;
// Switch type
switch (type) {
case MultiStatus:
if (len >= 5)
processMultiStatusPacket(data);
processMultiStatusPacket(data, module);
break;
case DSMBindPacket:
if (len >= 10)
processDSMBindPacket(data);
processDSMBindPacket(module, data);
break;
case SpektrumTelemetry:
@@ -131,6 +313,20 @@ static void processMultiTelemetryPaket(const uint8_t *packet)
TRACE("[MP] Received IBUS telemetry len %d < 28", len);
break;
case FlyskyIBusTelemetryAC:
if (len >= 28)
processFlySkyPacketAC(data);
else
TRACE("[MP] Received IBUS telemetry AC len %d < 28", len);
break;
case HitecTelemetry:
if (len >= 8)
processHitecPacket(data);
else
TRACE("[MP] Received Hitec telemetry len %d < 8", len);
break;
case FrSkyHubTelemetry:
if (len >= 4)
frskyDProcessPacket(data);
@@ -138,7 +334,7 @@ static void processMultiTelemetryPaket(const uint8_t *packet)
TRACE("[MP] Received Frsky HUB telemetry len %d < 4", len);
break;
case FrSkySportTelemtry:
case FrSkySportTelemetry:
if (len >= 4)
sportProcessTelemetryPacket(data);
else
@@ -147,7 +343,7 @@ static void processMultiTelemetryPaket(const uint8_t *packet)
case InputSync:
if (len >= 6)
processMultiSyncPacket(data);
processMultiSyncPacket(data, module);
else
TRACE("[MP] Received input sync len %d < 6", len);
break;
@@ -164,6 +360,21 @@ static void processMultiTelemetryPaket(const uint8_t *packet)
}
break;
#endif
case SpectrumScannerPacket:
if (len == 6)
processMultiScannerPacket(data);
else
TRACE("[MP] Received spectrum scanner len %d != 6", len);
break;
#if defined(PCBTARANIS) || defined(PCBHORUS)
case MultiRxChannels:
if (len >= 4)
processMultiRxChannels(data, len);
else
TRACE("[MP] Received RX channels len %d < 4", len);
break;
#endif
default:
TRACE("[MP] Unkown multi packet type 0x%02X, len %d", type, len);
@@ -173,7 +384,7 @@ static void processMultiTelemetryPaket(const uint8_t *packet)
// sprintf does not work AVR ARM
// use a small helper function
static void appendInt(char *buf, uint32_t val)
static void appendInt(char * buf, uint32_t val)
{
while (*buf)
buf++;
@@ -193,8 +404,8 @@ void MultiModuleSyncStatus::calcAdjustedRefreshRate(uint16_t newRefreshRate, uin
uint16_t targetRefreshRate = (uint16_t) (newRefreshRate * ((MIN_REFRESH_RATE / (newRefreshRate - 1)) + 1));
// Overflow, reverse sample
if (lagDifference < -targetRefreshRate/2)
lagDifference= -lagDifference;
if (lagDifference < -targetRefreshRate / 2)
lagDifference = -lagDifference;
// Reset adjusted refresh if rate has changed
@@ -205,7 +416,7 @@ void MultiModuleSyncStatus::calcAdjustedRefreshRate(uint16_t newRefreshRate, uin
adjustedRefreshRate /= 2;
// Our refresh rate in ps
adjustedRefreshRate*=1000;
adjustedRefreshRate *= 1000;
return;
}
@@ -213,16 +424,16 @@ void MultiModuleSyncStatus::calcAdjustedRefreshRate(uint16_t newRefreshRate, uin
int numsamples = interval * 10000 / targetRefreshRate;
// Convert lagDifference to ps
lagDifference=lagDifference*1000;
lagDifference = lagDifference * 1000;
// Calculate the time we intentionally were late/early
if (inputLag > target*10 +30)
lagDifference += numsamples*500;
else if (inputLag < target*10 - 30)
lagDifference -= numsamples*500;
if (inputLag > target * 10 + 30)
lagDifference += numsamples * 500;
else if (inputLag < target * 10 - 30)
lagDifference -= numsamples * 500;
// Caculate the time in ps each frame is to slow (positive), fast(negative)
int perframeps = lagDifference*10/ numsamples;
int perframeps = lagDifference * 10 / numsamples;
if (perframeps > 20000)
perframeps = 20000;
@@ -230,20 +441,21 @@ void MultiModuleSyncStatus::calcAdjustedRefreshRate(uint16_t newRefreshRate, uin
if (perframeps < -20000)
perframeps = -20000;
adjustedRefreshRate =(adjustedRefreshRate + perframeps);
adjustedRefreshRate = (adjustedRefreshRate + perframeps);
// Safeguards
if (adjustedRefreshRate < 6*1000*1000)
adjustedRefreshRate = 6*1000*1000;
if (adjustedRefreshRate > 30*1000*1000)
adjustedRefreshRate = 30*1000*1000;
if (adjustedRefreshRate < 6 * 1000 * 1000)
adjustedRefreshRate = 6 * 1000 * 1000;
if (adjustedRefreshRate > 30 * 1000 * 1000)
adjustedRefreshRate = 30 * 1000 * 1000;
inputLag = newInputLag;
}
static uint8_t counter;
uint16_t MultiModuleSyncStatus::getAdjustedRefreshRate() {
uint16_t MultiModuleSyncStatus::getAdjustedRefreshRate()
{
if (!isValid() || refreshRate == 0)
return 18000;
@@ -251,9 +463,9 @@ uint16_t MultiModuleSyncStatus::getAdjustedRefreshRate() {
counter = (uint8_t) (counter + 1 % 10);
uint16_t rate = (uint16_t) ((adjustedRefreshRate + counter * 50) / 500);
// Check how far off we are from our target, positive means we are too slow, negative we are too fast
if (inputLag > target*10 +30)
return (uint16_t) (rate - 1);
else if (inputLag < target*10 - 30)
if (inputLag > target * 10 + 30)
return (uint16_t) (rate - 1);
else if (inputLag < target * 10 - 30)
return (uint16_t) (rate + 1);
else
return rate;
@@ -265,17 +477,16 @@ static void prependSpaces(char * buf, int val)
while (*buf)
buf++;
int k=10000;
while(val/k==0 && k > 0)
{
*buf=' ';
int k = 10000;
while (val / k == 0 && k > 0) {
*buf = ' ';
buf++;
k/= 10;
k /= 10;
}
*buf='\0';
*buf = '\0';
}
void MultiModuleSyncStatus::getRefreshString(char *statusText)
void MultiModuleSyncStatus::getRefreshString(char * statusText)
{
if (!isValid()) {
return;
@@ -285,20 +496,22 @@ void MultiModuleSyncStatus::getRefreshString(char *statusText)
prependSpaces(statusText, inputLag);
appendInt(statusText, inputLag);
strcat(statusText, "ns R ");
prependSpaces(statusText, adjustedRefreshRate/1000);
prependSpaces(statusText, adjustedRefreshRate / 1000);
appendInt(statusText, (uint32_t) (adjustedRefreshRate / 1000));
strcat(statusText, "ns");
}
void MultiModuleStatus::getStatusString(char *statusText)
void MultiModuleStatus::getStatusString(char * statusText)
{
if (!isValid()) {
#if defined(PCBFRSKY)
if (IS_INTERNAL_MODULE_ENABLED())
#if !defined(INTERNAL_MODULE_MULTI)
if (isSportLineUsedByInternalModule())
strcpy(statusText, STR_DISABLE_INTERNAL);
else
#endif
strcpy(statusText, STR_MODULE_NO_TELEMETRY);
#endif
strcpy(statusText, STR_MODULE_NO_TELEMETRY);
return;
}
if (!protocolValid()) {
@@ -313,7 +526,7 @@ void MultiModuleStatus::getStatusString(char *statusText)
strcpy(statusText, STR_MODULE_NO_INPUT);
return;
}
else if(isWaitingforBind()) {
else if (isWaitingforBind()) {
strcpy(statusText, STR_MODULE_WAITFORBIND);
return;
}
@@ -333,50 +546,71 @@ void MultiModuleStatus::getStatusString(char *statusText)
strcat(statusText, STR_MODULE_BINDING);
}
static MultiBufferState multiTelemetryBufferState;
static void processMultiTelemetryByte(const uint8_t data)
static uint8_t * getRxBuffer(uint8_t moduleIdx)
{
if (telemetryRxBufferCount < TELEMETRY_RX_PACKET_SIZE) {
telemetryRxBuffer[telemetryRxBufferCount++] = data;
#if defined(INTERNAL_MODULE_MULTI)
if (moduleIdx == INTERNAL_MODULE)
return intTelemetryRxBuffer;
#endif
return telemetryRxBuffer;
}
static uint8_t &getRxBufferCount(uint8_t moduleIdx)
{
#if defined(INTERNAL_MODULE_MULTI)
if (moduleIdx == INTERNAL_MODULE)
return intTelemetryRxBufferCount;
#endif
return telemetryRxBufferCount;
}
static void processMultiTelemetryByte(const uint8_t data, uint8_t module)
{
uint8_t * rxBuffer = getRxBuffer(module);
uint8_t &rxBufferCount = getRxBufferCount(module);
if (rxBufferCount < TELEMETRY_RX_PACKET_SIZE) {
rxBuffer[rxBufferCount++] = data;
}
else {
TRACE("[MP] array size %d error", telemetryRxBufferCount);
multiTelemetryBufferState = NoProtocolDetected;
TRACE("[MP] array size %d error", rxBufferCount);
setMultiTelemetryBufferState(module, NoProtocolDetected);
}
// Length field does not count the header
if (telemetryRxBufferCount >= 2 && telemetryRxBuffer[1] == telemetryRxBufferCount - 2) {
if (rxBufferCount >= 2 && rxBuffer[1] == rxBufferCount - 2) {
// debug print the content of the packet
#if 0
debugPrintf("[MP] Packet type %02X len 0x%02X: ",
telemetryRxBuffer[0], telemetryRxBuffer[1]);
for (int i=0; i<(telemetryRxBufferCount+3)/4; i++) {
debugPrintf("[%02X%02X %02X%02X] ", telemetryRxBuffer[i*4+2], telemetryRxBuffer[i*4 + 3],
telemetryRxBuffer[i*4 + 4], telemetryRxBuffer[i*4 + 5]);
rxBuffer[0], rxBuffer[1]);
for (int i=0; i<(rxBufferCount+3)/4; 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
// Packet is complete, process it
processMultiTelemetryPaket(telemetryRxBuffer);
multiTelemetryBufferState = NoProtocolDetected;
processMultiTelemetryPaket(rxBuffer, module);
setMultiTelemetryBufferState(module, NoProtocolDetected);
}
}
void processMultiTelemetryData(const uint8_t data)
void processMultiTelemetryData(uint8_t data, uint8_t module)
{
// debugPrintf("State: %d, byte received %02X, buflen: %d\r\n", multiTelemetryBufferState, data, telemetryRxBufferCount);
switch (multiTelemetryBufferState) {
uint8_t * rxBuffer = getRxBuffer(module);
uint8_t &rxBufferCount = getRxBufferCount(module);
//debugPrintf("State: %d, byte received %02X, buflen: %d\r\n", multiTelemetryBufferState, data, rxBufferCount);
switch (getMultiTelemetryBufferState(module)) {
case NoProtocolDetected:
if (data == 'M') {
multiTelemetryBufferState = MultiFirstByteReceived;
setMultiTelemetryBufferState(module, MultiFirstByteReceived);
}
else if (data == 0xAA || data == 0x7e) {
multiTelemetryBufferState = guessProtocol();
setMultiTelemetryBufferState(module, guessProtocol(module));
// Process the first byte by the protocol
processMultiTelemetryData(data);
processMultiTelemetryData(data, module);
}
else {
TRACE("[MP] invalid start byte 0x%02X", data);
@@ -384,91 +618,92 @@ void processMultiTelemetryData(const uint8_t data)
break;
case FrskyTelemetryFallback:
multiTelemetryBufferState = FrskyTelemetryFallbackFirstByte;
setMultiTelemetryBufferState(module, FrskyTelemetryFallbackFirstByte);
processFrskyTelemetryData(data);
break;
case FrskyTelemetryFallbackFirstByte:
if (data == 'M') {
multiTelemetryBufferState = MultiStatusOrFrskyData;
setMultiTelemetryBufferState(module, MultiStatusOrFrskyData);
}
else {
processFrskyTelemetryData(data);
if (data != 0x7e)
multiTelemetryBufferState = FrskyTelemetryFallbackNextBytes;
setMultiTelemetryBufferState(module, FrskyTelemetryFallbackNextBytes);
}
break;
case FrskyTelemetryFallbackNextBytes:
processFrskyTelemetryData(data);
if (data == 0x7e)
if (data == 0x7e) {
// end of packet or start of new packet
multiTelemetryBufferState = FrskyTelemetryFallbackFirstByte;
setMultiTelemetryBufferState(module, FrskyTelemetryFallbackFirstByte);
}
break;
case FlyskyTelemetryFallback:
processFlySkyTelemetryData(data);
if (telemetryRxBufferCount == 0)
multiTelemetryBufferState = NoProtocolDetected;
processFlySkyTelemetryData(data, rxBuffer, rxBufferCount);
if (rxBufferCount == 0) {
setMultiTelemetryBufferState(module, NoProtocolDetected);
}
break;
case SpektrumTelemetryFallback:
processSpektrumTelemetryData(data);
if (telemetryRxBufferCount == 0)
multiTelemetryBufferState = NoProtocolDetected;
processSpektrumTelemetryData(module, data, rxBuffer, rxBufferCount);
if (rxBufferCount == 0) {
setMultiTelemetryBufferState(module, NoProtocolDetected);
}
break;
case MultiFirstByteReceived:
telemetryRxBufferCount = 0;
rxBufferCount = 0;
if (data == 'P') {
multiTelemetryBufferState = ReceivingMultiProtocol;
setMultiTelemetryBufferState(module, ReceivingMultiProtocol);
}
else if (data >= 5 && data <= 10) {
// Protocol indented for er9x/ersky9, accept only 5-10 as packet length to have
// a bit of validation
multiTelemetryBufferState = ReceivingMultiStatus;
processMultiTelemetryData(data);
setMultiTelemetryBufferState(module, ReceivingMultiStatus);
processMultiTelemetryData(data, module);
}
else {
TRACE("[MP] invalid second byte 0x%02X", data);
multiTelemetryBufferState = NoProtocolDetected;
setMultiTelemetryBufferState(module, NoProtocolDetected);
}
break;
case ReceivingMultiProtocol:
processMultiTelemetryByte(data);
processMultiTelemetryByte(data, module);
break;
case MultiStatusOrFrskyData:
// Check len byte if it makes sense for multi
if (data >= 5 && data <= 10) {
multiTelemetryBufferState = ReceivingMultiStatus;
telemetryRxBufferCount = 0;
setMultiTelemetryBufferState(module, ReceivingMultiStatus);
rxBufferCount = 0;
}
else {
multiTelemetryBufferState = FrskyTelemetryFallbackNextBytes;
processMultiTelemetryData('M');
setMultiTelemetryBufferState(module, FrskyTelemetryFallbackNextBytes);
processMultiTelemetryData('M', module);
}
processMultiTelemetryData(data);
processMultiTelemetryData(data, module);
break;
case ReceivingMultiStatus:
telemetryRxBuffer[telemetryRxBufferCount++] = data;
if (telemetryRxBufferCount > 5 && telemetryRxBuffer[0] == telemetryRxBufferCount-1) {
processMultiStatusPacket(telemetryRxBuffer+1);
telemetryRxBufferCount = 0;
multiTelemetryBufferState = NoProtocolDetected;
rxBuffer[rxBufferCount++] = data;
if (rxBufferCount > 5 && rxBuffer[0] == rxBufferCount - 1) {
processMultiStatusPacket(rxBuffer + 1, module);
rxBufferCount = 0;
setMultiTelemetryBufferState(module, NoProtocolDetected);
}
if (telemetryRxBufferCount > 10) {
if (rxBufferCount > 10) {
// too long ignore
TRACE("Overlong multi status packet detected ignoring, wanted %d", telemetryRxBuffer[0]);
telemetryRxBufferCount =0;
multiTelemetryBufferState = NoProtocolDetected;
TRACE("Overlong multi status packet detected ignoring, wanted %d", rxBuffer[0]);
rxBufferCount = 0;
setMultiTelemetryBufferState(module, NoProtocolDetected);
}
break;
}
}
+20 -12
View File
@@ -80,11 +80,16 @@ Type 0x06 Flysky AFHDS2 telemetry data
data[0] = RSSI value
data[1-28] telemetry data
Type 0x0B Spectrum Scanner telemetry data
length: 6
data[0] = start channel (2400 + x*0.333 Mhz)
data[1-5] power levels
*/
void processMultiTelemetryData(uint8_t data, uint8_t module);
void processMultiTelemetryData(uint8_t data);
#define MULTI_SCANNER_MAX_CHANNEL 249
// This should be put into the Module definition if other modules gain this functionality
struct MultiModuleSyncStatus {
@@ -107,7 +112,7 @@ struct MultiModuleSyncStatus {
};
extern MultiModuleSyncStatus multiSyncStatus;
MultiModuleSyncStatus& getMultiSyncStatus(uint8_t module);
struct MultiModuleStatus {
@@ -118,26 +123,29 @@ struct MultiModuleStatus {
uint8_t patch;
uint8_t flags;
uint8_t requiresFailsafeCheck;
tmr10ms_t lastUpdate;
void getStatusString(char* statusText);
inline bool isValid() { return (bool)(get_tmr10ms() - lastUpdate < 200); }
inline bool supportsFailsafe() { return (bool) (flags & 0x20); }
inline bool isWaitingforBind() { return (bool) (flags & 0x10); }
inline bool isBinding() { return (bool) (flags & 0x08); }
inline bool protocolValid() { return (bool) (flags & 0x04); }
inline bool serialMode() { return (bool) (flags & 0x02); }
inline bool inputDetected() { return (bool) (flags & 0x01); }
inline bool isValid() const { return (bool)(get_tmr10ms() - lastUpdate < 200); }
inline bool supportsFailsafe() const { return (bool) (flags & 0x20); }
inline bool isWaitingforBind() const { return (bool) (flags & 0x10); }
inline bool isBinding() const { return (bool) (flags & 0x08); }
inline bool protocolValid() const { return (bool) (flags & 0x04); }
inline bool serialMode() const { return (bool) (flags & 0x02); }
inline bool inputDetected() const { return (bool) (flags & 0x01); }
};
extern MultiModuleStatus multiModuleStatus;
MultiModuleStatus& getMultiModuleStatus(uint8_t module);
enum MultiBindStatus : uint8_t {
MULTI_NORMAL_OPERATION,
MULTI_BIND_INITIATED,
MULTI_BIND_FINISHED,
};
extern uint8_t multiBindStatus;
uint8_t getMultiBindStatus(uint8_t module);
void setMultiBindStatus(uint8_t module, uint8_t bindStatus);
#endif //OPENTX_MULTI_H
+25 -26
View File
@@ -370,28 +370,27 @@ void processSpektrumPacket(const uint8_t *packet)
LemonRX+Sat+tele 0xb2 07 1
*/
void processDSMBindPacket(const uint8_t *packet)
void processDSMBindPacket(uint8_t module, const uint8_t *packet)
{
uint32_t debugval;
if (g_model.moduleData[EXTERNAL_MODULE].type == MODULE_TYPE_MULTIMODULE && g_model.moduleData[EXTERNAL_MODULE].getMultiProtocol(true) == MODULE_SUBTYPE_MULTI_DSM2
&& g_model.moduleData[EXTERNAL_MODULE].multi.autoBindMode) {
if (g_model.moduleData[module].type == MODULE_TYPE_MULTIMODULE && g_model.moduleData[module].getMultiProtocol(true) == MODULE_SUBTYPE_MULTI_DSM2
&& g_model.moduleData[module].multi.autoBindMode) {
int channels = packet[5];
// Only sets channel etc when in DSM multi mode
g_model.moduleData[EXTERNAL_MODULE].channelsCount = channels - 8;
g_model.moduleData[module].channelsCount = channels - 8;
// bool use11ms = (packet[8] & 0x10) ;
if (packet[6] >= 0xb2)
g_model.moduleData[EXTERNAL_MODULE].subType = MM_RF_DSM2_SUBTYPE_DSMX_11;
g_model.moduleData[module].subType = MM_RF_DSM2_SUBTYPE_DSMX_11;
else if (packet[6] >= 0xa2)
g_model.moduleData[EXTERNAL_MODULE].subType = MM_RF_DSM2_SUBTYPE_DSMX_22;
g_model.moduleData[module].subType = MM_RF_DSM2_SUBTYPE_DSMX_22;
else if (packet[6] >= 0x12)
g_model.moduleData[EXTERNAL_MODULE].subType = MM_RF_DSM2_SUBTYPE_DSM2_11;
g_model.moduleData[module].subType = MM_RF_DSM2_SUBTYPE_DSM2_11;
else
g_model.moduleData[EXTERNAL_MODULE].subType = MM_RF_DSM2_SUBTYPE_DSM2_22;
g_model.moduleData[module].subType = MM_RF_DSM2_SUBTYPE_DSM2_22;
storageDirty(EE_MODEL);
}
debugval = packet[7] << 24 | packet[6] << 16 | packet[5] << 8 | packet[4];
@@ -400,45 +399,45 @@ void processDSMBindPacket(const uint8_t *packet)
setTelemetryValue(PROTOCOL_TELEMETRY_SPEKTRUM, (I2C_PSEUDO_TX << 8) + 4, 0, 0, debugval, UNIT_RAW, 0);
/* Finally stop binding as the rx just told us that it is bound */
if (g_model.moduleData[EXTERNAL_MODULE].type == MODULE_TYPE_MULTIMODULE && g_model.moduleData[EXTERNAL_MODULE].getMultiProtocol(true) == MODULE_SUBTYPE_MULTI_DSM2 && moduleState[EXTERNAL_MODULE].mode == MODULE_MODE_BIND) {
multiBindStatus=MULTI_BIND_FINISHED;
if (g_model.moduleData[module].type == MODULE_TYPE_MULTIMODULE && g_model.moduleData[module].getMultiProtocol(true) == MODULE_SUBTYPE_MULTI_DSM2 && moduleState[module].mode == MODULE_MODE_BIND) {
setMultiBindStatus(module, MULTI_BIND_FINISHED);
}
}
void processSpektrumTelemetryData(uint8_t data)
void processSpektrumTelemetryData(uint8_t module, uint8_t data, uint8_t* rxBuffer, uint8_t& rxBufferCount)
{
if (telemetryRxBufferCount == 0 && data != 0xAA) {
if (rxBufferCount == 0 && data != 0xAA) {
TRACE("[SPK] invalid start byte 0x%02X", data);
return;
}
if (telemetryRxBufferCount < TELEMETRY_RX_PACKET_SIZE) {
telemetryRxBuffer[telemetryRxBufferCount++] = data;
if (rxBufferCount < TELEMETRY_RX_PACKET_SIZE) {
rxBuffer[rxBufferCount++] = data;
}
else {
TRACE("[SPK] array size %d error", telemetryRxBufferCount);
telemetryRxBufferCount = 0;
TRACE("[SPK] array size %d error", rxBufferCount);
rxBufferCount = 0;
}
if (telemetryRxBuffer[1] == 0x80 && telemetryRxBufferCount >= DSM_BIND_PACKET_LENGTH) {
processDSMBindPacket(telemetryRxBuffer+2);
telemetryRxBufferCount = 0;
if (rxBuffer[1] == 0x80 && rxBufferCount >= DSM_BIND_PACKET_LENGTH) {
processDSMBindPacket(module, rxBuffer+2);
rxBufferCount = 0;
return;
}
if (telemetryRxBufferCount >= SPEKTRUM_TELEMETRY_LENGTH) {
if (rxBufferCount >= SPEKTRUM_TELEMETRY_LENGTH) {
// Debug print content of Telemetry to console
#if 0
debugPrintf("[SPK] Packet 0x%02X rssi 0x%02X: ic2 0x%02x, %02x: ",
telemetryRxBuffer[0], telemetryRxBuffer[1], telemetryRxBuffer[2], telemetryRxBuffer[3]);
rxBuffer[0], rxBuffer[1], rxBuffer[2], rxBuffer[3]);
for (int i=4; i<SPEKTRUM_TELEMETRY_LENGTH; i+=4) {
debugPrintf("%02X%02X %02X%02X ", telemetryRxBuffer[i], telemetryRxBuffer[i + 1],
telemetryRxBuffer[i + 2], telemetryRxBuffer[i + 3]);
debugPrintf("%02X%02X %02X%02X ", rxBuffer[i], rxBuffer[i + 1],
rxBuffer[i + 2], rxBuffer[i + 3]);
}
debugPrintf("\r\n");
#endif
processSpektrumPacket(telemetryRxBuffer);
telemetryRxBufferCount = 0;
processSpektrumPacket(rxBuffer);
rxBufferCount = 0;
}
}
+2 -2
View File
@@ -21,10 +21,10 @@
#ifndef _SPEKTRUM_H
#define _SPEKTRUM_H
void processSpektrumTelemetryData(uint8_t data);
void processSpektrumTelemetryData(uint8_t module, uint8_t data, uint8_t* rxBuffer, uint8_t& rxBufferCount);
void spektrumSetDefault(int index, uint16_t id, uint8_t subId, uint8_t instance);
// Used directly by multi telemetry protocol
void processSpektrumPacket(const uint8_t *packet);
void processDSMBindPacket(const uint8_t *packet);
void processDSMBindPacket(uint8_t module, const uint8_t *packet);
#endif
+20 -12
View File
@@ -19,6 +19,7 @@
*/
#include "opentx.h"
#include "multi.h"
uint8_t telemetryStreaming = 0;
uint8_t telemetryRxBuffer[TELEMETRY_RX_PACKET_SIZE]; // Receive buffer. 9 bytes (full packet), worst case 18 bytes with byte-stuffing (+1)
@@ -45,15 +46,15 @@ void processTelemetryData(uint8_t data)
#if defined(MULTIMODULE)
if (telemetryProtocol == PROTOCOL_TELEMETRY_SPEKTRUM) {
processSpektrumTelemetryData(data);
processSpektrumTelemetryData(EXTERNAL_MODULE, data, telemetryRxBuffer, telemetryRxBufferCount);
return;
}
if (telemetryProtocol == PROTOCOL_TELEMETRY_FLYSKY_IBUS) {
processFlySkyTelemetryData(data);
processFlySkyTelemetryData(data, telemetryRxBuffer, telemetryRxBufferCount);
return;
}
if (telemetryProtocol == PROTOCOL_TELEMETRY_MULTIMODULE) {
processMultiTelemetryData(data);
processMultiTelemetryData(data, EXTERNAL_MODULE);
return;
}
#endif
@@ -91,22 +92,29 @@ void telemetryWakeup()
}
#endif
#if defined(INTMODULE_USART) || defined(EXTMODULE_USART)
#if defined(INTERNAL_MODULE_PXX2) || defined(EXTMODULE_USART)
uint8_t frame[PXX2_FRAME_MAXLENGTH];
#if defined(INTMODULE_USART)
while (intmoduleFifo.getFrame(frame)) {
processPXX2Frame(INTERNAL_MODULE, frame);
}
#if defined(INTERNAL_MODULE_PXX2)
while (intmoduleFifo.getFrame(frame)) {
processPXX2Frame(INTERNAL_MODULE, frame);
}
#endif
#if defined(EXTMODULE_USART)
while (extmoduleFifo.getFrame(frame)) {
processPXX2Frame(EXTERNAL_MODULE, frame);
}
while (extmoduleFifo.getFrame(frame)) {
processPXX2Frame(EXTERNAL_MODULE, frame);
}
#endif
#endif
#if defined(INTERNAL_MODULE_MULTI)
while(!intmoduleFifo.isEmpty()) {
uint8_t b=0;
intmoduleFifo.pop(b);
processMultiTelemetryData(b, INTERNAL_MODULE);
}
#endif
#if defined(STM32)
uint8_t data;
if (telemetryGetByte(&data)) {
+17 -15
View File
@@ -27,6 +27,7 @@
#if defined(MULTIMODULE)
#include "spektrum.h"
#include "flysky_ibus.h"
#include "hitec.h"
#include "multi.h"
#endif
#include "myeeprom.h"
@@ -112,24 +113,37 @@ extern uint8_t telemetryProtocol;
#endif
#define IS_SPEKTRUM_PROTOCOL() (telemetryProtocol == PROTOCOL_TELEMETRY_SPEKTRUM)
#if defined(PCBTARANIS) || defined(PCBHORUS)
inline bool isSportLineUsedByInternalModule()
{
return g_model.moduleData[INTERNAL_MODULE].type == MODULE_TYPE_XJT_PXX1;
}
#else
inline bool isSportLineUsedByInternalModule()
{
return false;
}
#endif
inline uint8_t modelTelemetryProtocol()
{
bool sportUsed = isSportLineUsedByInternalModule();
#if defined(CROSSFIRE)
if (g_model.moduleData[EXTERNAL_MODULE].type == MODULE_TYPE_CROSSFIRE) {
return PROTOCOL_TELEMETRY_CROSSFIRE;
}
#endif
if (!IS_INTERNAL_MODULE_ENABLED() && g_model.moduleData[EXTERNAL_MODULE].type == MODULE_TYPE_PPM) {
if (!sportUsed && g_model.moduleData[EXTERNAL_MODULE].type == MODULE_TYPE_PPM) {
return g_model.telemetryProtocol;
}
#if defined(MULTIMODULE)
if (!IS_INTERNAL_MODULE_ENABLED() && g_model.moduleData[EXTERNAL_MODULE].type == MODULE_TYPE_MULTIMODULE) {
if (!sportUsed && g_model.moduleData[EXTERNAL_MODULE].type == MODULE_TYPE_MULTIMODULE) {
return PROTOCOL_TELEMETRY_MULTIMODULE;
}
#endif
// default choice
return PROTOCOL_TELEMETRY_FRSKY_SPORT;
}
@@ -232,18 +246,6 @@ extern OutputTelemetryBuffer outputTelemetryBuffer __DMA;
extern Fifo<uint8_t, LUA_TELEMETRY_INPUT_FIFO_SIZE> * luaInputTelemetryFifo;
#endif
#if defined(PCBTARANIS) || defined(PCBHORUS)
inline bool isSportLineUsedByInternalModule()
{
return g_model.moduleData[INTERNAL_MODULE].type == MODULE_TYPE_XJT_PXX1;
}
#else
inline bool isSportLineUsedByInternalModule()
{
return false;
}
#endif
void processPXX2Frame(uint8_t module, uint8_t *frame);
#endif // _TELEMETRY_H_
+5 -2
View File
@@ -527,6 +527,9 @@ int setTelemetryValue(TelemetryProtocol protocol, uint16_t id, uint8_t subId, ui
case PROTOCOL_TELEMETRY_FLYSKY_IBUS:
flySkySetDefault(index,id, subId, instance);
break;
case PROTOCOL_TELEMETRY_HITEC:
hitecSetDefault(index, id, subId, instance);
break;
#endif
#if defined(LUA)
case PROTOCOL_TELEMETRY_LUA:
@@ -599,10 +602,10 @@ const UnitConversionRule unitConversionTable[] = {
{ UNIT_KMH, UNIT_FEET_PER_SECOND, 911, 1000}, // 1 km/h = 0.911344415 feet per second
{ UNIT_MILLILITERS, UNIT_FLOZ, 100, 2957},
{ UNIT_RADIANS, UNIT_DEGREE, 10000, 175}, // 1 rad = 57.29578 deg
{ UNIT_DEGREE, UNIT_RADIANS, 175, 10000}, // 1 deg = ‪0,0174533‬ rad
{ 0, 0, 0, 0} // termination
};