diff --git a/lib/Espfc/src/Connect/Cli.cpp b/lib/Espfc/src/Connect/Cli.cpp index 97dd3f12..f686852a 100644 --- a/lib/Espfc/src/Connect/Cli.cpp +++ b/lib/Espfc/src/Connect/Cli.cpp @@ -506,6 +506,8 @@ const Cli::Param * Cli::initialize(ModelConfig& c) Param(PSTR("failsafe_delay"), &c.failsafe.delay), Param(PSTR("failsafe_kill_switch"), &c.failsafe.killSwitch), + Param(PSTR("arming_small_angle"), &c.arming.smallAngle), + Param(PSTR("vtx_power"), &c.vtx.power), Param(PSTR("vtx_channel"), &c.vtx.channel), Param(PSTR("vtx_band"), &c.vtx.band), diff --git a/lib/Espfc/src/Connect/MspProcessor.cpp b/lib/Espfc/src/Connect/MspProcessor.cpp index 35787bfa..9808a470 100644 --- a/lib/Espfc/src/Connect/MspProcessor.cpp +++ b/lib/Espfc/src/Connect/MspProcessor.cpp @@ -9,319 +9,384 @@ #define VTXCOMMON_MSP_BANDCHAN_CHKVAL ((uint16_t)((7 << 3) + 7)) -extern "C" { - #include "msp/msp_protocol.h" - #include "msp/msp_protocol_v2_common.h" - #include "msp/msp_protocol_v2_betaflight.h" - #include "io/serial_4way.h" - #include "blackbox/blackbox_io.h" +extern "C" +{ +#include "msp/msp_protocol.h" +#include "msp/msp_protocol_v2_common.h" +#include "msp/msp_protocol_v2_betaflight.h" +#include "io/serial_4way.h" +#include "blackbox/blackbox_io.h" int blackboxCalculatePDenom(int rateNum, int rateDenom); uint8_t blackboxCalculateSampleRate(uint16_t pRatio); uint8_t blackboxGetRateDenom(void); uint16_t blackboxGetPRatio(void); } -namespace { - -enum SerialSpeedIndex { - SERIAL_SPEED_INDEX_AUTO = 0, - SERIAL_SPEED_INDEX_9600, - SERIAL_SPEED_INDEX_19200, - SERIAL_SPEED_INDEX_38400, - SERIAL_SPEED_INDEX_57600, - SERIAL_SPEED_INDEX_115200, - SERIAL_SPEED_INDEX_230400, - SERIAL_SPEED_INDEX_250000, - SERIAL_SPEED_INDEX_400000, - SERIAL_SPEED_INDEX_460800, - SERIAL_SPEED_INDEX_500000, - SERIAL_SPEED_INDEX_921600, - SERIAL_SPEED_INDEX_1000000, - SERIAL_SPEED_INDEX_1500000, - SERIAL_SPEED_INDEX_2000000, - SERIAL_SPEED_INDEX_2470000, -}; - -static SerialSpeedIndex toBaudIndex(int32_t speed) +namespace { - using namespace Espfc; - if(speed >= SERIAL_SPEED_2470000) return SERIAL_SPEED_INDEX_2470000; - if(speed >= SERIAL_SPEED_2000000) return SERIAL_SPEED_INDEX_2000000; - if(speed >= SERIAL_SPEED_1500000) return SERIAL_SPEED_INDEX_1500000; - if(speed >= SERIAL_SPEED_1000000) return SERIAL_SPEED_INDEX_1000000; - if(speed >= SERIAL_SPEED_921600) return SERIAL_SPEED_INDEX_921600; - if(speed >= SERIAL_SPEED_500000) return SERIAL_SPEED_INDEX_500000; - if(speed >= SERIAL_SPEED_460800) return SERIAL_SPEED_INDEX_460800; - if(speed >= SERIAL_SPEED_400000) return SERIAL_SPEED_INDEX_400000; - if(speed >= SERIAL_SPEED_250000) return SERIAL_SPEED_INDEX_250000; - if(speed >= SERIAL_SPEED_230400) return SERIAL_SPEED_INDEX_230400; - if(speed >= SERIAL_SPEED_115200) return SERIAL_SPEED_INDEX_115200; - if(speed >= SERIAL_SPEED_57600) return SERIAL_SPEED_INDEX_57600; - if(speed >= SERIAL_SPEED_38400) return SERIAL_SPEED_INDEX_38400; - if(speed >= SERIAL_SPEED_19200) return SERIAL_SPEED_INDEX_19200; - if(speed >= SERIAL_SPEED_9600) return SERIAL_SPEED_INDEX_9600; - return SERIAL_SPEED_INDEX_AUTO; -} -static Espfc::SerialSpeed fromBaudIndex(SerialSpeedIndex index) -{ - using namespace Espfc; - switch(index) + enum SerialSpeedIndex + { + SERIAL_SPEED_INDEX_AUTO = 0, + SERIAL_SPEED_INDEX_9600, + SERIAL_SPEED_INDEX_19200, + SERIAL_SPEED_INDEX_38400, + SERIAL_SPEED_INDEX_57600, + SERIAL_SPEED_INDEX_115200, + SERIAL_SPEED_INDEX_230400, + SERIAL_SPEED_INDEX_250000, + SERIAL_SPEED_INDEX_400000, + SERIAL_SPEED_INDEX_460800, + SERIAL_SPEED_INDEX_500000, + SERIAL_SPEED_INDEX_921600, + SERIAL_SPEED_INDEX_1000000, + SERIAL_SPEED_INDEX_1500000, + SERIAL_SPEED_INDEX_2000000, + SERIAL_SPEED_INDEX_2470000, + }; + + static SerialSpeedIndex toBaudIndex(int32_t speed) { - case SERIAL_SPEED_INDEX_9600: return SERIAL_SPEED_9600; - case SERIAL_SPEED_INDEX_19200: return SERIAL_SPEED_19200; - case SERIAL_SPEED_INDEX_38400: return SERIAL_SPEED_38400; - case SERIAL_SPEED_INDEX_57600: return SERIAL_SPEED_57600; - case SERIAL_SPEED_INDEX_115200: return SERIAL_SPEED_115200; - case SERIAL_SPEED_INDEX_230400: return SERIAL_SPEED_230400; - case SERIAL_SPEED_INDEX_250000: return SERIAL_SPEED_250000; - case SERIAL_SPEED_INDEX_400000: return SERIAL_SPEED_400000; - case SERIAL_SPEED_INDEX_460800: return SERIAL_SPEED_460800; - case SERIAL_SPEED_INDEX_500000: return SERIAL_SPEED_500000; - case SERIAL_SPEED_INDEX_921600: return SERIAL_SPEED_921600; - case SERIAL_SPEED_INDEX_1000000: return SERIAL_SPEED_1000000; - case SERIAL_SPEED_INDEX_1500000: return SERIAL_SPEED_1500000; - case SERIAL_SPEED_INDEX_2000000: return SERIAL_SPEED_2000000; - case SERIAL_SPEED_INDEX_2470000: return SERIAL_SPEED_2470000; + using namespace Espfc; + if (speed >= SERIAL_SPEED_2470000) + return SERIAL_SPEED_INDEX_2470000; + if (speed >= SERIAL_SPEED_2000000) + return SERIAL_SPEED_INDEX_2000000; + if (speed >= SERIAL_SPEED_1500000) + return SERIAL_SPEED_INDEX_1500000; + if (speed >= SERIAL_SPEED_1000000) + return SERIAL_SPEED_INDEX_1000000; + if (speed >= SERIAL_SPEED_921600) + return SERIAL_SPEED_INDEX_921600; + if (speed >= SERIAL_SPEED_500000) + return SERIAL_SPEED_INDEX_500000; + if (speed >= SERIAL_SPEED_460800) + return SERIAL_SPEED_INDEX_460800; + if (speed >= SERIAL_SPEED_400000) + return SERIAL_SPEED_INDEX_400000; + if (speed >= SERIAL_SPEED_250000) + return SERIAL_SPEED_INDEX_250000; + if (speed >= SERIAL_SPEED_230400) + return SERIAL_SPEED_INDEX_230400; + if (speed >= SERIAL_SPEED_115200) + return SERIAL_SPEED_INDEX_115200; + if (speed >= SERIAL_SPEED_57600) + return SERIAL_SPEED_INDEX_57600; + if (speed >= SERIAL_SPEED_38400) + return SERIAL_SPEED_INDEX_38400; + if (speed >= SERIAL_SPEED_19200) + return SERIAL_SPEED_INDEX_19200; + if (speed >= SERIAL_SPEED_9600) + return SERIAL_SPEED_INDEX_9600; + return SERIAL_SPEED_INDEX_AUTO; + } + + static Espfc::SerialSpeed fromBaudIndex(SerialSpeedIndex index) + { + using namespace Espfc; + switch (index) + { + case SERIAL_SPEED_INDEX_9600: + return SERIAL_SPEED_9600; + case SERIAL_SPEED_INDEX_19200: + return SERIAL_SPEED_19200; + case SERIAL_SPEED_INDEX_38400: + return SERIAL_SPEED_38400; + case SERIAL_SPEED_INDEX_57600: + return SERIAL_SPEED_57600; + case SERIAL_SPEED_INDEX_115200: + return SERIAL_SPEED_115200; + case SERIAL_SPEED_INDEX_230400: + return SERIAL_SPEED_230400; + case SERIAL_SPEED_INDEX_250000: + return SERIAL_SPEED_250000; + case SERIAL_SPEED_INDEX_400000: + return SERIAL_SPEED_400000; + case SERIAL_SPEED_INDEX_460800: + return SERIAL_SPEED_460800; + case SERIAL_SPEED_INDEX_500000: + return SERIAL_SPEED_500000; + case SERIAL_SPEED_INDEX_921600: + return SERIAL_SPEED_921600; + case SERIAL_SPEED_INDEX_1000000: + return SERIAL_SPEED_1000000; + case SERIAL_SPEED_INDEX_1500000: + return SERIAL_SPEED_1500000; + case SERIAL_SPEED_INDEX_2000000: + return SERIAL_SPEED_2000000; + case SERIAL_SPEED_INDEX_2470000: + return SERIAL_SPEED_2470000; case SERIAL_SPEED_INDEX_AUTO: default: return SERIAL_SPEED_NONE; + } } -} -static uint8_t toFilterTypeDerivative(uint8_t t) -{ - switch(t) { - case 0: return Espfc::FILTER_NONE; - case 1: return Espfc::FILTER_PT3; - case 2: return Espfc::FILTER_BIQUAD; - default: return Espfc::FILTER_PT3; + static uint8_t toFilterTypeDerivative(uint8_t t) + { + switch (t) + { + case 0: + return Espfc::FILTER_NONE; + case 1: + return Espfc::FILTER_PT3; + case 2: + return Espfc::FILTER_BIQUAD; + default: + return Espfc::FILTER_PT3; + } } -} -static uint8_t fromFilterTypeDerivative(uint8_t t) -{ - switch(t) { - case Espfc::FILTER_NONE: return 0; - case Espfc::FILTER_PT3: return 1; - case Espfc::FILTER_BIQUAD: return 2; - default: return 1; + static uint8_t fromFilterTypeDerivative(uint8_t t) + { + switch (t) + { + case Espfc::FILTER_NONE: + return 0; + case Espfc::FILTER_PT3: + return 1; + case Espfc::FILTER_BIQUAD: + return 2; + default: + return 1; + } } -} -static uint8_t fromGyroDlpf(uint8_t t) -{ - switch(t) { - case Espfc::GYRO_DLPF_256: return 0; - case Espfc::GYRO_DLPF_EX: return 1; - default: return 2; + static uint8_t fromGyroDlpf(uint8_t t) + { + switch (t) + { + case Espfc::GYRO_DLPF_256: + return 0; + case Espfc::GYRO_DLPF_EX: + return 1; + default: + return 2; + } } -} -static int8_t toVbatSource(uint8_t t) -{ - switch(t) { - case 0: return 0; // none - case 1: return 1; // internal adc - default: return 0; + static int8_t toVbatSource(uint8_t t) + { + switch (t) + { + case 0: + return 0; // none + case 1: + return 1; // internal adc + default: + return 0; + } } -} -static int8_t toIbatSource(uint8_t t) -{ - switch(t) { - case 0: return 0; // none - case 1: return 1; // internal adc - default: return 0; + static int8_t toIbatSource(uint8_t t) + { + switch (t) + { + case 0: + return 0; // none + case 1: + return 1; // internal adc + default: + return 0; + } } -} -static uint8_t toVbatVoltageLegacy(float voltage) -{ - return constrain(lrintf(voltage * 10.0f), 0, 255); -} + static uint8_t toVbatVoltageLegacy(float voltage) + { + return constrain(lrintf(voltage * 10.0f), 0, 255); + } -static uint16_t toVbatVoltage(float voltage) -{ - return constrain(lrintf(voltage * 100.0f), 0, 32000); -} + static uint16_t toVbatVoltage(float voltage) + { + return constrain(lrintf(voltage * 100.0f), 0, 32000); + } -static uint16_t toIbatCurrent(float current) -{ - return constrain(lrintf(current * 100.0f), -32000, 32000); -} + static uint16_t toIbatCurrent(float current) + { + return constrain(lrintf(current * 100.0f), -32000, 32000); + } -constexpr uint8_t MSP_PASSTHROUGH_ESC_4WAY = 0xff; + constexpr uint8_t MSP_PASSTHROUGH_ESC_4WAY = 0xff; } -namespace Espfc { - -namespace Connect { - -MspProcessor::MspProcessor(Model& model): _model(model) {} - -bool MspProcessor::parse(char c, MspMessage& msg) +namespace Espfc { - _parser.parse(c, msg); - if(msg.isReady()) debugMessage(msg); + namespace Connect + { - return !msg.isIdle(); -} + MspProcessor::MspProcessor(Model &model) : _model(model) {} -void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialDevice& s) -{ - r.cmd = m.cmd; - r.version = m.version; - r.result = 1; - switch(m.cmd) - { - case MSP_API_VERSION: - r.writeU8(MSP_PROTOCOL_VERSION); - r.writeU8(API_VERSION_MAJOR); - r.writeU8(API_VERSION_MINOR); - break; + bool MspProcessor::parse(char c, MspMessage &msg) + { + _parser.parse(c, msg); - case MSP_FC_VARIANT: - r.writeData(flightControllerIdentifier, FLIGHT_CONTROLLER_IDENTIFIER_LENGTH); - break; + if (msg.isReady()) + debugMessage(msg); - case MSP_FC_VERSION: - r.writeU8(FC_VERSION_MAJOR); - r.writeU8(FC_VERSION_MINOR); - r.writeU8(FC_VERSION_PATCH_LEVEL); - break; + return !msg.isIdle(); + } - case MSP_BOARD_INFO: - r.writeData(boardIdentifier, BOARD_IDENTIFIER_LENGTH); - r.writeU16(0); // No other build targets currently have hardware revision detection. - r.writeU8(0); // 0 == FC - r.writeU8(0); // target capabilities - r.writeU8(strlen(targetName)); // target name - r.writeData(targetName, strlen(targetName)); - r.writeU8(0); // board name - r.writeU8(0); // manufacturer name - for(size_t i = 0; i < 32; i++) r.writeU8(0); // signature - r.writeU8(255); // mcu id: unknown - // 1.42 - r.writeU8(2); // configuration state: configured - // 1.43 - r.writeU16(_model.state.gyro.present ? _model.state.gyro.timer.rate : 0); // sample rate + void MspProcessor::processCommand(MspMessage &m, MspResponse &r, Device::SerialDevice &s) + { + r.cmd = m.cmd; + r.version = m.version; + r.result = 1; + switch (m.cmd) { - uint32_t problems = 0; - if(_model.state.accel.bias.x == 0 && _model.state.accel.bias.y == 0 && _model.state.accel.bias.z == 0) { - problems |= 1 << 0; // acc calibration required + case MSP_API_VERSION: + r.writeU8(MSP_PROTOCOL_VERSION); + r.writeU8(API_VERSION_MAJOR); + r.writeU8(API_VERSION_MINOR); + break; + + case MSP_FC_VARIANT: + r.writeData(flightControllerIdentifier, FLIGHT_CONTROLLER_IDENTIFIER_LENGTH); + break; + + case MSP_FC_VERSION: + r.writeU8(FC_VERSION_MAJOR); + r.writeU8(FC_VERSION_MINOR); + r.writeU8(FC_VERSION_PATCH_LEVEL); + break; + + case MSP_BOARD_INFO: + r.writeData(boardIdentifier, BOARD_IDENTIFIER_LENGTH); + r.writeU16(0); // No other build targets currently have hardware revision detection. + r.writeU8(0); // 0 == FC + r.writeU8(0); // target capabilities + r.writeU8(strlen(targetName)); // target name + r.writeData(targetName, strlen(targetName)); + r.writeU8(0); // board name + r.writeU8(0); // manufacturer name + for (size_t i = 0; i < 32; i++) + r.writeU8(0); // signature + r.writeU8(255); // mcu id: unknown + // 1.42 + r.writeU8(2); // configuration state: configured + // 1.43 + r.writeU16(_model.state.gyro.present ? _model.state.gyro.timer.rate : 0); // sample rate + { + uint32_t problems = 0; + if (_model.state.accel.bias.x == 0 && _model.state.accel.bias.y == 0 && _model.state.accel.bias.z == 0) + { + problems |= 1 << 0; // acc calibration required + } + if (_model.config.output.protocol == ESC_PROTOCOL_DISABLED) + { + problems |= 1 << 1; // no motor protocol + } + r.writeU32(problems); // configuration problems } - if(_model.config.output.protocol == ESC_PROTOCOL_DISABLED) { - problems |= 1 << 1; // no motor protocol + // 1.44 + r.writeU8(0); // spi dev count + r.writeU8(0); // i2c dev count + break; + + case MSP_BUILD_INFO: + r.writeData(buildDate, BUILD_DATE_LENGTH); + r.writeData(buildTime, BUILD_TIME_LENGTH); + r.writeData(shortGitRevision, GIT_SHORT_REVISION_LENGTH); + break; + + case MSP_UID: + r.writeU32(getBoardId0()); + r.writeU32(getBoardId1()); + r.writeU32(getBoardId2()); + break; + + case MSP_STATUS_EX: + case MSP_STATUS: + // r.writeU16(_model.state.loopTimer.delta); + r.writeU16(_model.state.stats.loopTime()); + r.writeU16(_model.state.i2cErrorCount); // i2c error count + // acc, baro, mag, gps, sonar, gyro + r.writeU16(_model.accelActive() | _model.baroActive() << 1 | _model.magActive() << 2 | _model.gpsActive() << 3 | 0 << 4 | _model.gyroActive() << 5); + r.writeU32(_model.state.mode.mask); // flight mode flags + r.writeU8(0); // pid profile + r.writeU16(lrintf(_model.state.stats.getCpuLoad())); + if (m.cmd == MSP_STATUS_EX) + { + r.writeU8(1); // max profile count + r.writeU8(0); // current rate profile index + } + else + { // MSP_STATUS + // r.writeU16(_model.state.gyro.timer.interval); // gyro cycle time + r.writeU16(0); } - r.writeU32(problems); // configuration problems - } - // 1.44 - r.writeU8(0); // spi dev count - r.writeU8(0); // i2c dev count - break; - - case MSP_BUILD_INFO: - r.writeData(buildDate, BUILD_DATE_LENGTH); - r.writeData(buildTime, BUILD_TIME_LENGTH); - r.writeData(shortGitRevision, GIT_SHORT_REVISION_LENGTH); - break; - - case MSP_UID: - r.writeU32(getBoardId0()); - r.writeU32(getBoardId1()); - r.writeU32(getBoardId2()); - break; - - case MSP_STATUS_EX: - case MSP_STATUS: - //r.writeU16(_model.state.loopTimer.delta); - r.writeU16(_model.state.stats.loopTime()); - r.writeU16(_model.state.i2cErrorCount); // i2c error count - // acc, baro, mag, gps, sonar, gyro - r.writeU16(_model.accelActive() | _model.baroActive() << 1 | _model.magActive() << 2 | _model.gpsActive() << 3 | 0 << 4 | _model.gyroActive() << 5); - r.writeU32(_model.state.mode.mask); // flight mode flags - r.writeU8(0); // pid profile - r.writeU16(lrintf(_model.state.stats.getCpuLoad())); - if (m.cmd == MSP_STATUS_EX) { - r.writeU8(1); // max profile count - r.writeU8(0); // current rate profile index - } else { // MSP_STATUS - //r.writeU16(_model.state.gyro.timer.interval); // gyro cycle time - r.writeU16(0); - } - - // flight mode flags (above 32 bits) - r.writeU8(0); // count - - // Write arming disable flags - r.writeU8(ARMING_DISABLED_FLAGS_COUNT); // 1 byte, flag count - r.writeU32(_model.state.mode.armingDisabledFlags); // 4 bytes, flags - r.writeU8(0); // reboot required - break; - - case MSP_NAME: - r.writeString(_model.config.modelName); - break; - case MSP_SET_NAME: - memset(&_model.config.modelName, 0, MODEL_NAME_LEN + 1); - for(size_t i = 0; i < std::min((size_t)m.received, MODEL_NAME_LEN); i++) - { - _model.config.modelName[i] = m.readU8(); - } - break; + // flight mode flags (above 32 bits) + r.writeU8(0); // count - case MSP_BOXNAMES: - r.writeString(F("ARM;AIRMODE;ANGLE;ALTHOLD;BEEPER;FAILSAFE;BLACKBOX;BLACKBOXERASE;")); - break; + // Write arming disable flags + r.writeU8(ARMING_DISABLED_FLAGS_COUNT); // 1 byte, flag count + r.writeU32(_model.state.mode.armingDisabledFlags); // 4 bytes, flags + r.writeU8(0); // reboot required + break; - case MSP_BOXIDS: - r.writeU8(MODE_ARMED); - r.writeU8(MODE_AIRMODE); - r.writeU8(MODE_ANGLE); - r.writeU8(MODE_ALTHOLD); - r.writeU8(MODE_BUZZER); - r.writeU8(MODE_FAILSAFE); - r.writeU8(MODE_BLACKBOX); - r.writeU8(MODE_BLACKBOX_ERASE); - break; + case MSP_NAME: + r.writeString(_model.config.modelName); + break; - case MSP_MODE_RANGES: - for(size_t i = 0; i < ACTUATOR_CONDITIONS; i++) - { - r.writeU8(_model.config.conditions[i].id); - r.writeU8(_model.config.conditions[i].ch - AXIS_AUX_1); - r.writeU8((_model.config.conditions[i].min - 900) / 25); - r.writeU8((_model.config.conditions[i].max - 900) / 25); - } - break; + case MSP_SET_NAME: + memset(&_model.config.modelName, 0, MODEL_NAME_LEN + 1); + for (size_t i = 0; i < std::min((size_t)m.received, MODEL_NAME_LEN); i++) + { + _model.config.modelName[i] = m.readU8(); + } + break; + + case MSP_BOXNAMES: + r.writeString(F("ARM;AIRMODE;ANGLE;ALTHOLD;BEEPER;FAILSAFE;BLACKBOX;BLACKBOXERASE;")); + break; + + case MSP_BOXIDS: + r.writeU8(MODE_ARMED); + r.writeU8(MODE_AIRMODE); + r.writeU8(MODE_ANGLE); + r.writeU8(MODE_ALTHOLD); + r.writeU8(MODE_BUZZER); + r.writeU8(MODE_FAILSAFE); + r.writeU8(MODE_BLACKBOX); + r.writeU8(MODE_BLACKBOX_ERASE); + break; + + case MSP_MODE_RANGES: + for (size_t i = 0; i < ACTUATOR_CONDITIONS; i++) + { + r.writeU8(_model.config.conditions[i].id); + r.writeU8(_model.config.conditions[i].ch - AXIS_AUX_1); + r.writeU8((_model.config.conditions[i].min - 900) / 25); + r.writeU8((_model.config.conditions[i].max - 900) / 25); + } + break; - case MSP_MODE_RANGES_EXTRA: - r.writeU8(ACTUATOR_CONDITIONS); - for(size_t i = 0; i < ACTUATOR_CONDITIONS; i++) - { - r.writeU8(_model.config.conditions[i].id); - r.writeU8(_model.config.conditions[i].logicMode); - r.writeU8(_model.config.conditions[i].linkId); - } + case MSP_MODE_RANGES_EXTRA: + r.writeU8(ACTUATOR_CONDITIONS); + for (size_t i = 0; i < ACTUATOR_CONDITIONS; i++) + { + r.writeU8(_model.config.conditions[i].id); + r.writeU8(_model.config.conditions[i].logicMode); + r.writeU8(_model.config.conditions[i].linkId); + } - break; + break; - case MSP_SET_MODE_RANGE: + case MSP_SET_MODE_RANGE: { size_t i = m.readU8(); - if(i < ACTUATOR_CONDITIONS) + if (i < ACTUATOR_CONDITIONS) { _model.config.conditions[i].id = m.readU8(); _model.config.conditions[i].ch = m.readU8() + AXIS_AUX_1; _model.config.conditions[i].min = m.readU8() * 25 + 900; _model.config.conditions[i].max = m.readU8() * 25 + 900; - if(m.remain() >= 2) { + if (m.remain() >= 2) + { _model.config.conditions[i].logicMode = m.readU8(); // mode logic - _model.config.conditions[i].linkId = m.readU8(); // link to + _model.config.conditions[i].linkId = m.readU8(); // link to } } else @@ -331,99 +396,99 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD } break; - case MSP_ANALOG: - r.writeU8(toVbatVoltageLegacy(_model.state.battery.voltage)); // voltage in 0.1V - r.writeU16(0); // mah drawn - r.writeU16(_model.getRssi()); // rssi - r.writeU16(toIbatCurrent(_model.state.battery.current)); // amperage in 0.01A - r.writeU16(toVbatVoltage(_model.state.battery.voltage)); // voltage in 0.01V - break; - - case MSP_FEATURE_CONFIG: - r.writeU32(_model.config.featureMask); - break; - - case MSP_SET_FEATURE_CONFIG: - _model.config.featureMask = m.readU32(); - _model.reload(); - break; - - case MSP_BATTERY_CONFIG: - r.writeU8(34); // vbatmincellvoltage - r.writeU8(42); // vbatmaxcellvoltage - r.writeU8((_model.config.vbat.cellWarning + 5) / 10); // vbatwarningcellvoltage - r.writeU16(0); // batteryCapacity - r.writeU8(_model.config.vbat.source); // voltageMeterSource - r.writeU8(_model.config.ibat.source); // currentMeterSource - r.writeU16(340); // vbatmincellvoltage - r.writeU16(420); // vbatmaxcellvoltage - r.writeU16(_model.config.vbat.cellWarning); // vbatwarningcellvoltage - break; - - case MSP_SET_BATTERY_CONFIG: - m.readU8(); // vbatmincellvoltage - m.readU8(); // vbatmaxcellvoltage - _model.config.vbat.cellWarning = m.readU8() * 10; // vbatwarningcellvoltage - m.readU16(); // batteryCapacity - _model.config.vbat.source = toVbatSource(m.readU8()); // voltageMeterSource - _model.config.ibat.source = toIbatSource(m.readU8()); // currentMeterSource - if(m.remain() >= 6) - { - m.readU16(); // vbatmincellvoltage - m.readU16(); // vbatmaxcellvoltage - _model.config.vbat.cellWarning = m.readU16(); - } - break; + case MSP_ANALOG: + r.writeU8(toVbatVoltageLegacy(_model.state.battery.voltage)); // voltage in 0.1V + r.writeU16(0); // mah drawn + r.writeU16(_model.getRssi()); // rssi + r.writeU16(toIbatCurrent(_model.state.battery.current)); // amperage in 0.01A + r.writeU16(toVbatVoltage(_model.state.battery.voltage)); // voltage in 0.01V + break; + + case MSP_FEATURE_CONFIG: + r.writeU32(_model.config.featureMask); + break; + + case MSP_SET_FEATURE_CONFIG: + _model.config.featureMask = m.readU32(); + _model.reload(); + break; + + case MSP_BATTERY_CONFIG: + r.writeU8(34); // vbatmincellvoltage + r.writeU8(42); // vbatmaxcellvoltage + r.writeU8((_model.config.vbat.cellWarning + 5) / 10); // vbatwarningcellvoltage + r.writeU16(0); // batteryCapacity + r.writeU8(_model.config.vbat.source); // voltageMeterSource + r.writeU8(_model.config.ibat.source); // currentMeterSource + r.writeU16(340); // vbatmincellvoltage + r.writeU16(420); // vbatmaxcellvoltage + r.writeU16(_model.config.vbat.cellWarning); // vbatwarningcellvoltage + break; + + case MSP_SET_BATTERY_CONFIG: + m.readU8(); // vbatmincellvoltage + m.readU8(); // vbatmaxcellvoltage + _model.config.vbat.cellWarning = m.readU8() * 10; // vbatwarningcellvoltage + m.readU16(); // batteryCapacity + _model.config.vbat.source = toVbatSource(m.readU8()); // voltageMeterSource + _model.config.ibat.source = toIbatSource(m.readU8()); // currentMeterSource + if (m.remain() >= 6) + { + m.readU16(); // vbatmincellvoltage + m.readU16(); // vbatmaxcellvoltage + _model.config.vbat.cellWarning = m.readU16(); + } + break; - case MSP_BATTERY_STATE: - // battery characteristics - r.writeU8(_model.state.battery.cells); // cell count, 0 indicates battery not detected. - r.writeU16(0); // capacity in mAh + case MSP_BATTERY_STATE: + // battery characteristics + r.writeU8(_model.state.battery.cells); // cell count, 0 indicates battery not detected. + r.writeU16(0); // capacity in mAh - // battery state - r.writeU8(toVbatVoltageLegacy(_model.state.battery.voltage)); // in 0.1V steps - r.writeU16(0); // milliamp hours drawn from battery - r.writeU16(toIbatCurrent(_model.state.battery.current)); // send current in 0.01 A steps, range is -320A to 320A + // battery state + r.writeU8(toVbatVoltageLegacy(_model.state.battery.voltage)); // in 0.1V steps + r.writeU16(0); // milliamp hours drawn from battery + r.writeU16(toIbatCurrent(_model.state.battery.current)); // send current in 0.01 A steps, range is -320A to 320A - // battery alerts - r.writeU8(0); - r.writeU16(toVbatVoltage(_model.state.battery.voltage)); // in 0.01 steps - break; + // battery alerts + r.writeU8(0); + r.writeU16(toVbatVoltage(_model.state.battery.voltage)); // in 0.01 steps + break; - case MSP_VOLTAGE_METERS: - for(int i = 0; i < 1; i++) - { - r.writeU8(i + 10); // meter id (10-19 vbat adc) - r.writeU8(toVbatVoltageLegacy(_model.state.battery.voltage)); // meter value - } - break; + case MSP_VOLTAGE_METERS: + for (int i = 0; i < 1; i++) + { + r.writeU8(i + 10); // meter id (10-19 vbat adc) + r.writeU8(toVbatVoltageLegacy(_model.state.battery.voltage)); // meter value + } + break; - case MSP_CURRENT_METERS: - for(int i = 0; i < 1; i++) - { - r.writeU8(i + 10); // meter id (10-19 ibat adc) - r.writeU16(0); // mah drawn - r.writeU16(constrain(toIbatCurrent(_model.state.battery.current) * 10, 0, 0xffff)); // meter value - } - break; + case MSP_CURRENT_METERS: + for (int i = 0; i < 1; i++) + { + r.writeU8(i + 10); // meter id (10-19 ibat adc) + r.writeU16(0); // mah drawn + r.writeU16(constrain(toIbatCurrent(_model.state.battery.current) * 10, 0, 0xffff)); // meter value + } + break; - case MSP_VOLTAGE_METER_CONFIG: - r.writeU8(1); // num voltage sensors - for(int i = 0; i < 1; i++) - { - r.writeU8(5); // frame size (5) - r.writeU8(i + 10); // id (10-19 vbat adc) - r.writeU8(0); // type resistor divider - r.writeU8(_model.config.vbat.scale); // scale - r.writeU8(_model.config.vbat.resDiv); // resdivval - r.writeU8(_model.config.vbat.resMult); // resdivmultiplier - } - break; + case MSP_VOLTAGE_METER_CONFIG: + r.writeU8(1); // num voltage sensors + for (int i = 0; i < 1; i++) + { + r.writeU8(5); // frame size (5) + r.writeU8(i + 10); // id (10-19 vbat adc) + r.writeU8(0); // type resistor divider + r.writeU8(_model.config.vbat.scale); // scale + r.writeU8(_model.config.vbat.resDiv); // resdivval + r.writeU8(_model.config.vbat.resMult); // resdivmultiplier + } + break; - case MSP_SET_VOLTAGE_METER_CONFIG: + case MSP_SET_VOLTAGE_METER_CONFIG: { int id = m.readU8(); - if(id == 10 + 0) // id (10-19 vbat adc, allow only 10) + if (id == 10 + 0) // id (10-19 vbat adc, allow only 10) { _model.config.vbat.scale = m.readU8(); _model.config.vbat.resDiv = m.readU8(); @@ -432,22 +497,22 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD } break; - case MSP_CURRENT_METER_CONFIG: - r.writeU8(1); // num voltage sensors - for(int i = 0; i < 1; i++) - { - r.writeU8(6); // frame size (6) - r.writeU8(i + 10); // id (10-19 ibat adc) - r.writeU8(1); // type adc - r.writeU16(_model.config.ibat.scale); // scale - r.writeU16(_model.config.ibat.offset); // offset - } - break; + case MSP_CURRENT_METER_CONFIG: + r.writeU8(1); // num voltage sensors + for (int i = 0; i < 1; i++) + { + r.writeU8(6); // frame size (6) + r.writeU8(i + 10); // id (10-19 ibat adc) + r.writeU8(1); // type adc + r.writeU16(_model.config.ibat.scale); // scale + r.writeU16(_model.config.ibat.offset); // offset + } + break; - case MSP_SET_CURRENT_METER_CONFIG: + case MSP_SET_CURRENT_METER_CONFIG: { int id = m.readU8(); - if(id == 10 + 0) // id (10-19 ibat adc, allow only 10) + if (id == 10 + 0) // id (10-19 ibat adc, allow only 10) { _model.config.ibat.scale = m.readU16(); _model.config.ibat.offset = m.readU16(); @@ -455,8 +520,8 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD } break; - case MSP_DATAFLASH_SUMMARY: -#ifdef USE_FLASHFS + case MSP_DATAFLASH_SUMMARY: +#ifdef USE_FLASHFS { uint8_t flags = flashfsIsSupported() ? 2 : 0; flags |= flashfsIsReady() ? 1 : 0; @@ -466,20 +531,20 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD r.writeU32(flashfsGetOffset()); } #else - r.writeU8(0); - r.writeU32(0); - r.writeU32(0); - r.writeU32(0); + r.writeU8(0); + r.writeU32(0); + r.writeU32(0); + r.writeU32(0); #endif break; - case MSP_DATAFLASH_ERASE: + case MSP_DATAFLASH_ERASE: #ifdef USE_FLASHFS - blackboxEraseAll(); + blackboxEraseAll(); #endif - break; + break; - case MSP_DATAFLASH_READ: + case MSP_DATAFLASH_READ: #ifdef USE_FLASHFS { const unsigned int dataSize = m.remain(); @@ -488,115 +553,139 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD bool allowCompression = false; bool useLegacyFormat; - if (dataSize >= sizeof(uint32_t) + sizeof(uint16_t)) { - readLength = m.readU16(); - if (m.remain()) { - allowCompression = m.readU8(); - } - useLegacyFormat = false; - } else { - readLength = 128; - useLegacyFormat = true; + if (dataSize >= sizeof(uint32_t) + sizeof(uint16_t)) + { + readLength = m.readU16(); + if (m.remain()) + { + allowCompression = m.readU8(); + } + useLegacyFormat = false; + } + else + { + readLength = 128; + useLegacyFormat = true; } serializeFlashData(r, readAddress, readLength, useLegacyFormat, allowCompression); } -#endif - break; - - case MSP_ACC_TRIM: - r.writeU16(0); // pitch - r.writeU16(0); // roll - break; - - case MSP_MIXER_CONFIG: - r.writeU8(_model.config.mixer.type); // mixerMode, QUAD_X - r.writeU8(_model.config.mixer.yawReverse); // yaw_motors_reversed - break; - - case MSP_SET_MIXER_CONFIG: - _model.config.mixer.type = m.readU8(); // mixerMode, QUAD_X - _model.config.mixer.yawReverse = m.readU8(); // yaw_motors_reversed - break; - - case MSP_SENSOR_CONFIG: - r.writeU8(_model.config.accel.dev); // 3 acc mpu6050 - r.writeU8(_model.config.baro.dev); // 2 baro bmp085 - r.writeU8(_model.config.mag.dev); // 3 mag hmc5883l - break; - - case MSP_SET_SENSOR_CONFIG: - _model.config.accel.dev = m.readU8(); // 3 acc mpu6050 - _model.config.baro.dev = m.readU8(); // 2 baro bmp085 - _model.config.mag.dev = m.readU8(); // 3 mag hmc5883l - _model.reload(); +#endif break; - case MSP_SENSOR_ALIGNMENT: - r.writeU8(_model.config.gyro.align); // gyro align - r.writeU8(_model.config.gyro.align); // acc align, Starting with 4.0 gyro and acc alignment are the same - r.writeU8(_model.config.mag.align); // mag align - //1.41+ - r.writeU8(_model.state.gyro.present ? 1 : 0); // gyro detection mask GYRO_1_MASK - r.writeU8(0); // gyro_to_use - r.writeU8(_model.config.gyro.align); // gyro 1 - r.writeU8(0); // gyro 2 - break; + case MSP_ACC_TRIM: + r.writeU16(_model.config.accel.trim[0]); // pitch + r.writeU16(_model.config.accel.trim[1]); // roll + break; - case MSP_SET_SENSOR_ALIGNMENT: + case MSP_SET_ACC_TRIM: + _model.config.accel.trim[0] = m.readU16(); // pitch + _model.config.accel.trim[1] = m.readU16(); // roll + { + const float trimPitch = Utils::toRad(_model.config.accel.trim[0] * 0.1f); + const float trimRoll = Utils::toRad(_model.config.accel.trim[1] * 0.1f); + if (trimPitch != 0.f || trimRoll != 0.f) + { + _model.state.trimRotation.init(VectorFloat(trimRoll, trimPitch, 0.f)); + } + else + { + _model.state.trimRotation.init(VectorFloat(0.f, 0.f, 0.f)); + } + } + break; + + case MSP_MIXER_CONFIG: + r.writeU8(_model.config.mixer.type); // mixerMode, QUAD_X + r.writeU8(_model.config.mixer.yawReverse); // yaw_motors_reversed + break; + + case MSP_SET_MIXER_CONFIG: + _model.config.mixer.type = m.readU8(); // mixerMode, QUAD_X + _model.config.mixer.yawReverse = m.readU8(); // yaw_motors_reversed + break; + + case MSP_SENSOR_CONFIG: + r.writeU8(_model.config.accel.dev); // 3 acc mpu6050 + r.writeU8(_model.config.baro.dev); // 2 baro bmp085 + r.writeU8(_model.config.mag.dev); // 3 mag hmc5883l + break; + + case MSP_SET_SENSOR_CONFIG: + _model.config.accel.dev = m.readU8(); // 3 acc mpu6050 + _model.config.baro.dev = m.readU8(); // 2 baro bmp085 + _model.config.mag.dev = m.readU8(); // 3 mag hmc5883l + _model.reload(); + break; + + case MSP_SENSOR_ALIGNMENT: + r.writeU8(_model.config.gyro.align); // gyro align + r.writeU8(_model.config.gyro.align); // acc align, Starting with 4.0 gyro and acc alignment are the same + r.writeU8(_model.config.mag.align); // mag align + // 1.41+ + r.writeU8(_model.state.gyro.present ? 1 : 0); // gyro detection mask GYRO_1_MASK + r.writeU8(0); // gyro_to_use + r.writeU8(_model.config.gyro.align); // gyro 1 + r.writeU8(0); // gyro 2 + break; + + case MSP_SET_SENSOR_ALIGNMENT: { - uint8_t gyroAlign = m.readU8(); // gyro align - m.readU8(); // discard deprecated acc align + uint8_t gyroAlign = m.readU8(); // gyro align + m.readU8(); // discard deprecated acc align _model.config.mag.align = m.readU8(); // mag align // API >= 1.41 - support the gyro_to_use and alignment for gyros 1 & 2 - if(m.remain() >= 3) + if (m.remain() >= 3) { - m.readU8(); // gyro_to_use + m.readU8(); // gyro_to_use gyroAlign = m.readU8(); // gyro 1 align - m.readU8(); // gyro 2 align + m.readU8(); // gyro 2 align } _model.config.gyro.align = gyroAlign; } break; - case MSP_CF_SERIAL_CONFIG: - for(int i = 0; i < SERIAL_UART_COUNT; i++) - { - if(_model.config.serial[i].id >= SERIAL_ID_SOFTSERIAL_1 && !_model.isFeatureActive(FEATURE_SOFTSERIAL)) continue; - r.writeU8(_model.config.serial[i].id); // identifier - r.writeU16(_model.config.serial[i].functionMask); // functionMask - r.writeU8(toBaudIndex(_model.config.serial[i].baud)); // msp_baudrateIndex - r.writeU8(0); // gps_baudrateIndex - r.writeU8(0); // telemetry_baudrateIndex - r.writeU8(toBaudIndex(_model.config.serial[i].blackboxBaud)); // blackbox_baudrateIndex - } - break; + case MSP_CF_SERIAL_CONFIG: + for (int i = 0; i < SERIAL_UART_COUNT; i++) + { + if (_model.config.serial[i].id >= SERIAL_ID_SOFTSERIAL_1 && !_model.isFeatureActive(FEATURE_SOFTSERIAL)) + continue; + r.writeU8(_model.config.serial[i].id); // identifier + r.writeU16(_model.config.serial[i].functionMask); // functionMask + r.writeU8(toBaudIndex(_model.config.serial[i].baud)); // msp_baudrateIndex + r.writeU8(0); // gps_baudrateIndex + r.writeU8(0); // telemetry_baudrateIndex + r.writeU8(toBaudIndex(_model.config.serial[i].blackboxBaud)); // blackbox_baudrateIndex + } + break; - case MSP2_COMMON_SERIAL_CONFIG: + case MSP2_COMMON_SERIAL_CONFIG: { uint8_t count = 0; for (int i = 0; i < SERIAL_UART_COUNT; i++) { - if(_model.config.serial[i].id >= SERIAL_ID_SOFTSERIAL_1 && !_model.isFeatureActive(FEATURE_SOFTSERIAL)) continue; + if (_model.config.serial[i].id >= SERIAL_ID_SOFTSERIAL_1 && !_model.isFeatureActive(FEATURE_SOFTSERIAL)) + continue; count++; } r.writeU8(count); for (int i = 0; i < SERIAL_UART_COUNT; i++) { - if(_model.config.serial[i].id >= SERIAL_ID_SOFTSERIAL_1 && !_model.isFeatureActive(FEATURE_SOFTSERIAL)) continue; - r.writeU8(_model.config.serial[i].id); // identifier - r.writeU32(_model.config.serial[i].functionMask); // functionMask - r.writeU8(toBaudIndex(_model.config.serial[i].baud)); // msp_baudrateIndex - r.writeU8(0); // gps_baudrateIndex - r.writeU8(0); // telemetry_baudrateIndex + if (_model.config.serial[i].id >= SERIAL_ID_SOFTSERIAL_1 && !_model.isFeatureActive(FEATURE_SOFTSERIAL)) + continue; + r.writeU8(_model.config.serial[i].id); // identifier + r.writeU32(_model.config.serial[i].functionMask); // functionMask + r.writeU8(toBaudIndex(_model.config.serial[i].baud)); // msp_baudrateIndex + r.writeU8(0); // gps_baudrateIndex + r.writeU8(0); // telemetry_baudrateIndex r.writeU8(toBaudIndex(_model.config.serial[i].blackboxBaud)); // blackbox_baudrateIndex } } break; - case MSP_SET_CF_SERIAL_CONFIG: + case MSP_SET_CF_SERIAL_CONFIG: { const int packetSize = 1 + 2 + 4; - while(m.remain() >= packetSize) + while (m.remain() >= packetSize) { int id = m.readU8(); int k = _model.getSerialIndex((SerialPortId)id); @@ -612,19 +701,19 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD _model.config.serial[k].blackboxBaud = fromBaudIndex((SerialSpeedIndex)m.readU8()); } } - _model.reload(); - break; + _model.reload(); + break; - case MSP2_COMMON_SET_SERIAL_CONFIG: + case MSP2_COMMON_SET_SERIAL_CONFIG: { size_t count = m.readU8(); (void)count; // ignore const int packetSize = 1 + 4 + 4; - while(m.remain() >= packetSize) + while (m.remain() >= packetSize) { int id = m.readU8(); int k = _model.getSerialIndex((SerialPortId)id); - if(k == -1) + if (k == -1) { m.advance(packetSize - 1); continue; @@ -637,739 +726,774 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD _model.config.serial[k].blackboxBaud = fromBaudIndex((SerialSpeedIndex)m.readU8()); } } - _model.reload(); - break; - - case MSP_BLACKBOX_CONFIG: - r.writeU8(1); // Blackbox supported - r.writeU8(_model.config.blackbox.dev); // device serial or none - r.writeU8(1); // blackboxGetRateNum()); // unused - r.writeU8(1); // blackboxGetRateDenom()); - r.writeU16(_model.config.blackbox.pDenom);//blackboxGetPRatio()); // p_denom - //r.writeU8(_model.config.blackbox.pDenom); // sample_rate - //r.writeU32(~_model.config.blackbox.fieldsMask); - break; - - case MSP_SET_BLACKBOX_CONFIG: - // Don't allow config to be updated while Blackbox is logging - if (true) { - _model.config.blackbox.dev = m.readU8(); - const int rateNum = m.readU8(); // was rate_num - const int rateDenom = m.readU8(); // was rate_denom - uint16_t pRatio = 0; - if (m.remain() >= 2) { + _model.reload(); + break; + + case MSP_BLACKBOX_CONFIG: + r.writeU8(1); // Blackbox supported + r.writeU8(_model.config.blackbox.dev); // device serial or none + r.writeU8(1); // blackboxGetRateNum()); // unused + r.writeU8(1); // blackboxGetRateDenom()); + r.writeU16(_model.config.blackbox.pDenom); // blackboxGetPRatio()); // p_denom + // r.writeU8(_model.config.blackbox.pDenom); // sample_rate + // r.writeU32(~_model.config.blackbox.fieldsMask); + break; + + case MSP_SET_BLACKBOX_CONFIG: + // Don't allow config to be updated while Blackbox is logging + if (true) + { + _model.config.blackbox.dev = m.readU8(); + const int rateNum = m.readU8(); // was rate_num + const int rateDenom = m.readU8(); // was rate_denom + uint16_t pRatio = 0; + if (m.remain() >= 2) + { pRatio = m.readU16(); // p_denom specified, so use it directly - } else { + } + else + { // p_denom not specified in MSP, so calculate it from old rateNum and rateDenom - //pRatio = blackboxCalculatePDenom(rateNum, rateDenom); + // pRatio = blackboxCalculatePDenom(rateNum, rateDenom); (void)(rateNum + rateDenom); - } - _model.config.blackbox.pDenom = pRatio; + } + _model.config.blackbox.pDenom = pRatio; - /*if (m.remain() >= 1) { - _model.config.blackbox.pDenom = m.readU8(); - } else if(pRatio > 0) { - _model.config.blackbox.pDenom = blackboxCalculateSampleRate(pRatio); - //_model.config.blackbox.pDenom = pRatio; + /*if (m.remain() >= 1) { + _model.config.blackbox.pDenom = m.readU8(); + } else if(pRatio > 0) { + _model.config.blackbox.pDenom = blackboxCalculateSampleRate(pRatio); + //_model.config.blackbox.pDenom = pRatio; + } + if (m.remain() >= 4) { + _model.config.blackbox.fieldsMask = ~m.readU32(); + }*/ } - if (m.remain() >= 4) { - _model.config.blackbox.fieldsMask = ~m.readU32(); - }*/ - } - break; - - case MSP_ATTITUDE: - r.writeU16(lrintf(Utils::toDeg(_model.state.attitude.euler.x) * 10.f)); // roll [decidegrees] - r.writeU16(lrintf(Utils::toDeg(_model.state.attitude.euler.y) * 10.f)); // pitch [decidegrees] - r.writeU16(lrintf(Utils::toDeg(-_model.state.attitude.euler.z))); // yaw [degrees] - break; - - case MSP_ALTITUDE: - r.writeU32(lrintf(_model.state.altitude.height * 100.f)); // alt [cm] - r.writeU16(lrintf(_model.state.altitude.vario * 100.f)); // vario [cm/s] - break; - - case MSP_BEEPER_CONFIG: - r.writeU32(~_model.config.buzzer.beeperMask); // beeper mask - r.writeU8(0); // dshot beacon tone - r.writeU32(0); // dshot beacon off flags - break; - - case MSP_SET_BEEPER_CONFIG: - _model.config.buzzer.beeperMask = ~m.readU32(); // beeper mask - break; - - case MSP_BOARD_ALIGNMENT_CONFIG: - r.writeU16(_model.config.boardAlignment[0]); // roll - r.writeU16(_model.config.boardAlignment[1]); // pitch - r.writeU16(_model.config.boardAlignment[2]); // yaw - break; + break; + + case MSP_ATTITUDE: + r.writeU16(lrintf(Utils::toDeg(_model.state.attitude.euler.x) * 10.f)); // roll [decidegrees] + r.writeU16(lrintf(Utils::toDeg(_model.state.attitude.euler.y) * 10.f)); // pitch [decidegrees] + r.writeU16(lrintf(Utils::toDeg(-_model.state.attitude.euler.z))); // yaw [degrees] + break; + + case MSP_ALTITUDE: + r.writeU32(lrintf(_model.state.altitude.height * 100.f)); // alt [cm] + r.writeU16(lrintf(_model.state.altitude.vario * 100.f)); // vario [cm/s] + break; + + case MSP_BEEPER_CONFIG: + r.writeU32(~_model.config.buzzer.beeperMask); // beeper mask + r.writeU8(0); // dshot beacon tone + r.writeU32(0); // dshot beacon off flags + break; + + case MSP_SET_BEEPER_CONFIG: + _model.config.buzzer.beeperMask = ~m.readU32(); // beeper mask + break; + + case MSP_BOARD_ALIGNMENT_CONFIG: + r.writeU16(_model.config.boardAlignment[0]); // roll + r.writeU16(_model.config.boardAlignment[1]); // pitch + r.writeU16(_model.config.boardAlignment[2]); // yaw + break; + + case MSP_SET_BOARD_ALIGNMENT_CONFIG: + _model.config.boardAlignment[0] = m.readU16(); + _model.config.boardAlignment[1] = m.readU16(); + _model.config.boardAlignment[2] = m.readU16(); + break; + + case MSP_RX_MAP: + for (size_t i = 0; i < INPUT_CHANNELS; i++) + { + r.writeU8(_model.config.input.channel[i].map); + } + break; - case MSP_SET_BOARD_ALIGNMENT_CONFIG: - _model.config.boardAlignment[0] = m.readU16(); - _model.config.boardAlignment[1] = m.readU16(); - _model.config.boardAlignment[2] = m.readU16(); - break; + case MSP_SET_RX_MAP: + for (size_t i = 0; i < 8; i++) + { + _model.config.input.channel[i].map = m.readU8(); + } + break; + + case MSP_RSSI_CONFIG: + r.writeU8(_model.config.input.rssiChannel); + break; + + case MSP_SET_RSSI_CONFIG: + _model.config.input.rssiChannel = m.readU8(); + break; + + case MSP_MOTOR_CONFIG: + r.writeU16(_model.config.output.minThrottle); // minthrottle + r.writeU16(_model.config.output.maxThrottle); // maxthrottle + r.writeU16(_model.config.output.minCommand); // mincommand + r.writeU8(_model.state.currentMixer.count); // motor count + // 1.42+ + r.writeU8(_model.config.output.motorPoles); // motor pole count + r.writeU8(_model.config.output.dshotTelemetry); // dshot telemtery + r.writeU8(0); // esc sensor + break; + + case MSP_SET_MOTOR_CONFIG: + _model.config.output.minThrottle = m.readU16(); // minthrottle + _model.config.output.maxThrottle = m.readU16(); // maxthrottle + _model.config.output.minCommand = m.readU16(); // mincommand + if (m.remain() >= 2) + { +#ifdef ESPFC_DSHOT_TELEMETRY + _model.config.output.motorPoles = m.readU8(); + _model.config.output.dshotTelemetry = m.readU8(); +#else + m.readU8(); + m.readU8(); +#endif + } + _model.reload(); + break; + + case MSP_MOTOR_3D_CONFIG: + r.writeU16(1406); // deadband3d_low; + r.writeU16(1514); // deadband3d_high; + r.writeU16(1460); // neutral3d; + break; + + case MSP_ARMING_CONFIG: + r.writeU8(5); // auto_disarm delay + r.writeU8(0); // disarm kill switch + r.writeU8(_model.config.arming.smallAngle); // small angle + break; + + case MSP_SET_ARMING_CONFIG: + (void)m.readU8(); // auto_disarm delay (ignored) + (void)m.readU8(); // disarm kill switch (ignored) + _model.config.arming.smallAngle = m.readU8(); // small angle + break; + + case MSP_RC_DEADBAND: + r.writeU8(_model.config.input.deadband); + r.writeU8(0); // yaw deadband + r.writeU8(0); // alt hold deadband + r.writeU16(0); // deadband 3d throttle + break; + + case MSP_SET_RC_DEADBAND: + _model.config.input.deadband = m.readU8(); + m.readU8(); // yaw deadband + m.readU8(); // alt hod deadband + m.readU16(); // deadband 3d throttle + break; + + case MSP_RX_CONFIG: + r.writeU8(_model.config.input.serialRxProvider); // serialrx_provider + r.writeU16(_model.config.input.maxCheck); // maxcheck + r.writeU16(_model.config.input.midRc); // midrc + r.writeU16(_model.config.input.minCheck); // mincheck + r.writeU8(0); // spectrum bind + r.writeU16(_model.config.input.minRc); // min_us + r.writeU16(_model.config.input.maxRc); // max_us + r.writeU8(_model.config.input.interpolationMode); // rc interpolation + r.writeU8(_model.config.input.interpolationInterval); // rc interpolation interval + r.writeU16(1500); // airmode activate threshold + r.writeU8(0); // rx spi prot + r.writeU32(0); // rx spi id + r.writeU8(0); // rx spi chan count + r.writeU8(0); // fpv camera angle + r.writeU8(2); // rc iterpolation channels: RPYT + r.writeU8(_model.config.input.filterType); // rc_smoothing_type + r.writeU8(_model.config.input.filter.freq); // rc_smoothing_input_cutoff + r.writeU8(_model.config.input.filterDerivative.freq); // rc_smoothing_derivative_cutoff + r.writeU8(0); //_model.config.input.filter.type); // rc_smoothing_input_type + r.writeU8(fromFilterTypeDerivative(_model.config.input.filterDerivative.type)); // rc_smoothing_derivative_type + r.writeU8(0); // usb type + // 1.42+ + r.writeU8(_model.config.input.filterAutoFactor); // rc_smoothing_auto_factor + break; + + case MSP_SET_RX_CONFIG: + _model.config.input.serialRxProvider = m.readU8(); // serialrx_provider + _model.config.input.maxCheck = m.readU16(); // maxcheck + _model.config.input.midRc = m.readU16(); // midrc + _model.config.input.minCheck = m.readU16(); // mincheck + m.readU8(); // spectrum bind + _model.config.input.minRc = m.readU16(); // min_us + _model.config.input.maxRc = m.readU16(); // max_us + if (m.remain() >= 4) + { + _model.config.input.interpolationMode = m.readU8(); // rc interpolation + _model.config.input.interpolationInterval = m.readU8(); // rc interpolation interval + m.readU16(); // airmode activate threshold + } + if (m.remain() >= 6) + { + m.readU8(); // rx spi prot + m.readU32(); // rx spi id + m.readU8(); // rx spi chan count + } + if (m.remain() >= 1) + { + m.readU8(); // fpv camera angle + } + // 1.40+ + if (m.remain() >= 6) + { + m.readU8(); // rc iterpolation channels + _model.config.input.filterType = m.readU8(); // rc_smoothing_type + _model.config.input.filter.freq = m.readU8(); // rc_smoothing_input_cutoff + _model.config.input.filterDerivative.freq = m.readU8(); // rc_smoothing_derivative_cutoff + //_model.config.input.filter.type = m.readU8() == 1 ? FILTER_BIQUAD : FILTER_PT1; // rc_smoothing_input_type + m.readU8(); + _model.config.input.filterDerivative.type = toFilterTypeDerivative(m.readU8()); // rc_smoothing_derivative_type + } + if (m.remain() >= 1) + { + m.readU8(); // usb type + } + // 1.42+ + if (m.remain() >= 1) + { + _model.config.input.filterAutoFactor = m.readU8(); // rc_smoothing_auto_factor + } - case MSP_RX_MAP: - for(size_t i = 0; i < INPUT_CHANNELS; i++) - { - r.writeU8(_model.config.input.channel[i].map); - } - break; + _model.reload(); + break; + + case MSP_FAILSAFE_CONFIG: + r.writeU8(_model.config.failsafe.delay); // failsafe_delay + r.writeU8(0); // failsafe_off_delay + r.writeU16(1000); // failsafe_throttle + r.writeU8(_model.config.failsafe.killSwitch); // failsafe_kill_switch + r.writeU16(0); // failsafe_throttle_low_delay + r.writeU8(1); // failsafe_procedure; default drop + break; + + case MSP_SET_FAILSAFE_CONFIG: + _model.config.failsafe.delay = m.readU8(); // failsafe_delay + m.readU8(); // failsafe_off_delay + m.readU16(); // failsafe_throttle + _model.config.failsafe.killSwitch = m.readU8(); // failsafe_kill_switch + m.readU16(); // failsafe_throttle_low_delay + m.readU8(); // failsafe_procedure + break; + + case MSP_RXFAIL_CONFIG: + for (size_t i = 0; i < _model.state.input.channelCount; i++) + { + r.writeU8(_model.config.input.channel[i].fsMode); + r.writeU16(_model.config.input.channel[i].fsValue); + } + break; - case MSP_SET_RX_MAP: - for(size_t i = 0; i < 8; i++) + case MSP_SET_RXFAIL_CONFIG: { - _model.config.input.channel[i].map = m.readU8(); + size_t i = m.readU8(); + if (i < INPUT_CHANNELS) + { + _model.config.input.channel[i].fsMode = m.readU8(); // mode + _model.config.input.channel[i].fsValue = m.readU16(); // pulse + } + else + { + r.result = -1; + } } break; - case MSP_RSSI_CONFIG: - r.writeU8(_model.config.input.rssiChannel); - break; - - case MSP_SET_RSSI_CONFIG: - _model.config.input.rssiChannel = m.readU8(); - break; - - case MSP_MOTOR_CONFIG: - r.writeU16(_model.config.output.minThrottle); // minthrottle - r.writeU16(_model.config.output.maxThrottle); // maxthrottle - r.writeU16(_model.config.output.minCommand); // mincommand - r.writeU8(_model.state.currentMixer.count); // motor count - // 1.42+ - r.writeU8(_model.config.output.motorPoles); // motor pole count - r.writeU8(_model.config.output.dshotTelemetry); // dshot telemtery - r.writeU8(0); // esc sensor - break; - - case MSP_SET_MOTOR_CONFIG: - _model.config.output.minThrottle = m.readU16(); // minthrottle - _model.config.output.maxThrottle = m.readU16(); // maxthrottle - _model.config.output.minCommand = m.readU16(); // mincommand - if(m.remain() >= 2) - { -#ifdef ESPFC_DSHOT_TELEMETRY - _model.config.output.motorPoles = m.readU8(); - _model.config.output.dshotTelemetry = m.readU8(); -#else - m.readU8(); - m.readU8(); -#endif - } - _model.reload(); - break; + case MSP_RC: + for (size_t i = 0; i < _model.state.input.channelCount; i++) + { + r.writeU16(lrintf(_model.state.input.us[i])); + } + break; - case MSP_MOTOR_3D_CONFIG: - r.writeU16(1406); // deadband3d_low; - r.writeU16(1514); // deadband3d_high; - r.writeU16(1460); // neutral3d; - break; + case MSP_RC_TUNING: + r.writeU8(_model.config.input.rate[AXIS_ROLL]); + r.writeU8(_model.config.input.expo[AXIS_ROLL]); + for (size_t i = 0; i < AXIS_COUNT_RPY; i++) + { + r.writeU8(_model.config.input.superRate[i]); + } + r.writeU8(_model.config.controller.tpaScale); // dyn thr pid + r.writeU8(50); // thrMid8 + r.writeU8(0); // thr expo + r.writeU16(_model.config.controller.tpaBreakpoint); // tpa breakpoint + r.writeU8(_model.config.input.expo[AXIS_YAW]); // yaw expo + r.writeU8(_model.config.input.rate[AXIS_YAW]); // yaw rate + r.writeU8(_model.config.input.rate[AXIS_PITCH]); // pitch rate + r.writeU8(_model.config.input.expo[AXIS_PITCH]); // pitch expo + // 1.41+ + r.writeU8(_model.config.output.throttleLimitType); // throttle_limit_type (off) + r.writeU8(_model.config.output.throttleLimitPercent); // throtle_limit_percent (100%) + // 1.42+ + r.writeU16(_model.config.input.rateLimit[0]); // rate limit roll + r.writeU16(_model.config.input.rateLimit[1]); // rate limit pitch + r.writeU16(_model.config.input.rateLimit[2]); // rate limit yaw + // 1.43+ + r.writeU8(_model.config.input.rateType); // rates type + + break; + + case MSP_SET_RC_TUNING: + if (m.remain() >= 10) + { + const uint8_t rate = m.readU8(); + if (_model.config.input.rate[AXIS_PITCH] == _model.config.input.rate[AXIS_ROLL]) + { + _model.config.input.rate[AXIS_PITCH] = rate; + } + _model.config.input.rate[AXIS_ROLL] = rate; - case MSP_ARMING_CONFIG: - r.writeU8(5); // auto_disarm delay - r.writeU8(0); // disarm kill switch - r.writeU8(180); // small angle - break; + const uint8_t expo = m.readU8(); + if (_model.config.input.expo[AXIS_PITCH] == _model.config.input.expo[AXIS_ROLL]) + { + _model.config.input.expo[AXIS_PITCH] = expo; + } + _model.config.input.expo[AXIS_ROLL] = expo; - case MSP_RC_DEADBAND: - r.writeU8(_model.config.input.deadband); - r.writeU8(0); // yaw deadband - r.writeU8(0); // alt hold deadband - r.writeU16(0); // deadband 3d throttle - break; - - case MSP_SET_RC_DEADBAND: - _model.config.input.deadband = m.readU8(); - m.readU8(); // yaw deadband - m.readU8(); // alt hod deadband - m.readU16(); // deadband 3d throttle - break; - - case MSP_RX_CONFIG: - r.writeU8(_model.config.input.serialRxProvider); // serialrx_provider - r.writeU16(_model.config.input.maxCheck); //maxcheck - r.writeU16(_model.config.input.midRc); //midrc - r.writeU16(_model.config.input.minCheck); //mincheck - r.writeU8(0); // spectrum bind - r.writeU16(_model.config.input.minRc); //min_us - r.writeU16(_model.config.input.maxRc); //max_us - r.writeU8(_model.config.input.interpolationMode); // rc interpolation - r.writeU8(_model.config.input.interpolationInterval); // rc interpolation interval - r.writeU16(1500); // airmode activate threshold - r.writeU8(0); // rx spi prot - r.writeU32(0); // rx spi id - r.writeU8(0); // rx spi chan count - r.writeU8(0); // fpv camera angle - r.writeU8(2); // rc iterpolation channels: RPYT - r.writeU8(_model.config.input.filterType); // rc_smoothing_type - r.writeU8(_model.config.input.filter.freq); // rc_smoothing_input_cutoff - r.writeU8(_model.config.input.filterDerivative.freq); // rc_smoothing_derivative_cutoff - r.writeU8(0);//_model.config.input.filter.type); // rc_smoothing_input_type - r.writeU8(fromFilterTypeDerivative(_model.config.input.filterDerivative.type)); // rc_smoothing_derivative_type - r.writeU8(0); // usb type - // 1.42+ - r.writeU8(_model.config.input.filterAutoFactor); // rc_smoothing_auto_factor - break; - - case MSP_SET_RX_CONFIG: - _model.config.input.serialRxProvider = m.readU8(); // serialrx_provider - _model.config.input.maxCheck = m.readU16(); //maxcheck - _model.config.input.midRc = m.readU16(); //midrc - _model.config.input.minCheck = m.readU16(); //mincheck - m.readU8(); // spectrum bind - _model.config.input.minRc = m.readU16(); //min_us - _model.config.input.maxRc = m.readU16(); //max_us - if (m.remain() >= 4) { - _model.config.input.interpolationMode = m.readU8(); // rc interpolation - _model.config.input.interpolationInterval = m.readU8(); // rc interpolation interval - m.readU16(); // airmode activate threshold - } - if (m.remain() >= 6) { - m.readU8(); // rx spi prot - m.readU32(); // rx spi id - m.readU8(); // rx spi chan count - } - if (m.remain() >= 1) { - m.readU8(); // fpv camera angle - } - // 1.40+ - if (m.remain() >= 6) { - m.readU8(); // rc iterpolation channels - _model.config.input.filterType = m.readU8(); // rc_smoothing_type - _model.config.input.filter.freq = m.readU8(); // rc_smoothing_input_cutoff - _model.config.input.filterDerivative.freq = m.readU8(); // rc_smoothing_derivative_cutoff - //_model.config.input.filter.type = m.readU8() == 1 ? FILTER_BIQUAD : FILTER_PT1; // rc_smoothing_input_type - m.readU8(); - _model.config.input.filterDerivative.type = toFilterTypeDerivative(m.readU8()); // rc_smoothing_derivative_type - } - if (m.remain() >= 1) { - m.readU8(); // usb type - } - // 1.42+ - if (m.remain() >= 1) { - _model.config.input.filterAutoFactor = m.readU8(); // rc_smoothing_auto_factor - } - - _model.reload(); - break; - - case MSP_FAILSAFE_CONFIG: - r.writeU8(_model.config.failsafe.delay); // failsafe_delay - r.writeU8(0); // failsafe_off_delay - r.writeU16(1000); //failsafe_throttle - r.writeU8(_model.config.failsafe.killSwitch); // failsafe_kill_switch - r.writeU16(0); // failsafe_throttle_low_delay - r.writeU8(1); //failsafe_procedure; default drop - break; - - case MSP_SET_FAILSAFE_CONFIG: - _model.config.failsafe.delay = m.readU8(); //failsafe_delay - m.readU8(); //failsafe_off_delay - m.readU16(); //failsafe_throttle - _model.config.failsafe.killSwitch = m.readU8(); //failsafe_kill_switch - m.readU16(); //failsafe_throttle_low_delay - m.readU8(); //failsafe_procedure - break; - - case MSP_RXFAIL_CONFIG: - for (size_t i = 0; i < _model.state.input.channelCount; i++) - { - r.writeU8(_model.config.input.channel[i].fsMode); - r.writeU16(_model.config.input.channel[i].fsValue); - } - break; - - case MSP_SET_RXFAIL_CONFIG: - { - size_t i = m.readU8(); - if(i < INPUT_CHANNELS) - { - _model.config.input.channel[i].fsMode = m.readU8(); // mode - _model.config.input.channel[i].fsValue = m.readU16(); // pulse + for (size_t i = 0; i < AXIS_COUNT_RPY; i++) + { + _model.config.input.superRate[i] = m.readU8(); + } + _model.config.controller.tpaScale = Utils::clamp(m.readU8(), (uint8_t)0, (uint8_t)90); // dyn thr pid + m.readU8(); // thrMid8 + m.readU8(); // thr expo + _model.config.controller.tpaBreakpoint = Utils::clamp(m.readU16(), (uint16_t)1000, (uint16_t)2000); // tpa breakpoint + if (m.remain() >= 1) + { + _model.config.input.expo[AXIS_YAW] = m.readU8(); // yaw expo + } + if (m.remain() >= 1) + { + _model.config.input.rate[AXIS_YAW] = m.readU8(); // yaw rate + } + if (m.remain() >= 1) + { + _model.config.input.rate[AXIS_PITCH] = m.readU8(); // pitch rate + } + if (m.remain() >= 1) + { + _model.config.input.expo[AXIS_PITCH] = m.readU8(); // pitch expo + } + // 1.41 + if (m.remain() >= 2) + { + _model.config.output.throttleLimitType = m.readU8(); // throttle_limit_type + _model.config.output.throttleLimitPercent = m.readU8(); // throttle_limit_percent + } + // 1.42 + if (m.remain() >= 6) + { + _model.config.input.rateLimit[0] = m.readU16(); // roll + _model.config.input.rateLimit[1] = m.readU16(); // pitch + _model.config.input.rateLimit[2] = m.readU16(); // yaw + } + // 1.43 + if (m.remain() >= 1) + { + _model.config.input.rateType = m.readU8(); + } } else { r.result = -1; + // error } - } - break; - - case MSP_RC: - for(size_t i = 0; i < _model.state.input.channelCount; i++) - { - r.writeU16(lrintf(_model.state.input.us[i])); - } - break; - - case MSP_RC_TUNING: - r.writeU8(_model.config.input.rate[AXIS_ROLL]); - r.writeU8(_model.config.input.expo[AXIS_ROLL]); - for(size_t i = 0; i < AXIS_COUNT_RPY; i++) - { - r.writeU8(_model.config.input.superRate[i]); - } - r.writeU8(_model.config.controller.tpaScale); // dyn thr pid - r.writeU8(50); // thrMid8 - r.writeU8(0); // thr expo - r.writeU16(_model.config.controller.tpaBreakpoint); // tpa breakpoint - r.writeU8(_model.config.input.expo[AXIS_YAW]); // yaw expo - r.writeU8(_model.config.input.rate[AXIS_YAW]); // yaw rate - r.writeU8(_model.config.input.rate[AXIS_PITCH]); // pitch rate - r.writeU8(_model.config.input.expo[AXIS_PITCH]); // pitch expo - // 1.41+ - r.writeU8(_model.config.output.throttleLimitType); // throttle_limit_type (off) - r.writeU8(_model.config.output.throttleLimitPercent); // throtle_limit_percent (100%) - //1.42+ - r.writeU16(_model.config.input.rateLimit[0]); // rate limit roll - r.writeU16(_model.config.input.rateLimit[1]); // rate limit pitch - r.writeU16(_model.config.input.rateLimit[2]); // rate limit yaw - // 1.43+ - r.writeU8(_model.config.input.rateType); // rates type - - break; - - case MSP_SET_RC_TUNING: - if(m.remain() >= 10) - { - const uint8_t rate = m.readU8(); - if(_model.config.input.rate[AXIS_PITCH] == _model.config.input.rate[AXIS_ROLL]) + break; + + case MSP_ADVANCED_CONFIG: + r.writeU8(1); // gyroSync unused + r.writeU8(_model.config.loopSync); + r.writeU8(_model.config.output.async); + r.writeU8(_model.config.output.protocol); + r.writeU16(_model.config.output.rate); + r.writeU16(_model.config.output.dshotIdle); + r.writeU8(0); // 32k gyro + r.writeU8(0); // PWM inversion + r.writeU8(0); // gyro_to_use: {1:0, 2:1. 2:both} + r.writeU8(0); // gyro high fsr (flase) + r.writeU8(48); // gyro cal threshold + r.writeU16(125); // gyro cal duration (1.25s) + r.writeU16(0); // gyro offset yaw + r.writeU8(0); // check overflow + r.writeU8(_model.config.debug.mode); + r.writeU8(DEBUG_COUNT); + break; + + case MSP_SET_ADVANCED_CONFIG: + m.readU8(); // ignore gyroSync, removed in 1.43 + _model.config.loopSync = m.readU8(); + _model.config.output.async = m.readU8(); + _model.config.output.protocol = m.readU8(); + _model.config.output.rate = m.readU16(); + if (m.remain() >= 2) { - _model.config.input.rate[AXIS_PITCH] = rate; + _model.config.output.dshotIdle = m.readU16(); // dshot idle } - _model.config.input.rate[AXIS_ROLL] = rate; - - const uint8_t expo = m.readU8(); - if(_model.config.input.expo[AXIS_PITCH] == _model.config.input.expo[AXIS_ROLL]) + if (m.remain()) { - _model.config.input.expo[AXIS_PITCH] = expo; + m.readU8(); // 32k gyro } - _model.config.input.expo[AXIS_ROLL] = expo; - - for(size_t i = 0; i < AXIS_COUNT_RPY; i++) + if (m.remain()) { - _model.config.input.superRate[i] = m.readU8(); + m.readU8(); // PWM inversion } - _model.config.controller.tpaScale = Utils::clamp(m.readU8(), (uint8_t)0, (uint8_t)90); // dyn thr pid - m.readU8(); // thrMid8 - m.readU8(); // thr expo - _model.config.controller.tpaBreakpoint = Utils::clamp(m.readU16(), (uint16_t)1000, (uint16_t)2000); // tpa breakpoint - if(m.remain() >= 1) + if (m.remain() >= 8) { - _model.config.input.expo[AXIS_YAW] = m.readU8(); // yaw expo + m.readU8(); // gyro_to_use + m.readU8(); // gyro high fsr + m.readU8(); // gyro cal threshold + m.readU16(); // gyro cal duration + m.readU16(); // gyro offset yaw + m.readU8(); // check overflow } - if(m.remain() >= 1) + if (m.remain()) { - _model.config.input.rate[AXIS_YAW] = m.readU8(); // yaw rate + _model.config.debug.mode = m.readU8(); + } + _model.reload(); + break; + + // case MSP_COMPASS_CONFIG: + // r.writeU16(0); // mag_declination * 10 + // break; + + case MSP_FILTER_CONFIG: + r.writeU8(_model.config.gyro.filter.freq); // gyro lpf + r.writeU16(_model.config.dterm.filter.freq); // dterm lpf + r.writeU16(_model.config.yaw.filter.freq); // yaw lpf + r.writeU16(_model.config.gyro.notch1Filter.freq); // gyro notch 1 hz + r.writeU16(_model.config.gyro.notch1Filter.cutoff); // gyro notch 1 cutoff + r.writeU16(_model.config.dterm.notchFilter.freq); // dterm notch hz + r.writeU16(_model.config.dterm.notchFilter.cutoff); // dterm notch cutoff + r.writeU16(_model.config.gyro.notch2Filter.freq); // gyro notch 2 hz + r.writeU16(_model.config.gyro.notch2Filter.cutoff); // gyro notch 2 cutoff + r.writeU8(_model.config.dterm.filter.type); // dterm type + r.writeU8(fromGyroDlpf(_model.config.gyro.dlpf)); + r.writeU8(0); // dlfp 32khz type + r.writeU16(_model.config.gyro.filter.freq); // lowpass1 freq + r.writeU16(_model.config.gyro.filter2.freq); // lowpass2 freq + r.writeU8(_model.config.gyro.filter.type); // lowpass1 type + r.writeU8(_model.config.gyro.filter2.type); // lowpass2 type + r.writeU16(_model.config.dterm.filter2.freq); // dterm lopwass2 freq + // 1.41+ + r.writeU8(_model.config.dterm.filter2.type); // dterm lopwass2 type + r.writeU16(_model.config.gyro.dynLpfFilter.cutoff); // dyn lpf gyro min + r.writeU16(_model.config.gyro.dynLpfFilter.freq); // dyn lpf gyro max + r.writeU16(_model.config.dterm.dynLpfFilter.cutoff); // dyn lpf dterm min + r.writeU16(_model.config.dterm.dynLpfFilter.freq); // dyn lpf dterm max + // gyro analyse + r.writeU8(3); // deprecated dyn notch range + r.writeU8(_model.config.gyro.dynamicFilter.count); // dyn_notch_width_percent + r.writeU16(_model.config.gyro.dynamicFilter.q); // dyn_notch_q + r.writeU16(_model.config.gyro.dynamicFilter.min_freq); // dyn_notch_min_hz + // rpm filter + r.writeU8(_model.config.gyro.rpmFilter.harmonics); // gyro_rpm_notch_harmonics + r.writeU8(_model.config.gyro.rpmFilter.minFreq); // gyro_rpm_notch_min + // 1.43+ + r.writeU16(_model.config.gyro.dynamicFilter.max_freq); // dyn_notch_max_hz + break; + + case MSP_SET_FILTER_CONFIG: + _model.config.gyro.filter.freq = m.readU8(); + _model.config.dterm.filter.freq = m.readU16(); + _model.config.yaw.filter.freq = m.readU16(); + if (m.remain() >= 8) + { + _model.config.gyro.notch1Filter.freq = m.readU16(); + _model.config.gyro.notch1Filter.cutoff = m.readU16(); + _model.config.dterm.notchFilter.freq = m.readU16(); + _model.config.dterm.notchFilter.cutoff = m.readU16(); } - if(m.remain() >= 1) + if (m.remain() >= 4) { - _model.config.input.rate[AXIS_PITCH] = m.readU8(); // pitch rate + _model.config.gyro.notch2Filter.freq = m.readU16(); + _model.config.gyro.notch2Filter.cutoff = m.readU16(); } - if(m.remain() >= 1) + if (m.remain() >= 1) { - _model.config.input.expo[AXIS_PITCH] = m.readU8(); // pitch expo + _model.config.dterm.filter.type = (FilterType)m.readU8(); } - // 1.41 - if(m.remain() >= 2) + if (m.remain() >= 10) { - _model.config.output.throttleLimitType = m.readU8(); // throttle_limit_type - _model.config.output.throttleLimitPercent = m.readU8(); // throttle_limit_percent + m.readU8(); // dlfp type + m.readU8(); // 32k dlfp type + _model.config.gyro.filter.freq = m.readU16(); + _model.config.gyro.filter2.freq = m.readU16(); + _model.config.gyro.filter.type = m.readU8(); + _model.config.gyro.filter2.type = m.readU8(); + _model.config.dterm.filter2.freq = m.readU16(); } - // 1.42 - if(m.remain() >= 6) + // 1.41+ + if (m.remain() >= 9) { - _model.config.input.rateLimit[0] = m.readU16(); // roll - _model.config.input.rateLimit[1] = m.readU16(); // pitch - _model.config.input.rateLimit[2] = m.readU16(); // yaw + _model.config.dterm.filter2.type = m.readU8(); + _model.config.gyro.dynLpfFilter.cutoff = m.readU16(); // dyn gyro lpf min + _model.config.gyro.dynLpfFilter.freq = m.readU16(); // dyn gyro lpf max + _model.config.dterm.dynLpfFilter.cutoff = m.readU16(); // dyn dterm lpf min + _model.config.dterm.dynLpfFilter.freq = m.readU16(); // dyn dterm lpf min } - // 1.43 + if (m.remain() >= 8) + { + m.readU8(); // deprecated dyn_notch_range + _model.config.gyro.dynamicFilter.count = m.readU8(); // dyn_notch_width_percent + _model.config.gyro.dynamicFilter.q = m.readU16(); // dyn_notch_q + _model.config.gyro.dynamicFilter.min_freq = m.readU16(); // dyn_notch_min_hz + _model.config.gyro.rpmFilter.harmonics = m.readU8(); // gyro_rpm_notch_harmonics + _model.config.gyro.rpmFilter.minFreq = m.readU8(); // gyro_rpm_notch_min + } + // 1.43+ if (m.remain() >= 1) { - _model.config.input.rateType = m.readU8(); + _model.config.gyro.dynamicFilter.max_freq = m.readU16(); // dyn_notch_max_hz } - } - else - { - r.result = -1; - // error - } - break; - - case MSP_ADVANCED_CONFIG: - r.writeU8(1); // gyroSync unused - r.writeU8(_model.config.loopSync); - r.writeU8(_model.config.output.async); - r.writeU8(_model.config.output.protocol); - r.writeU16(_model.config.output.rate); - r.writeU16(_model.config.output.dshotIdle); - r.writeU8(0); // 32k gyro - r.writeU8(0); // PWM inversion - r.writeU8(0); // gyro_to_use: {1:0, 2:1. 2:both} - r.writeU8(0); // gyro high fsr (flase) - r.writeU8(48); // gyro cal threshold - r.writeU16(125); // gyro cal duration (1.25s) - r.writeU16(0); // gyro offset yaw - r.writeU8(0); // check overflow - r.writeU8(_model.config.debug.mode); - r.writeU8(DEBUG_COUNT); - break; - - case MSP_SET_ADVANCED_CONFIG: - m.readU8(); // ignore gyroSync, removed in 1.43 - _model.config.loopSync = m.readU8(); - _model.config.output.async = m.readU8(); - _model.config.output.protocol = m.readU8(); - _model.config.output.rate = m.readU16(); - if(m.remain() >= 2) { - _model.config.output.dshotIdle = m.readU16(); // dshot idle - } - if(m.remain()) { - m.readU8(); // 32k gyro - } - if(m.remain()) { - m.readU8(); // PWM inversion - } - if(m.remain() >= 8) { - m.readU8(); // gyro_to_use - m.readU8(); // gyro high fsr - m.readU8(); // gyro cal threshold - m.readU16(); // gyro cal duration - m.readU16(); // gyro offset yaw - m.readU8(); // check overflow - } - if(m.remain()) { - _model.config.debug.mode = m.readU8(); - } - _model.reload(); - break; - - //case MSP_COMPASS_CONFIG: - // r.writeU16(0); // mag_declination * 10 - // break; - - case MSP_FILTER_CONFIG: - r.writeU8(_model.config.gyro.filter.freq); // gyro lpf - r.writeU16(_model.config.dterm.filter.freq); // dterm lpf - r.writeU16(_model.config.yaw.filter.freq); // yaw lpf - r.writeU16(_model.config.gyro.notch1Filter.freq); // gyro notch 1 hz - r.writeU16(_model.config.gyro.notch1Filter.cutoff); // gyro notch 1 cutoff - r.writeU16(_model.config.dterm.notchFilter.freq); // dterm notch hz - r.writeU16(_model.config.dterm.notchFilter.cutoff); // dterm notch cutoff - r.writeU16(_model.config.gyro.notch2Filter.freq); // gyro notch 2 hz - r.writeU16(_model.config.gyro.notch2Filter.cutoff); // gyro notch 2 cutoff - r.writeU8(_model.config.dterm.filter.type); // dterm type - r.writeU8(fromGyroDlpf(_model.config.gyro.dlpf)); - r.writeU8(0); // dlfp 32khz type - r.writeU16(_model.config.gyro.filter.freq); // lowpass1 freq - r.writeU16(_model.config.gyro.filter2.freq); // lowpass2 freq - r.writeU8(_model.config.gyro.filter.type); // lowpass1 type - r.writeU8(_model.config.gyro.filter2.type); // lowpass2 type - r.writeU16(_model.config.dterm.filter2.freq); // dterm lopwass2 freq - // 1.41+ - r.writeU8(_model.config.dterm.filter2.type); // dterm lopwass2 type - r.writeU16(_model.config.gyro.dynLpfFilter.cutoff); // dyn lpf gyro min - r.writeU16(_model.config.gyro.dynLpfFilter.freq); // dyn lpf gyro max - r.writeU16(_model.config.dterm.dynLpfFilter.cutoff); // dyn lpf dterm min - r.writeU16(_model.config.dterm.dynLpfFilter.freq); // dyn lpf dterm max - // gyro analyse - r.writeU8(3); // deprecated dyn notch range - r.writeU8(_model.config.gyro.dynamicFilter.count); // dyn_notch_width_percent - r.writeU16(_model.config.gyro.dynamicFilter.q); // dyn_notch_q - r.writeU16(_model.config.gyro.dynamicFilter.min_freq); // dyn_notch_min_hz - // rpm filter - r.writeU8(_model.config.gyro.rpmFilter.harmonics); // gyro_rpm_notch_harmonics - r.writeU8(_model.config.gyro.rpmFilter.minFreq); // gyro_rpm_notch_min - // 1.43+ - r.writeU16(_model.config.gyro.dynamicFilter.max_freq); // dyn_notch_max_hz - break; - - case MSP_SET_FILTER_CONFIG: - _model.config.gyro.filter.freq = m.readU8(); - _model.config.dterm.filter.freq = m.readU16(); - _model.config.yaw.filter.freq = m.readU16(); - if (m.remain() >= 8) { - _model.config.gyro.notch1Filter.freq = m.readU16(); - _model.config.gyro.notch1Filter.cutoff = m.readU16(); - _model.config.dterm.notchFilter.freq = m.readU16(); - _model.config.dterm.notchFilter.cutoff = m.readU16(); - } - if (m.remain() >= 4) { - _model.config.gyro.notch2Filter.freq = m.readU16(); - _model.config.gyro.notch2Filter.cutoff = m.readU16(); - } - if (m.remain() >= 1) { - _model.config.dterm.filter.type = (FilterType)m.readU8(); - } - if (m.remain() >= 10) { - m.readU8(); // dlfp type - m.readU8(); // 32k dlfp type - _model.config.gyro.filter.freq = m.readU16(); - _model.config.gyro.filter2.freq = m.readU16(); - _model.config.gyro.filter.type = m.readU8(); - _model.config.gyro.filter2.type = m.readU8(); - _model.config.dterm.filter2.freq = m.readU16(); - } - // 1.41+ - if (m.remain() >= 9) { - _model.config.dterm.filter2.type = m.readU8(); - _model.config.gyro.dynLpfFilter.cutoff = m.readU16(); // dyn gyro lpf min - _model.config.gyro.dynLpfFilter.freq = m.readU16(); // dyn gyro lpf max - _model.config.dterm.dynLpfFilter.cutoff = m.readU16(); // dyn dterm lpf min - _model.config.dterm.dynLpfFilter.freq = m.readU16(); // dyn dterm lpf min - } - if (m.remain() >= 8) { - m.readU8(); // deprecated dyn_notch_range - _model.config.gyro.dynamicFilter.count = m.readU8(); // dyn_notch_width_percent - _model.config.gyro.dynamicFilter.q = m.readU16(); // dyn_notch_q - _model.config.gyro.dynamicFilter.min_freq = m.readU16(); // dyn_notch_min_hz - _model.config.gyro.rpmFilter.harmonics = m.readU8(); // gyro_rpm_notch_harmonics - _model.config.gyro.rpmFilter.minFreq = m.readU8(); // gyro_rpm_notch_min - } - // 1.43+ - if (m.remain() >= 1) { - _model.config.gyro.dynamicFilter.max_freq = m.readU16(); // dyn_notch_max_hz - } - _model.reload(); - break; + _model.reload(); + break; - case MSP_PID_CONTROLLER: - r.writeU8(1); // betaflight controller id - break; + case MSP_PID_CONTROLLER: + r.writeU8(1); // betaflight controller id + break; - case MSP_PIDNAMES: - r.writeString(F("ROLL;PITCH;YAW;ALT;Pos;PosR;NavR;LEVEL;MAG;VEL;")); - break; + case MSP_PIDNAMES: + r.writeString(F("ROLL;PITCH;YAW;ALT;Pos;PosR;NavR;LEVEL;MAG;VEL;")); + break; - case MSP_PID: - for(size_t i = 0; i < PID_ITEM_COUNT; i++) - { - r.writeU8(_model.config.pid[i].P); - r.writeU8(_model.config.pid[i].I); - r.writeU8(_model.config.pid[i].D); - } - break; - - case MSP_SET_PID: - for (int i = 0; i < PID_ITEM_COUNT; i++) - { - _model.config.pid[i].P = m.readU8(); - _model.config.pid[i].I = m.readU8(); - _model.config.pid[i].D = m.readU8(); - } - _model.reload(); - break; + case MSP_PID: + for (size_t i = 0; i < PID_ITEM_COUNT; i++) + { + r.writeU8(_model.config.pid[i].P); + r.writeU8(_model.config.pid[i].I); + r.writeU8(_model.config.pid[i].D); + } + break; - case MSP_PID_ADVANCED: /// !!!FINISHED HERE!!! - r.writeU16(0); - r.writeU16(0); - r.writeU16(0); // was pidProfile.yaw_p_limit - r.writeU8(0); // reserved - r.writeU8(0); // vbatPidCompensation; - r.writeU8(0); // feedForwardTransition; - r.writeU8((uint8_t)std::min(_model.config.dterm.setpointWeight, (int16_t)255)); // was low byte of dtermSetpointWeight - r.writeU8(0); // reserved - r.writeU8(0); // reserved - r.writeU8(0); // reserved - r.writeU16(0); // rateAccelLimit; - r.writeU16(0); // yawRateAccelLimit; - r.writeU8(_model.config.level.angleLimit); // levelAngleLimit; - r.writeU8(0); // was pidProfile.levelSensitivity - r.writeU16(0); // itermThrottleThreshold; - r.writeU16(1000); // itermAcceleratorGain; anti_gravity_gain, 0 in 1.45+ - r.writeU16(_model.config.dterm.setpointWeight); - r.writeU8(0); // iterm rotation - r.writeU8(0); // smart feed forward - r.writeU8(_model.config.iterm.relax); // iterm relax - r.writeU8(1); // iterm relax type (setpoint only) - r.writeU8(0); // abs control gain - r.writeU8(0); // throttle boost - r.writeU8(0); // acro trainer max angle - r.writeU16(_model.config.pid[FC_PID_ROLL].F); //pid roll f - r.writeU16(_model.config.pid[FC_PID_PITCH].F); //pid pitch f - r.writeU16(_model.config.pid[FC_PID_YAW].F); //pid yaw f - r.writeU8(0); // antigravity mode - // 1.41+ - r.writeU8(0); // d min roll - r.writeU8(0); // d min pitch - r.writeU8(0); // d min yaw - r.writeU8(0); // d min gain - r.writeU8(0); // d min advance - r.writeU8(0); // use_integrated_yaw - r.writeU8(0); // integrated_yaw_relax - // 1.42+ - r.writeU8(_model.config.iterm.relaxCutoff); // iterm_relax_cutoff - // 1.43+ - r.writeU8(_model.config.output.motorLimit); // motor_output_limit - r.writeU8(0); // auto_profile_cell_count - r.writeU8(0); // idle_min_rpm - break; + case MSP_SET_PID: + for (int i = 0; i < PID_ITEM_COUNT; i++) + { + _model.config.pid[i].P = m.readU8(); + _model.config.pid[i].I = m.readU8(); + _model.config.pid[i].D = m.readU8(); + } + _model.reload(); + break; - case MSP_SET_PID_ADVANCED: - m.readU16(); - m.readU16(); - m.readU16(); // was pidProfile.yaw_p_limit - m.readU8(); // reserved - m.readU8(); - m.readU8(); - _model.config.dterm.setpointWeight = m.readU8(); - m.readU8(); // reserved - m.readU8(); // reserved - m.readU8(); // reserved - m.readU16(); - m.readU16(); - if (m.remain() >= 2) { + case MSP_PID_ADVANCED: /// !!!FINISHED HERE!!! + r.writeU16(0); + r.writeU16(0); + r.writeU16(0); // was pidProfile.yaw_p_limit + r.writeU8(0); // reserved + r.writeU8(0); // vbatPidCompensation; + r.writeU8(0); // feedForwardTransition; + r.writeU8((uint8_t)std::min(_model.config.dterm.setpointWeight, (int16_t)255)); // was low byte of dtermSetpointWeight + r.writeU8(0); // reserved + r.writeU8(0); // reserved + r.writeU8(0); // reserved + r.writeU16(0); // rateAccelLimit; + r.writeU16(0); // yawRateAccelLimit; + r.writeU8(_model.config.level.angleLimit); // levelAngleLimit; + r.writeU8(0); // was pidProfile.levelSensitivity + r.writeU16(0); // itermThrottleThreshold; + r.writeU16(1000); // itermAcceleratorGain; anti_gravity_gain, 0 in 1.45+ + r.writeU16(_model.config.dterm.setpointWeight); + r.writeU8(0); // iterm rotation + r.writeU8(0); // smart feed forward + r.writeU8(_model.config.iterm.relax); // iterm relax + r.writeU8(1); // iterm relax type (setpoint only) + r.writeU8(0); // abs control gain + r.writeU8(0); // throttle boost + r.writeU8(0); // acro trainer max angle + r.writeU16(_model.config.pid[FC_PID_ROLL].F); // pid roll f + r.writeU16(_model.config.pid[FC_PID_PITCH].F); // pid pitch f + r.writeU16(_model.config.pid[FC_PID_YAW].F); // pid yaw f + r.writeU8(0); // antigravity mode + // 1.41+ + r.writeU8(0); // d min roll + r.writeU8(0); // d min pitch + r.writeU8(0); // d min yaw + r.writeU8(0); // d min gain + r.writeU8(0); // d min advance + r.writeU8(0); // use_integrated_yaw + r.writeU8(0); // integrated_yaw_relax + // 1.42+ + r.writeU8(_model.config.iterm.relaxCutoff); // iterm_relax_cutoff + // 1.43+ + r.writeU8(_model.config.output.motorLimit); // motor_output_limit + r.writeU8(0); // auto_profile_cell_count + r.writeU8(0); // idle_min_rpm + break; + + case MSP_SET_PID_ADVANCED: + m.readU16(); + m.readU16(); + m.readU16(); // was pidProfile.yaw_p_limit + m.readU8(); // reserved + m.readU8(); + m.readU8(); + _model.config.dterm.setpointWeight = m.readU8(); + m.readU8(); // reserved + m.readU8(); // reserved + m.readU8(); // reserved + m.readU16(); + m.readU16(); + if (m.remain() >= 2) + { _model.config.level.angleLimit = m.readU8(); m.readU8(); // was pidProfile.levelSensitivity - } - if (m.remain() >= 4) { + } + if (m.remain() >= 4) + { m.readU16(); // itermThrottleThreshold; m.readU16(); // itermAcceleratorGain; anti_gravity_gain - } - if (m.remain() >= 2) { - _model.config.dterm.setpointWeight = m.readU16(); - } - if (m.remain() >= 14) { - m.readU8(); //iterm rotation - m.readU8(); //smart feed forward - _model.config.iterm.relax = m.readU8(); //iterm relax - m.readU8(); //iterm relax type - m.readU8(); //abs control gain - m.readU8(); //throttle boost - m.readU8(); //acro trainer max angle - _model.config.pid[FC_PID_ROLL].F = m.readU16(); // pid roll f - _model.config.pid[FC_PID_PITCH].F = m.readU16(); // pid pitch f - _model.config.pid[FC_PID_YAW].F = m.readU16(); // pid yaw f - m.readU8(); //antigravity mode - } - // 1.41+ - if (m.remain() >= 7) { - m.readU8(); // d min roll - m.readU8(); // d min pitch - m.readU8(); // d min yaw - m.readU8(); // d min gain - m.readU8(); // d min advance - m.readU8(); // use_integrated_yaw - m.readU8(); // integrated_yaw_relax - } - // 1.42+ - if (m.remain() >= 1) { - _model.config.iterm.relaxCutoff = m.readU8(); // iterm_relax_cutoff - } - // 1.43+ - if (m.remain() >= 3) { - _model.config.output.motorLimit = m.readU8(); // motor_output_limit - m.readU8(); // auto_profile_cell_count - m.readU8(); // idle_min_rpm - } - _model.reload(); - break; + } + if (m.remain() >= 2) + { + _model.config.dterm.setpointWeight = m.readU16(); + } + if (m.remain() >= 14) + { + m.readU8(); // iterm rotation + m.readU8(); // smart feed forward + _model.config.iterm.relax = m.readU8(); // iterm relax + m.readU8(); // iterm relax type + m.readU8(); // abs control gain + m.readU8(); // throttle boost + m.readU8(); // acro trainer max angle + _model.config.pid[FC_PID_ROLL].F = m.readU16(); // pid roll f + _model.config.pid[FC_PID_PITCH].F = m.readU16(); // pid pitch f + _model.config.pid[FC_PID_YAW].F = m.readU16(); // pid yaw f + m.readU8(); // antigravity mode + } + // 1.41+ + if (m.remain() >= 7) + { + m.readU8(); // d min roll + m.readU8(); // d min pitch + m.readU8(); // d min yaw + m.readU8(); // d min gain + m.readU8(); // d min advance + m.readU8(); // use_integrated_yaw + m.readU8(); // integrated_yaw_relax + } + // 1.42+ + if (m.remain() >= 1) + { + _model.config.iterm.relaxCutoff = m.readU8(); // iterm_relax_cutoff + } + // 1.43+ + if (m.remain() >= 3) + { + _model.config.output.motorLimit = m.readU8(); // motor_output_limit + m.readU8(); // auto_profile_cell_count + m.readU8(); // idle_min_rpm + } + _model.reload(); + break; - case MSP_RAW_IMU: - for (int i = 0; i < AXIS_COUNT_RPY; i++) - { - r.writeU16(lrintf(_model.state.accel.adc[i] * ACCEL_G_INV * 2048.f)); - } - for (int i = 0; i < AXIS_COUNT_RPY; i++) - { - r.writeU16(lrintf(Utils::toDeg(_model.state.gyro.adc[i]))); - } - for (int i = 0; i < AXIS_COUNT_RPY; i++) - { - r.writeU16(lrintf(_model.state.mag.adc[i] * 1090)); - } - break; + case MSP_RAW_IMU: + for (int i = 0; i < AXIS_COUNT_RPY; i++) + { + r.writeU16(lrintf(_model.state.accel.adc[i] * ACCEL_G_INV * 2048.f)); + } + for (int i = 0; i < AXIS_COUNT_RPY; i++) + { + r.writeU16(lrintf(Utils::toDeg(_model.state.gyro.adc[i]))); + } + for (int i = 0; i < AXIS_COUNT_RPY; i++) + { + r.writeU16(lrintf(_model.state.mag.adc[i] * 1090)); + } + break; - case MSP_MOTOR: - for (size_t i = 0; i < 8; i++) - { - if (i >= OUTPUT_CHANNELS || _model.config.pin[i + PIN_OUTPUT_0] == -1) + case MSP_MOTOR: + for (size_t i = 0; i < 8; i++) { - r.writeU16(0); - continue; + if (i >= OUTPUT_CHANNELS || _model.config.pin[i + PIN_OUTPUT_0] == -1) + { + r.writeU16(0); + continue; + } + r.writeU16(_model.state.output.us[i]); } - r.writeU16(_model.state.output.us[i]); - } - break; + break; - case MSP_MOTOR_TELEMETRY: - r.writeU8(OUTPUT_CHANNELS); - for (size_t i = 0; i < OUTPUT_CHANNELS; i++) - { - int rpm = 0; - uint16_t invalidPct = 0; - uint8_t escTemperature = 0; // degrees celcius - uint16_t escVoltage = 0; // 0.01V per unit - uint16_t escCurrent = 0; // 0.01A per unit - uint16_t escConsumption = 0; // mAh - - if (_model.config.pin[i + PIN_OUTPUT_0] != -1) - { - rpm = lrintf(_model.state.output.telemetry.rpm[i]); - invalidPct = _model.state.output.telemetry.errors[i]; - escTemperature = _model.state.output.telemetry.temperature[i]; - escVoltage = _model.state.output.telemetry.voltage[i]; - escCurrent = _model.state.output.telemetry.current[i]; - } - - r.writeU32(rpm); - r.writeU16(invalidPct); - r.writeU8(escTemperature); - r.writeU16(escVoltage); - r.writeU16(escCurrent); - r.writeU16(escConsumption); - } - break; + case MSP_MOTOR_TELEMETRY: + r.writeU8(OUTPUT_CHANNELS); + for (size_t i = 0; i < OUTPUT_CHANNELS; i++) + { + int rpm = 0; + uint16_t invalidPct = 0; + uint8_t escTemperature = 0; // degrees celcius + uint16_t escVoltage = 0; // 0.01V per unit + uint16_t escCurrent = 0; // 0.01A per unit + uint16_t escConsumption = 0; // mAh + + if (_model.config.pin[i + PIN_OUTPUT_0] != -1) + { + rpm = lrintf(_model.state.output.telemetry.rpm[i]); + invalidPct = _model.state.output.telemetry.errors[i]; + escTemperature = _model.state.output.telemetry.temperature[i]; + escVoltage = _model.state.output.telemetry.voltage[i]; + escCurrent = _model.state.output.telemetry.current[i]; + } - case MSP_SET_MOTOR: - if(_model.isFeatureActive(FEATURE_GPS) && _model.config.blackbox.mode > 0) - { - _model.setGpsHome(true); - } - for(size_t i = 0; i < OUTPUT_CHANNELS; i++) - { - _model.state.output.disarmed[i] = m.readU16(); - } - break; + r.writeU32(rpm); + r.writeU16(invalidPct); + r.writeU8(escTemperature); + r.writeU16(escVoltage); + r.writeU16(escCurrent); + r.writeU16(escConsumption); + } + break; - case MSP_SERVO: - for(size_t i = 0; i < OUTPUT_CHANNELS; i++) - { - if (i >= OUTPUT_CHANNELS || _model.config.pin[i + PIN_OUTPUT_0] == -1) + case MSP_SET_MOTOR: + if (_model.isFeatureActive(FEATURE_GPS) && _model.config.blackbox.mode > 0) { - r.writeU16(1500); - continue; + _model.setGpsHome(true); } - r.writeU16(_model.state.output.us[i]); - } - break; + for (size_t i = 0; i < OUTPUT_CHANNELS; i++) + { + _model.state.output.disarmed[i] = m.readU16(); + } + break; - case MSP_SERVO_CONFIGURATIONS: - for(size_t i = 0; i < 8; i++) - { - if(i < OUTPUT_CHANNELS) + case MSP_SERVO: + for (size_t i = 0; i < OUTPUT_CHANNELS; i++) { - r.writeU16(_model.config.output.channel[i].min); - r.writeU16(_model.config.output.channel[i].max); - r.writeU16(_model.config.output.channel[i].neutral); + if (i >= OUTPUT_CHANNELS || _model.config.pin[i + PIN_OUTPUT_0] == -1) + { + r.writeU16(1500); + continue; + } + r.writeU16(_model.state.output.us[i]); } - else + break; + + case MSP_SERVO_CONFIGURATIONS: + for (size_t i = 0; i < 8; i++) { - r.writeU16(1000); - r.writeU16(2000); - r.writeU16(1500); + if (i < OUTPUT_CHANNELS) + { + r.writeU16(_model.config.output.channel[i].min); + r.writeU16(_model.config.output.channel[i].max); + r.writeU16(_model.config.output.channel[i].neutral); + } + else + { + r.writeU16(1000); + r.writeU16(2000); + r.writeU16(1500); + } + r.writeU8(100); + r.writeU8(-1); + r.writeU32(0); } - r.writeU8(100); - r.writeU8(-1); - r.writeU32(0); - } - break; + break; - case MSP_SET_SERVO_CONFIGURATION: + case MSP_SET_SERVO_CONFIGURATION: { uint8_t i = m.readU8(); - if(i < OUTPUT_CHANNELS) + if (i < OUTPUT_CHANNELS) { _model.config.output.channel[i].min = m.readU16(); _model.config.output.channel[i].max = m.readU16(); @@ -1385,81 +1509,91 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD } break; - case MSP_ACC_CALIBRATION: - if(!_model.isModeActive(MODE_ARMED)) _model.calibrateGyro(); - break; + case MSP_ACC_CALIBRATION: + if (!_model.isModeActive(MODE_ARMED)) + _model.calibrateGyro(); + break; - case MSP_MAG_CALIBRATION: - if(!_model.isModeActive(MODE_ARMED)) _model.calibrateMag(); - break; + case MSP_MAG_CALIBRATION: + if (!_model.isModeActive(MODE_ARMED)) + _model.calibrateMag(); + break; - case MSP_VTX_CONFIG: - if (!_model.state.vtx.active) { - r.writeU8(0); // vtx type - r.writeU8(0); // band - r.writeU8(0); // channel - r.writeU8(0); // power - r.writeU8(0); // status - r.writeU16(0); // freq - r.writeU8(0); // ready - r.writeU8(0); // low power disarm - } else { - r.writeU8(3 /* SMARTAUDIO */); // vtx type unknown - r.writeU8(_model.config.vtx.band); // band - r.writeU8(_model.config.vtx.channel); // channel - r.writeU8(_model.config.vtx.power); // power - r.writeU8(0); // status (looks like 1 means pit mode :shrug:) - r.writeU16(0); // freq - r.writeU8(1); // ready - r.writeU8(_model.config.vtx.lowPowerDisarm); // low power disarm - } - // 1.42 - r.writeU16(0); // pit mode freq - r.writeU8(0); // vtx table available (no) - r.writeU8(0); // vtx table bands - r.writeU8(0); // vtx table channels - r.writeU8(0); // vtx power levels - break; - - case MSP_SET_VTX_CONFIG: + case MSP_VTX_CONFIG: + if (!_model.state.vtx.active) + { + r.writeU8(0); // vtx type + r.writeU8(0); // band + r.writeU8(0); // channel + r.writeU8(0); // power + r.writeU8(0); // status + r.writeU16(0); // freq + r.writeU8(0); // ready + r.writeU8(0); // low power disarm + } + else + { + r.writeU8(3 /* SMARTAUDIO */); // vtx type unknown + r.writeU8(_model.config.vtx.band); // band + r.writeU8(_model.config.vtx.channel); // channel + r.writeU8(_model.config.vtx.power); // power + r.writeU8(0); // status (looks like 1 means pit mode :shrug:) + r.writeU16(0); // freq + r.writeU8(1); // ready + r.writeU8(_model.config.vtx.lowPowerDisarm); // low power disarm + } + // 1.42 + r.writeU16(0); // pit mode freq + r.writeU8(0); // vtx table available (no) + r.writeU8(0); // vtx table bands + r.writeU8(0); // vtx table channels + r.writeU8(0); // vtx power levels + break; + + case MSP_SET_VTX_CONFIG: { uint16_t freq = m.readU16(); - if (freq <= VTXCOMMON_MSP_BANDCHAN_CHKVAL) { // Value is band and channel - //const uint8_t newBand = (freq / 8) + 1; - //const uint8_t newChannel = (freq % 8) + 1; + if (freq <= VTXCOMMON_MSP_BANDCHAN_CHKVAL) + { // Value is band and channel + // const uint8_t newBand = (freq / 8) + 1; + // const uint8_t newChannel = (freq % 8) + 1; } - if (m.remain() >= 2) { - _model.config.vtx.power = m.readU8(); - /*const uint8_t newPitmode = */m.readU8(); + if (m.remain() >= 2) + { + _model.config.vtx.power = m.readU8(); + /*const uint8_t newPitmode = */ m.readU8(); } - if (m.remain()) { + if (m.remain()) + { _model.config.vtx.lowPowerDisarm = m.readU8(); } // API version 1.42 - this parameter kept separate since clients may already be supplying - if (m.remain() >= 2) { - /*const uint16_t pitModeFreq = */m.readU16(); + if (m.remain() >= 2) + { + /*const uint16_t pitModeFreq = */ m.readU16(); } // API version 1.42 - extensions for non-encoded versions of the band, channel or frequency - if (m.remain() >= 4) { + if (m.remain() >= 4) + { // Added standalone values for band, channel and frequency to move // away from the flawed encoded combined method originally implemented. - _model.config.vtx.band = m.readU8(); + _model.config.vtx.band = m.readU8(); _model.config.vtx.channel = m.readU8(); - /*uint16_t newFreq = */m.readU16(); + /*uint16_t newFreq = */ m.readU16(); } } break; - - case MSP_SET_ARMING_DISABLED: + case MSP_SET_ARMING_DISABLED: { const uint8_t cmd = m.readU8(); uint8_t disableRunawayTakeoff = 0; - if(m.remain()) { + if (m.remain()) + { disableRunawayTakeoff = m.readU8(); } (void)disableRunawayTakeoff; @@ -1467,232 +1601,261 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD (void)cmd; #warning "Danger macro used ESPFC_DEV_PRESET_UNSAFE_ARMING" #else - _model.setArmingDisabled(ARMING_DISABLED_MSP, cmd); + _model.setArmingDisabled(ARMING_DISABLED_MSP, cmd); #endif - if (_model.isModeActive(MODE_ARMED)) _model.disarm(DISARM_REASON_ARMING_DISABLED); + if (_model.isModeActive(MODE_ARMED)) + _model.disarm(DISARM_REASON_ARMING_DISABLED); } break; - case MSP_SET_PASSTHROUGH: + case MSP_SET_PASSTHROUGH: { uint8_t ptMode = MSP_PASSTHROUGH_ESC_4WAY; uint8_t ptArg = 0; - if(m.remain() >= 2) { + if (m.remain() >= 2) + { ptMode = m.readU8(); ptArg = m.readU8(); } switch (ptMode) { - case MSP_PASSTHROUGH_ESC_4WAY: - r.writeU8(esc4wayInit()); - serialDeviceInit(&s, 0); - _postCommand = std::bind(&MspProcessor::processEsc4way, this); - break; - default: - r.writeU8(0); - break; + case MSP_PASSTHROUGH_ESC_4WAY: + r.writeU8(esc4wayInit()); + serialDeviceInit(&s, 0); + _postCommand = std::bind(&MspProcessor::processEsc4way, this); + break; + default: + r.writeU8(0); + break; } (void)ptArg; } break; - case MSP_DEBUG: - for (int i = 0; i < 8; i++) { - r.writeU16(_model.state.debug[i]); - } - break; - - case MSP_SET_GPS_CONFIG: - m.readU8(); // provider - m.readU8(); // sbas mode - m.readU8(); // auto config - m.readU8(); // auto baud - if (m.remain() >= 2) { + case MSP_DEBUG: + for (int i = 0; i < 8; i++) + { + r.writeU16(_model.state.debug[i]); + } + break; + + case MSP_SET_GPS_CONFIG: + m.readU8(); // provider + m.readU8(); // sbas mode + m.readU8(); // auto config + m.readU8(); // auto baud + if (m.remain() >= 2) + { // Added in API version 1.43 _model.config.gps.setHomeOnce = m.readU8(); // gps_set_home_point_once - m.readU8(); // gps_ublox_use_galileo - } - break; - - case MSP_GPS_CONFIG: - r.writeU8(1); // provider - r.writeU8(0); // sbasMode, 0: auto - r.writeU8(1); // autoConfig, 0: off, 1: on - r.writeU8(1); // autoBaud, 0: off, 1: on - // Added in API version 1.43 - r.writeU8(_model.config.gps.setHomeOnce); // gps_set_home_point_once - r.writeU8(1); // gps_ublox_use_galileo - break; - - case MSP_RAW_GPS: - r.writeU8(_model.state.gps.fixType > 2); // STATE(GPS_FIX)); - r.writeU8(_model.state.gps.numSats); // numSat - r.writeU32(_model.state.gps.location.raw.lat); // lat - r.writeU32(_model.state.gps.location.raw.lon); // lon - r.writeU16(std::clamp((int)_model.state.gps.location.raw.height / 1000, 0, (int)std::numeric_limits::max())); // height [m] - r.writeU16(_model.state.gps.velocity.raw.groundSpeed / 10); // cm/s - r.writeU16(_model.state.gps.velocity.raw.heading / 10000); // deg * 10 - // Added in API version 1.44 - r.writeU16(_model.state.gps.accuracy.pDop); // pDOP - break; + m.readU8(); // gps_ublox_use_galileo + } + break; + + case MSP_GPS_CONFIG: + r.writeU8(1); // provider + r.writeU8(0); // sbasMode, 0: auto + r.writeU8(1); // autoConfig, 0: off, 1: on + r.writeU8(1); // autoBaud, 0: off, 1: on + // Added in API version 1.43 + r.writeU8(_model.config.gps.setHomeOnce); // gps_set_home_point_once + r.writeU8(1); // gps_ublox_use_galileo + break; + + case MSP_RAW_GPS: + r.writeU8(_model.state.gps.fixType > 2); // STATE(GPS_FIX)); + r.writeU8(_model.state.gps.numSats); // numSat + r.writeU32(_model.state.gps.location.raw.lat); // lat + r.writeU32(_model.state.gps.location.raw.lon); // lon + r.writeU16(std::clamp((int)_model.state.gps.location.raw.height / 1000, 0, (int)std::numeric_limits::max())); // height [m] + r.writeU16(_model.state.gps.velocity.raw.groundSpeed / 10); // cm/s + r.writeU16(_model.state.gps.velocity.raw.heading / 10000); // deg * 10 + // Added in API version 1.44 + r.writeU16(_model.state.gps.accuracy.pDop); // pDOP + break; + + case MSP_COMP_GPS: + r.writeU16(0); // GPS_distanceToHome + r.writeU16(0); // GPS_directionToHome / 10 // resolution increased in Betaflight 4.4 by factor of 10, this maintains backwards compatibility for DJI OSD + r.writeU8(0); // GPS_update & 1 // direct or msp + break; + + case MSP_GPSSVINFO: + r.writeU8(_model.state.gps.numCh); // GPS_numCh + for (size_t i = 0; i < _model.state.gps.numCh; i++) + { + r.writeU8(_model.state.gps.svinfo[i].gnssId); // GPS_svinfo_chn[i] + r.writeU8(_model.state.gps.svinfo[i].id); // GPS_svinfo_svid[i] + r.writeU8(static_cast(_model.state.gps.svinfo[i].quality.value & 0xff)); // GPS_svinfo_quality[i] + r.writeU8(_model.state.gps.svinfo[i].cno); // GPS_svinfo_cno[i] + } + break; - case MSP_COMP_GPS: - r.writeU16(0); // GPS_distanceToHome - r.writeU16(0); // GPS_directionToHome / 10 // resolution increased in Betaflight 4.4 by factor of 10, this maintains backwards compatibility for DJI OSD - r.writeU8(0); // GPS_update & 1 // direct or msp - break; + case MSP_EEPROM_WRITE: + _model.save(); + break; - case MSP_GPSSVINFO: - r.writeU8(_model.state.gps.numCh); // GPS_numCh - for (size_t i = 0; i < _model.state.gps.numCh; i++) { - r.writeU8(_model.state.gps.svinfo[i].gnssId); // GPS_svinfo_chn[i] - r.writeU8(_model.state.gps.svinfo[i].id); // GPS_svinfo_svid[i] - r.writeU8(static_cast(_model.state.gps.svinfo[i].quality.value & 0xff)); // GPS_svinfo_quality[i] - r.writeU8(_model.state.gps.svinfo[i].cno); // GPS_svinfo_cno[i] - } - break; + case MSP_RESET_CONF: + if (!_model.isModeActive(MODE_ARMED)) + { + _model.reset(); + _model.save(); + } + break; - case MSP_EEPROM_WRITE: - _model.save(); - break; + case MSP_REBOOT: + r.writeU8(0); // reboot to firmware + _postCommand = std::bind(&MspProcessor::processRestart, this); + break; - case MSP_RESET_CONF: - if(!_model.isModeActive(MODE_ARMED)) - { - _model.reset(); - _model.save(); + default: + r.result = 0; + break; } - break; - - case MSP_REBOOT: - r.writeU8(0); // reboot to firmware - _postCommand = std::bind(&MspProcessor::processRestart, this); - break; - - default: - r.result = 0; - break; - } -} + } -void MspProcessor::processEsc4way() -{ + void MspProcessor::processEsc4way() + { #if defined(ESPFC_MULTI_CORE) && defined(ESPFC_FREE_RTOS) - timer_pause(TIMER_GROUP_0, TIMER_0); + timer_pause(TIMER_GROUP_0, TIMER_0); #endif - esc4wayProcess(getSerialPort()); - processRestart(); -} + esc4wayProcess(getSerialPort()); + processRestart(); + } -void MspProcessor::processRestart() -{ - Hardware::restart(_model); -} + void MspProcessor::processRestart() + { + Hardware::restart(_model); + } -void MspProcessor::serializeFlashData(MspResponse& r, uint32_t address, const uint16_t size, bool useLegacyFormat, bool allowCompression) -{ + void MspProcessor::serializeFlashData(MspResponse &r, uint32_t address, const uint16_t size, bool useLegacyFormat, bool allowCompression) + { #ifdef USE_FLASHFS - (void)allowCompression; // not supported + (void)allowCompression; // not supported - const uint32_t allowedToRead = r.remain() - 16; - const uint32_t flashfsSize = flashfsGetSize(); + const uint32_t allowedToRead = r.remain() - 16; + const uint32_t flashfsSize = flashfsGetSize(); - uint16_t readLen = std::min(std::min((uint32_t)size, allowedToRead), flashfsSize - address); + uint16_t readLen = std::min(std::min((uint32_t)size, allowedToRead), flashfsSize - address); - r.writeU32(address); + r.writeU32(address); - uint16_t *readLenPtr = (uint16_t*)&r.data[r.len]; - if (!useLegacyFormat) - { - // new format supports variable read lengths - r.writeU16(readLen); - r.writeU8(0); // NO_COMPRESSION - } + uint16_t *readLenPtr = (uint16_t *)&r.data[r.len]; + if (!useLegacyFormat) + { + // new format supports variable read lengths + r.writeU16(readLen); + r.writeU8(0); // NO_COMPRESSION + } - const size_t bytesRead = flashfsReadAbs(address, &r.data[r.len], readLen); - r.advance(bytesRead); + const size_t bytesRead = flashfsReadAbs(address, &r.data[r.len], readLen); + r.advance(bytesRead); - if (!useLegacyFormat) - { - // update the 'read length' with the actual amount read from flash. - *readLenPtr = bytesRead; - } - else - { - // pad the buffer with zeros - //for (int i = bytesRead; i < allowedToRead; i++) r.writeU8(0); - } + if (!useLegacyFormat) + { + // update the 'read length' with the actual amount read from flash. + *readLenPtr = bytesRead; + } + else + { + // pad the buffer with zeros + // for (int i = bytesRead; i < allowedToRead; i++) r.writeU8(0); + } #endif -} - -void MspProcessor::sendResponse(MspResponse& r, Device::SerialDevice& s) -{ - debugResponse(r); - uint8_t buff[256]; - size_t len = r.serialize(buff, 256); - s.write(buff, len); -} - -void MspProcessor::postCommand() -{ - if(!_postCommand) return; - std::function cb = _postCommand; - _postCommand = {}; - cb(); -} - -bool MspProcessor::debugSkip(uint8_t cmd) -{ - //return true; - //return false; - if(cmd == MSP_STATUS) return true; - if(cmd == MSP_STATUS_EX) return true; - if(cmd == MSP_BOXNAMES) return true; - if(cmd == MSP_ANALOG) return true; - if(cmd == MSP_ATTITUDE) return true; - if(cmd == MSP_ALTITUDE) return true; - if(cmd == MSP_RC) return true; - if(cmd == MSP_RAW_IMU) return true; - if(cmd == MSP_MOTOR) return true; - if(cmd == MSP_SERVO) return true; - if(cmd == MSP_BATTERY_STATE) return true; - if(cmd == MSP_VOLTAGE_METERS) return true; - if(cmd == MSP_CURRENT_METERS) return true; - return false; -} - -void MspProcessor::debugMessage(const MspMessage& m) -{ - if(debugSkip(m.cmd)) return; - Device::SerialDevice * s = _model.getSerialStream(SERIAL_FUNCTION_TELEMETRY_HOTT); - if(!s) return; - - s->print(m.dir == MSP_TYPE_REPLY ? '>' : '<'); - s->print(m.cmd); s->print('.'); - s->print(m.expected); s->print(' '); - for(size_t i = 0; i < m.expected; i++) - { - s->print(m.buffer[i], HEX); s->print(' '); - } - s->println(); -} + } + + void MspProcessor::sendResponse(MspResponse &r, Device::SerialDevice &s) + { + debugResponse(r); + uint8_t buff[256]; + size_t len = r.serialize(buff, 256); + s.write(buff, len); + } + + void MspProcessor::postCommand() + { + if (!_postCommand) + return; + std::function cb = _postCommand; + _postCommand = {}; + cb(); + } + + bool MspProcessor::debugSkip(uint8_t cmd) + { + // return true; + // return false; + if (cmd == MSP_STATUS) + return true; + if (cmd == MSP_STATUS_EX) + return true; + if (cmd == MSP_BOXNAMES) + return true; + if (cmd == MSP_ANALOG) + return true; + if (cmd == MSP_ATTITUDE) + return true; + if (cmd == MSP_ALTITUDE) + return true; + if (cmd == MSP_RC) + return true; + if (cmd == MSP_RAW_IMU) + return true; + if (cmd == MSP_MOTOR) + return true; + if (cmd == MSP_SERVO) + return true; + if (cmd == MSP_BATTERY_STATE) + return true; + if (cmd == MSP_VOLTAGE_METERS) + return true; + if (cmd == MSP_CURRENT_METERS) + return true; + return false; + } + + void MspProcessor::debugMessage(const MspMessage &m) + { + if (debugSkip(m.cmd)) + return; + Device::SerialDevice *s = _model.getSerialStream(SERIAL_FUNCTION_TELEMETRY_HOTT); + if (!s) + return; + + s->print(m.dir == MSP_TYPE_REPLY ? '>' : '<'); + s->print(m.cmd); + s->print('.'); + s->print(m.expected); + s->print(' '); + for (size_t i = 0; i < m.expected; i++) + { + s->print(m.buffer[i], HEX); + s->print(' '); + } + s->println(); + } + + void MspProcessor::debugResponse(const MspResponse &r) + { + if (debugSkip(r.cmd)) + return; + Device::SerialDevice *s = _model.getSerialStream(SERIAL_FUNCTION_TELEMETRY_HOTT); + if (!s) + return; + + s->print(r.result == 1 ? '>' : (r.result == -1 ? '!' : '@')); + s->print(r.cmd); + s->print('.'); + s->print(r.len); + s->print(' '); + for (size_t i = 0; i < r.len; i++) + { + s->print(r.data[i], HEX); + s->print(' '); + } + s->println(); + } -void MspProcessor::debugResponse(const MspResponse& r) -{ - if(debugSkip(r.cmd)) return; - Device::SerialDevice * s = _model.getSerialStream(SERIAL_FUNCTION_TELEMETRY_HOTT); - if(!s) return; - - s->print(r.result == 1 ? '>' : (r.result == -1 ? '!' : '@')); - s->print(r.cmd); s->print('.'); - s->print(r.len); s->print(' '); - for(size_t i = 0; i < r.len; i++) - { - s->print(r.data[i], HEX); s->print(' '); } - s->println(); -} - -} } diff --git a/lib/Espfc/src/Control/Actuator.cpp b/lib/Espfc/src/Control/Actuator.cpp index 3f346fba..37cd3338 100644 --- a/lib/Espfc/src/Control/Actuator.cpp +++ b/lib/Espfc/src/Control/Actuator.cpp @@ -96,6 +96,20 @@ void Actuator::updateArmingDisabled() _model.setArmingDisabled(ARMING_DISABLED_CALIBRATING, _model.calibrationActive()); _model.setArmingDisabled(ARMING_DISABLED_MOTOR_PROTOCOL, _model.config.output.protocol == ESC_PROTOCOL_DISABLED); _model.setArmingDisabled(ARMING_DISABLED_REBOOT_REQUIRED, _model.state.mode.rescueConfigMode == RESCUE_CONFIG_ACTIVE); + + // Check small angle - prevent arming if tilted beyond configured angle + if(_model.config.arming.smallAngle < 180 && _model.accelActive()) + { + const float maxTiltRad = Utils::toRad(_model.config.arming.smallAngle); + const float roll = _model.state.attitude.euler[AXIS_ROLL]; + const float pitch = _model.state.attitude.euler[AXIS_PITCH]; + const float currentTilt = acosf(cosf(roll) * cosf(pitch)); + _model.setArmingDisabled(ARMING_DISABLED_ANGLE, currentTilt > maxTiltRad); + } + else + { + _model.setArmingDisabled(ARMING_DISABLED_ANGLE, false); + } if(_model.isFeatureActive(FEATURE_GPS)) { _model.setArmingDisabled(ARMING_DISABLED_GPS, !_model.state.gps.present || _model.state.gps.numSats < _model.config.gps.minSats); diff --git a/lib/Espfc/src/ModelConfig.h b/lib/Espfc/src/ModelConfig.h index 198af605..1c2baf2b 100644 --- a/lib/Espfc/src/ModelConfig.h +++ b/lib/Espfc/src/ModelConfig.h @@ -591,6 +591,7 @@ struct AccelConfig int8_t bus = BUS_AUTO; int8_t dev = GYRO_AUTO; int16_t bias[3] = { 0, 0, 0 }; + int16_t trim[2] = { 0, 0 }; FilterConfig filter{FILTER_BIQUAD, 15}; }; @@ -678,6 +679,11 @@ struct LedConfig int8_t type = 0; }; +struct ArmingConfig +{ + uint8_t smallAngle = 25; +}; + // persistent data class ModelConfig { @@ -694,6 +700,7 @@ class ModelConfig IBatConfig ibat; VtxConfig vtx; GpsConfig gps; + ArmingConfig arming; ActuatorCondition conditions[ACTUATOR_CONDITIONS]; ScalerConfig scaler[SCALER_COUNT]; diff --git a/lib/Espfc/src/ModelState.h b/lib/Espfc/src/ModelState.h index db3bc254..aed047b1 100644 --- a/lib/Espfc/src/ModelState.h +++ b/lib/Espfc/src/ModelState.h @@ -16,47 +16,51 @@ #include "Connect/Msp.hpp" #include "Connect/StatusLed.hpp" -namespace Espfc { +namespace Espfc +{ -constexpr size_t DEBUG_VALUE_COUNT = 8; -constexpr size_t CLI_BUFF_SIZE = 128; -constexpr size_t CLI_ARGS_SIZE = 12; + constexpr size_t DEBUG_VALUE_COUNT = 8; + constexpr size_t CLI_BUFF_SIZE = 128; + constexpr size_t CLI_ARGS_SIZE = 12; -class CliCmd -{ + class CliCmd + { public: - CliCmd(): buff{0}, index{0} { + CliCmd() : buff{0}, index{0} + { std::fill_n(args, CLI_ARGS_SIZE, nullptr); } - const char * args[CLI_ARGS_SIZE]; + const char *args[CLI_ARGS_SIZE]; char buff[CLI_BUFF_SIZE]; size_t index; -}; + }; -class SerialPortState -{ + class SerialPortState + { public: Connect::MspMessage mspRequest; Connect::MspResponse mspResponse; CliCmd cliCmd; - Device::SerialDevice * stream; -}; + Device::SerialDevice *stream; + }; -class BuzzerState -{ + class BuzzerState + { public: - BuzzerState(): idx(0) {} + BuzzerState() : idx(0) {} void play(BuzzerEvent e) // play continously, repeat while condition is true { - if(!empty()) return; + if (!empty()) + return; push(e); } void push(BuzzerEvent e) // play once { - if(full()) return; - if(beeperMask & (1 << (e - 1))) + if (full()) + return; + if (beeperMask & (1 << (e - 1))) { events[idx++] = e; } @@ -64,7 +68,8 @@ class BuzzerState BuzzerEvent pop() { - if(empty()) return BUZZER_SILENCE; + if (empty()) + return BUZZER_SILENCE; return events[--idx]; } @@ -82,14 +87,15 @@ class BuzzerState BuzzerEvent events[BUZZER_MAX_EVENTS]; size_t idx; int32_t beeperMask; -}; + }; -class BatteryState -{ + class BatteryState + { public: bool warn(int vbatCellWarning) const { - if(voltage < 2.0) return false; // no battery connected + if (voltage < 2.0) + return false; // no battery connected return !samples && cellVoltage < vbatCellWarning * 0.01f; } @@ -104,426 +110,432 @@ class BatteryState int8_t cells; int8_t samples; Utils::Timer timer; -}; - -enum CalibrationState { - CALIBRATION_IDLE = 0, - CALIBRATION_START = 1, - CALIBRATION_UPDATE = 2, - CALIBRATION_APPLY = 3, - CALIBRATION_SAVE = 4, -}; - -enum FailsafePhase { - FC_FAILSAFE_IDLE = 0, - FC_FAILSAFE_RX_LOSS_DETECTED, - FC_FAILSAFE_LANDING, - FC_FAILSAFE_LANDED, - FC_FAILSAFE_RX_LOSS_MONITORING, - FC_FAILSAFE_RX_LOSS_RECOVERED -}; - -class FailsafeState -{ + }; + + enum CalibrationState + { + CALIBRATION_IDLE = 0, + CALIBRATION_START = 1, + CALIBRATION_UPDATE = 2, + CALIBRATION_APPLY = 3, + CALIBRATION_SAVE = 4, + }; + + enum FailsafePhase + { + FC_FAILSAFE_IDLE = 0, + FC_FAILSAFE_RX_LOSS_DETECTED, + FC_FAILSAFE_LANDING, + FC_FAILSAFE_LANDED, + FC_FAILSAFE_RX_LOSS_MONITORING, + FC_FAILSAFE_RX_LOSS_RECOVERED + }; + + class FailsafeState + { public: FailsafePhase phase; uint32_t timeout; -}; - -constexpr float ACCEL_G = 9.80665f; -constexpr float ACCEL_G_INV = 1.f / ACCEL_G; - -enum RescueConfigMode { - RESCUE_CONFIG_PENDING, - RESCUE_CONFIG_ACTIVE, - RESCUE_CONFIG_DISABLED, -}; - -struct OutputTelemetryState -{ - int16_t errors[OUTPUT_CHANNELS]; - int32_t errorsSum[OUTPUT_CHANNELS]; - int32_t errorsCount[OUTPUT_CHANNELS]; - - uint32_t erpm[OUTPUT_CHANNELS]; - float rpm[OUTPUT_CHANNELS]; - float freq[OUTPUT_CHANNELS]; - - int8_t temperature[OUTPUT_CHANNELS]; - int8_t voltage[OUTPUT_CHANNELS]; - int8_t current[OUTPUT_CHANNELS]; - int8_t debug1[OUTPUT_CHANNELS]; - int8_t debug2[OUTPUT_CHANNELS]; - int8_t debug3[OUTPUT_CHANNELS]; - int8_t events[OUTPUT_CHANNELS]; -}; - -struct OutputState -{ - float ch[OUTPUT_CHANNELS]; - int16_t us[OUTPUT_CHANNELS]; - int16_t disarmed[OUTPUT_CHANNELS]; - bool saturated; - OutputTelemetryState telemetry; -}; - -struct InputState -{ - size_t channelCount; - bool channelsValid; - bool rxLoss; - bool rxFailSafe; - - uint32_t frameTime; - uint32_t frameDelta; - uint32_t frameRate; - uint32_t frameCount; - uint32_t lossTime; - - float interpolationDelta; - float interpolationStep; - float autoFactor; - float autoFreq; - - int16_t raw[INPUT_CHANNELS]; - int16_t buffer[INPUT_CHANNELS]; - int16_t bufferPrevious[INPUT_CHANNELS]; - - float us[INPUT_CHANNELS]; - float ch[INPUT_CHANNELS]; - - Utils::Filter filter[AXIS_COUNT_RPYT]; - - Utils::Timer timer; -}; - -struct MixerState -{ - Utils::Timer timer; - float minThrottle; - float maxThrottle; - bool digitalOutput; - - EscDriver * escMotor; - EscDriver * escServo; -}; - -struct MagState -{ - Device::MagDevice* dev; - bool present; - int rate; - - VectorInt16 raw; - VectorFloat adc; - Utils::Filter filter[3]; - Utils::Timer timer; - - int calibrationSamples; - int calibrationState; - bool calibrationValid; - VectorFloat calibrationMin; - VectorFloat calibrationMax; - VectorFloat calibrationScale; - VectorFloat calibrationOffset; - - VectorFloat pose; -}; - -struct BaroState -{ - Device::BaroDevice* dev; - bool present; - int32_t rate; - - float temperatureRaw; - float temperature; - float pressureRaw; - float pressure; - float altitudeRaw; - float altitude; - float altitudeGround; - float altitudeBias; - float altitudePrev; - float vario; - int32_t altitudeBiasSamples; -}; - -struct GyroState -{ - Device::GyroDevice* dev; - bool present; - int32_t rate; - int32_t clock = 1000; - - VectorInt16 raw; - VectorFloat adc; - VectorFloat sampled; - VectorFloat scaled; - VectorFloat dynNotch; - - float scale; - VectorFloat bias; - float biasAlpha; - int biasSamples; - int calibrationState; - int calibrationRate; - - Utils::Filter filter[AXIS_COUNT_RPY]; - Utils::Filter filter2[AXIS_COUNT_RPY]; - Utils::Filter filter3[AXIS_COUNT_RPY]; - Utils::Filter notch1Filter[AXIS_COUNT_RPY]; - Utils::Filter notch2Filter[AXIS_COUNT_RPY]; - Utils::Filter dynNotchFilter[DYN_NOTCH_COUNT_MAX][AXIS_COUNT_RPY]; - Utils::Filter rpmFilter[RPM_FILTER_MOTOR_MAX][RPM_FILTER_HARMONICS_MAX][AXIS_COUNT_RPY]; - Utils::Filter rpmFreqFilter[RPM_FILTER_MOTOR_MAX]; - - Utils::Timer timer; - Utils::Timer dynamicFilterTimer; -}; - -struct AccelState -{ - bool present; - VectorInt16 raw; - VectorFloat adc; - VectorFloat prev; - Utils::Filter filter[AXIS_COUNT_RPY]; - Utils::Timer timer; - - VectorFloat world; - - float scale; - VectorFloat bias; - float biasAlpha; - int biasSamples; - int calibrationState; -}; - -struct AttitudeState -{ - VectorFloat rate; - Utils::Filter filter[AXIS_COUNT_RPY]; - VectorFloat euler; - Quaternion quaternion; - float cosTheta; -}; - -struct SetpointState -{ - VectorFloat angle; - float rate[AXIS_COUNT_RPYT]; -}; - -struct ModeState -{ - uint32_t mask; - uint32_t maskPrev; - uint32_t maskSwitch; - uint32_t maskPresent; - uint32_t disarmReason; - uint32_t armingDisabledFlags; - RescueConfigMode rescueConfigMode; - bool airmodeAllowed; - uint32_t button; - bool isSingleClickActive() const { return button & (1 << 0); } - bool isDoubleClickActive() const { return button & (1 << 1); } - bool isLongClickActive() const { return button & (1 << 2); } -}; - -struct AltitudeState -{ - float height; - float vario; -}; - -struct VtxState -{ - uint8_t active = false; -}; + }; + + constexpr float ACCEL_G = 9.80665f; + constexpr float ACCEL_G_INV = 1.f / ACCEL_G; + + enum RescueConfigMode + { + RESCUE_CONFIG_PENDING, + RESCUE_CONFIG_ACTIVE, + RESCUE_CONFIG_DISABLED, + }; + + struct OutputTelemetryState + { + int16_t errors[OUTPUT_CHANNELS]; + int32_t errorsSum[OUTPUT_CHANNELS]; + int32_t errorsCount[OUTPUT_CHANNELS]; + + uint32_t erpm[OUTPUT_CHANNELS]; + float rpm[OUTPUT_CHANNELS]; + float freq[OUTPUT_CHANNELS]; + + int8_t temperature[OUTPUT_CHANNELS]; + int8_t voltage[OUTPUT_CHANNELS]; + int8_t current[OUTPUT_CHANNELS]; + int8_t debug1[OUTPUT_CHANNELS]; + int8_t debug2[OUTPUT_CHANNELS]; + int8_t debug3[OUTPUT_CHANNELS]; + int8_t events[OUTPUT_CHANNELS]; + }; + + struct OutputState + { + float ch[OUTPUT_CHANNELS]; + int16_t us[OUTPUT_CHANNELS]; + int16_t disarmed[OUTPUT_CHANNELS]; + bool saturated; + OutputTelemetryState telemetry; + }; + + struct InputState + { + size_t channelCount; + bool channelsValid; + bool rxLoss; + bool rxFailSafe; + + uint32_t frameTime; + uint32_t frameDelta; + uint32_t frameRate; + uint32_t frameCount; + uint32_t lossTime; + + float interpolationDelta; + float interpolationStep; + float autoFactor; + float autoFreq; + + int16_t raw[INPUT_CHANNELS]; + int16_t buffer[INPUT_CHANNELS]; + int16_t bufferPrevious[INPUT_CHANNELS]; + + float us[INPUT_CHANNELS]; + float ch[INPUT_CHANNELS]; + + Utils::Filter filter[AXIS_COUNT_RPYT]; -enum GpsDeviceVersion -{ - GPS_UNKNOWN, - GPS_M8, - GPS_M9, - GPS_F9, -}; - -struct GpsSupportState -{ - GpsDeviceVersion version = GPS_UNKNOWN; - bool glonass = false; - bool galileo = false; - bool beidou = false; - bool sbas = false; -}; - -template -struct GpsCoordinate -{ - T lat = T{}; // deg * 1e7 - T lon = T{}; // deg * 1e7 - T height = T{}; // mm (1e3) -}; - -struct GpsPosition -{ - GpsCoordinate raw; - GpsCoordinate home; -}; - -template -struct GpsSpeed -{ - T north = T{}; // mm/s (1e3) - T east = T{}; // mm/s (1e3) - T down = T{}; // mm/s (1e3) - T groundSpeed = T{}; // mm/s (1e3) - T heading = T{}; // deg * 1e5 - T speed3d = T{}; // mm/s (1e3) -}; - -struct GpsVelocity -{ - GpsSpeed raw; -}; - -struct GpsAccuracy -{ - uint32_t horizontal = 0; // mm (1e3) - uint32_t vertical = 0; // mm (1e3) - uint32_t speed = 0; // mm/s (1e3) - uint32_t heading = 0; // deg * 1e5 - uint32_t pDop = 0; // (1e2) -}; - -struct GpsSatelite -{ - uint8_t gnssId = 0; - uint8_t id = 0; - uint8_t cno = 0; - union { - uint32_t value; - struct { - uint8_t qualityInd: 3; // quality indicatopr: 0-no signal, 1-searching, 2-aquired, 3-unstable, 4-code locked, 5,6,7-code and carrier locked - uint8_t svUsed: 1; // used for navigation - uint8_t health: 2; // signal health 0-unknown, 1-healthy, 2-unhealty - uint8_t difCorr: 1; // differential correction available for this SV - uint8_t smoothed: 1; // carrier smotthed pseudorange used - uint8_t orbitSource: 3; // orbit source: 0-no inform, 1-ephemeris, 2-almanac, 3-assistnow offline, 4-assistnow autonomous, 5,6,7-other - uint8_t ephAvail: 1; // ephemeris available - uint8_t elmAvail: 1; // almanac available - uint8_t enoAvail: 1; // assistnow offline available - uint8_t eopAvail: 1; // assistnow autonomous available - uint8_t reserved: 1; - uint8_t sbasCorrUsed: 1; // SBAS corrections used - uint8_t rtcmCorrUsed: 1; // RTCM corrections used - uint8_t slasCorrUsed: 1; // SLAS corrections used - uint8_t spartnCorrUsed: 1; // SPARTN corrections used - uint8_t prCorrUsed: 1; // Pseudorange corrections used - uint8_t crCorrUsed: 1; // Carrier range corrections used - uint8_t doCorrUsed: 1; // Range rate (Doppler) corrections used - uint8_t clasCorrUsed: 1; // CLAS corrections used - }; - } quality = { .value = 0 }; -}; - -struct GpsDateTime -{ - uint16_t year; // full year - uint8_t month; // 1-12 - uint8_t day; // 1-31 - uint8_t hour; // 0-23 - uint8_t minute; // 0-59 - uint8_t second; // 0-59 - uint16_t msec; // 0-999 -}; - -static constexpr size_t SAT_MAX = 32u; - -struct GpsState -{ - bool fix = 0; - uint8_t fixType = 0; - uint8_t numSats = 0; - uint8_t numCh = 0; - bool present = false; - bool frameError = false; - bool wasLocked = false; - bool homeSet = false; - uint32_t interval; - uint32_t lastMsgTs; - GpsSupportState support; - GpsPosition location; - GpsVelocity velocity; - GpsAccuracy accuracy; - GpsDateTime dateTime; - GpsSatelite svinfo[SAT_MAX]; -}; - -// runtime data -struct ModelState -{ - GyroState gyro; - AccelState accel; - MagState mag; - BaroState baro; - GpsState gps; - - InputState input; - FailsafeState failsafe; - - AttitudeState attitude; - RotationMatrixFloat boardAlignment; - - AltitudeState altitude; - - SetpointState setpoint; - Control::Pid innerPid[AXIS_COUNT_RPYT]; - Control::Pid outerPid[AXIS_COUNT_RPYT]; - - MixerState mixer; - OutputState output; - VtxState vtx; - - int32_t loopRate; - Utils::Timer loopTimer; - - Utils::Timer actuatorTimer; - Utils::Timer telemetryTimer; - - ModeState mode; - Utils::Stats stats; - - int16_t debug[DEBUG_VALUE_COUNT]; - - BuzzerState buzzer; - Connect::StatusLed led; - - BatteryState battery; - - MixerConfig currentMixer; - MixerConfig customMixer; + Utils::Timer timer; + }; - int16_t i2cErrorCount; - int16_t i2cErrorDelta; + struct MixerState + { + Utils::Timer timer; + float minThrottle; + float maxThrottle; + bool digitalOutput; + + EscDriver *escMotor; + EscDriver *escServo; + }; + + struct MagState + { + Device::MagDevice *dev; + bool present; + int rate; + + VectorInt16 raw; + VectorFloat adc; + Utils::Filter filter[3]; + Utils::Timer timer; - SerialPortState serial[SERIAL_UART_COUNT]; - Utils::Timer serialTimer; + int calibrationSamples; + int calibrationState; + bool calibrationValid; + VectorFloat calibrationMin; + VectorFloat calibrationMax; + VectorFloat calibrationScale; + VectorFloat calibrationOffset; + + VectorFloat pose; + }; + + struct BaroState + { + Device::BaroDevice *dev; + bool present; + int32_t rate; + + float temperatureRaw; + float temperature; + float pressureRaw; + float pressure; + float altitudeRaw; + float altitude; + float altitudeGround; + float altitudeBias; + float altitudePrev; + float vario; + int32_t altitudeBiasSamples; + }; + + struct GyroState + { + Device::GyroDevice *dev; + bool present; + int32_t rate; + int32_t clock = 1000; + + VectorInt16 raw; + VectorFloat adc; + VectorFloat sampled; + VectorFloat scaled; + VectorFloat dynNotch; + + float scale; + VectorFloat bias; + float biasAlpha; + int biasSamples; + int calibrationState; + int calibrationRate; + + Utils::Filter filter[AXIS_COUNT_RPY]; + Utils::Filter filter2[AXIS_COUNT_RPY]; + Utils::Filter filter3[AXIS_COUNT_RPY]; + Utils::Filter notch1Filter[AXIS_COUNT_RPY]; + Utils::Filter notch2Filter[AXIS_COUNT_RPY]; + Utils::Filter dynNotchFilter[DYN_NOTCH_COUNT_MAX][AXIS_COUNT_RPY]; + Utils::Filter rpmFilter[RPM_FILTER_MOTOR_MAX][RPM_FILTER_HARMONICS_MAX][AXIS_COUNT_RPY]; + Utils::Filter rpmFreqFilter[RPM_FILTER_MOTOR_MAX]; - Target::Queue appQueue; + Utils::Timer timer; + Utils::Timer dynamicFilterTimer; + }; + + struct AccelState + { + bool present; + VectorInt16 raw; + VectorFloat adc; + VectorFloat prev; + Utils::Filter filter[AXIS_COUNT_RPY]; + Utils::Timer timer; - // other state - Kalman kalman[AXIS_COUNT_RPYT]; - VectorFloat gyroPose; - Quaternion gyroPoseQ; - VectorFloat accelPose; - VectorFloat accelPose2; - Quaternion accelPoseQ; - VectorFloat pose; - Quaternion poseQ; -}; + VectorFloat world; + + float scale; + VectorFloat bias; + float biasAlpha; + int biasSamples; + int calibrationState; + }; + + struct AttitudeState + { + VectorFloat rate; + Utils::Filter filter[AXIS_COUNT_RPY]; + VectorFloat euler; + Quaternion quaternion; + float cosTheta; + }; + + struct SetpointState + { + VectorFloat angle; + float rate[AXIS_COUNT_RPYT]; + }; + + struct ModeState + { + uint32_t mask; + uint32_t maskPrev; + uint32_t maskSwitch; + uint32_t maskPresent; + uint32_t disarmReason; + uint32_t armingDisabledFlags; + RescueConfigMode rescueConfigMode; + bool airmodeAllowed; + uint32_t button; + bool isSingleClickActive() const { return button & (1 << 0); } + bool isDoubleClickActive() const { return button & (1 << 1); } + bool isLongClickActive() const { return button & (1 << 2); } + }; + + struct AltitudeState + { + float height; + float vario; + }; + + struct VtxState + { + uint8_t active = false; + }; + + enum GpsDeviceVersion + { + GPS_UNKNOWN, + GPS_M8, + GPS_M9, + GPS_F9, + }; + + struct GpsSupportState + { + GpsDeviceVersion version = GPS_UNKNOWN; + bool glonass = false; + bool galileo = false; + bool beidou = false; + bool sbas = false; + }; + + template + struct GpsCoordinate + { + T lat = T{}; // deg * 1e7 + T lon = T{}; // deg * 1e7 + T height = T{}; // mm (1e3) + }; + + struct GpsPosition + { + GpsCoordinate raw; + GpsCoordinate home; + }; + + template + struct GpsSpeed + { + T north = T{}; // mm/s (1e3) + T east = T{}; // mm/s (1e3) + T down = T{}; // mm/s (1e3) + T groundSpeed = T{}; // mm/s (1e3) + T heading = T{}; // deg * 1e5 + T speed3d = T{}; // mm/s (1e3) + }; + + struct GpsVelocity + { + GpsSpeed raw; + }; + + struct GpsAccuracy + { + uint32_t horizontal = 0; // mm (1e3) + uint32_t vertical = 0; // mm (1e3) + uint32_t speed = 0; // mm/s (1e3) + uint32_t heading = 0; // deg * 1e5 + uint32_t pDop = 0; // (1e2) + }; + + struct GpsSatelite + { + uint8_t gnssId = 0; + uint8_t id = 0; + uint8_t cno = 0; + union + { + uint32_t value; + struct + { + uint8_t qualityInd : 3; // quality indicator: 0-no signal, 1-searching, 2-aquired, 3-unstable, 4-code locked, 5,6,7-code and carrier locked + uint8_t svUsed : 1; // used for navigation + uint8_t health : 2; // signal health 0-unknown, 1-healthy, 2-unhealty + uint8_t difCorr : 1; // differential correction available for this SV + uint8_t smoothed : 1; // carrier smotthed pseudorange used + uint8_t orbitSource : 3; // orbit source: 0-no inform, 1-ephemeris, 2-almanac, 3-assistnow offline, 4-assistnow autonomous, 5,6,7-other + uint8_t ephAvail : 1; // ephemeris available + uint8_t elmAvail : 1; // almanac available + uint8_t enoAvail : 1; // assistnow offline available + uint8_t eopAvail : 1; // assistnow autonomous available + uint8_t reserved : 1; + uint8_t sbasCorrUsed : 1; // SBAS corrections used + uint8_t rtcmCorrUsed : 1; // RTCM corrections used + uint8_t slasCorrUsed : 1; // SLAS corrections used + uint8_t spartnCorrUsed : 1; // SPARTN corrections used + uint8_t prCorrUsed : 1; // Pseudorange corrections used + uint8_t crCorrUsed : 1; // Carrier range corrections used + uint8_t doCorrUsed : 1; // Range rate (Doppler) corrections used + uint8_t clasCorrUsed : 1; // CLAS corrections used + }; + } quality = {.value = 0}; + }; + + struct GpsDateTime + { + uint16_t year; // full year + uint8_t month; // 1-12 + uint8_t day; // 1-31 + uint8_t hour; // 0-23 + uint8_t minute; // 0-59 + uint8_t second; // 0-59 + uint16_t msec; // 0-999 + }; + + static constexpr size_t SAT_MAX = 32u; + + struct GpsState + { + bool fix = 0; + uint8_t fixType = 0; + uint8_t numSats = 0; + uint8_t numCh = 0; + bool present = false; + bool frameError = false; + bool wasLocked = false; + bool homeSet = false; + uint32_t interval; + uint32_t lastMsgTs; + GpsSupportState support; + GpsPosition location; + GpsVelocity velocity; + GpsAccuracy accuracy; + GpsDateTime dateTime; + GpsSatelite svinfo[SAT_MAX]; + }; + + // runtime data + struct ModelState + { + GyroState gyro; + AccelState accel; + MagState mag; + BaroState baro; + GpsState gps; + + InputState input; + FailsafeState failsafe; + + AttitudeState attitude; + RotationMatrixFloat boardAlignment; + RotationMatrixFloat trimRotation; + + AltitudeState altitude; + + SetpointState setpoint; + Control::Pid innerPid[AXIS_COUNT_RPYT]; + Control::Pid outerPid[AXIS_COUNT_RPYT]; + + MixerState mixer; + OutputState output; + VtxState vtx; + + int32_t loopRate; + Utils::Timer loopTimer; + + Utils::Timer actuatorTimer; + Utils::Timer telemetryTimer; + + ModeState mode; + Utils::Stats stats; + + int16_t debug[DEBUG_VALUE_COUNT]; + + BuzzerState buzzer; + Connect::StatusLed led; + + BatteryState battery; + + MixerConfig currentMixer; + MixerConfig customMixer; + + int16_t i2cErrorCount; + int16_t i2cErrorDelta; + + SerialPortState serial[SERIAL_UART_COUNT]; + Utils::Timer serialTimer; + + Target::Queue appQueue; + + // other state + Kalman kalman[AXIS_COUNT_RPYT]; + VectorFloat gyroPose; + Quaternion gyroPoseQ; + VectorFloat accelPose; + VectorFloat accelPose2; + Quaternion accelPoseQ; + VectorFloat pose; + Quaternion poseQ; + }; } diff --git a/lib/Espfc/src/Sensor/AccelSensor.cpp b/lib/Espfc/src/Sensor/AccelSensor.cpp index 542e775d..9083a900 100644 --- a/lib/Espfc/src/Sensor/AccelSensor.cpp +++ b/lib/Espfc/src/Sensor/AccelSensor.cpp @@ -1,85 +1,97 @@ #include "Sensor/AccelSensor.h" #include "Utils/FilterHelper.h" -namespace Espfc::Sensor { - -static constexpr float ESPFC_FUZZY_ACCEL_ZERO = 0.05f; -static constexpr float ESPFC_FUZZY_GYRO_ZERO = 0.20f; - -AccelSensor::AccelSensor(Model& model): _model(model) {} - -int AccelSensor::begin() +namespace Espfc::Sensor { - _model.state.accel.adc.z = ACCEL_G; - _gyro = _model.state.gyro.dev; - if(!_gyro) return 0; + static constexpr float ESPFC_FUZZY_ACCEL_ZERO = 0.05f; + static constexpr float ESPFC_FUZZY_GYRO_ZERO = 0.20f; - _model.state.accel.scale = 16.f * ACCEL_G / 32768.f; + AccelSensor::AccelSensor(Model &model) : _model(model) {} - for(size_t i = 0; i < AXIS_COUNT_RPY; i++) + int AccelSensor::begin() { - _filter[i].begin(FilterConfig(FILTER_FIR2, 1), _model.state.accel.timer.rate); - _model.state.accel.filter[i].begin(_model.config.accel.filter, _model.state.accel.timer.rate); - } + _model.state.accel.adc.z = ACCEL_G; - _model.state.accel.biasAlpha = 5.0f / _model.state.accel.timer.rate; - _model.state.accel.calibrationState = CALIBRATION_IDLE; + _gyro = _model.state.gyro.dev; + if (!_gyro) + return 0; - _model.logger.info().log(F("ACCEL INIT")).log(FPSTR(Device::GyroDevice::getName(_gyro->getType()))).log(_gyro->getAddress()).log(_model.state.accel.timer.rate).log(_model.state.accel.timer.interval).logln(_model.state.accel.present); + _model.state.accel.scale = 16.f * ACCEL_G / 32768.f; - return 1; -} + for (size_t i = 0; i < AXIS_COUNT_RPY; i++) + { + _filter[i].begin(FilterConfig(FILTER_FIR2, 1), _model.state.accel.timer.rate); + _model.state.accel.filter[i].begin(_model.config.accel.filter, _model.state.accel.timer.rate); + } -int FAST_CODE_ATTR AccelSensor::update() -{ - int status = read(); + _model.state.accel.biasAlpha = 5.0f / _model.state.accel.timer.rate; + _model.state.accel.calibrationState = CALIBRATION_IDLE; - if (status) filter(); + updateTrimRotation(); - return status; -} + _model.logger.info().log(F("ACCEL INIT")).log(FPSTR(Device::GyroDevice::getName(_gyro->getType()))).log(_gyro->getAddress()).log(_model.state.accel.timer.rate).log(_model.state.accel.timer.interval).logln(_model.state.accel.present); -int FAST_CODE_ATTR AccelSensor::read() -{ - if(!_model.accelActive()) return 0; + return 1; + } - Utils::Stats::Measure measure(_model.state.stats, COUNTER_ACCEL_READ); - _gyro->readAccel(_model.state.accel.raw); + int FAST_CODE_ATTR AccelSensor::update() + { + int status = read(); - return 1; -} + if (status) + filter(); -int FAST_CODE_ATTR AccelSensor::filter() -{ - if(!_model.accelActive()) return 0; + return status; + } - Utils::Stats::Measure measure(_model.state.stats, COUNTER_ACCEL_FILTER); + int FAST_CODE_ATTR AccelSensor::read() + { + if (!_model.accelActive()) + return 0; - _model.state.accel.adc = (VectorFloat)_model.state.accel.raw * _model.state.accel.scale; + Utils::Stats::Measure measure(_model.state.stats, COUNTER_ACCEL_READ); + _gyro->readAccel(_model.state.accel.raw); - align(_model.state.accel.adc, _model.config.gyro.align); - _model.state.accel.adc = _model.state.boardAlignment.apply(_model.state.accel.adc); + return 1; + } - for(size_t i = 0; i < AXIS_COUNT_RPY; i++) + int FAST_CODE_ATTR AccelSensor::filter() { - if(_model.config.debug.mode == DEBUG_ACCELEROMETER) + if (!_model.accelActive()) + return 0; + + Utils::Stats::Measure measure(_model.state.stats, COUNTER_ACCEL_FILTER); + + _model.state.accel.adc = (VectorFloat)_model.state.accel.raw * _model.state.accel.scale; + + align(_model.state.accel.adc, _model.config.gyro.align); + _model.state.accel.adc = _model.state.boardAlignment.apply(_model.state.accel.adc); + + for (size_t i = 0; i < AXIS_COUNT_RPY; i++) { - _model.state.debug[i] = _model.state.accel.raw[i]; + if (_model.config.debug.mode == DEBUG_ACCELEROMETER) + { + _model.state.debug[i] = _model.state.accel.raw[i]; + } + _model.state.accel.adc.set(i, _filter[i].update(_model.state.accel.adc[i])); + _model.state.accel.adc.set(i, _model.state.accel.filter[i].update(_model.state.accel.adc[i])); } - _model.state.accel.adc.set(i, _filter[i].update(_model.state.accel.adc[i])); - _model.state.accel.adc.set(i, _model.state.accel.filter[i].update(_model.state.accel.adc[i])); - } - calibrate(); + calibrate(); - return 1; -} + if (_gyro && _model.state.accel.calibrationState == CALIBRATION_IDLE) + { + _model.state.accel.adc = _model.state.trimRotation.apply(_model.state.accel.adc); + } -void FAST_CODE_ATTR AccelSensor::calibrate() -{ - switch(_model.state.accel.calibrationState) + return 1; + } + + void FAST_CODE_ATTR AccelSensor::calibrate() { + switch (_model.state.accel.calibrationState) + { case CALIBRATION_IDLE: _model.state.accel.adc -= _model.state.accel.bias; break; @@ -91,7 +103,8 @@ void FAST_CODE_ATTR AccelSensor::calibrate() case CALIBRATION_UPDATE: _model.state.accel.bias += (_model.state.accel.adc - _model.state.accel.bias) * _model.state.accel.biasAlpha; _model.state.accel.biasSamples--; - if(_model.state.accel.biasSamples <= 0) _model.state.accel.calibrationState = CALIBRATION_APPLY; + if (_model.state.accel.biasSamples <= 0) + _model.state.accel.calibrationState = CALIBRATION_APPLY; break; case CALIBRATION_APPLY: _model.state.accel.bias.z -= ACCEL_G; @@ -104,7 +117,21 @@ void FAST_CODE_ATTR AccelSensor::calibrate() default: _model.state.accel.calibrationState = CALIBRATION_IDLE; break; + } + } + + void AccelSensor::updateTrimRotation() + { + const float trimPitch = Utils::toRad(_model.config.accel.trim[0] * 0.1f); + const float trimRoll = Utils::toRad(_model.config.accel.trim[1] * 0.1f); + if (trimPitch != 0.f || trimRoll != 0.f) + { + _model.state.trimRotation.init(VectorFloat(trimRoll, trimPitch, 0.f)); + } + else + { + _model.state.trimRotation.init(VectorFloat(0.f, 0.f, 0.f)); + } } -} } diff --git a/lib/Espfc/src/Sensor/AccelSensor.h b/lib/Espfc/src/Sensor/AccelSensor.h index b6a8618a..f0bc176d 100644 --- a/lib/Espfc/src/Sensor/AccelSensor.h +++ b/lib/Espfc/src/Sensor/AccelSensor.h @@ -4,24 +4,26 @@ #include "BaseSensor.h" #include "Device/GyroDevice.h" -namespace Espfc::Sensor { - -class AccelSensor: public BaseSensor +namespace Espfc::Sensor { + + class AccelSensor : public BaseSensor + { public: - AccelSensor(Model& model); - + AccelSensor(Model &model); + int begin(); int update(); int read(); int filter(); + void updateTrimRotation(); private: void calibrate(); - Model& _model; - Device::GyroDevice * _gyro; + Model &_model; + Device::GyroDevice *_gyro; Utils::Filter _filter[AXIS_COUNT_RPY]; -}; + }; }