piotr/edgetx-gx12
1
0
Fork 0
Code Issues 2 Pull Requests Actions Packages Projects Releases Wiki Activity

feat(afhds3): iBUS2 support (#6914)

Browse Source 
Co-authored-by: xlong <984929678@qq.com>
Co-authored-by: Peter Feerick <peter.feerick@gmail.com>
Co-authored-by: Claude Sonnet 5 <noreply@anthropic.com>
This commit is contained in:
richardclli
2026-08-05 12:56:32 +10:00
committed by GitHub
co-authored by xlong Peter Feerick Claude Sonnet 5
parent 992096f4c9
commit 256dc98917
33 changed files with 1092 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
radio/src/dataconstants.h
+1
View File
@@ -276,6 +276,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,
radio/src/gui/colorlcd/module/afhds3_options.cpp
+150 -11
View File
@@ -34,9 +34,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 };
@@ -123,6 +122,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);
@@ -159,7 +165,7 @@ AFHDS3_Options::AFHDS3_Options(uint8_t moduleIdx) : Page(ICON_MODEL_SETUP)
temp_str = STR_CH;
temp_str += " 1";
new StaticText(line, rect_t{}, temp_str );
new Choice(line, rect_t{}, _analog_outputs,
new Choice(line, rect_t{}, STR_FLYSKY_PULSE_PROTO,
afhds3::SES_ANALOG_OUTPUT_PWM, afhds3::SES_ANALOG_OUTPUT_PPM,
GET_SET_AND_SYNC(cfg, vCfg->AnalogOutput, afhds3::DirtyConfig::DC_RX_CMD_OUT_PWM_PPM_MODE));
@@ -196,8 +202,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)
{
@@ -206,14 +212,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);
}
}
@@ -237,3 +278,101 @@ 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 += STR_TELEMETRY_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) {
new StaticText(line, rect_t{}, STR_IBUS2_SENSORS_MODE_ONLY);
return;
}
if (cfg->others.sensorOnLine & (1 << afhds3::DirtyIbus2Sensor::IBUS2_SENSOR_GPS))
{
temp_str = STR_IMU;
temp_str += " ";
temp_str += STR_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 = STR_SENSOR_DIST;
temp_str += " ";
temp_str += STR_CURRENTSENSOR;
line = body->newLine(grid);
new StaticText(line, rect_t{}, temp_str);
auto dist_btn = new TextButton(line, rect_t{}, STR_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)) {
line = body->newLine(grid);
new StaticText(line, rect_t{}, STR_ALTSENSOR);
auto alt_btn = new TextButton(line, rect_t{}, STR_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 += STR_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 += STR_CALIBRATION;
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");
}
}
radio/src/gui/colorlcd/module/afhds3_options.h
+8
View File
@@ -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);
};
radio/src/gui/colorlcd/module/afhds3_settings.cpp
+5
View File
@@ -89,6 +89,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) {
radio/src/pulses/afhds3.cpp
+109 -2
View File
@@ -29,6 +29,7 @@
#include "../definitions.h"
#include "telemetry/telemetry.h"
#include "telemetry/flysky_ibus2.h"
#include "mixer_scheduler.h"
#include "hal/module_driver.h"
#include "hal/module_port.h"
@@ -285,6 +286,8 @@ class ProtoState
bool syncSettings();
bool sensorCalibration();
// void requestInfoAndRun(bool send = false);
uint8_t setFailSafe(int16_t* target, uint8_t rfchannelcount=AFHDS3_MAX_CHANNELS);
@@ -413,6 +416,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;
@@ -559,6 +574,8 @@ void ProtoState::setupFrame()
// Sync config, with commands
if (syncSettings()) { return; }
if (sensorCalibration()) { return; }
// Send channels data
sendChannelsData();
} else {
@@ -705,6 +722,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();
@@ -756,7 +774,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;
}
}
@@ -812,6 +842,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;
}
@@ -836,6 +880,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()
{
@@ -888,7 +976,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;
}
radio/src/pulses/afhds3_config.h
+17 -1
View File
@@ -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
radio/src/pulses/afhds3_transport.h
+2
View File
@@ -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
{
radio/src/targets/common/arm/CMakeLists.txt
+2
View File
@@ -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)
radio/src/targets/simu/timers_driver.cpp
+1
View File
@@ -22,5 +22,6 @@
#include "timers_driver.h"
void watchdogSuspend(unsigned int) {}
uint32_t timersGetMsTick() { return 0; }
uint32_t timersGetUsTick() { return 0; }
radio/src/telemetry/flysky_ibus2.cpp
+730
View File
@@ -0,0 +1,730 @@
/*
* 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 <math.h>
#include "edgetx.h"
#include "pulses/afhds3_config.h"
#include "pulses/afhds3_transport.h"
#define FLYSKY_TELEMETRY_LENGTH (2 + 7 * 4)
#define ALT_PRECISION 15
#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 {
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;
};
// clang-format off
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 on
#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;
// clang-format off
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),
};
// clang-format off
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<uint8_t>(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 << afhds3::IBUS2_SENSOR_IBC;
}
if (now - gps_update_tick < 2000) {
sensor_on_line |= 1 << afhds3::IBUS2_SENSOR_GPS;
}
if (now - rpm_update_tick < 2000) {
sensor_on_line |= 1 << afhds3::IBUS2_SENSOR_RPM;
}
if (now - pres_update_tick < 2000) {
sensor_on_line |= 1 << afhds3::IBUS2_SENSOR_PRES;
}
return sensor_on_line;
}
radio/src/telemetry/flysky_ibus2.h
+36
View File
@@ -0,0 +1,36 @@
/*
* 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);
void processFlySkyIbus2AFHDS3Sensor(const uint8_t* packet, uint8_t len);
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();
radio/src/telemetry/telemetry_sensors.cpp
+10
View File
@@ -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(RADIO_NV14_FAMILY)
case PROTOCOL_TELEMETRY_FLYSKY_NV14:
flySkyNv14SetDefault(index, id, subId, instance);
radio/src/translations/i18n/cn.h
+1
View File
@@ -712,6 +712,7 @@
#define TR_AFHDS3_ONE_TO_MANY "Multicast"
#define TR_AFHDS3_ACTUAL_POWER TR("Act. pow", "Actual power")
#define TR_AFHDS3_POWER_SOURCE TR("Power src.", "Power source")
#define TR_IBUS2_SENSORS_MODE_ONLY "Only in the iBUS2 mode can the sensors be set."
#define TR_FLYSKY_TELEMETRY TR("FlySky RSSI #", "使用 FlySky RSSI 值无需重缩放")
#define TR_GPS_COORDS_FORMAT TR("方向角", "方向角格式")
#define TR_VARIO TR("变化率", "高度变化率")
radio/src/translations/i18n/cz.h
+1
View File
@@ -1072,6 +1072,7 @@
#define TR_AFHDS3_ONE_TO_MANY "Multicast"
#define TR_AFHDS3_ACTUAL_POWER TR("Act. pow", "Aktuální výkon")
#define TR_AFHDS3_POWER_SOURCE TR("Power src.", TR("Power src.", "Power source"))
#define TR_IBUS2_SENSORS_MODE_ONLY "Only in the iBUS2 mode can the sensors be set."
#define TR_ANTENNACONFIRM1 "Opravdu přepnout?"
#define TR_ANTENNA_MODES_1 "Interní"
#define TR_ANTENNA_MODES_2 "Zeptat se"
radio/src/translations/i18n/da.h
+1
View File
@@ -717,6 +717,7 @@
#define TR_AFHDS3_ONE_TO_MANY "Multicast"
#define TR_AFHDS3_ACTUAL_POWER TR("Akt. strøm", "Aktuel strøm")
#define TR_AFHDS3_POWER_SOURCE TR("Strøm", "Strøm kilde")
#define TR_IBUS2_SENSORS_MODE_ONLY "Only in the iBUS2 mode can the sensors be set."
#define TR_FLYSKY_TELEMETRY TR("FlySky RSSI #", "Brug FlySky RSSI værdi uden reskalering")
#define TR_GPS_COORDS_FORMAT TR("GPS kordi.", "Kordinat format")
#define TR_VARIO TR("Højde", "Højdemåler")
radio/src/translations/i18n/de.h
+1
View File
@@ -712,6 +712,7 @@
#define TR_AFHDS3_ONE_TO_MANY "Multicast"
#define TR_AFHDS3_ACTUAL_POWER TR("Ist-Leist.", "Aktuelle Leistung")
#define TR_AFHDS3_POWER_SOURCE TR("Versorg.", "Spannungsquelle")
#define TR_IBUS2_SENSORS_MODE_ONLY "Only in the iBUS2 mode can the sensors be set."
#define TR_FLYSKY_TELEMETRY TR("FlySky RSSI #", "Verwende FlySky RSSI (skalierungsfrei)")
#define TR_GPS_COORDS_FORMAT TR("GPS-Koord.", "GPS-Koordinaten-Format")
#define TR_VARIO TR("Vario", "Variometer")
radio/src/translations/i18n/en.h
+1
View File
@@ -713,6 +713,7 @@
#define TR_AFHDS3_ONE_TO_MANY "Multicast"
#define TR_AFHDS3_ACTUAL_POWER TR("Act. pow", "Actual power")
#define TR_AFHDS3_POWER_SOURCE TR("Power src.", "Power source")
#define TR_IBUS2_SENSORS_MODE_ONLY "Only in the iBUS2 mode can the sensors be set."
#define TR_FLYSKY_TELEMETRY TR("FlySky RSSI #", "Use FlySky RSSI value without rescalling")
#define TR_GPS_COORDS_FORMAT TR("GPS Coords", "Coordinate format")
#define TR_VARIO TR("Vario", "Variometer")
radio/src/translations/i18n/es.h
+1
View File
@@ -1069,6 +1069,7 @@
#define TR_AFHDS3_ONE_TO_MANY "Multicast"
#define TR_AFHDS3_ACTUAL_POWER TR("Act. pow", "Actual power")
#define TR_AFHDS3_POWER_SOURCE TR("Power src.", "Power source")
#define TR_IBUS2_SENSORS_MODE_ONLY "Only in the iBUS2 mode can the sensors be set."
#define TR_ANTENNACONFIRM1 "ANTENA EXT."
#define TR_ANTENNA_MODES_1 "Interna"
#define TR_ANTENNA_MODES_2 "Preguntar"
radio/src/translations/i18n/fi.h
+1
View File
@@ -1069,6 +1069,7 @@
#define TR_AFHDS3_ONE_TO_MANY "Multicast"
#define TR_AFHDS3_ACTUAL_POWER TR("Act. pow", "Actual power")
#define TR_AFHDS3_POWER_SOURCE TR("Power src.", "Power source")
#define TR_IBUS2_SENSORS_MODE_ONLY "Only in the iBUS2 mode can the sensors be set."
#define TR_ANTENNACONFIRM1 "EXT. ANTENNA"
#define TR_ANTENNA_MODES_1 "Internal"
#define TR_ANTENNA_MODES_2 "Ask"
radio/src/translations/i18n/fr.h
+1
View File
@@ -1072,6 +1072,7 @@
#define TR_AFHDS3_ONE_TO_MANY "Multicast"
#define TR_AFHDS3_ACTUAL_POWER TR("Act. pow", "Puissance actuelle")
#define TR_AFHDS3_POWER_SOURCE TR("Power src.", "Source puissance")
#define TR_IBUS2_SENSORS_MODE_ONLY "Only in the iBUS2 mode can the sensors be set."
#define TR_ANTENNACONFIRM1 "Vraiment changer?"
#define TR_ANTENNA_MODES_1 "Interne"
#define TR_ANTENNA_MODES_2 "Demander"
radio/src/translations/i18n/he.h
+1
View File
@@ -715,6 +715,7 @@
#define TR_AFHDS3_ONE_TO_MANY "Multicast"
#define TR_AFHDS3_ACTUAL_POWER TR("Act. pow", "Actual power")
#define TR_AFHDS3_POWER_SOURCE TR("Power src.", "Power source")
#define TR_IBUS2_SENSORS_MODE_ONLY "Only in the iBUS2 mode can the sensors be set."
#define TR_FLYSKY_TELEMETRY TR("FlySky RSSI #", "Use FlySky RSSI value without rescalling")
#define TR_GPS_COORDS_FORMAT TR("GPS Coords", "Coordinate format")
#define TR_VARIO TR("Vario", "הגדרות תדר")
radio/src/translations/i18n/it.h
+1
View File
@@ -712,6 +712,7 @@
#define TR_AFHDS3_ONE_TO_MANY "Multicast"
#define TR_AFHDS3_ACTUAL_POWER TR("Pot. att.", "Potenza Attuale")
#define TR_AFHDS3_POWER_SOURCE TR("Fonte pot.", "Fonte Potenza")
#define TR_IBUS2_SENSORS_MODE_ONLY "Only in the iBUS2 mode can the sensors be set."
#define TR_FLYSKY_TELEMETRY TR("FlySky RSSI #", "Usa valore RSSI FlySky senza scalare")
#define TR_GPS_COORDS_FORMAT TR("GPS Coords", "Formato Coordinate")
#define TR_VARIO TR("Vario", "Variometro")
radio/src/translations/i18n/jp.h
+1
View File
@@ -711,6 +711,7 @@
#define TR_AFHDS3_ONE_TO_MANY "マルチキャスト"
#define TR_AFHDS3_ACTUAL_POWER TR("Act. pow", "実電力")
#define TR_AFHDS3_POWER_SOURCE TR("Power src.", "出力ソース")
#define TR_IBUS2_SENSORS_MODE_ONLY "Only in the iBUS2 mode can the sensors be set."
#define TR_FLYSKY_TELEMETRY TR("FlySky RSSI #", "再コールせずFlySky RSSI値を使用")
#define TR_GPS_COORDS_FORMAT TR("GPS Coords", "GPS座標形式")
#define TR_VARIO TR("Vario", "バリオメーター")
radio/src/translations/i18n/ko.h
+1
View File
@@ -740,6 +740,7 @@
#define TR_AFHDS3_ACTUAL_POWER TR("실제 출력", "실제 RF 출력")
#define TR_AFHDS3_POWER_SOURCE TR("전원 소스", "전원 공급 방식")
#define TR_IBUS2_SENSORS_MODE_ONLY "Only in the iBUS2 mode can the sensors be set."
#define TR_FLYSKY_TELEMETRY TR("FlySky RSSI 값", "RSSI 값을 리스케일 없이 사용")
#define TR_GPS_COORDS_FORMAT TR("GPS 좌표", "좌표 표시 형식")
#define TR_VARIO TR("바리오", "수직 속도계")
radio/src/translations/i18n/nl.h
+1
View File
@@ -1070,6 +1070,7 @@
#define TR_AFHDS3_ONE_TO_MANY "Multicast"
#define TR_AFHDS3_ACTUAL_POWER TR("Act. pow", "Actual power")
#define TR_AFHDS3_POWER_SOURCE TR("Power src.", "Power source")
#define TR_IBUS2_SENSORS_MODE_ONLY "Only in the iBUS2 mode can the sensors be set."
#define TR_ANTENNACONFIRM1 "Antennes wisselen?"
#define TR_ANTENNA_MODES_1 "Internal"
#define TR_ANTENNA_MODES_2 "Ask"
radio/src/translations/i18n/pl.h
+1
View File
@@ -1067,6 +1067,7 @@
#define TR_AFHDS3_ONE_TO_MANY "Multicast"
#define TR_AFHDS3_ACTUAL_POWER TR("Act. pow", "Actual power")
#define TR_AFHDS3_POWER_SOURCE TR("Power src.", "Power source")
#define TR_IBUS2_SENSORS_MODE_ONLY "Only in the iBUS2 mode can the sensors be set."
#define TR_ANTENNACONFIRM1 "EXT. ANTENNA"
#define TR_ANTENNA_MODES_1 "Internal"
#define TR_ANTENNA_MODES_2 "Ask"
radio/src/translations/i18n/pt.h
+1
View File
@@ -712,6 +712,7 @@
#define TR_AFHDS3_ONE_TO_MANY "Multicast"
#define TR_AFHDS3_ACTUAL_POWER TR("Act. pow", "Actual power")
#define TR_AFHDS3_POWER_SOURCE TR("Power src.", "Power source")
#define TR_IBUS2_SENSORS_MODE_ONLY "Only in the iBUS2 mode can the sensors be set."
#define TR_FLYSKY_TELEMETRY TR("FlySky RSSI #", "Use FlySky RSSI value without rescalling")
#define TR_GPS_COORDS_FORMAT TR("GPS Coords", "Coordinate format")
#define TR_VARIO TR("Vario", "Variometer")
radio/src/translations/i18n/ru.h
+1
View File
@@ -714,6 +714,7 @@
#define TR_AFHDS3_ONE_TO_MANY "Мультикаст"
#define TR_AFHDS3_ACTUAL_POWER TR("Акт. мощность", "Фактич мощность")
#define TR_AFHDS3_POWER_SOURCE TR("Источ питания", "Источ питания")
#define TR_IBUS2_SENSORS_MODE_ONLY "Only in the iBUS2 mode can the sensors be set."
#define TR_FLYSKY_TELEMETRY TR("FlySky RSSI #", "FlySky RSSI")
#define TR_GPS_COORDS_FORMAT TR("Формат коорд GPS", "Формат коорд")
#define TR_VARIO TR("Вариометр", "Вариометр")
radio/src/translations/i18n/se.h
+1
View File
@@ -1075,6 +1075,7 @@
#define TR_AFHDS3_ONE_TO_MANY "Multicast"
#define TR_AFHDS3_ACTUAL_POWER TR("Act. pow", "Actual power")
#define TR_AFHDS3_POWER_SOURCE "Strömkälla"
#define TR_IBUS2_SENSORS_MODE_ONLY "Only in the iBUS2 mode can the sensors be set."
#define TR_ANTENNACONFIRM1 "EXT. ANTENN"
#define TR_ANTENNA_MODES_1 "Intern"
radio/src/translations/i18n/tw.h
+1
View File
@@ -709,6 +709,7 @@
#define TR_AFHDS3_ONE_TO_MANY "Multicast"
#define TR_AFHDS3_ACTUAL_POWER TR("Act. pow", "Actual power")
#define TR_AFHDS3_POWER_SOURCE TR("Power src.", "Power source")
#define TR_IBUS2_SENSORS_MODE_ONLY "Only in the iBUS2 mode can the sensors be set."
#define TR_FLYSKY_TELEMETRY TR("FlySky RSSI #", "使用 FlySky RSSI 值無需重縮放")
#define TR_GPS_COORDS_FORMAT TR("方向角", "方向角格式")
#define TR_VARIO TR("變化率", "高度變化率")
radio/src/translations/i18n/ua.h
+1
View File
@@ -713,6 +713,7 @@
#define TR_AFHDS3_ONE_TO_MANY "Multicast"
#define TR_AFHDS3_ACTUAL_POWER TR("Факт.потужн.", "Фактична потужність")
#define TR_AFHDS3_POWER_SOURCE TR("Джерело живл.", "Джерело живлення")
#define TR_IBUS2_SENSORS_MODE_ONLY "Only in the iBUS2 mode can the sensors be set."
#define TR_FLYSKY_TELEMETRY TR("FlySky RSSI #", "Use FlySky RSSI") /* use english */
#define TR_GPS_COORDS_FORMAT TR("Формат коорд.", "Формат координат")
#define TR_VARIO TR("Варіо", "Варіометр")
radio/src/translations/sim_string_list.h
+1
View File
@@ -361,6 +361,7 @@
#define STR_AFHDS3_ONE_TO_ONE_TELEMETRY currentLangStrings->STR_AFHDS3_ONE_TO_ONE_TELEMETRY
#define STR_AFHDS3_POWER_SOURCE currentLangStrings->STR_AFHDS3_POWER_SOURCE
#define STR_AFHDS3_RX_FREQ currentLangStrings->STR_AFHDS3_RX_FREQ
#define STR_IBUS2_SENSORS_MODE_ONLY currentLangStrings->STR_IBUS2_SENSORS_MODE_ONLY
#endif
#if defined(BLUETOOTH)
radio/src/translations/string_list.h
+1
View File
@@ -360,6 +360,7 @@ STR(AFHDS3_ONE_TO_MANY)
STR(AFHDS3_ONE_TO_ONE_TELEMETRY)
STR(AFHDS3_POWER_SOURCE)
STR(AFHDS3_RX_FREQ)
STR(IBUS2_SENSORS_MODE_ONLY)
#endif
#if defined(BLUETOOTH)