From 6b57f7afa799095c275b686e6f8f5e2357d99a32 Mon Sep 17 00:00:00 2001 From: Arne Schwabe Date: Sun, 13 Nov 2016 13:09:33 -0500 Subject: [PATCH] Add FlySky Telemetry (#3997) Add FlySky Telemetry --- radio/src/gui/128x64/model_setup.cpp | 6 + radio/src/gui/212x64/model_setup.cpp | 4 +- radio/src/gui/480x272/model_setup.cpp | 6 + radio/src/gui/gui_common_arm.cpp | 4 +- radio/src/myeeprom.h | 3 +- radio/src/pulses/multi_arm.cpp | 7 +- radio/src/targets/common/arm/CMakeLists.txt | 2 +- radio/src/telemetry/flysky_ibus.cpp | 179 ++++++++++++++++++++ radio/src/telemetry/flysky_ibus.h | 27 +++ radio/src/telemetry/frsky.h | 3 +- radio/src/telemetry/spektrum.cpp | 4 + radio/src/telemetry/telemetry.cpp | 4 + radio/src/telemetry/telemetry.h | 8 +- radio/src/telemetry/telemetry_sensors.cpp | 3 + radio/src/translations/en.h.txt | 4 +- 15 files changed, 252 insertions(+), 12 deletions(-) create mode 100644 radio/src/telemetry/flysky_ibus.cpp create mode 100644 radio/src/telemetry/flysky_ibus.h diff --git a/radio/src/gui/128x64/model_setup.cpp b/radio/src/gui/128x64/model_setup.cpp index 89cc64af5..0a66c77be 100644 --- a/radio/src/gui/128x64/model_setup.cpp +++ b/radio/src/gui/128x64/model_setup.cpp @@ -829,6 +829,12 @@ void menuModelSetup(event_t event) lcdDrawText(MODEL_SETUP_2ND_COLUMN+xOffsetBind, y, STR_MODULE_BIND, l_posHorz==1 ? attr : 0); lcdDrawText(MODEL_SETUP_2ND_COLUMN+MODEL_SETUP_RANGE_OFS+xOffsetBind, y, STR_MODULE_RANGE, l_posHorz==2 ? attr : 0); uint8_t newFlag = 0; +#if defined(MULTIMODULE) + if (spektrumBindFinished) { + spektrumBindFinished = false; + s_editMode=0; + } +#endif if (attr && l_posHorz>0 && s_editMode>0) { if (l_posHorz == 1) newFlag = MODULE_BIND; diff --git a/radio/src/gui/212x64/model_setup.cpp b/radio/src/gui/212x64/model_setup.cpp index 49394caca..66eab8417 100644 --- a/radio/src/gui/212x64/model_setup.cpp +++ b/radio/src/gui/212x64/model_setup.cpp @@ -936,8 +936,8 @@ void menuModelSetup(event_t event) uint8_t newFlag = 0; #if defined(MULTIMODULE) if (spektrumBindFinished) { - spektrumBindFinished = false; - s_editMode=0; + spektrumBindFinished = false; + s_editMode=0; } #endif if (attr && l_posHorz>0 && s_editMode>0) { diff --git a/radio/src/gui/480x272/model_setup.cpp b/radio/src/gui/480x272/model_setup.cpp index 600e2f135..d98e1b4a1 100644 --- a/radio/src/gui/480x272/model_setup.cpp +++ b/radio/src/gui/480x272/model_setup.cpp @@ -849,6 +849,12 @@ bool menuModelSetup(event_t event) drawButton(MODEL_SETUP_2ND_COLUMN+xOffsetBind, y, STR_MODULE_BIND, (moduleFlag[moduleIdx] == MODULE_BIND ? BUTTON_ON : BUTTON_OFF) | (l_posHorz==1 ? attr : 0)); drawButton(MODEL_SETUP_2ND_COLUMN+MODEL_SETUP_RANGE_OFS+xOffsetBind, y, STR_MODULE_RANGE, (moduleFlag[moduleIdx] == MODULE_RANGECHECK ? BUTTON_ON : BUTTON_OFF) | (l_posHorz==2 ? attr : 0)); uint8_t newFlag = 0; +#if defined(MULTIMODULE) + if (spektrumBindFinished) { + spektrumBindFinished = false; + s_editMode=0; + } +#endif if (attr && l_posHorz>0 && s_editMode>0) { if (l_posHorz == 1) newFlag = MODULE_BIND; diff --git a/radio/src/gui/gui_common_arm.cpp b/radio/src/gui/gui_common_arm.cpp index fb7b5d0bf..e736bdd04 100644 --- a/radio/src/gui/gui_common_arm.cpp +++ b/radio/src/gui/gui_common_arm.cpp @@ -533,12 +533,12 @@ bool isTelemetryProtocolAvailable(int protocol) } #if !defined(MULTIMODULE) - if (protocol== PROTOCOL_SPEKTRUM) { + if (protocol == PROTOCOL_SPEKTRUM || protocol == PROTOCOL_FLYSKY_IBUS) { return false; } #endif -#if defined(PBHORUS) +#if defined(PCBHORUS) if (protocol == PROTOCOL_FRSKY_D_SECONDARY) { return false; } diff --git a/radio/src/myeeprom.h b/radio/src/myeeprom.h index ae5efcc98..70b7d72e6 100644 --- a/radio/src/myeeprom.h +++ b/radio/src/myeeprom.h @@ -617,7 +617,8 @@ enum TelemetryType PROTOCOL_FRSKY_D_SECONDARY, PROTOCOL_PULSES_CROSSFIRE, PROTOCOL_SPEKTRUM, - PROTOCOL_TELEMETRY_LAST=PROTOCOL_SPEKTRUM + PROTOCOL_FLYSKY_IBUS, + PROTOCOL_TELEMETRY_LAST=PROTOCOL_FLYSKY_IBUS }; enum DisplayTrims diff --git a/radio/src/pulses/multi_arm.cpp b/radio/src/pulses/multi_arm.cpp index 81a358d68..59cb0c18d 100644 --- a/radio/src/pulses/multi_arm.cpp +++ b/radio/src/pulses/multi_arm.cpp @@ -164,7 +164,12 @@ void setupPulsesMultimodule(uint8_t port) } } - // For custom protocol send unmofied type byte + // Set the highest bit of option byte in AFHDS2A protocol to instruct MULTI to passthrough telemetry bytes instead + // of sending Frsky D telemetry + if (g_model.moduleData[port].getMultiProtocol(false) == MM_RF_PROTO_FS_AFHDS2A) + optionValue = optionValue | 0x80; + + // For custom protocol send unmodified type byte if (g_model.moduleData[port].getMultiProtocol(true) == MM_RF_CUSTOM_SELECTED) type = g_model.moduleData[port].getMultiProtocol(false); diff --git a/radio/src/targets/common/arm/CMakeLists.txt b/radio/src/targets/common/arm/CMakeLists.txt index 414ef62b7..d34d9f7e5 100644 --- a/radio/src/targets/common/arm/CMakeLists.txt +++ b/radio/src/targets/common/arm/CMakeLists.txt @@ -70,7 +70,7 @@ if(HAPTIC) endif() if(MULTIMODULE) add_definitions(-DMULTIMODULE) - set(SRC ${SRC} pulses/multi_arm.cpp telemetry/spektrum.cpp) + set(SRC ${SRC} pulses/multi_arm.cpp telemetry/spektrum.cpp telemetry/flysky_ibus.cpp) endif() add_definitions(-DCPUARM -DVIRTUAL_INPUTS) add_definitions(-DTELEMETRY_FRSKY -DTELEMETRY_FRSKY_SPORT -DFRSKY_HUB -DGPS -DPXX -DDSM2) diff --git a/radio/src/telemetry/flysky_ibus.cpp b/radio/src/telemetry/flysky_ibus.cpp new file mode 100644 index 000000000..749e3a6bf --- /dev/null +++ b/radio/src/telemetry/flysky_ibus.cpp @@ -0,0 +1,179 @@ +/* + * 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" + +/* + * 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 + * + */ + + +#define FLYSKY_TELEMETRY_LENGTH (2+7*4) +#define TX_RSSI_ID 300 // Pseudo id outside 1 byte range of FlySky sensors +#define FS_ID_RSSI 0x7c + +struct FlySkySensor { + const uint16_t id; + const char *name; + const TelemetryUnit unit; + const uint8_t precision; +}; + + + +const FlySkySensor flySkySensors[] = { + + // RX Voltage (remapped, really 0x0) + {0x100, ZSTR_A1, UNIT_VOLTS, 2}, + // Temperature + {0x01, ZSTR_TEMP1, UNIT_CELSIUS, 1}, + // RPM + {0x02, ZSTR_RPM, UNIT_RAW, 0}, + // External voltage + {0x03, ZSTR_A3, UNIT_VOLTS, 1}, + // RX Noise + {0xfb, ZSTR_RX_SNR, UNIT_DB, 0}, + // RX RSSI + {FS_ID_RSSI, ZSTR_RSSI, UNIT_DB, 0}, + // RX error rate + {0xfe, ZSTR_RX_QUALITY, UNIT_RAW, 1}, + // 0xff is an used sensor slot) + // Pseud sensor for TRSSI + {TX_RSSI_ID, ZSTR_TX_RSSI, UNIT_RAW, 0}, + // sentinel + {0x00, NULL, UNIT_RAW, 0}, +}; + +static void processFlySkySensor(const uint8_t *packet) +{ + uint16_t id = packet[0]; + const uint8_t instance = packet[1]; + const int32_t value = (packet[3] << 8) + packet[2]; + + if (id == 0xff) { + // No sensor + 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_RSSI) { + telemetryData.rssi.set(value); + } + + for (const FlySkySensor * sensor = flySkySensors; sensor->id; sensor++) { + // Extract value, skip header + if (sensor->id == id) { + setTelemetryValue(TELEM_PROTO_FLYSKY_IBUS, id, 0, instance, value, sensor->unit, sensor->precision); + return; + } + } + setTelemetryValue(TELEM_PROTO_FLYSKY_IBUS, id, 0, instance, value, UNIT_RAW, 0); +} + +static void processFlySkyPacket(const uint8_t *packet) +{ + // Set TX RSSI Value, reverse MULTIs scaling + setTelemetryValue(TELEM_PROTO_FLYSKY_IBUS, TX_RSSI_ID, 0, 0, packet[1], UNIT_RAW, 0); + + for (int sensor = 0; sensor < 7; sensor++) { + int index = 2 + (4 * sensor); + processFlySkySensor(packet+index); + } + telemetryStreaming = TELEMETRY_TIMEOUT10ms; +} + +void processFlySkyTelemetryData(uint8_t data) +{ + if (telemetryRxBufferCount == 0 && data != 0xAA) { + TRACE("[IBUS] invalid start byte 0x%02X", data); + return; + } + + if (telemetryRxBufferCount < TELEMETRY_RX_PACKET_SIZE) { + telemetryRxBuffer[telemetryRxBufferCount++] = data; + } + else { + TRACE("[IBUS] array size %d error", telemetryRxBufferCount); + telemetryRxBufferCount = 0; + } + + + if (telemetryRxBufferCount >= FLYSKY_TELEMETRY_LENGTH) { + // debug print the content of the packets +#if 0 + debugPrintf("[IBUS] Packet 0x%02X rssi 0x%02X: ", + telemetryRxBuffer[0], telemetryRxBuffer[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("\r\n"); +#endif + processFlySkyPacket(telemetryRxBuffer); + telemetryRxBufferCount = 0; + } +} + +const FlySkySensor *getFlySkySensor(uint16_t id) +{ + for (const FlySkySensor * sensor = flySkySensors; sensor->id; sensor++) { + if (id == sensor->id) + 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(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); +} diff --git a/radio/src/telemetry/flysky_ibus.h b/radio/src/telemetry/flysky_ibus.h new file mode 100644 index 000000000..15eb59fc2 --- /dev/null +++ b/radio/src/telemetry/flysky_ibus.h @@ -0,0 +1,27 @@ +/* + * 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 _FLYSKY_IBUS_H +#define _FLYSKY_IBUS_H + +void processFlySkyTelemetryData(uint8_t data); +void flySkySetDefault(int index, uint16_t id, uint8_t subId, uint8_t instance); + +#endif diff --git a/radio/src/telemetry/frsky.h b/radio/src/telemetry/frsky.h index 299f8584e..cbb0b11c6 100644 --- a/radio/src/telemetry/frsky.h +++ b/radio/src/telemetry/frsky.h @@ -432,7 +432,8 @@ enum TelemetryProtocol TELEM_PROTO_FRSKY_SPORT, TELEM_PROTO_CROSSFIRE, TELEM_PROTO_SPEKTRUM, - TELEM_PROTO_LUA + TELEM_PROTO_LUA, + TELEM_PROTO_FLYSKY_IBUS, }; enum TelemAnas { diff --git a/radio/src/telemetry/spektrum.cpp b/radio/src/telemetry/spektrum.cpp index 5b0242bda..1a1edb78c 100644 --- a/radio/src/telemetry/spektrum.cpp +++ b/radio/src/telemetry/spektrum.cpp @@ -403,7 +403,10 @@ void processSpektrumTelemetryData(uint8_t data) } + if (telemetryRxBufferCount >= 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]); for (int i=4; i0 (true) == data is streaming in. 0 = no data detected for some time @@ -69,10 +70,11 @@ enum TelemetrySerialMode { TELEMETRY_SERIAL_8E2 }; -#if defined(CROSSFIRE) +#if defined(CROSSFIRE) || defined(MULTIMODULE) #define TELEMETRY_RX_PACKET_SIZE 128 +// multi module Spektrum telemetry is 18 bytes, FlySky is 37 bytes #else -#define TELEMETRY_RX_PACKET_SIZE 19 // 9 bytes (full packet), worst case 18 bytes with byte-stuffing (+1), multimodule Spektrum telemetry is 18 bytes +#define TELEMETRY_RX_PACKET_SIZE 19 // 9 bytes (full packet), worst case 18 bytes with byte-stuffing (+1) #endif extern uint8_t telemetryRxBuffer[TELEMETRY_RX_PACKET_SIZE]; @@ -149,6 +151,8 @@ inline uint8_t modelTelemetryProtocol() if (g_model.moduleData[INTERNAL_MODULE].rfProtocol == RF_PROTO_OFF && g_model.moduleData[EXTERNAL_MODULE].type == MODULE_TYPE_MULTIMODULE) { if (g_model.moduleData[EXTERNAL_MODULE].getMultiProtocol(false) == MM_RF_PROTO_DSM2) return PROTOCOL_SPEKTRUM; + else if (g_model.moduleData[EXTERNAL_MODULE].getMultiProtocol(false) == MM_RF_PROTO_FS_AFHDS2A) + return PROTOCOL_FLYSKY_IBUS; else if ((g_model.moduleData[EXTERNAL_MODULE].getMultiProtocol(false) == MM_RF_PROTO_FRSKY) && (g_model.moduleData[EXTERNAL_MODULE].subType == MM_RF_FRSKY_SUBTYPE_D16 || g_model.moduleData[EXTERNAL_MODULE].subType == MM_RF_FRSKY_SUBTYPE_D16_8CH)) return PROTOCOL_FRSKY_SPORT; diff --git a/radio/src/telemetry/telemetry_sensors.cpp b/radio/src/telemetry/telemetry_sensors.cpp index f7b987551..76dfe982c 100644 --- a/radio/src/telemetry/telemetry_sensors.cpp +++ b/radio/src/telemetry/telemetry_sensors.cpp @@ -458,6 +458,9 @@ int setTelemetryValue(TelemetryProtocol protocol, uint16_t id, uint8_t subId, ui case TELEM_PROTO_SPEKTRUM: spektrumSetDefault(index, id, subId, instance); break; + case TELEM_PROTO_FLYSKY_IBUS: + flySkySetDefault(index,id, subId, instance); + break; #endif #if defined(LUA) case TELEM_PROTO_LUA: diff --git a/radio/src/translations/en.h.txt b/radio/src/translations/en.h.txt index db2ea896e..006e50fc0 100644 --- a/radio/src/translations/en.h.txt +++ b/radio/src/translations/en.h.txt @@ -115,8 +115,8 @@ #define TR_MULTI_CUSTOM "Custom" -#define LEN_SUBTYPE_FLYSKY "\007" -#define TR_SUBTYPE_FLYSKY "Std\0 ""V9x9 ""V6x6 ""V912 " +#define LEN_SUBTYPE_FLYSKY "\004" +#define TR_SUBTYPE_FLYSKY "Std\0""V9x9""V6x6""V912" #define LEN_SUBTYPE_AFHDS2A "\010" #define TR_SUBTYPE_AFHDS2A "PWM,IBUS""PPM,IBUS""PWM,SBUS""PPM,SBUS"