diff --git a/radio/src/dataconstants.h b/radio/src/dataconstants.h index d1e7a2ce2e1..31f71bade8a 100644 --- a/radio/src/dataconstants.h +++ b/radio/src/dataconstants.h @@ -271,6 +271,7 @@ enum TelemetryProtocol PROTOCOL_TELEMETRY_CROSSFIRE, PROTOCOL_TELEMETRY_SPEKTRUM, PROTOCOL_TELEMETRY_FLYSKY_IBUS, + PROTOCOL_TELEMETRY_FLYSKY_IBUS2, PROTOCOL_TELEMETRY_HITEC, PROTOCOL_TELEMETRY_HOTT, PROTOCOL_TELEMETRY_MLINK, diff --git a/radio/src/gui/colorlcd/module/afhds3_options.cpp b/radio/src/gui/colorlcd/module/afhds3_options.cpp index 73f5bdc14a9..317d5a87d8e 100644 --- a/radio/src/gui/colorlcd/module/afhds3_options.cpp +++ b/radio/src/gui/colorlcd/module/afhds3_options.cpp @@ -30,8 +30,8 @@ static const lv_coord_t row_dsc[] = {LV_GRID_CONTENT, #define SET_DIRTY() static const char* const _analog_outputs[] = { "PWM", "PPM" }; -static const char* const _bus_types[] = { "iBUS OUT", "iBUS IN", "SBUS" }; -static const char* const _v1_bus_types[] = { "PWM", "PPM", "SBUS", "iBUS IN", "iBUS OUT" }; +static const char* const _bus_types[] = { "iBUS OUT", "iBUS IN", "SBUS"}; +static const char* const _v1_bus_types[] = { "PWM", "PPM", "SBUS", "iBUS IN", "iBUS OUT", "iBUS2" }; static const char* const _v1_pwmfreq_types[] = { STR_ANALOG_SERVO, STR_DIGITAL_SERVO, "SR833HZ", "SFR1000HZ", STR_MULTI_CUSTOM }; static const char* const _v0_pwmfreq_types[] = { STR_ANALOG_SERVO, STR_DIGITAL_SERVO, STR_MULTI_CUSTOM }; @@ -118,6 +118,13 @@ PWMfrequencyChoice::PWMfrequencyChoice(Window* parent, uint8_t moduleIdx ) : num_edit->show(pwmvalue_type == 2); } +uint8_t GetDisplayPortType(uint8_t NP) { + if (NP == afhds3::SES_NPT_IBUS2_HUB_PORT) + return afhds3::SES_NPT_IBUS2; + else + return NP; +} + AFHDS3_Options::AFHDS3_Options(uint8_t moduleIdx) : Page(ICON_MODEL_SETUP) { cfg = afhds3::getConfig(moduleIdx); @@ -191,8 +198,8 @@ AFHDS3_Options::AFHDS3_Options(uint8_t moduleIdx) : Page(ICON_MODEL_SETUP) portName += 'A' + i; new StaticText(line, rect_t{}, portName.c_str()); new Choice(line, rect_t{}, _v1_bus_types, afhds3::SES_NPT_PWM, - afhds3::SES_NPT_IBUS1_OUT, - GET_DEFAULT(vCfg->NewPortTypes[i]), + afhds3::SES_NPT_IBUS2, + GET_DEFAULT(GetDisplayPortType(vCfg->NewPortTypes[i])), [=](int32_t newValue) { if(!newValue) { @@ -201,14 +208,49 @@ AFHDS3_Options::AFHDS3_Options(uint8_t moduleIdx) : Page(ICON_MODEL_SETUP) } else { uint8_t j = 0; - for ( j = 0; j < SES_NPT_NB_MAX_PORTS; j++) { - if ( vCfg->NewPortTypes[j]== newValue && i != j ) - break; + bool isNewValueIBUS2 = (newValue == afhds3::SES_NPT_IBUS2 || newValue == afhds3::SES_NPT_IBUS2_HUB_PORT); + bool isNewValueIBUS1_IN = (newValue == afhds3::SES_NPT_IBUS1_IN); + bool isNewValueIBUS1_OUT = (newValue == afhds3::SES_NPT_IBUS1_OUT); + bool conflict = false; + for (j = 0; j < SES_NPT_NB_MAX_PORTS; j++) { + if (j == i) continue; + + uint8_t existingType = vCfg->NewPortTypes[j]; + bool isExistingIBUS1_IN = (existingType == afhds3::SES_NPT_IBUS1_IN); + bool isExistingIBUS1_OUT = (existingType == afhds3::SES_NPT_IBUS1_OUT); + bool isExistingIBUS2 = (existingType == afhds3::SES_NPT_IBUS2 || existingType == afhds3::SES_NPT_IBUS2_HUB_PORT); + + // Check for mutual exclusivity of iBUS1 and iBUS2 + if ((isNewValueIBUS1_IN || isNewValueIBUS1_OUT) && isExistingIBUS2) { + conflict = true; + break; + } + if (isNewValueIBUS2 && (isExistingIBUS1_IN || isExistingIBUS1_OUT)) { + conflict = true; + break; + } + + // Check for iBUS1 conflicts in the same direction + if (isNewValueIBUS1_IN && isExistingIBUS1_IN) { + conflict = true; + break; + } + if (isNewValueIBUS1_OUT && isExistingIBUS1_OUT) { + conflict = true; + break; + } + + // Check for duplicate entries of non-iBUS type + if (!isNewValueIBUS2 && !isNewValueIBUS1_IN && !isNewValueIBUS1_OUT && + existingType == newValue) { + conflict = true; + break; + } } - //The RX does not support two or more ports to output IBUS (the same is true for PPM and SBUS). if(j==SES_NPT_NB_MAX_PORTS ) { - vCfg->NewPortTypes[i] = newValue; + if (!conflict) + vCfg->NewPortTypes[i] = newValue; DIRTY_CMD(cfg, afhds3::DirtyConfig::DC_RX_CMD_PORT_TYPE_V1); } } @@ -232,3 +274,103 @@ AFHDS3_Options::AFHDS3_Options(uint8_t moduleIdx) : Page(ICON_MODEL_SETUP) DIRTY_CMD(cfg, afhds3::DirtyConfig::DC_RX_CMD_RSSI_CHANNEL_SETUP); }); } + +AFHDS3_Sensors::AFHDS3_Sensors(uint8_t moduleIdx) : Page(ICON_MODEL_SETUP) +{ + cfg = afhds3::getConfig(moduleIdx); + std::string title = + moduleIdx == INTERNAL_MODULE ? STR_INTERNALRF : STR_EXTERNALRF; + header->setTitle(title); + + title = "AFHDS3 ("; + title += "SENSORS"; + title += ")"; + header->setTitle2(title); + + body->setFlexLayout(LV_FLEX_FLOW_COLUMN, PAD_TINY); + + FlexGridLayout grid(col_dsc, row_dsc, PAD_TINY); + + bool ibus2_mode = false; + + if (cfg->version == 0) { + return; + } + auto vCfg = &cfg->v1; + + for (uint8_t j = 0; j < SES_NPT_NB_MAX_PORTS; j++) { + if (vCfg->NewPortTypes[j] == afhds3::SES_NPT_IBUS2 || vCfg->NewPortTypes[j] == afhds3::SES_NPT_IBUS2_HUB_PORT) { + ibus2_mode = true; + break; + } + } + std::string temp_str; + auto line = body->newLine(grid); + + if (!ibus2_mode) { + temp_str = "Only in the iBUS2 mode can the sensors be set."; + new StaticText(line, rect_t{}, temp_str); + return; + } + + if (cfg->others.sensorOnLine & (1 << afhds3::DirtyIbus2Sensor::IBUS2_SENSOR_GPS)) + { + temp_str = STR_IMU; + temp_str += " "; + temp_str += TR_CURRENTSENSOR; + new StaticText(line, rect_t{}, temp_str); + auto imu_btn = new TextButton(line, rect_t{}, STR_CALIBRATION); + imu_btn->setPressHandler([=]() -> uint8_t { + DIRTY_CMD(cfg, afhds3::DirtyConfig::DC_RX_CMD_CALIB_GYRO); + return 0; + }); + + temp_str = "DIST"; + temp_str += " "; + temp_str += TR_CURRENTSENSOR; + line = body->newLine(grid); + new StaticText(line, rect_t{}, temp_str); + auto dist_btn = new TextButton(line, rect_t{}, TR_RESET); + dist_btn->setPressHandler([=]() -> uint8_t { + DIRTY_CMD(cfg, afhds3::DirtyConfig::DC_RX_CMD_CALIB_DIST); + return 0; + }); + } + + if (cfg->others.sensorOnLine & (1 << afhds3::DirtyIbus2Sensor::IBUS2_SENSOR_GPS) || cfg->others.sensorOnLine & (1 << afhds3::DirtyIbus2Sensor::IBUS2_SENSOR_PRES)) { + temp_str = TR_ALTSENSOR; + line = body->newLine(grid); + new StaticText(line, rect_t{}, temp_str); + auto alt_btn = new TextButton(line, rect_t{}, TR_RESET); + alt_btn->setPressHandler([=]() -> uint8_t { + DIRTY_CMD(cfg, afhds3::DirtyConfig::DC_RX_CMD_CALIB_ALT); + return 0; + }); + } + + // if (cfg->others.sensorOnLine & (1 << afhds3::DirtyIbus2Sensor::IBUS2_SENSOR_RPM)) { + // temp_str = "RPM"; + // temp_str += " "; + // temp_str += TR_CURRENTSENSOR; + // line = body->newLine(grid); + // new StaticText(line, rect_t{}, temp_str); + // new NumberEdit(line, rect_t{0, 0, EdgeTxStyles::EDIT_FLD_WIDTH_NARROW, 0}, 1, 12, GET_SET_DEFAULT(cfg->others.calibData[afhds3::DirtyIbus2Sensor::IBUS2_SENSOR_RPM])); + // } + + if (cfg->others.sensorOnLine & (1 << afhds3::DirtyIbus2Sensor::IBUS2_SENSOR_IBC)) { + // temp_str = "IBC01"; + // temp_str += " Auto "; + // temp_str += "Clear"; + // line = body->newLine(grid); + // new StaticText(line, rect_t{}, temp_str); + // new ToggleSwitch(line, rect_t{}, GET_SET_DEFAULT(cfg->others.calibData[afhds3::DirtyIbus2Sensor::IBUS2_SENSOR_IBC])); + + temp_str = "IBC01"; + temp_str += " "; + temp_str += "Calib"; + line = body->newLine(grid); + new StaticText(line, rect_t{}, temp_str); + auto edit = new NumberEdit(line, rect_t{0, 0, EdgeTxStyles::EDIT_FLD_WIDTH_NARROW, 0}, 0, 120, GET_SET_DEFAULT(cfg->others.calibData[afhds3::DirtyIbus2Sensor::IBUS2_SENSOR_IBC]), PREC1); + edit->setSuffix("v"); + } +} \ No newline at end of file diff --git a/radio/src/gui/colorlcd/module/afhds3_options.h b/radio/src/gui/colorlcd/module/afhds3_options.h index 3a7497b5987..6197afa8e2e 100644 --- a/radio/src/gui/colorlcd/module/afhds3_options.h +++ b/radio/src/gui/colorlcd/module/afhds3_options.h @@ -31,3 +31,11 @@ class AFHDS3_Options : public Page public: AFHDS3_Options(uint8_t moduleIdx); }; + +class AFHDS3_Sensors : public Page +{ + afhds3::Config_u* cfg; + + public: + AFHDS3_Sensors(uint8_t moduleIdx); +}; \ No newline at end of file diff --git a/radio/src/gui/colorlcd/module/afhds3_settings.cpp b/radio/src/gui/colorlcd/module/afhds3_settings.cpp index 97664c2f70f..a4bb68becd5 100644 --- a/radio/src/gui/colorlcd/module/afhds3_settings.cpp +++ b/radio/src/gui/colorlcd/module/afhds3_settings.cpp @@ -87,6 +87,11 @@ AFHDS3Settings::AFHDS3Settings(Window* parent, const FlexGridLayout& g, return 0; }); + new TextButton(afhds3TypeForm, rect_t{}, STR_TELEMETRY_SENSORS, [=]() { + new AFHDS3_Sensors(moduleIdx); + return 0; + }); + bool hasPowerOption = false; int maxPower; if (moduleIdx == INTERNAL_MODULE) { diff --git a/radio/src/pulses/afhds3.cpp b/radio/src/pulses/afhds3.cpp index 5f58c7f5e19..80b2c8f035e 100644 --- a/radio/src/pulses/afhds3.cpp +++ b/radio/src/pulses/afhds3.cpp @@ -51,8 +51,18 @@ extern uint16_t sns_RFCurrentPower; //get channel value outside of afhds3 namespace int32_t getChannelValue(uint8_t channel); void processFlySkyAFHDS3Sensor(const uint8_t * packet, uint8_t type); +void processFlySkyIbus2AFHDS3Sensor(const uint8_t * packet, uint8_t type); void processFlySkySensor(const uint8_t * packet, uint8_t type); +void flySkyIbus2CalGpsGyro(uint8_t* packet, uint8_t* len); +void flySkyIbus2CalibIBC(uint8_t* packet, uint8_t* len, short voltags); +void flySkyIbus2CalGpsAlt(); +void flySkyIbus2CalGpsDist(); +void Ibus2ParamCheck(uint8_t* packet, uint8_t len); +void flySkyIbus2ReadParamRPM(uint8_t* packet, uint8_t* len); +bool getIbus2IbcState(); +uint8_t flyskyIbus2SensorOnLine(); + namespace afhds3 { @@ -284,6 +294,8 @@ class ProtoState bool syncSettings(); + bool sensorCalibration(); + // void requestInfoAndRun(bool send = false); uint8_t setFailSafe(int16_t* target, uint8_t rfchannelcount=AFHDS3_MAX_CHANNELS); @@ -412,6 +424,18 @@ bool ProtoState::hasTelemetry() return cfg.v1.IsTwoWay; } +uint8_t ibus_type[SES_NPT_NB_MAX_PORTS] = {SES_NPT_IBUS1_IN}; +void setIbusType(uint8_t* ibus_type_buf) +{ + for(uint8_t i = 0; i< SES_NPT_NB_MAX_PORTS; i++) { + if (ibus_type_buf[i] == afhds3::SES_NPT_IBUS2 || ibus_type_buf[i] == afhds3::SES_NPT_IBUS2_HUB_PORT) { + ibus_type[i] = afhds3::SES_NPT_IBUS2; + } else { + ibus_type[i] = afhds3::SES_NPT_IBUS1_IN; + } + } +} + void ProtoState::setupFrame() { bool trsp_error = false; @@ -558,6 +582,8 @@ void ProtoState::setupFrame() // Sync config, with commands if (syncSettings()) { return; } + if (sensorCalibration()) { return; } + // Send channels data sendChannelsData(); } else { @@ -704,6 +730,7 @@ void ProtoState::parseData(uint8_t* rxBuffer, uint8_t rxBufferCount) this->rx_state = true; DIRTY_CMD( cfg, DC_RX_CMD_GET_RX_VERSION ); trsp.enqueue( COMMAND::MODULE_VERSION, FRAME_TYPE::REQUEST_GET_DATA ); + setIbusType(cfg->v1.NewPortTypes); // modelcfgGet = true; // cfg.others.isConnected = true; // cfg.others.lastUpdated = get_tmr10ms(); @@ -755,7 +782,19 @@ void ProtoState::parseData(uint8_t* rxBuffer, uint8_t rxBufferCount) break; } telemetry[0] = 0; - ::processFlySkyAFHDS3Sensor( telemetry, len-3 ); + uint8_t ibus_version = SES_NPT_IBUS1_IN; + for(uint8_t i = 0; i < SES_NPT_NB_MAX_PORTS; i++) { + // If ibus2 is configured, ignore ibus1. + if (ibus_type[i] == SES_NPT_IBUS2 || ibus_type[i] == SES_NPT_IBUS2_HUB_PORT) { + ibus_version = SES_NPT_IBUS2; + break; + } + } + if (ibus_version == afhds3::SES_NPT_IBUS2) { + ::processFlySkyIbus2AFHDS3Sensor(telemetry, len-3); + } else { + ::processFlySkyAFHDS3Sensor(telemetry, len-3); + } telemetry += len; } } @@ -811,6 +850,20 @@ void ProtoState::parseData(uint8_t* rxBuffer, uint8_t rxBufferCount) clearDirtyFlag(DC_RX_CMD_GET_RX_VERSION); } }break; + case RX_CMD_CODE_IBUS2_SET_PARAM: + { + uint8_t len = *data++; + ::Ibus2ParamCheck(data, len); + } + break; + case RX_CMD_CODE_IBUS2_GET_PARAM: + { + uint8_t len = *data++; + ::Ibus2ParamCheck(data, len); + // cfg->others.calibData[IBUS2_SENSOR_IBC] = getIbus2IbcState(); + // DIRTY_CMD(cfg, DC_RX_CMD_CLEAR_IBC); + } + break; default: break; } @@ -835,6 +888,50 @@ inline bool isPWM(uint8_t mode) return !(mode & 2); } +bool ProtoState::sensorCalibration() { + auto *cfg = this->getConfig(); + + static uint8_t data[30] = {0}; + uint8_t len = 0; + + uint8_t sensor_online = flyskyIbus2SensorOnLine(); + cfg->others.sensorOnLine = sensor_online; + + if (checkDirtyFlag(DC_RX_CMD_CALIB_GYRO)) { + + ::flySkyIbus2CalGpsGyro(data, &len); + trsp.putFrame( COMMAND::SEND_COMMAND, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, data, len); + clearDirtyFlag(DC_RX_CMD_CALIB_GYRO); + return true; + } + + if (checkDirtyFlag(DC_RX_CMD_CALIB_ALT)) { + ::flySkyIbus2CalGpsAlt(); + clearDirtyFlag(DC_RX_CMD_CALIB_ALT); + return true; + } + + if (checkDirtyFlag(DC_RX_CMD_CALIB_DIST) ){ + ::flySkyIbus2CalGpsDist(); + clearDirtyFlag(DC_RX_CMD_CALIB_DIST); + return true; + } + + static uint32_t ibc_update_tick = 0; + static short last_ibc_v = cfg->others.calibData[IBUS2_SENSOR_IBC]; + if (last_ibc_v != cfg->others.calibData[IBUS2_SENSOR_IBC] ) { + if (timersGetMsTick() - ibc_update_tick > 2000) { // 2-second check + ibc_update_tick = timersGetMsTick(); + last_ibc_v = cfg->others.calibData[IBUS2_SENSOR_IBC]; + ::flySkyIbus2CalibIBC(data, &len, cfg->others.calibData[IBUS2_SENSOR_IBC]); + trsp.putFrame( COMMAND::SEND_COMMAND, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, data, len); + return true; + } + } + + return false; +} + bool ProtoState::syncSettings() { @@ -887,7 +984,26 @@ bool ProtoState::syncSettings() { // TRACE("AFHDS3 [RX_CMD_PORT_TYPE_V1]"); uint8_t data[] = { (uint8_t)(RX_CMD_PORT_TYPE_V1&0xFF), (uint8_t)((RX_CMD_PORT_TYPE_V1>>8)&0xFF), 4, 0, 0, 0, 0 }; - std::memcpy(&data[3], &cfg->v1.NewPortTypes, SES_NPT_NB_MAX_PORTS); + setIbusType(cfg->v1.NewPortTypes); + // If pure is upgraded from multiple ibus2 to ibus2 hub + uint8_t tempPortTypes[SES_NPT_NB_MAX_PORTS] = {0}; + std::memcpy(tempPortTypes, cfg->v1.NewPortTypes, SES_NPT_NB_MAX_PORTS); + + uint8_t ibus2Count = 0; + for (uint8_t i = 0; i < SES_NPT_NB_MAX_PORTS; i++) { + if (tempPortTypes[i] == afhds3::SES_NPT_IBUS2) { + ibus2Count++; + } + } + if (ibus2Count >= 2) { + for (uint8_t i = 0; i < SES_NPT_NB_MAX_PORTS; i++) { + if (tempPortTypes[i] == afhds3::SES_NPT_IBUS2) { + tempPortTypes[i] = afhds3::SES_NPT_IBUS2_HUB_PORT; + } + } + } + + std::memcpy(&data[3], tempPortTypes, SES_NPT_NB_MAX_PORTS); trsp.putFrame(COMMAND::SEND_COMMAND, FRAME_TYPE::REQUEST_SET_EXPECT_DATA, data, sizeof(data)); return true; } diff --git a/radio/src/pulses/afhds3_config.h b/radio/src/pulses/afhds3_config.h index ea31c0733cc..c8ddda25c1b 100644 --- a/radio/src/pulses/afhds3_config.h +++ b/radio/src/pulses/afhds3_config.h @@ -100,7 +100,21 @@ enum DirtyConfig { DC_RX_CMD_BUS_DIRECTION, DC_RX_CMD_BUS_FAILSAFE, DC_RX_CMD_GET_VERSION, - DC_RX_CMD_GET_RX_VERSION + DC_RX_CMD_GET_RX_VERSION, + DC_RX_CMD_CALIB_GYRO, + DC_RX_CMD_CALIB_ALT, + DC_RX_CMD_CALIB_DIST, + DC_RX_CMD_CALIB_RPM, + DC_RX_CMD_CALIB_IBC_V, + DC_RX_CMD_CLEAR_IBC, +}; + +enum DirtyIbus2Sensor { + IBUS2_SENSOR_RPM, + IBUS2_SENSOR_GPS, + IBUS2_SENSOR_IBC, + IBUS2_SENSOR_PRES, + IBUS2_SENSOR_NUM, }; #define SET_AND_SYNC(cfg, value, dirtyCmd) [=](int32_t newValue) { value = newValue; cfg->others.dirtyFlag |= (uint32_t) 1 << dirtyCmd; } @@ -137,6 +151,8 @@ PACK(struct sDATA_Others { tmr10ms_t lastUpdated; // last updated time bool isConnected; // specify if receiver is connected uint32_t dirtyFlag; // mapped to commands that need to be issued to sync settings + short calibData[4]; // sensor Calibration + uint8_t sensorOnLine; }); union Config_u diff --git a/radio/src/pulses/afhds3_transport.h b/radio/src/pulses/afhds3_transport.h index a7442ad460d..f1832cbb766 100644 --- a/radio/src/pulses/afhds3_transport.h +++ b/radio/src/pulses/afhds3_transport.h @@ -71,6 +71,8 @@ enum COMMAND : uint8_t { #define RX_CMD_IBUS_DIRECTION ( 0x7020 ) //IBUS INPUT or OUTPUT #define RX_CMD_BUS_FAILSAFE ( 0x702A ) #define RX_CMD_GET_VERSION ( 0x701F ) +#define RX_CMD_CODE_IBUS2_SET_PARAM ( 0x7025 ) +#define RX_CMD_CODE_IBUS2_GET_PARAM ( 0x7026 ) enum RX_CMDRESULT: uint8_t { diff --git a/radio/src/targets/common/arm/CMakeLists.txt b/radio/src/targets/common/arm/CMakeLists.txt index 7e1b4ff8dd6..283c1df40f6 100644 --- a/radio/src/targets/common/arm/CMakeLists.txt +++ b/radio/src/targets/common/arm/CMakeLists.txt @@ -228,6 +228,7 @@ endif() if(MULTIMODULE OR AFHDS3) set(SRC ${SRC} telemetry/flysky_ibus.cpp) + set(SRC ${SRC} telemetry/flysky_ibus2.cpp) endif() if(AFHDS2 AND INTERNAL_MODULE_AFHDS2A) @@ -259,6 +260,7 @@ endif() if(MULTIMODULE OR AFHDS3) set(SRC ${SRC} telemetry/flysky_ibus.cpp ) + set(SRC ${SRC} telemetry/flysky_ibus2.cpp) endif() if(GHOST) diff --git a/radio/src/telemetry/flysky_ibus2.cpp b/radio/src/telemetry/flysky_ibus2.cpp new file mode 100644 index 00000000000..21241b2caf9 --- /dev/null +++ b/radio/src/telemetry/flysky_ibus2.cpp @@ -0,0 +1,743 @@ +/* + * 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" +#include + +#define FLYSKY_TELEMETRY_LENGTH (2+7*4) +#define ALT_PRECISION 15 +#define RX_CMD_CODE_IBUS2_SET_PARAM ( 0x7025 ) +#define RX_CMD_CODE_IBUS2_GET_PARAM ( 0x7026 ) +#define PRESSURE_MASK 0x7FFFF +#define REMAP_CONST 0x1000 // Some part of OpenTX does not like sensor with id and instance 0, remap to 0x1000 +#define IBUS2_CALIB_SET_PARAM 0x1234 +#define IBUS2_CALIB_IBC01 0x0003 +#define IBUS2_CALIB_RPM 0xD001 + +enum Ibus2SensorOnLine { + IBUS2_SENSOR_RPM, + IBUS2_SENSOR_GPS, + IBUS2_SENSOR_IBC, + IBUS2_SENSOR_PRES, + IBUS2_SENSOR_NUM, +}; + +enum +{ + GPS_MSG_TYPE_PACK1 = 1, + GPS_MSG_TYPE_PACK2, + GPS_MSG_TYPE_PACK3, + GPS_MSG_TYPE_PACK4, + GPS_MSG_TYPE_CALI = 0x0F, + GPS_MSG_TYPE_NUM, +}; + +enum +{ + GPS_STATE_GET_POSITION = 3, + GPS_STATE_NUM, +}; + +typedef struct +{ + unsigned char NbSatellites; + unsigned char PositionStatus; //- + unsigned char DateDay; + unsigned char DateMonth; + unsigned short DateYear; // Year from 2000 + unsigned short Speed; // In 1/100th of meters + unsigned long UTCTime; // In milliseconds from midnight + int32_t Latitude; // In 1/1000000th of degree from -90 to +90 degrees + int32_t Longitude; // In 1/1000000th of degree from -180 to +180 degrees + int32_t Direction; // True direction in 1/10 of degree (0=North) + int32_t Altitude; // In 1/100th of meters + float Pitch; //- + float Roll; //- + float Yaw; //- + + int32_t Distance; //- + int16_t Acceleration;//acc + int16_t HeightChange;// +} Ibus2Gps_t; + +typedef struct +{ + int32_t Latitude; // In 1/1000000th of degree from -90 to +90 degrees + int32_t Longitude; // In 1/1000000th of degree from -180 to +180 degrees + int32_t Altitude; // In 1/100th of meters + int32_t Boar_Altitude; +} StartPos_t; + +typedef struct +{ + short Voltage; //unit 0.1V + short Current; // unit 0.1A + uint16_t UsedCapacity; //unit 1mAh + uint16_t RunTime; //unit 1second + short AverageCurrent; //unit 0.1A + short MaxVoltage; //unit 0.1V + short MinVoltage; //unit 0.1V + short MaxCurrent; //unit 0.1A +} Ibus2Ibc_t; + +typedef struct flysky_ibus2 +{ + uint8_t id; + uint16_t type; + uint8_t ParameterData[16]; +} Ibus2Param_t; + + +Ibus2Gps_t GPSData = {0}; +Ibus2Ibc_t IBCDate = {0}; +StartPos_t StartPos = {0}; +Ibus2Param_t Ibus2DevPara = {0}; + +struct FlySkySensor +{ + const uint16_t type; + const TelemetryUnit unit; + const uint8_t precision; + const char * name; +}; + +enum +{ + IBDT_INT_VOLTAGE = 0x00 | REMAP_CONST, + IBDT_TEMPERATURE = 0x01, // Temperature + IBDT_ROTATION_SPEED = 0x02, // RPM + IBDT_EXT_VOLTAGE = 0x03, // Sensor voltage + IBDT_GPS = 0x40, // GPS + IBDT_PRESSURE = 0x41, // Pressure + IBDT_COMPASS = 0x42, + IBDT_IBC01 = 0x43, // Voltage and current sensor + IBDT_REDUNDANT_RECEIVER = 0x44, + IBDT_REDUNDANT_RECEIVER_E = 0x45, + IBDT_FLIGHT = 0x70, + IBDT_TX_VOLTAGE = 0x7F, + IBDT_PRESSURE_SENSOR = 0x80, + IBDT_AIRSPEED_SENSOR = 0x81, + IBDT_EXT_BVD = 0xA0, + IBDT_HUB_1_PORTS_PWM = 0xE0, // 1-port hub in PWM mode + IBDT_HUB_2_PORTS_PWM = 0xE1, // 2-port hub in PWM mode + IBDT_HUB_3_PORTS_PWM = 0xE2, // 3-port hub in PWM mode + IBDT_HUB_4_PORTS_PWM = 0xE3, // 4-port hub in PWM mode + IBDT_HUB_5_PORTS_PWM = 0xE4, // 5-port hub in PWM mode + IBDT_HUB_6_PORTS_PWM = 0xE5, // 6-port hub in PWM mode + IBDT_HUB_7_PORTS_PWM = 0xE6, // 7-port hub in PWM mode + IBDT_8_PORTS_PWM_ADAPTER = 0xE7, // 8-port PWM adapter + IBDT_9_PORTS_PWM_ADAPTER = 0xE8, // 9-port PWM adapter + IBDT_10_PORTS_PWM_ADAPTER = 0xE9, // 10-port PWM adapter + IBDT_11_PORTS_PWM_ADAPTER = 0xEA, // 11-port PWM adapter + IBDT_12_PORTS_PWM_ADAPTER = 0xEB, // 12-port PWM adapter + IBDT_13_PORTS_PWM_ADAPTER = 0xEC, // 13-port PWM adapter + IBDT_14_PORTS_PWM_ADAPTER = 0xED, // 14-port PWM adapter + IBDT_15_PORTS_PWM_ADAPTER = 0xEE, // 15-port PWM adapter + IBDT_16_PORTS_PWM_ADAPTER = 0xEF, // 16-port PWM adapter + IBDT_17_PORTS_PWM_ADAPTER = 0xF0, // 17-port PWM adapter + IBDT_HUB_1_PORTS_HUB = 0xF1, // 1-port hub in hub mode + IBDT_HUB_2_PORTS_HUB = 0xF2, // 2-port hub in hub mode + IBDT_HUB_3_PORTS_HUB = 0xF3, // 3-port hub in hub mode + IBDT_HUB_4_PORTS_HUB = 0xF4, // 4-port hub in hub mode + IBDT_HUB_5_PORTS_HUB = 0xF5, // 5-port hub in hub mode + IBDT_HUB_6_PORTS_HUB = 0xF6, // 6-port hub in hub mode + IBDT_HUB_7_PORTS_HUB = 0xF7, // 7-port hub in hub mode + IBDT_DIGITAL_SERVO = 0xF8, + IBDT_SNR = 0xFA, + IBDT_BK_NOISE = 0xFB, + IBDT_RSSI = 0xFC, + IBDT_SERVO_HUB = 0xFD, // i-Bus 1 only + IBDT_SIGNAL_STRENGTH = 0xFE, + IBDT_NONE = 0xFF, + + VIRTUAL_ALT = 0x1041, // virtual + VIRTUAL_REL_ALT = 0x1141, // virtual + VIRTUAL_GPS_STAS = 0x1040, + VIRTUAL_GPS_TIME = 0x1140, + VIRTUAL_GPS_LAT = 0x1240, + VIRTUAL_GPS_LON = 0x1340, + VIRTUAL_GPS_KMH = 0x1440, + VIRTUAL_GPS_PITCH = 0x1540, + VIRTUAL_GPS_ROLL = 0x1640, + VIRTUAL_GPS_YAW = 0x1740, + VIRTUAL_GPS_DIST = 0x1840, + VIRTUAL_GPS_ACC = 0x1940, + VIRTUAL_GPS_SPEED = 0x1A40, + VIRTUAL_GPS_ALT = 0x1B40, + VIRTUAL_GPS_REL_ALT = 0x1C40, + + VIRTUAL_IBC_VOLTS = 0x1043, + VIRTUAL_IBC_CURR = 0x1143, + VIRTUAL_IBC_CAPA = 0x1243, + VIRTUAL_IBC_RUMTIME = 0x1343, + VIRTUAL_IBC_AVG_CURR = 0x1443, +}; + +// clang-format off +#define FS(type,name,unit,precision) {type,unit,precision,name} + +extern int32_t getALT(uint32_t value); + +void flyskyIbus2GPS(const uint8_t * pData, uint8_t len, uint8_t id); +void flyskyIbus2IBC(const uint8_t * pData, uint8_t len, uint8_t id); +void setIbus2Param(uint8_t* pData); +void getIbus2Param(uint8_t* pData); + +void sendFlyskytelemtry(uint16_t type, uint8_t id, int32_t value); + +int32_t GpsSensorDistanceGet(); +int32_t GpsSensorHeighthGet(); + +uint8_t flyskyGpsId = 0; +uint8_t flyskyIbcId = 0; +uint8_t flyskyRpmId = 0; +bool reset_gps_dist = false; +bool reset_gps_alt = false; +bool reset_baro_alt = false; + +uint32_t gps_update_tick = 0; +uint32_t ibc_update_tick = 0; +uint32_t rpm_update_tick = 0; +uint32_t pres_update_tick = 0; + +const FlySkySensor flySkySensors[] = { + // flysky start + FS(IBDT_INT_VOLTAGE, STR_SENSOR_BATT, UNIT_VOLTS, 2), + FS(IBDT_TEMPERATURE, STR_SENSOR_TEMP1, UNIT_CELSIUS, 1), + FS(IBDT_ROTATION_SPEED, STR_SENSOR_RPM, UNIT_RAW, 0), + FS(IBDT_EXT_VOLTAGE, STR_SENSOR_A3, UNIT_VOLTS, 2), + FS(IBDT_GPS, STR_SENSOR_GPS, UNIT_GPS, 2), + + FS(VIRTUAL_GPS_STAS, STR_SENSOR_SATELLITES, UNIT_RAW, 0), + FS(VIRTUAL_GPS_TIME, STR_SENSOR_GPSDATETIME, UNIT_DATETIME, 0), + FS(VIRTUAL_GPS_LAT, STR_SENSOR_GPS, UNIT_GPS_LATITUDE, 0), + FS(VIRTUAL_GPS_LON, STR_SENSOR_GPS, UNIT_GPS_LONGITUDE, 0), + FS(VIRTUAL_GPS_KMH, STR_SENSOR_GSPD, UNIT_KMH, 1), + FS(VIRTUAL_GPS_PITCH, STR_SENSOR_PITCH, UNIT_DEGREE, 1), + FS(VIRTUAL_GPS_ROLL, STR_SENSOR_ROLL, UNIT_DEGREE, 1), + FS(VIRTUAL_GPS_YAW, STR_SENSOR_YAW, UNIT_DEGREE, 1), + FS(VIRTUAL_GPS_DIST, STR_SENSOR_DIST, UNIT_METERS, 2), + FS(VIRTUAL_GPS_ACC, STR_SENSOR_ACC, UNIT_RAW, 0), + FS(VIRTUAL_GPS_SPEED, STR_SENSOR_GSPD, UNIT_SPEED, 2), + FS(VIRTUAL_GPS_ALT, STR_SENSOR_GPSALT, UNIT_METERS, 0), + FS(VIRTUAL_GPS_REL_ALT, "G_RH", UNIT_METERS, 0), + + FS(IBDT_PRESSURE, STR_SENSOR_PRES, UNIT_RAW, 2), + FS(VIRTUAL_ALT, STR_SENSOR_ALT, UNIT_METERS, 2), + FS(VIRTUAL_REL_ALT, "P_RH", UNIT_METERS, 2), + FS(IBDT_COMPASS, "COMPASS", UNIT_RAW, 2), + FS(IBDT_IBC01, "IBC", UNIT_RAW, 2), + FS(VIRTUAL_IBC_VOLTS, STR_SENSOR_BATT1_VOLTAGE, UNIT_VOLTS, 1), + FS(VIRTUAL_IBC_CURR, STR_SENSOR_CURR, UNIT_AMPS, 1), + FS(VIRTUAL_IBC_CAPA, STR_SENSOR_CAPACITY, UNIT_MAH, 0), + FS(VIRTUAL_IBC_AVG_CURR, "AVG_CURR", UNIT_AMPS, 1), + + FS(IBDT_REDUNDANT_RECEIVER, "REDUNDANT", UNIT_RAW, 0), + FS(IBDT_REDUNDANT_RECEIVER_E, "REDUNDANT_E", UNIT_RAW, 0), + FS(IBDT_FLIGHT, "FLIGHT", UNIT_RAW, 0), + FS(IBDT_TX_VOLTAGE, STR_SENSOR_TXV, UNIT_VOLTS, 2), + FS(IBDT_PRESSURE_SENSOR, "PRESSURE_SENSOR", UNIT_RAW, 2), + FS(IBDT_AIRSPEED_SENSOR, "AIRSPEED_SENSOR", UNIT_RAW, 2), + FS(IBDT_EXT_BVD, "BVD", UNIT_VOLTS, 2), + FS(IBDT_HUB_1_PORTS_PWM, "HUB_PWM1", UNIT_RAW, 0), + FS(IBDT_HUB_2_PORTS_PWM, "HUB_PWM2", UNIT_RAW, 0), + FS(IBDT_HUB_3_PORTS_PWM, "HUB_PWM3", UNIT_RAW, 0), + FS(IBDT_HUB_4_PORTS_PWM, "HUB_PWM4", UNIT_RAW, 0), + FS(IBDT_HUB_5_PORTS_PWM, "HUB_PWM5", UNIT_RAW, 0), + FS(IBDT_HUB_6_PORTS_PWM, "HUB_PWM6", UNIT_RAW, 0), + FS(IBDT_HUB_7_PORTS_PWM, "HUB_PWM7", UNIT_RAW, 0), + FS(IBDT_8_PORTS_PWM_ADAPTER, "ADAPTER_8", UNIT_RAW, 0), + FS(IBDT_9_PORTS_PWM_ADAPTER, "ADAPTER_9", UNIT_RAW, 0), + FS(IBDT_10_PORTS_PWM_ADAPTER, "ADAPTER_10", UNIT_RAW, 0), + FS(IBDT_11_PORTS_PWM_ADAPTER, "ADAPTER_11", UNIT_RAW, 0), + FS(IBDT_12_PORTS_PWM_ADAPTER, "ADAPTER_12", UNIT_RAW, 0), + FS(IBDT_13_PORTS_PWM_ADAPTER, "ADAPTER_13", UNIT_RAW, 0), + FS(IBDT_14_PORTS_PWM_ADAPTER, "ADAPTER_14", UNIT_RAW, 0), + FS(IBDT_15_PORTS_PWM_ADAPTER, "ADAPTER_15", UNIT_RAW, 0), + FS(IBDT_16_PORTS_PWM_ADAPTER, "ADAPTER_16", UNIT_RAW, 0), + FS(IBDT_17_PORTS_PWM_ADAPTER, "ADAPTER_17", UNIT_RAW, 0), + FS(IBDT_HUB_1_PORTS_HUB, "HUB1", UNIT_VOLTS, 3), + FS(IBDT_HUB_2_PORTS_HUB, "HUB2", UNIT_VOLTS, 3), + FS(IBDT_HUB_3_PORTS_HUB, "HUB3", UNIT_VOLTS, 3), + FS(IBDT_HUB_4_PORTS_HUB, "HUB4", UNIT_VOLTS, 3), + FS(IBDT_HUB_5_PORTS_HUB, "HUB5", UNIT_VOLTS, 3), + FS(IBDT_HUB_6_PORTS_HUB, "HUB6", UNIT_VOLTS, 3), + FS(IBDT_HUB_7_PORTS_HUB, "HUB7", UNIT_VOLTS, 3), + FS(IBDT_DIGITAL_SERVO, "DSERVO", UNIT_RAW, 0), + FS(IBDT_SNR, STR_SENSOR_RX_SNR, UNIT_DB, 0), + FS(IBDT_BK_NOISE, STR_SENSOR_RX_NOISE, UNIT_DBM, 0), + FS(IBDT_RSSI, STR_SENSOR_RSSI, UNIT_DBM, 0), + FS(IBDT_SERVO_HUB, "SERVO_HUB", UNIT_RAW, 0), + FS(IBDT_SIGNAL_STRENGTH, STR_SENSOR_RX_QUALITY, UNIT_PERCENT, 0), + FS(IBDT_NONE, NULL, UNIT_RAW, 0), +}; + +inline int setFlyskyTelemetryValue( int16_t type, uint8_t instance, int32_t value, uint32_t unit, uint32_t prec) +{ + return setTelemetryValue(PROTOCOL_TELEMETRY_FLYSKY_IBUS2, type, 0, instance, value, unit, prec ); +} + + +void processFlySkyIbus2AFHDS3Sensor(const uint8_t * packet, uint8_t len ) +{ + uint16_t type = (packet[0] << 8) | packet[1]; + uint8_t id = packet[2]; + int32_t value=0; + type = type ? type : IBDT_INT_VOLTAGE; // Remapped + + // TRACE("[IBUS] type x%02X, len %d", type, len); + if(len == 1) + { + value = packet[3]; // type 0xfe SIGNAL_STRENGTH + } + 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]; // PRESSURE + } else { + // gps & ibc01 + } + + if (IBDT_BK_NOISE == type || IBDT_RSSI == type) + { + value = -value; + } + + if ( (IBDT_EXT_VOLTAGE == type) ) + { + if ( id&0x80 ) + { + type = IBDT_EXT_BVD; + } + } + else if(IBDT_RSSI == type || IBDT_BK_NOISE == type || IBDT_SNR == type) + { + if( value>=0 ) + value = (value+2)/4; + else + value = (value-2)/4; + } + else if (IBDT_SIGNAL_STRENGTH == type) + { + telemetryData.rssi.set( value ); + if (value > 0) telemetryStreaming = TELEMETRY_TIMEOUT10ms; + } + else if (IBDT_PRESSURE == type) + { + int32_t alt = getALT(value); + // int16_t temp = (value >> 19); + if (reset_baro_alt) { + reset_baro_alt = !reset_baro_alt; + StartPos.Boar_Altitude = alt; + } else { + int32_t RH = alt - StartPos.Boar_Altitude; + type = VIRTUAL_REL_ALT; + sendFlyskytelemtry(type, id, RH); + // TRACE("[IBUS2] RH = %d", RH); + } + + type = VIRTUAL_ALT; + sendFlyskytelemtry(type, id, alt); + + pres_update_tick = timersGetMsTick(); + value &= PRESSURE_MASK; + type = IBDT_PRESSURE; + sendFlyskytelemtry(type, id, value); + return; + } else if (IBDT_ROTATION_SPEED == type) { + // Adjust the rotational speed based on the number of blades + // RPM = value; + flyskyRpmId = id; + rpm_update_tick = timersGetMsTick(); + } else if (IBDT_GPS == type) { + flyskyGpsId = id; + flyskyIbus2GPS(&packet[3], len, id); + return; + } else if (IBDT_IBC01 == type) { + flyskyIbus2IBC(&packet[3], len, id); + return; + } + + if(IBDT_TEMPERATURE == type) + { + value -= 400; // Temperature sensors have 40 degree offset + } + + sendFlyskytelemtry(type, id, value); +} + +void flyskyIbus2GPS(const uint8_t * pData, uint8_t len, uint8_t id) { + static uint16_t PSValue; + uint16_t n = 0; + int32_t value = 0; + uint16_t type = 0; + static bool get_start = true; // Get the location at startup + + gps_update_tick = timersGetMsTick(); + + PSValue = pData[n] | (pData[n+1] << 8); + if( GPS_MSG_TYPE_PACK1 == (PSValue & 0x000f) ) + { + GPSData.NbSatellites = (PSValue >> 4) & 0x003F ; n++; + GPSData.PositionStatus = (PSValue >> 10) & 0x003F ;n++; + value = GPSData.NbSatellites; + type = VIRTUAL_GPS_STAS; + sendFlyskytelemtry(type, id, value); + + PSValue = pData[n] | (pData[n+1] << 8); + GPSData.DateDay = (PSValue & 0x001F); + GPSData.DateMonth = (PSValue >> 5) & 0x000F; n++; + GPSData.DateYear = (PSValue >> 9) & 0x007F; n++; + value = GPSData.DateYear << 24 | GPSData.DateMonth << 16 | GPSData.DateDay << 8 | 0xff ; + type = VIRTUAL_GPS_TIME; + sendFlyskytelemtry(type, id, value); + + PSValue = pData[n] | (pData[n+1] << 8); + GPSData.Speed = (PSValue); n++;n++; + value = GPSData.Speed; + type = VIRTUAL_GPS_SPEED; + sendFlyskytelemtry(type, id, value); + + GPSData.UTCTime = pData[n] | (pData[n+1] << 8) | (pData[n+2] << 16) | (pData[n+3] << 24); + n++;n++;n++;n++; + // value = GPSData.UTCTime; + uint32_t total_seconds = GPSData.UTCTime; + uint8_t sec = total_seconds % 60; + uint32_t total_minutes = total_seconds / 60; + uint8_t min = total_minutes % 60; + uint8_t hour = total_minutes / 60; + type = VIRTUAL_GPS_TIME; + value = 0x00; + value = hour << 24 | min << 16 | sec << 8; + sendFlyskytelemtry(type, id, value); + // TRACE("[IBUS2] utc %d %d %d %d ", GPSData.DateYear, GPSData.DateMonth, GPSData.DateDay, GPSData.UTCTime); + + PSValue = pData[n] | (pData[n+1] << 8); + GPSData.Altitude = (PSValue); n++;n++; + type = VIRTUAL_GPS_ALT; + value = PSValue; + sendFlyskytelemtry(type, id, value); + + PSValue = pData[n] | (pData[n+1] << 8); + GPSData.Direction = (PSValue); n++;n++; + + type = VIRTUAL_GPS_REL_ALT; + if (!get_start) { + value = GpsSensorHeighthGet(); + } else { + value = 0; + } + sendFlyskytelemtry(type, id, value); + } + else if( GPS_MSG_TYPE_PACK2 == (PSValue & 0x000f) ) + { + GPSData.Pitch = (PSValue >> 4) & 0x0FFF ; n++;n++; + value = GPSData.Pitch - 1800; + type = VIRTUAL_GPS_PITCH; + sendFlyskytelemtry(type, id, value); + + PSValue = pData[n] | (pData[n+1] << 8); + GPSData.Roll = (PSValue); n++;n++; + value = GPSData.Roll - 1800; + type = VIRTUAL_GPS_ROLL; + sendFlyskytelemtry(type, id, value); + + PSValue = pData[n] | (pData[n+1] << 8); + GPSData.Yaw = (PSValue); n++;n++; + value = 3600 - GPSData.Yaw; + type = VIRTUAL_GPS_YAW; + sendFlyskytelemtry(type, id, value); + + GPSData.Latitude = (pData[n] | (pData[n+1] << 8) | (pData[n+2] << 16) | (pData[n+3] << 24)); + value = GPSData.Latitude; + type = VIRTUAL_GPS_LAT; + sendFlyskytelemtry(type, id, value); + + n++;n++;n++;n++; + GPSData.Longitude = (pData[n] | (pData[n+1] << 8) | (pData[n+2] << 16) | (pData[n+3] << 24)); + value = GPSData.Longitude; + type = VIRTUAL_GPS_LON; + sendFlyskytelemtry(type, id, value); + + + type = VIRTUAL_GPS_DIST; + if (!get_start) { + value = GpsSensorDistanceGet(); + } else { + value = 0; + } + sendFlyskytelemtry(type, id, value); + + } + + if (get_start && GPSData.PositionStatus == 3) { + get_start = false; + StartPos.Altitude = GPSData.Altitude; + StartPos.Latitude = GPSData.Latitude; + StartPos.Longitude = GPSData.Longitude; + } else { + if (reset_gps_alt) { + StartPos.Altitude = GPSData.Altitude; + reset_gps_alt = false; + } + if (reset_gps_dist) { + StartPos.Latitude = GPSData.Latitude; + StartPos.Longitude = GPSData.Longitude; + reset_gps_dist = false; + } + } +} + +void flyskyIbus2IBC(const uint8_t * pData, uint8_t len, uint8_t id) { + int32_t value = 0; + uint16_t type = 0; + memcpy(&IBCDate, pData, sizeof(IBCDate)); + + ibc_update_tick = timersGetMsTick(); + + type = VIRTUAL_IBC_VOLTS; + value = IBCDate.Voltage; + sendFlyskytelemtry(type, id, value); + + type = VIRTUAL_IBC_CURR; + value = IBCDate.Current; + sendFlyskytelemtry(type, id, value); + + type = VIRTUAL_IBC_CAPA; + value = IBCDate.UsedCapacity; + sendFlyskytelemtry(type, id, value); + + type = VIRTUAL_IBC_AVG_CURR; + value = IBCDate.AverageCurrent; + sendFlyskytelemtry(type, id, value); + + flyskyIbcId = id; +} + +uint8_t getIbcVoltags(void) { + return IBCDate.Voltage; +} + +void sendFlyskytelemtry(uint16_t type, uint8_t id, int32_t value) { + 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 + + // Remapped to single GPS sensor: + if (type == VIRTUAL_GPS_LON) type = VIRTUAL_GPS_LAT; + setFlyskyTelemetryValue(type, id, value, sensor->unit, sensor->precision); + return; + } +} + +const FlySkySensor * getFlySkyIbus2Sensor(uint16_t id) +{ + for (const FlySkySensor * sensor = flySkySensors; sensor->type; sensor++) { + if (id == sensor->type) + return sensor; + } + return nullptr; +} + +void flySkyIbus2SetDefault(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 = getFlySkyIbus2Sensor(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 if (unit == UNIT_GPS_LATITUDE || unit == UNIT_GPS_LONGITUDE) { + telemetrySensor.unit = UNIT_GPS; + } + } + else { + telemetrySensor.init(id); + } + + storageDirty(EE_MODEL); +} + +double deg2rad(double deg) { + return deg * M_PI / 180.0; +} + +static double GetDistance(double lat1, double lng1, double lat2, double lng2) +{ + lat1 =lat1/1000000; lng1= lng1/1000000; + lat2 =lat2/1000000; lng2= lng2/1000000; + + double radLat1 = deg2rad(lat1); + double radLat2 = deg2rad(lat2); + + double a = radLat1 - radLat2; + double b = deg2rad(lng1) - deg2rad(lng2); + + float s = 2.0f * asin(sqrt(pow(sin(a/2),2) + + cos(radLat1)*cos(radLat2)*pow(sin(b/2),2))); + + s = s * EARTH_RADIUS * 100; //cm + return s; +} + +int32_t GpsSensorDistanceGet() +{ + double lats ,lngs, late, lnge; + + lats = StartPos.Latitude; + lngs = StartPos.Longitude; + late = GPSData.Latitude; + lnge = GPSData.Longitude; + return GetDistance(lats, lngs, late, lnge); +} + +int32_t GpsSensorHeighthGet() +{ + return GPSData.Altitude - StartPos.Altitude; +} + +void flySkyIbus2CalGpsGyro(uint8_t* packet, uint8_t* len) +{ + Ibus2DevPara.type = IBUS2_CALIB_SET_PARAM; + Ibus2DevPara.id = flyskyGpsId; + Ibus2DevPara.ParameterData[0] = 0x55; + Ibus2DevPara.ParameterData[1] = 0x01; + Ibus2DevPara.ParameterData[2] = 0x01; + setIbus2Param(packet); + *len = 22; +} + +void flySkyIbus2CalGpsAlt() +{ + reset_gps_alt = true; + reset_baro_alt = true; +} + +void flySkyIbus2CalGpsDist() +{ + reset_gps_dist = true; +} + +void flySkyIbus2ReadParamRPM(uint8_t* packet, uint8_t* len) +{ + Ibus2DevPara.type = IBUS2_CALIB_RPM; + Ibus2DevPara.id = flyskyRpmId; + getIbus2Param(packet); + *len = 6; +} + +void flySkyIbus2CalibIBC(uint8_t* packet, uint8_t* len, short voltags) +{ + Ibus2DevPara.type = IBUS2_CALIB_SET_PARAM; + Ibus2DevPara.id = flyskyIbcId; + Ibus2DevPara.ParameterData[0] = (uint8_t)(voltags & 0xff); + Ibus2DevPara.ParameterData[1] = (uint8_t)(voltags >> 8); + setIbus2Param(packet); + *len = 22; +} + +void setIbus2Param(uint8_t* pData) { + uint8_t n = 0; + pData[n++] = ( uint8_t )( RX_CMD_CODE_IBUS2_SET_PARAM ); + pData[n++] = ( uint8_t )( RX_CMD_CODE_IBUS2_SET_PARAM >> 8 ); + pData[n++] = 19; //data length + pData[n++] = Ibus2DevPara.id; + pData[n++] = Ibus2DevPara.type; + pData[n++] = Ibus2DevPara.type >> 8; + pData[n++] = Ibus2DevPara.ParameterData[0]; + pData[n++] = Ibus2DevPara.ParameterData[1]; + pData[n++] = Ibus2DevPara.ParameterData[2]; + pData[n++] = Ibus2DevPara.ParameterData[3]; + pData[n++] = Ibus2DevPara.ParameterData[4]; + pData[n++] = Ibus2DevPara.ParameterData[5]; + pData[n++] = Ibus2DevPara.ParameterData[6]; + pData[n++] = Ibus2DevPara.ParameterData[7]; + pData[n++] = Ibus2DevPara.ParameterData[8]; + pData[n++] = Ibus2DevPara.ParameterData[9]; + pData[n++] = Ibus2DevPara.ParameterData[10]; + pData[n++] = Ibus2DevPara.ParameterData[11]; + pData[n++] = Ibus2DevPara.ParameterData[12]; + pData[n++] = Ibus2DevPara.ParameterData[13]; + pData[n++] = Ibus2DevPara.ParameterData[14]; + pData[n++] = Ibus2DevPara.ParameterData[15]; +} + +void getIbus2Param(uint8_t* pData) { + uint8_t n = 0; + pData[n++] = ( uint8_t )(RX_CMD_CODE_IBUS2_GET_PARAM); + pData[n++] = ( uint8_t )(RX_CMD_CODE_IBUS2_GET_PARAM >> 8); + pData[n++] = 3;//data length + pData[n++] = Ibus2DevPara.id; + pData[n++] = Ibus2DevPara.type; + pData[n++] = Ibus2DevPara.type >> 8; +} + +bool ibc_state = false; +void Ibus2ParamCheck(uint8_t* packet, uint8_t len) { + if (len != 4) { + return; + } + uint8_t id = packet[0]; + uint8_t code = packet[3]; + if (id == flyskyIbcId) { + ibc_state = code; + } else if (id == flyskyGpsId) { + // todo + } else if (id == flyskyRpmId) { + // todo + } +} + +bool getIbus2IbcState() { + return ibc_state; +} + +uint8_t flyskyIbus2SensorOnLine() { + uint32_t now = timersGetMsTick(); + uint8_t sensor_on_line = 0; + if (now - ibc_update_tick < 2000) { + sensor_on_line |= 1 << IBUS2_SENSOR_IBC; + } + if (now - gps_update_tick < 2000) { + sensor_on_line |= 1 << IBUS2_SENSOR_GPS; + } + if (now - rpm_update_tick < 2000) { + sensor_on_line |= 1 << IBUS2_SENSOR_RPM; + } + if (now - pres_update_tick < 2000) { + sensor_on_line |= 1 << IBUS2_SENSOR_PRES; + } + + return sensor_on_line; +} \ No newline at end of file diff --git a/radio/src/telemetry/flysky_ibus2.h b/radio/src/telemetry/flysky_ibus2.h new file mode 100644 index 00000000000..9f9a6107733 --- /dev/null +++ b/radio/src/telemetry/flysky_ibus2.h @@ -0,0 +1,24 @@ +/* + * 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. + */ + +#pragma once + +void flySkyIbus2SetDefault(int index, uint16_t id, uint8_t subId, uint8_t instance); diff --git a/radio/src/telemetry/telemetry_sensors.cpp b/radio/src/telemetry/telemetry_sensors.cpp index 16237c6fac2..d974de322c4 100644 --- a/radio/src/telemetry/telemetry_sensors.cpp +++ b/radio/src/telemetry/telemetry_sensors.cpp @@ -51,6 +51,10 @@ #include "flysky_ibus.h" #endif +#if defined(AFHDS3) + #include "flysky_ibus2.h" +#endif + TelemetryItem telemetryItems[MAX_TELEMETRY_SENSORS]; bool allowNewSensors; @@ -553,6 +557,12 @@ int setTelemetryValue(TelemetryProtocol protocol, uint16_t id, uint8_t subId, break; #endif +#if defined(AFHDS3) + case PROTOCOL_TELEMETRY_FLYSKY_IBUS2: + flySkyIbus2SetDefault(index, id, subId, instance); + break; +#endif + #if defined(AFHDS2) && defined(PCBNV14) case PROTOCOL_TELEMETRY_FLYSKY_NV14: flySkyNv14SetDefault(index, id, subId, instance);