diff --git a/bin/msp.py b/bin/msp.py new file mode 100755 index 00000000..ac3b9f90 --- /dev/null +++ b/bin/msp.py @@ -0,0 +1,358 @@ +#!/usr/bin/env python3 +# Requires pyserial: install with `python3 -m pip install pyserial` +# On Ubuntu you can also install it with `sudo apt install python3-serial` +import argparse +from dataclasses import dataclass +import sys +import time + +import serial + +""" +MSP debug script +""" + +MSP_BUF_SIZE = 192 +TIMEOUT_SECONDS = 3.0 + +MSP_STATE_IDLE = 0 +MSP_STATE_HEADER_START = 1 +MSP_STATE_HEADER_M = 2 +MSP_STATE_HEADER_V1 = 3 +MSP_STATE_PAYLOAD_V1 = 4 +MSP_STATE_CHECKSUM_V1 = 5 +MSP_STATE_HEADER_X = 6 +MSP_STATE_HEADER_V2 = 7 +MSP_STATE_PAYLOAD_V2 = 8 +MSP_STATE_CHECKSUM_V2 = 9 +MSP_STATE_RECEIVED = 10 + +MSP_TYPE_CMD = 0 +MSP_TYPE_REPLY = 1 + +MSP_V1 = 0 +MSP_V2 = 1 + +@dataclass +class ParsedFrame: + version: int + direction: int + frame_type: int + flags: int + cmd: int + payload: bytes + checksum: int + raw: bytes + + @property + def header_length(self) -> int: + return 5 if self.version == MSP_V1 else 8 + + @property + def header(self) -> bytes: + return self.raw[: self.header_length] + + +class MspParser: + def __init__(self) -> None: + self.reset(full=True) + + def reset(self, full: bool = False) -> None: + self.state = MSP_STATE_IDLE + self.version = MSP_V1 + self.direction = MSP_TYPE_CMD + self.frame_type = 0 + self.flags = 0 + self.cmd = 0 + self.expected = 0 + self.received = 0 + self.checksum = 0 + self.checksum2 = 0 + self.buffer = bytearray() + self.raw = bytearray() if full else self.raw[:0] + + def feed(self, byte: int): + c = byte & 0xFF + + if self.state == MSP_STATE_IDLE: + if c == ord('$'): + self.raw = bytearray((c,)) + self.state = MSP_STATE_HEADER_START + return None + + if self.state == MSP_STATE_HEADER_START: + self.received = 0 + self.checksum = 0 + self.checksum2 = 0 + self.buffer = bytearray() + self.raw.append(c) + if c == ord('M'): + self.version = MSP_V1 + self.state = MSP_STATE_HEADER_M + elif c == ord('X'): + self.version = MSP_V2 + self.state = MSP_STATE_HEADER_X + else: + self.reset() + return None + + if self.state == MSP_STATE_HEADER_M: + self.raw.append(c) + if c == ord('>'): + self.direction = MSP_TYPE_REPLY + self.frame_type = c + self.state = MSP_STATE_HEADER_V1 + elif c == ord('<'): + self.direction = MSP_TYPE_CMD + self.frame_type = c + self.state = MSP_STATE_HEADER_V1 + elif c == ord('!'): + self.direction = MSP_TYPE_REPLY + self.frame_type = c + self.state = MSP_STATE_HEADER_V1 + else: + self.reset() + return None + + if self.state == MSP_STATE_HEADER_X: + self.raw.append(c) + if c == ord('>'): + self.direction = MSP_TYPE_REPLY + self.frame_type = c + self.state = MSP_STATE_HEADER_V2 + elif c == ord('<'): + self.direction = MSP_TYPE_CMD + self.frame_type = c + self.state = MSP_STATE_HEADER_V2 + elif c == ord('!'): + self.direction = MSP_TYPE_REPLY + self.frame_type = c + self.state = MSP_STATE_HEADER_V2 + else: + self.reset() + return None + + if self.state == MSP_STATE_HEADER_V1: + self.buffer.append(c) + self.raw.append(c) + self.received += 1 + self.checksum ^= c + if self.received == 2: + size = self.buffer[0] + if size > MSP_BUF_SIZE: + self.reset() + else: + self.expected = size + self.cmd = self.buffer[1] + self.received = 0 + self.buffer = bytearray() + self.state = MSP_STATE_PAYLOAD_V1 if self.expected > 0 else MSP_STATE_CHECKSUM_V1 + return None + + if self.state == MSP_STATE_PAYLOAD_V1: + self.buffer.append(c) + self.raw.append(c) + self.received += 1 + self.checksum ^= c + if self.received == self.expected: + self.state = MSP_STATE_CHECKSUM_V1 + return None + + if self.state == MSP_STATE_CHECKSUM_V1: + self.raw.append(c) + if self.checksum != c: + self.reset() + return None + frame = ParsedFrame( + version=self.version, + direction=self.direction, + frame_type=self.frame_type, + flags=0, + cmd=self.cmd, + payload=bytes(self.buffer), + checksum=c, + raw=bytes(self.raw), + ) + self.state = MSP_STATE_RECEIVED + self.reset() + return frame + + if self.state == MSP_STATE_HEADER_V2: + self.buffer.append(c) + self.raw.append(c) + self.received += 1 + self.checksum2 = crc8_dvb_s2(self.checksum2, c) + if self.received == 5: + flags = self.buffer[0] + cmd = self.buffer[1] | (self.buffer[2] << 8) + size = self.buffer[3] | (self.buffer[4] << 8) + if size > MSP_BUF_SIZE: + self.reset() + else: + self.flags = flags + self.cmd = cmd + self.expected = size + self.received = 0 + self.buffer = bytearray() + self.state = MSP_STATE_PAYLOAD_V2 if self.expected > 0 else MSP_STATE_CHECKSUM_V2 + return None + + if self.state == MSP_STATE_PAYLOAD_V2: + self.buffer.append(c) + self.raw.append(c) + self.received += 1 + self.checksum2 = crc8_dvb_s2(self.checksum2, c) + if self.received == self.expected: + self.state = MSP_STATE_CHECKSUM_V2 + return None + + if self.state == MSP_STATE_CHECKSUM_V2: + self.raw.append(c) + if self.checksum2 != c: + self.reset() + return None + frame = ParsedFrame( + version=self.version, + direction=self.direction, + frame_type=self.frame_type, + flags=self.flags, + cmd=self.cmd, + payload=bytes(self.buffer), + checksum=c, + raw=bytes(self.raw), + ) + self.state = MSP_STATE_RECEIVED + self.reset() + return frame + + self.reset() + return None + + +def crc8_dvb_s2(crc: int, value: int) -> int: + crc ^= value & 0xFF + for _ in range(8): + if crc & 0x80: + crc = ((crc << 1) ^ 0xD5) & 0xFF + else: + crc = (crc << 1) & 0xFF + return crc + + +def parse_message_id(value: str) -> int: + try: + cmd = int(value, 0) + except ValueError as exc: + raise argparse.ArgumentTypeError(f"invalid message id: {value}") from exc + if not 0 <= cmd <= 0xFFFF: + raise argparse.ArgumentTypeError("message id must be in range 0..65535") + return cmd + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser() + parser.add_argument("port") + parser.add_argument("message_id", type=parse_message_id) + parser.add_argument("--baud", type=int, default=115200) + return parser.parse_args() + + +@dataclass +class RequestFrame: + version: int + header: bytes + payload: bytes + checksum: int + + @property + def raw(self) -> bytes: + return self.header + self.payload + bytes((self.checksum,)) + + +def build_request(cmd: int) -> RequestFrame: + if cmd <= 0xFF: + header = bytes((ord('$'), ord('M'), ord('<'), 0, cmd)) + checksum = 0 ^ header[3] ^ header[4] + return RequestFrame(MSP_V1, header, b"", checksum) + + header = bytes((ord('$'), ord('X'), ord('<'), 0, cmd & 0xFF, (cmd >> 8) & 0xFF, 0, 0)) + checksum = 0 + for value in header[3:8]: + checksum = crc8_dvb_s2(checksum, value) + return RequestFrame(MSP_V2, header, b"", checksum) + + +def read_response(ser: serial.Serial, expected_cmd: int, timeout: float) -> ParsedFrame: + parser = MspParser() + deadline = time.monotonic() + timeout + while True: + remaining = deadline - time.monotonic() + if remaining <= 0: + raise TimeoutError(f"timeout waiting for response to message {expected_cmd}") + ser.timeout = max(0.0, min(remaining, 0.2)) + chunk = ser.read(256) + if not chunk: + continue + for byte in chunk: + frame = parser.feed(byte) + if frame and frame.direction == MSP_TYPE_REPLY and frame.cmd == expected_cmd: + return frame + + +def open_serial(port: str, baud: int) -> serial.Serial: + try: + return serial.Serial( + port=port, + baudrate=baud, + parity=serial.PARITY_NONE, + stopbits=serial.STOPBITS_ONE, + bytesize=serial.EIGHTBITS, + timeout=0.2, + write_timeout=1.0, + ) + except (serial.SerialException, ValueError) as exc: + raise OSError(f"failed to open serial port {port}: {exc}") from exc + + +def format_bytes(data: bytes) -> str: + return " ".join(f"{byte:02X}" for byte in data) + + +def response_marker(response: ParsedFrame) -> str: + return "!" if response.frame_type == ord("!") else ">" + + +def format_header(header: bytes) -> str: + result = header[0:3].decode("ascii") + result += " " + " ".join(f"{byte:02X}" for byte in header[0:]) + return result + + +def print_frame_parts(request: RequestFrame, response: ParsedFrame) -> None: + marker = response_marker(response) + print("<", format_header(request.header), "..", format_bytes(bytes((request.checksum,)))) + print("<", format_bytes(request.payload)) + print(marker, format_header(response.header), "..", format_bytes(bytes((response.checksum,)))) + print(marker, format_bytes(response.payload)) + + +def main() -> int: + args = parse_args() + request = build_request(args.message_id) + ser = open_serial(args.port, args.baud) + try: + ser.write(request.raw) + ser.flush() + response = read_response(ser, args.message_id, TIMEOUT_SECONDS) + finally: + ser.close() + print_frame_parts(request, response) + return 0 + + +if __name__ == "__main__": + try: + raise SystemExit(main()) + except (OSError, TimeoutError, ValueError, serial.SerialException) as exc: + print(str(exc), file=sys.stderr) + raise SystemExit(1) diff --git a/docs/cli.md b/docs/cli.md index 2de98d8c..4c953dc4 100644 --- a/docs/cli.md +++ b/docs/cli.md @@ -450,8 +450,6 @@ set input_deadband 3 set input_min 885 set input_mid 1500 set input_max 2115 -set input_interpolation AUTO -set input_interpolation_interval 26 set input_filter_type FILTER set input_lpf_type PT3 set input_lpf_freq 0 diff --git a/docs/pyDrone_dump.txt b/docs/pyDrone_dump.txt index 5d505b3f..f5cfe44f 100644 --- a/docs/pyDrone_dump.txt +++ b/docs/pyDrone_dump.txt @@ -83,8 +83,6 @@ set input_deadband 3 set input_min 885 set input_mid 1500 set input_max 2115 -set input_interpolation AUTO -set input_interpolation_interval 26 set input_filter_type FILTER set input_lpf_type PT3 set input_lpf_freq 0 diff --git a/lib/Espfc/library.json b/lib/Espfc/library.json index 05d54da1..a375d314 100644 --- a/lib/Espfc/library.json +++ b/lib/Espfc/library.json @@ -1,4 +1,9 @@ { "name": "Espfc", - "version": "0.2.0" + "version": "0.3.0", + "license": "MIT", + "repository": { + "type": "git", + "url": "https://github.com/rtlopez/esp-fc.git" + } } diff --git a/lib/Espfc/src/Blackbox/Blackbox.cpp b/lib/Espfc/src/Blackbox/Blackbox.cpp index 456dea0c..f4824312 100644 --- a/lib/Espfc/src/Blackbox/Blackbox.cpp +++ b/lib/Espfc/src/Blackbox/Blackbox.cpp @@ -138,7 +138,7 @@ int Blackbox::begin() motorConfigMutable()->dev.motorPwmProtocol = _model.config.output.protocol; motorConfigMutable()->dev.motorPwmRate = _model.config.output.rate; motorConfigMutable()->mincommand = _model.config.output.minCommand; - motorConfigMutable()->digitalIdleOffsetValue = _model.config.output.dshotIdle; + motorConfigMutable()->digitalIdleOffsetValue = _model.config.output.motorIdle; motorConfigMutable()->minthrottle = _model.state.mixer.minThrottle; motorConfigMutable()->maxthrottle = _model.state.mixer.maxThrottle; motorConfigMutable()->dev.useDshotTelemetry = _model.config.output.dshotTelemetry; @@ -156,14 +156,7 @@ int Blackbox::begin() targetPidLooptime = _model.state.loopTimer.interval; activePidLoopDenom = _model.config.loopSync; - if(_model.config.blackbox.pDenom >= 0 && _model.config.blackbox.pDenom <= 4) - { - blackboxConfigMutable()->sample_rate = _model.config.blackbox.pDenom; - } - else - { - blackboxConfigMutable()->sample_rate = blackboxCalculateSampleRate(_model.config.blackbox.pDenom); - } + blackboxConfigMutable()->sample_rate = _model.config.blackbox.pDenom; blackboxConfigMutable()->device = _model.config.blackbox.dev; blackboxConfigMutable()->fields_disabled_mask = ~_model.config.blackbox.fieldsMask; blackboxConfigMutable()->mode = _model.config.blackbox.mode; @@ -176,10 +169,8 @@ int Blackbox::begin() batteryConfigMutable()->vbatmaxcellvoltage = 420; batteryConfigMutable()->vbatmincellvoltage = 340; - rxConfigMutable()->rcInterpolation = _model.config.input.interpolationMode; - rxConfigMutable()->rcInterpolationInterval = _model.config.input.interpolationInterval; rxConfigMutable()->rssi_channel = _model.config.input.rssiChannel; - rxConfigMutable()->airModeActivateThreshold = 40; + rxConfigMutable()->airModeActivateThreshold = _model.config.input.airModeActivateThreshold; rxConfigMutable()->serialrx_provider = _model.config.input.serialRxProvider; rpmFilterConfigMutable()->rpm_filter_harmonics = _model.config.gyro.rpmFilter.harmonics; diff --git a/lib/Espfc/src/Connect/Cli.cpp b/lib/Espfc/src/Connect/Cli.cpp index 411b1dab..84bf90ba 100644 --- a/lib/Espfc/src/Connect/Cli.cpp +++ b/lib/Espfc/src/Connect/Cli.cpp @@ -1,9 +1,11 @@ #include "Connect/Cli.hpp" #include "Device/GyroDevice.hpp" #include "Hardware.h" +#include "ModelConfig.h" #include "Utils/Filter.h" #include "msp/msp_protocol.h" #include +#include #include #include @@ -21,9 +23,7 @@ #include #endif -namespace Espfc { - -namespace Connect { +namespace Espfc::Connect { void Cli::Param::print(Stream& stream) const { @@ -283,9 +283,9 @@ void Cli::Param::write(ActuatorCondition& ac, const char** args) const void Cli::Param::write(MixerEntry& ac, const char** args) const { - if (args[2]) ac.src = constrain(String(args[2]).toInt(), 0, MIXER_SOURCE_MAX - 1); - if (args[3]) ac.dst = constrain(String(args[3]).toInt(), 0, (int)(OUTPUT_CHANNELS - 1)); - if (args[4]) ac.rate = constrain(String(args[4]).toInt(), -1000, 1000); + if (args[2]) ac.src = std::clamp(String(args[2]).toInt(), 0, MIXER_SOURCE_MAX - 1); + if (args[3]) ac.dst = std::clamp(String(args[3]).toInt(), 0, (int)(OUTPUT_CHANNELS - 1)); + if (args[4]) ac.rate = std::clamp(String(args[4]).toInt(), -1000, 1000); } void Cli::Param::write(SerialPortConfig& sc, const char** args) const @@ -314,7 +314,7 @@ int32_t Cli::Param::parse(const char* v) const return tmp.toInt(); } -Cli::Cli(Model& model): _model(model), _ignore(false), _active(false) +Cli::Cli(Model& model): _model(model), _ignore(false), _active(false), _interactive(false) { _params = initialize(_model.config); } @@ -331,15 +331,21 @@ const Cli::Param* Cli::initialize(ModelConfig& c) // clang-format off static const char* gyroDlpfChoices[] = { "256Hz", "188Hz", "98Hz", "42Hz", "20Hz", "10Hz", "5Hz", "EXPERIMENTAL", nullptr }; - static const char* debugModeChoices[] = { "NONE", "CYCLETIME", "BATTERY", "GYRO_FILTERED", "ACCELEROMETER", "PIDLOOP", "GYRO_SCALED", "RC_INTERPOLATION", + static const char* debugModeChoices[] = { "NONE", "CYCLETIME", "BATTERY", "GYRO_FILTERED", "ACCELEROMETER", "PIDLOOP", "RC_INTERPOLATION", "ANGLERATE", "ESC_SENSOR", "SCHEDULER", "STACK", "ESC_SENSOR_RPM", "ESC_SENSOR_TMP", "ALTITUDE", "FFT", - "FFT_TIME", "FFT_FREQ", "RX_FRSKY_SPI", "RX_SFHSS_SPI", "GYRO_RAW", "DUAL_GYRO_RAW", "DUAL_GYRO_DIFF", - "MAX7456_SIGNAL", "MAX7456_SPICLOCK", "SBUS", "FPORT", "RANGEFINDER", "RANGEFINDER_QUALITY", "LIDAR_TF", + "FFT_TIME", "FFT_FREQ", "RX_FRSKY_SPI", "RX_SFHSS_SPI", "GYRO_RAW", "MULTI_GYRO_RAW", "MULTI_GYRO_DIFF", + "MAX7456_SIGNAL", "MAX7456_SPICLOCK", "SBUS", "FPORT", "RANGEFINDER", "RANGEFINDER_QUALITY", "OPTICALFLOW", "LIDAR_TF", "ADC_INTERNAL", "RUNAWAY_TAKEOFF", "SDIO", "CURRENT_SENSOR", "USB", "SMARTAUDIO", "RTH", "ITERM_RELAX", "ACRO_TRAINER", "RC_SMOOTHING", "RX_SIGNAL_LOSS", "RC_SMOOTHING_RATE", "ANTI_GRAVITY", "DYN_LPF", "RX_SPEKTRUM_SPI", - "DSHOT_RPM_TELEMETRY", "RPM_FILTER", "D_MIN", "AC_CORRECTION", "AC_ERROR", "DUAL_GYRO_SCALED", "DSHOT_RPM_ERRORS", - "CRSF_LINK_STATISTICS_UPLINK", "CRSF_LINK_STATISTICS_PWR", "CRSF_LINK_STATISTICS_DOWN", "BARO", "GPS_RESCUE_THROTTLE_PID", - "DYN_IDLE", "FF_LIMIT", "FF_INTERPOLATED", "BLACKBOX_OUTPUT", "GYRO_SAMPLE", "RX_TIMING", nullptr }; + "DSHOT_RPM_TELEMETRY", "RPM_FILTER", "D_MAX", "AC_CORRECTION", "AC_ERROR", "MULTI_GYRO_SCALED", "DSHOT_RPM_ERRORS", + "CRSF_LINK_STATISTICS_UPLINK", "CRSF_LINK_STATISTICS_PWR", "CRSF_LINK_STATISTICS_DOWN", "BARO", "AUTOPILOT_ALTITUDE", + "DYN_IDLE", "FEEDFORWARD_LIMIT", "FEEDFORWARD", "BLACKBOX_OUTPUT", "GYRO_SAMPLE", "RX_TIMING", "D_LPF", + "VTX_TRAMP", "GHST", "GHST_MSP", "SCHEDULER_DETERMINISM", "TIMING_ACCURACY", "RX_EXPRESSLRS_SPI", + "RX_EXPRESSLRS_PHASELOCK", "RX_STATE_TIME", "GPS_RESCUE_VELOCITY", "GPS_RESCUE_HEADING", "GPS_RESCUE_TRACKING", + "GPS_CONNECTION", "ATTITUDE", "VTX_MSP", "GPS_DOP", "FAILSAFE", "GYRO_CALIBRATION", "ANGLE_MODE", "ANGLE_TARGET", + "CURRENT_ANGLE", "DSHOT_TELEMETRY_COUNTS", "RPM_LIMIT", "RC_STATS", "MAG_CALIB", "MAG_TASK_RATE", "EZLANDING", "TPA", + "S_TERM", "SPA", "TASK", "GIMBAL", "WING_SETPOINT", "CHIRP", "FLASH_TEST_PRBS", "MAVLINK_TELEMETRY", + "AUTOPILOT_PID", "POSITION_NAV", "AUTOPILOT_STOP", "PITOT", nullptr }; static const char* filterTypeChoices[] = { "PT1", "BIQUAD", "PT2", "PT3", "NOTCH", "NOTCH_DF1", "BPF", "FO", "FIR2", "MEDIAN3", "NONE", nullptr }; static const char* alignChoices[] = { "DEFAULT", "CW0", "CW90", "CW180", "CW270", "CW0_FLIP", "CW90_FLIP", "CW180_FLIP", "CW270_FLIP", "CUSTOM", nullptr }; static const char* mixerTypeChoices[] = { "NONE", "TRI", "QUADP", "QUADX", "BI", @@ -348,10 +354,8 @@ const Cli::Param* Cli::initialize(ModelConfig& c) "HELI120", "HELI90", "VTAIL4", "HEX6H", "PPMSERVO", "DUALCOPTER", "SINGLECOPTER", "ATAIL4", "CUSTOM", "CUSTOMAIRPLANE", "CUSTOMTRI", "QUADX1234", nullptr }; - static const char* interpolChoices[] = { "NONE", "DEFAULT", "AUTO", "MANUAL", nullptr }; static const char* inputRateTypeChoices[] = { "BETAFLIGHT", "RACEFLIGHT", "KISS", "ACTUAL", "QUICK", nullptr }; static const char* throtleLimitTypeChoices[] = { "NONE", "SCALE", "CLIP", nullptr }; - static const char* inputFilterChoices[] = { "INTERPOLATION", "FILTER", nullptr }; static const char* inputItermRelaxChoices[] = { "OFF", "RP", "RPY", "RP_INC", "RPY_INC", nullptr }; static const char* voltageSourceChoices[] = { "NONE", "ADC", nullptr }; @@ -359,113 +363,179 @@ const Cli::Param* Cli::initialize(ModelConfig& c) static const char* blackboxDevChoices[] = { "NONE", "FLASH", "SD_CARD", "SERIAL", nullptr }; static const char* blackboxModeChoices[] = { "NORMAL", "TEST", "ALWAYS", nullptr }; static const char* ledTypeChoices[] = { "SIMPLE", "STRIP", nullptr }; - // clang-format on + static const char* simplifiedTunigModeChoices[] = { "OFF", "RP", "RPY", nullptr }; + static const char* tpaModeChoices[] = { "PD", "D", nullptr }; size_t i = 0; static const Param params[] = { - Param("feature_gps", &c.featureMask, 7), Param("feature_dyn_notch", &c.featureMask, 29), - Param("feature_motor_stop", &c.featureMask, 4), Param("feature_rx_ppm", &c.featureMask, 0), - Param("feature_rx_serial", &c.featureMask, 3), Param("feature_rx_spi", &c.featureMask, 25), - Param("feature_soft_serial", &c.featureMask, 6), Param("feature_telemetry", &c.featureMask, 10), - - Param("debug_mode", &c.debug.mode, debugModeChoices), Param("debug_axis", &c.debug.axis), - - Param("gyro_bus", &c.gyro.bus, busDevChoices), Param("gyro_dev", &c.gyro.dev, gyroDevChoices), - Param("gyro_dlpf", &c.gyro.dlpf, gyroDlpfChoices), Param("gyro_align", &c.gyro.align, alignChoices), - Param("gyro_lpf_type", &c.gyro.filter.type, filterTypeChoices), Param("gyro_lpf_freq", &c.gyro.filter.freq), - Param("gyro_lpf2_type", &c.gyro.filter2.type, filterTypeChoices), Param("gyro_lpf2_freq", &c.gyro.filter2.freq), - Param("gyro_lpf3_type", &c.gyro.filter3.type, filterTypeChoices), Param("gyro_lpf3_freq", &c.gyro.filter3.freq), - Param("gyro_notch1_freq", &c.gyro.notch1Filter.freq), Param("gyro_notch1_cutoff", &c.gyro.notch1Filter.cutoff), - Param("gyro_notch2_freq", &c.gyro.notch2Filter.freq), Param("gyro_notch2_cutoff", &c.gyro.notch2Filter.cutoff), - Param("gyro_dyn_lpf_min", &c.gyro.dynLpfFilter.cutoff), Param("gyro_dyn_lpf_max", &c.gyro.dynLpfFilter.freq), - Param("gyro_dyn_notch_q", &c.gyro.dynamicFilter.q), Param("gyro_dyn_notch_count", &c.gyro.dynamicFilter.count), + Param("feature_gps", &c.featureMask, 7), + Param("feature_dyn_notch", &c.featureMask, 29), + Param("feature_motor_stop", &c.featureMask, 4), + Param("feature_rx_ppm", &c.featureMask, 0), + Param("feature_rx_serial", &c.featureMask, 3), + Param("feature_rx_spi", &c.featureMask, 25), + Param("feature_soft_serial", &c.featureMask, 6), + Param("feature_telemetry", &c.featureMask, 10), + + Param("debug_mode", &c.debug.mode, debugModeChoices), + Param("debug_axis", &c.debug.axis), + + Param("gyro_bus", &c.gyro.bus, busDevChoices), + Param("gyro_dev", &c.gyro.dev, gyroDevChoices), + Param("gyro_dlpf", &c.gyro.dlpf, gyroDlpfChoices), + Param("gyro_align", &c.gyro.align, alignChoices), + Param("gyro_lpf_type", &c.gyro.filter.type, filterTypeChoices), + Param("gyro_lpf_freq", &c.gyro.filter.freq), + Param("gyro_lpf2_type", &c.gyro.filter2.type, filterTypeChoices), + Param("gyro_lpf2_freq", &c.gyro.filter2.freq), + Param("gyro_lpf3_type", &c.gyro.filter3.type, filterTypeChoices), + Param("gyro_lpf3_freq", &c.gyro.filter3.freq), + Param("gyro_notch1_freq", &c.gyro.notch1Filter.freq), + Param("gyro_notch1_cutoff", &c.gyro.notch1Filter.cutoff), + Param("gyro_notch2_freq", &c.gyro.notch2Filter.freq), + Param("gyro_notch2_cutoff", &c.gyro.notch2Filter.cutoff), + Param("gyro_dyn_lpf_min", &c.gyro.dynLpfFilter.cutoff), + Param("gyro_dyn_lpf_max", &c.gyro.dynLpfFilter.freq), + Param("gyro_dyn_notch_q", &c.gyro.dynamicFilter.q), + Param("gyro_dyn_notch_count", &c.gyro.dynamicFilter.count), Param("gyro_dyn_notch_min", &c.gyro.dynamicFilter.min_freq), Param("gyro_dyn_notch_max", &c.gyro.dynamicFilter.max_freq), - Param("gyro_rpm_harmonics", &c.gyro.rpmFilter.harmonics), Param("gyro_rpm_q", &c.gyro.rpmFilter.q), - Param("gyro_rpm_min_freq", &c.gyro.rpmFilter.minFreq), Param("gyro_rpm_fade", &c.gyro.rpmFilter.fade), + Param("gyro_rpm_harmonics", &c.gyro.rpmFilter.harmonics), + Param("gyro_rpm_q", &c.gyro.rpmFilter.q), + Param("gyro_rpm_min_freq", &c.gyro.rpmFilter.minFreq), + Param("gyro_rpm_fade", &c.gyro.rpmFilter.fade), Param("gyro_rpm_weight_1", &c.gyro.rpmFilter.weights[0]), Param("gyro_rpm_weight_2", &c.gyro.rpmFilter.weights[1]), Param("gyro_rpm_weight_3", &c.gyro.rpmFilter.weights[2]), - Param("gyro_rpm_tlm_lpf_freq", &c.gyro.rpmFilter.freqLpf), Param("gyro_offset_x", &c.gyro.bias[0]), - Param("gyro_offset_y", &c.gyro.bias[1]), Param("gyro_offset_z", &c.gyro.bias[2]), - - Param("accel_bus", &c.accel.bus, busDevChoices), Param("accel_dev", &c.accel.dev, gyroDevChoices), - Param("accel_lpf_type", &c.accel.filter.type, filterTypeChoices), Param("accel_lpf_freq", &c.accel.filter.freq), - Param("accel_offset_x", &c.accel.bias[0]), Param("accel_offset_y", &c.accel.bias[1]), - Param("accel_offset_z", &c.accel.bias[2]), Param("accel_trim_roll", &c.accel.trim[1]), + Param("gyro_rpm_tlm_lpf_freq", &c.gyro.rpmFilter.freqLpf), + Param("gyro_offset_x", &c.gyro.bias[0]), + Param("gyro_offset_y", &c.gyro.bias[1]), + Param("gyro_offset_z", &c.gyro.bias[2]), + + Param("gyro_tuning", &c.simplifiedTuning.gyroFilter), + Param("gyro_tuning_gain", &c.simplifiedTuning.gyroFilterMultiplier), + + Param("accel_bus", &c.accel.bus, busDevChoices), + Param("accel_dev", &c.accel.dev, gyroDevChoices), + Param("accel_lpf_type", &c.accel.filter.type, filterTypeChoices), + Param("accel_lpf_freq", &c.accel.filter.freq), + Param("accel_offset_x", &c.accel.bias[0]), + Param("accel_offset_y", &c.accel.bias[1]), + Param("accel_offset_z", &c.accel.bias[2]), + Param("accel_trim_roll", &c.accel.trim[1]), Param("accel_trim_pitch", &c.accel.trim[0]), - Param("mag_bus", &c.mag.bus, busDevChoices), Param("mag_dev", &c.mag.dev, magDevChoices), - Param("mag_align", &c.mag.align, alignChoices), Param("mag_filter_type", &c.mag.filter.type, filterTypeChoices), - Param("mag_filter_lpf", &c.mag.filter.freq), Param("mag_offset_x", &c.mag.offset[0]), - Param("mag_offset_y", &c.mag.offset[1]), Param("mag_offset_z", &c.mag.offset[2]), - Param("mag_scale_x", &c.mag.scale[0]), Param("mag_scale_y", &c.mag.scale[1]), + Param("mag_bus", &c.mag.bus, busDevChoices), + Param("mag_dev", &c.mag.dev, magDevChoices), + Param("mag_align", &c.mag.align, alignChoices), + Param("mag_declination", &c.mag.declination), + Param("mag_filter_type", &c.mag.filter.type, filterTypeChoices), + Param("mag_filter_lpf", &c.mag.filter.freq), + Param("mag_offset_x", &c.mag.offset[0]), + Param("mag_offset_y", &c.mag.offset[1]), + Param("mag_offset_z", &c.mag.offset[2]), + Param("mag_scale_x", &c.mag.scale[0]), + Param("mag_scale_y", &c.mag.scale[1]), Param("mag_scale_z", &c.mag.scale[2]), - Param("baro_bus", &c.baro.bus, busDevChoices), Param("baro_dev", &c.baro.dev, baroDevChoices), - Param("baro_lpf_type", &c.baro.filter.type, filterTypeChoices), Param("baro_lpf_freq", &c.baro.filter.freq), - - Param("gps_min_sats", &c.gps.minSats), Param("gps_set_home_once", &c.gps.setHomeOnce), - - Param("gps_gnss_mode", &c.gps.gnssMode), Param("gps_enable_dual_band", &c.gps.enableDualBand), - Param("gps_enable_gps", &c.gps.enableGPS), Param("gps_enable_glonass", &c.gps.enableGLONASS), - Param("gps_enable_galileo", &c.gps.enableGalileo), Param("gps_enable_beidou", &c.gps.enableBeiDou), - Param("gps_enable_qzss", &c.gps.enableQZSS), Param("gps_enable_sbas", &c.gps.enableSBAS), - - Param("board_align_roll", &c.boardAlignment[0]), Param("board_align_pitch", &c.boardAlignment[1]), + Param("baro_bus", &c.baro.bus, busDevChoices), + Param("baro_dev", &c.baro.dev, baroDevChoices), + Param("baro_lpf_type", &c.baro.filter.type, filterTypeChoices), + Param("baro_lpf_freq", &c.baro.filter.freq), + + Param("gps_min_sats", &c.gps.minSats), + Param("gps_set_home_once", &c.gps.setHomeOnce), + + Param("gps_gnss_mode", &c.gps.gnssMode), + Param("gps_enable_dual_band", &c.gps.enableDualBand), + Param("gps_enable_gps", &c.gps.enableGPS), + Param("gps_enable_glonass", &c.gps.enableGLONASS), + Param("gps_enable_galileo", &c.gps.enableGalileo), + Param("gps_enable_beidou", &c.gps.enableBeiDou), + Param("gps_enable_qzss", &c.gps.enableQZSS), + Param("gps_enable_sbas", &c.gps.enableSBAS), + + Param("board_align_roll", &c.boardAlignment[0]), + Param("board_align_pitch", &c.boardAlignment[1]), Param("board_align_yaw", &c.boardAlignment[2]), - Param("vbat_source", &c.vbat.source, voltageSourceChoices), Param("vbat_scale", &c.vbat.scale), - Param("vbat_mul", &c.vbat.resMult), Param("vbat_div", &c.vbat.resDiv), + Param("vbat_source", &c.vbat.source, voltageSourceChoices), + Param("vbat_scale", &c.vbat.scale), + Param("vbat_mul", &c.vbat.resMult), + Param("vbat_div", &c.vbat.resDiv), Param("vbat_cell_warn", &c.vbat.cellWarning), - Param("ibat_source", &c.ibat.source, currentSourceChoices), Param("ibat_scale", &c.ibat.scale), + Param("ibat_source", &c.ibat.source, currentSourceChoices), + Param("ibat_scale", &c.ibat.scale), Param("ibat_offset", &c.ibat.offset), - Param("fusion_mode", &c.fusion.mode, fusionModeChoices), Param("fusion_gain_p", &c.fusion.gain), - Param("fusion_gain_i", &c.fusion.gainI), Param("fusion_use_mag", &c.fusion.useMag), + Param("fusion_mode", &c.fusion.mode, fusionModeChoices), + Param("fusion_gain_p", &c.fusion.gain), + Param("fusion_gain_i", &c.fusion.gainI), + Param("fusion_use_mag", &c.fusion.useMag), Param("input_rate_type", &c.input.rateType, inputRateTypeChoices), - Param("input_roll_rate", &c.input.rate[0]), Param("input_roll_srate", &c.input.superRate[0]), - Param("input_roll_expo", &c.input.expo[0]), Param("input_roll_limit", &c.input.rateLimit[0]), + Param("input_roll_rate", &c.input.rate[0]), + Param("input_roll_srate", &c.input.superRate[0]), + Param("input_roll_expo", &c.input.expo[0]), + Param("input_roll_limit", &c.input.rateLimit[0]), - Param("input_pitch_rate", &c.input.rate[1]), Param("input_pitch_srate", &c.input.superRate[1]), - Param("input_pitch_expo", &c.input.expo[1]), Param("input_pitch_limit", &c.input.rateLimit[1]), + Param("input_pitch_rate", &c.input.rate[1]), + Param("input_pitch_srate", &c.input.superRate[1]), + Param("input_pitch_expo", &c.input.expo[1]), + Param("input_pitch_limit", &c.input.rateLimit[1]), - Param("input_yaw_rate", &c.input.rate[2]), Param("input_yaw_srate", &c.input.superRate[2]), - Param("input_yaw_expo", &c.input.expo[2]), Param("input_yaw_limit", &c.input.rateLimit[2]), + Param("input_yaw_rate", &c.input.rate[2]), + Param("input_yaw_srate", &c.input.superRate[2]), + Param("input_yaw_expo", &c.input.expo[2]), + Param("input_yaw_limit", &c.input.rateLimit[2]), Param("input_deadband", &c.input.deadband), + Param("input_airmode_threshold", &c.input.airModeActivateThreshold), - Param("input_min", &c.input.minRc), Param("input_mid", &c.input.midRc), Param("input_max", &c.input.maxRc), - - Param("input_interpolation", &c.input.interpolationMode, interpolChoices), - Param("input_interpolation_interval", &c.input.interpolationInterval), + Param("input_min", &c.input.minRc), + Param("input_mid", &c.input.midRc), + Param("input_max", &c.input.maxRc), - Param("input_filter_type", &c.input.filterType, inputFilterChoices), - Param("input_lpf_type", &c.input.filter.type, filterTypeChoices), Param("input_lpf_freq", &c.input.filter.freq), + Param("input_filter", &c.input.filterEnable), + Param("input_lpf_type", &c.input.filter.type, filterTypeChoices), + Param("input_lpf_freq", &c.input.filter.freq), Param("input_lpf_factor", &c.input.filterAutoFactor), + Param("input_lpf_throttle_type", &c.input.filterThrottle.type, filterTypeChoices), + Param("input_lpf_throttle_freq", &c.input.filterThrottle.freq), + Param("input_lpf_throttle_factor", &c.input.filterAutoThrottleFactor), Param("input_ff_lpf_type", &c.input.filterDerivative.type, filterTypeChoices), Param("input_ff_lpf_freq", &c.input.filterDerivative.freq), Param("input_rssi_channel", &c.input.rssiChannel), - Param("input_0", &c.input.channel[0]), Param("input_1", &c.input.channel[1]), - Param("input_2", &c.input.channel[2]), Param("input_3", &c.input.channel[3]), - Param("input_4", &c.input.channel[4]), Param("input_5", &c.input.channel[5]), - Param("input_6", &c.input.channel[6]), Param("input_7", &c.input.channel[7]), - Param("input_8", &c.input.channel[8]), Param("input_9", &c.input.channel[9]), - Param("input_10", &c.input.channel[10]), Param("input_11", &c.input.channel[11]), - Param("input_12", &c.input.channel[12]), Param("input_13", &c.input.channel[13]), - Param("input_14", &c.input.channel[14]), Param("input_15", &c.input.channel[15]), - - Param("failsafe_delay", &c.failsafe.delay), Param("failsafe_kill_switch", &c.failsafe.killSwitch), + Param("input_0", &c.input.channel[0]), + Param("input_1", &c.input.channel[1]), + Param("input_2", &c.input.channel[2]), + Param("input_3", &c.input.channel[3]), + Param("input_4", &c.input.channel[4]), + Param("input_5", &c.input.channel[5]), + Param("input_6", &c.input.channel[6]), + Param("input_7", &c.input.channel[7]), + Param("input_8", &c.input.channel[8]), + Param("input_9", &c.input.channel[9]), + Param("input_10", &c.input.channel[10]), + Param("input_11", &c.input.channel[11]), + Param("input_12", &c.input.channel[12]), + Param("input_13", &c.input.channel[13]), + Param("input_14", &c.input.channel[14]), + Param("input_15", &c.input.channel[15]), + + Param("failsafe_delay", &c.failsafe.delay), + Param("failsafe_kill_switch", &c.failsafe.killSwitch), Param("arming_small_angle", &c.arming.smallAngle), - Param("vtx_power", &c.vtx.power), Param("vtx_channel", &c.vtx.channel), Param("vtx_band", &c.vtx.band), + Param("vtx_power", &c.vtx.power), + Param("vtx_channel", &c.vtx.channel), + Param("vtx_band", &c.vtx.band), Param("vtx_low_power_disarm", &c.vtx.lowPowerDisarm), #ifdef ESPFC_SERIAL_0 @@ -484,71 +554,109 @@ const Cli::Param* Cli::initialize(ModelConfig& c) Param("serial_usb", &c.serial[SERIAL_USB]), #endif - Param("scaler_0", &c.scaler[0]), Param("scaler_1", &c.scaler[1]), Param("scaler_2", &c.scaler[2]), + Param("scaler_0", &c.scaler[0]), + Param("scaler_1", &c.scaler[1]), + Param("scaler_2", &c.scaler[2]), - Param("mode_0", &c.conditions[0]), Param("mode_1", &c.conditions[1]), Param("mode_2", &c.conditions[2]), - Param("mode_3", &c.conditions[3]), Param("mode_4", &c.conditions[4]), Param("mode_5", &c.conditions[5]), - Param("mode_6", &c.conditions[6]), Param("mode_7", &c.conditions[7]), + Param("mode_0", &c.conditions[0]), + Param("mode_1", &c.conditions[1]), + Param("mode_2", &c.conditions[2]), + Param("mode_3", &c.conditions[3]), + Param("mode_4", &c.conditions[4]), + Param("mode_5", &c.conditions[5]), + Param("mode_6", &c.conditions[6]), + Param("mode_7", &c.conditions[7]), Param("pid_sync", &c.loopSync), - - Param("pid_roll_p", &c.pid[FC_PID_ROLL].P), Param("pid_roll_i", &c.pid[FC_PID_ROLL].I), - Param("pid_roll_d", &c.pid[FC_PID_ROLL].D), Param("pid_roll_f", &c.pid[FC_PID_ROLL].F), - - Param("pid_pitch_p", &c.pid[FC_PID_PITCH].P), Param("pid_pitch_i", &c.pid[FC_PID_PITCH].I), - Param("pid_pitch_d", &c.pid[FC_PID_PITCH].D), Param("pid_pitch_f", &c.pid[FC_PID_PITCH].F), - - Param("pid_yaw_p", &c.pid[FC_PID_YAW].P), Param("pid_yaw_i", &c.pid[FC_PID_YAW].I), - Param("pid_yaw_d", &c.pid[FC_PID_YAW].D), Param("pid_yaw_f", &c.pid[FC_PID_YAW].F), - - Param("pid_level_p", &c.pid[FC_PID_LEVEL].P), Param("pid_level_i", &c.pid[FC_PID_LEVEL].I), - Param("pid_level_d", &c.pid[FC_PID_LEVEL].D), Param("pid_level_f", &c.pid[FC_PID_LEVEL].F), - - Param("pid_level_angle_limit", &c.level.angleLimit), Param("pid_level_rate_limit", &c.level.rateLimit), + Param("pid_tuning", &c.simplifiedTuning.pidsMode, simplifiedTunigModeChoices), + Param("pid_tuning_gain", &c.simplifiedTuning.masterMultiplier), + Param("pid_tuning_rp_ratio", &c.simplifiedTuning.rollPitchRatio), + Param("pid_tuning_i_gain", &c.simplifiedTuning.iGain), + Param("pid_tuning_d_gain", &c.simplifiedTuning.dGain), + Param("pid_tuning_pi_gain", &c.simplifiedTuning.piGain), + // Param("pid_tuning_d_max_gain", &c.simplifiedTuning.dMaxGain), + Param("pid_tuning_ff_gain", &c.simplifiedTuning.ffGain), + Param("pid_tuning_pitch_pi_gain", &c.simplifiedTuning.pitchPiGain), + + Param("pid_roll_p", &c.pid[FC_PID_ROLL].P), + Param("pid_roll_i", &c.pid[FC_PID_ROLL].I), + Param("pid_roll_d", &c.pid[FC_PID_ROLL].D), + Param("pid_roll_f", &c.pid[FC_PID_ROLL].F), + + Param("pid_pitch_p", &c.pid[FC_PID_PITCH].P), + Param("pid_pitch_i", &c.pid[FC_PID_PITCH].I), + Param("pid_pitch_d", &c.pid[FC_PID_PITCH].D), + Param("pid_pitch_f", &c.pid[FC_PID_PITCH].F), + + Param("pid_yaw_p", &c.pid[FC_PID_YAW].P), + Param("pid_yaw_i", &c.pid[FC_PID_YAW].I), + Param("pid_yaw_d", &c.pid[FC_PID_YAW].D), + Param("pid_yaw_f", &c.pid[FC_PID_YAW].F), + + Param("pid_level_p", &c.pid[FC_PID_LEVEL].P), + Param("pid_level_i", &c.pid[FC_PID_LEVEL].I), + Param("pid_level_d", &c.pid[FC_PID_LEVEL].D), + Param("pid_level_f", &c.pid[FC_PID_LEVEL].F), + + Param("pid_level_angle_limit", &c.level.angleLimit), + Param("pid_level_rate_limit", &c.level.rateLimit), Param("pid_level_lpf_type", &c.level.ptermFilter.type, filterTypeChoices), Param("pid_level_lpf_freq", &c.level.ptermFilter.freq), - Param("pid_althold_vel_p", &c.pid[FC_PID_VEL].P), Param("pid_althold_vel_i", &c.pid[FC_PID_VEL].I), - Param("pid_althold_vel_d", &c.pid[FC_PID_VEL].D), Param("pid_althold_vel_f", &c.pid[FC_PID_VEL].F), + Param("pid_althold_vel_p", &c.pid[FC_PID_VEL].P), + Param("pid_althold_vel_i", &c.pid[FC_PID_VEL].I), + Param("pid_althold_vel_d", &c.pid[FC_PID_VEL].D), + Param("pid_althold_vel_f", &c.pid[FC_PID_VEL].F), Param("pid_althold_iterm_center", &c.altHold.itermCenter), - Param("pid_althold_iterm_range", &c.altHold.itermRange), Param("pid_althold_baro_tau", &c.altHold.baroTau), + Param("pid_althold_iterm_range", &c.altHold.itermRange), + Param("pid_althold_baro_tau", &c.altHold.baroTau), - Param("pid_yaw_lpf_type", &c.yaw.filter.type, filterTypeChoices), Param("pid_yaw_lpf_freq", &c.yaw.filter.freq), + Param("pid_yaw_lpf_type", &c.yaw.filter.type, filterTypeChoices), + Param("pid_yaw_lpf_freq", &c.yaw.filter.freq), Param("pid_dterm_lpf_type", &c.dterm.filter.type, filterTypeChoices), Param("pid_dterm_lpf_freq", &c.dterm.filter.freq), Param("pid_dterm_lpf2_type", &c.dterm.filter2.type, filterTypeChoices), - Param("pid_dterm_lpf2_freq", &c.dterm.filter2.freq), Param("pid_dterm_notch_freq", &c.dterm.notchFilter.freq), + Param("pid_dterm_lpf2_freq", &c.dterm.filter2.freq), + Param("pid_dterm_notch_freq", &c.dterm.notchFilter.freq), Param("pid_dterm_notch_cutoff", &c.dterm.notchFilter.cutoff), Param("pid_dterm_dyn_lpf_min", &c.dterm.dynLpfFilter.cutoff), Param("pid_dterm_dyn_lpf_max", &c.dterm.dynLpfFilter.freq), + Param("pid_dterm_tuning", &c.simplifiedTuning.dtermFilter), + Param("pid_dterm_tuning_gain", &c.simplifiedTuning.dtermFilterMultiplier), - Param("pid_dterm_weight", &c.dterm.setpointWeight), Param("pid_iterm_limit", &c.iterm.limit), + Param("pid_iterm_limit", &c.iterm.limit), Param("pid_iterm_zero", &c.iterm.lowThrottleZeroIterm), Param("pid_iterm_relax", &c.iterm.relax, inputItermRelaxChoices), - Param("pid_iterm_relax_cutoff", &c.iterm.relaxCutoff), Param("pid_tpa_scale", &c.controller.tpaScale), + Param("pid_iterm_relax_cutoff", &c.iterm.relaxCutoff), + + Param("pid_tpa_mode", &c.controller.tpaMode, tpaModeChoices), + Param("pid_tpa_scale", &c.controller.tpaScale), Param("pid_tpa_breakpoint", &c.controller.tpaBreakpoint), - Param("mixer_sync", &c.mixerSync), Param("mixer_type", &c.mixer.type, mixerTypeChoices), + Param("mixer_sync", &c.mixerSync), + Param("mixer_type", &c.mixer.type, mixerTypeChoices), Param("mixer_yaw_reverse", &c.mixer.yawReverse), Param("mixer_throttle_limit_type", &c.output.throttleLimitType, throtleLimitTypeChoices), Param("mixer_throttle_limit_percent", &c.output.throttleLimitPercent), Param("mixer_output_limit", &c.output.motorLimit), - Param("output_motor_protocol", &c.output.protocol, protocolChoices), Param("output_motor_async", &c.output.async), + Param("output_motor_protocol", &c.output.protocol, protocolChoices), + Param("output_motor_async", &c.output.async), Param("output_motor_rate", &c.output.rate), + Param("output_motor_idle", &c.output.motorIdle), #ifdef ESPFC_DSHOT_TELEMETRY Param("output_motor_poles", &c.output.motorPoles), + Param("output_dshot_telemetry", &c.output.dshotTelemetry), #endif Param("output_servo_rate", &c.output.servoRate), - Param("output_min_command", &c.output.minCommand), Param("output_min_throttle", &c.output.minThrottle), - Param("output_max_throttle", &c.output.maxThrottle), Param("output_dshot_idle", &c.output.dshotIdle), -#ifdef ESPFC_DSHOT_TELEMETRY - Param("output_dshot_telemetry", &c.output.dshotTelemetry), -#endif - Param("output_0", &c.output.channel[0]), Param("output_1", &c.output.channel[1]), - Param("output_2", &c.output.channel[2]), Param("output_3", &c.output.channel[3]), + Param("output_min_command", &c.output.minCommand), + Param("output_max_throttle", &c.output.maxThrottle), + Param("output_0", &c.output.channel[0]), + Param("output_1", &c.output.channel[1]), + Param("output_2", &c.output.channel[2]), + Param("output_3", &c.output.channel[3]), #if ESPFC_OUTPUT_COUNT > 4 Param("output_4", &c.output.channel[4]), #endif @@ -564,8 +672,10 @@ const Cli::Param* Cli::initialize(ModelConfig& c) #ifdef ESPFC_INPUT Param("pin_input_rx", &c.pin[PIN_INPUT_RX]), #endif - Param("pin_output_0", &c.pin[PIN_OUTPUT_0]), Param("pin_output_1", &c.pin[PIN_OUTPUT_1]), - Param("pin_output_2", &c.pin[PIN_OUTPUT_2]), Param("pin_output_3", &c.pin[PIN_OUTPUT_3]), + Param("pin_output_0", &c.pin[PIN_OUTPUT_0]), + Param("pin_output_1", &c.pin[PIN_OUTPUT_1]), + Param("pin_output_2", &c.pin[PIN_OUTPUT_2]), + Param("pin_output_3", &c.pin[PIN_OUTPUT_3]), #if ESPFC_OUTPUT_COUNT > 4 Param("pin_output_4", &c.pin[PIN_OUTPUT_4]), #endif @@ -578,19 +688,24 @@ const Cli::Param* Cli::initialize(ModelConfig& c) #if ESPFC_OUTPUT_COUNT > 7 Param("pin_output_7", &c.pin[PIN_OUTPUT_7]), #endif - Param("pin_button", &c.pin[PIN_BUTTON]), Param("pin_buzzer", &c.pin[PIN_BUZZER]), + Param("pin_button", &c.pin[PIN_BUTTON]), + Param("pin_buzzer", &c.pin[PIN_BUZZER]), Param("pin_led", &c.pin[PIN_LED_BLINK]), #if defined(ESPFC_SERIAL_0) && defined(ESPFC_SERIAL_REMAP_PINS) - Param("pin_serial_0_tx", &c.pin[PIN_SERIAL_0_TX]), Param("pin_serial_0_rx", &c.pin[PIN_SERIAL_0_RX]), + Param("pin_serial_0_tx", &c.pin[PIN_SERIAL_0_TX]), + Param("pin_serial_0_rx", &c.pin[PIN_SERIAL_0_RX]), #endif #if defined(ESPFC_SERIAL_1) && defined(ESPFC_SERIAL_REMAP_PINS) - Param("pin_serial_1_tx", &c.pin[PIN_SERIAL_1_TX]), Param("pin_serial_1_rx", &c.pin[PIN_SERIAL_1_RX]), + Param("pin_serial_1_tx", &c.pin[PIN_SERIAL_1_TX]), + Param("pin_serial_1_rx", &c.pin[PIN_SERIAL_1_RX]), #endif #if defined(ESPFC_SERIAL_2) && defined(ESPFC_SERIAL_REMAP_PINS) - Param("pin_serial_2_tx", &c.pin[PIN_SERIAL_2_TX]), Param("pin_serial_2_rx", &c.pin[PIN_SERIAL_2_RX]), + Param("pin_serial_2_tx", &c.pin[PIN_SERIAL_2_TX]), + Param("pin_serial_2_rx", &c.pin[PIN_SERIAL_2_RX]), #endif #ifdef ESPFC_I2C_0 - Param("pin_i2c_scl", &c.pin[PIN_I2C_0_SCL]), Param("pin_i2c_sda", &c.pin[PIN_I2C_0_SDA]), + Param("pin_i2c_scl", &c.pin[PIN_I2C_0_SCL]), + Param("pin_i2c_sda", &c.pin[PIN_I2C_0_SDA]), #endif #ifdef ESPFC_ADC_0 Param("pin_input_adc_0", &c.pin[PIN_INPUT_ADC_0]), @@ -599,11 +714,15 @@ const Cli::Param* Cli::initialize(ModelConfig& c) Param("pin_input_adc_1", &c.pin[PIN_INPUT_ADC_1]), #endif #ifdef ESPFC_SPI_0 - Param("pin_spi_0_sck", &c.pin[PIN_SPI_0_SCK]), Param("pin_spi_0_mosi", &c.pin[PIN_SPI_0_MOSI]), - Param("pin_spi_0_miso", &c.pin[PIN_SPI_0_MISO]), Param("pin_spi_cs_0", &c.pin[PIN_SPI_CS0]), - Param("pin_spi_cs_1", &c.pin[PIN_SPI_CS1]), Param("pin_spi_cs_2", &c.pin[PIN_SPI_CS2]), + Param("pin_spi_0_sck", &c.pin[PIN_SPI_0_SCK]), + Param("pin_spi_0_mosi", &c.pin[PIN_SPI_0_MOSI]), + Param("pin_spi_0_miso", &c.pin[PIN_SPI_0_MISO]), + Param("pin_spi_cs_0", &c.pin[PIN_SPI_CS0]), + Param("pin_spi_cs_1", &c.pin[PIN_SPI_CS1]), + Param("pin_spi_cs_2", &c.pin[PIN_SPI_CS2]), #endif - Param("pin_buzzer_invert", &c.buzzer.inverted), Param("pin_led_invert", &c.led.invert), + Param("pin_buzzer_invert", &c.buzzer.inverted), + Param("pin_led_invert", &c.led.invert), Param("pin_led_type", &c.led.type, ledTypeChoices), #ifdef ESPFC_I2C_0 @@ -615,7 +734,8 @@ const Cli::Param* Cli::initialize(ModelConfig& c) Param("telemetry_interval", &c.telemetryInterval), Param("blackbox_dev", &c.blackbox.dev, blackboxDevChoices), - Param("blackbox_mode", &c.blackbox.mode, blackboxModeChoices), Param("blackbox_rate", &c.blackbox.pDenom), + Param("blackbox_mode", &c.blackbox.mode, blackboxModeChoices), + Param("blackbox_rate", &c.blackbox.pDenom), Param("blackbox_log_acc", &c.blackbox.fieldsMask, BLACKBOX_FIELD_ACC), Param("blackbox_log_alt", &c.blackbox.fieldsMask, BLACKBOX_FIELD_ALTITUDE), Param("blackbox_log_bat", &c.blackbox.fieldsMask, BLACKBOX_FIELD_BATTERY), @@ -639,31 +759,76 @@ const Cli::Param* Cli::initialize(ModelConfig& c) Param("wifi_tcp_port", &c.wireless.port), #endif - Param("mix_outputs", &c.customMixerCount), Param("mix_0", &c.customMixes[i++]), - Param("mix_1", &c.customMixes[i++]), Param("mix_2", &c.customMixes[i++]), Param("mix_3", &c.customMixes[i++]), - Param("mix_4", &c.customMixes[i++]), Param("mix_5", &c.customMixes[i++]), Param("mix_6", &c.customMixes[i++]), - Param("mix_7", &c.customMixes[i++]), Param("mix_8", &c.customMixes[i++]), Param("mix_9", &c.customMixes[i++]), - Param("mix_10", &c.customMixes[i++]), Param("mix_11", &c.customMixes[i++]), Param("mix_12", &c.customMixes[i++]), - Param("mix_13", &c.customMixes[i++]), Param("mix_14", &c.customMixes[i++]), Param("mix_15", &c.customMixes[i++]), - Param("mix_16", &c.customMixes[i++]), Param("mix_17", &c.customMixes[i++]), Param("mix_18", &c.customMixes[i++]), - Param("mix_19", &c.customMixes[i++]), Param("mix_20", &c.customMixes[i++]), Param("mix_21", &c.customMixes[i++]), - Param("mix_22", &c.customMixes[i++]), Param("mix_23", &c.customMixes[i++]), Param("mix_24", &c.customMixes[i++]), - Param("mix_25", &c.customMixes[i++]), Param("mix_26", &c.customMixes[i++]), Param("mix_27", &c.customMixes[i++]), - Param("mix_28", &c.customMixes[i++]), Param("mix_29", &c.customMixes[i++]), Param("mix_30", &c.customMixes[i++]), - Param("mix_31", &c.customMixes[i++]), Param("mix_32", &c.customMixes[i++]), Param("mix_33", &c.customMixes[i++]), - Param("mix_34", &c.customMixes[i++]), Param("mix_35", &c.customMixes[i++]), Param("mix_36", &c.customMixes[i++]), - Param("mix_37", &c.customMixes[i++]), Param("mix_38", &c.customMixes[i++]), Param("mix_39", &c.customMixes[i++]), - Param("mix_40", &c.customMixes[i++]), Param("mix_41", &c.customMixes[i++]), Param("mix_42", &c.customMixes[i++]), - Param("mix_43", &c.customMixes[i++]), Param("mix_44", &c.customMixes[i++]), Param("mix_45", &c.customMixes[i++]), - Param("mix_46", &c.customMixes[i++]), Param("mix_47", &c.customMixes[i++]), Param("mix_48", &c.customMixes[i++]), - Param("mix_49", &c.customMixes[i++]), Param("mix_50", &c.customMixes[i++]), Param("mix_51", &c.customMixes[i++]), - Param("mix_52", &c.customMixes[i++]), Param("mix_53", &c.customMixes[i++]), Param("mix_54", &c.customMixes[i++]), - Param("mix_55", &c.customMixes[i++]), Param("mix_56", &c.customMixes[i++]), Param("mix_57", &c.customMixes[i++]), - Param("mix_58", &c.customMixes[i++]), Param("mix_59", &c.customMixes[i++]), Param("mix_60", &c.customMixes[i++]), - Param("mix_61", &c.customMixes[i++]), Param("mix_62", &c.customMixes[i++]), Param("mix_63", &c.customMixes[i++]), + Param("mix_outputs", &c.customMixerCount), + Param("mix_0", &c.customMixes[i++]), + Param("mix_1", &c.customMixes[i++]), + Param("mix_2", &c.customMixes[i++]), + Param("mix_3", &c.customMixes[i++]), + Param("mix_4", &c.customMixes[i++]), + Param("mix_5", &c.customMixes[i++]), + Param("mix_6", &c.customMixes[i++]), + Param("mix_7", &c.customMixes[i++]), + Param("mix_8", &c.customMixes[i++]), + Param("mix_9", &c.customMixes[i++]), + Param("mix_10", &c.customMixes[i++]), + Param("mix_11", &c.customMixes[i++]), + Param("mix_12", &c.customMixes[i++]), + Param("mix_13", &c.customMixes[i++]), + Param("mix_14", &c.customMixes[i++]), + Param("mix_15", &c.customMixes[i++]), + Param("mix_16", &c.customMixes[i++]), + Param("mix_17", &c.customMixes[i++]), + Param("mix_18", &c.customMixes[i++]), + Param("mix_19", &c.customMixes[i++]), + Param("mix_20", &c.customMixes[i++]), + Param("mix_21", &c.customMixes[i++]), + Param("mix_22", &c.customMixes[i++]), + Param("mix_23", &c.customMixes[i++]), + Param("mix_24", &c.customMixes[i++]), + Param("mix_25", &c.customMixes[i++]), + Param("mix_26", &c.customMixes[i++]), + Param("mix_27", &c.customMixes[i++]), + Param("mix_28", &c.customMixes[i++]), + Param("mix_29", &c.customMixes[i++]), + Param("mix_30", &c.customMixes[i++]), + Param("mix_31", &c.customMixes[i++]), + Param("mix_32", &c.customMixes[i++]), + Param("mix_33", &c.customMixes[i++]), + Param("mix_34", &c.customMixes[i++]), + Param("mix_35", &c.customMixes[i++]), + Param("mix_36", &c.customMixes[i++]), + Param("mix_37", &c.customMixes[i++]), + Param("mix_38", &c.customMixes[i++]), + Param("mix_39", &c.customMixes[i++]), + Param("mix_40", &c.customMixes[i++]), + Param("mix_41", &c.customMixes[i++]), + Param("mix_42", &c.customMixes[i++]), + Param("mix_43", &c.customMixes[i++]), + Param("mix_44", &c.customMixes[i++]), + Param("mix_45", &c.customMixes[i++]), + Param("mix_46", &c.customMixes[i++]), + Param("mix_47", &c.customMixes[i++]), + Param("mix_48", &c.customMixes[i++]), + Param("mix_49", &c.customMixes[i++]), + Param("mix_50", &c.customMixes[i++]), + Param("mix_51", &c.customMixes[i++]), + Param("mix_52", &c.customMixes[i++]), + Param("mix_53", &c.customMixes[i++]), + Param("mix_54", &c.customMixes[i++]), + Param("mix_55", &c.customMixes[i++]), + Param("mix_56", &c.customMixes[i++]), + Param("mix_57", &c.customMixes[i++]), + Param("mix_58", &c.customMixes[i++]), + Param("mix_59", &c.customMixes[i++]), + Param("mix_60", &c.customMixes[i++]), + Param("mix_61", &c.customMixes[i++]), + Param("mix_62", &c.customMixes[i++]), + Param("mix_63", &c.customMixes[i++]), Param() // terminate }; + // clang-format on + return params; } @@ -674,47 +839,86 @@ bool Cli::process(const char c, CliCmd& cmd, Stream& stream) { // FIXME: detect disconnection _active = true; + _interactive = true; stream.println(); stream.println("Entering CLI Mode, type 'exit' to return, or 'help'"); stream.print("# "); printVersion(stream); stream.println(); _model.setArmingDisabled(ARMING_DISABLED_CLI, true); - cmd = CliCmd(); + cmd = {}; return true; } - if (_active && c == 4) // CTRL-D + + // non-interactive session enter byte 0x02 + if (c == 0x02) + { + _active = true; + stream.write(0x02); + cmd = {}; + return true; + } + // non-interactive session exit byte 0x03 + if (c == 0x03) + { + _active = false; + stream.write(0x03); + cmd = {}; + return true; + } + + // CTRL-D + if (c == 0x04) { stream.println(); - stream.println(" #leaving CLI mode, unsaved changes lost"); + stream.println("# leaving CLI mode, unsaved changes lost"); _active = false; - cmd = CliCmd(); + _interactive = false; + _model.setArmingDisabled(ARMING_DISABLED_CLI, false); + cmd = {}; return true; } + // execute on end line bool endl = c == '\n' || c == '\r'; if (cmd.index && endl) { parse(cmd); execute(cmd, stream); - cmd = CliCmd(); + cmd = {}; return true; } + // ignore comments if (c == '#') + { _ignore = true; + } else if (endl) + { _ignore = false; + } // don't put characters into buffer in specific conditions - if (_ignore || endl || cmd.index >= CLI_BUFF_SIZE - 1) return false; + if (_ignore || endl || cmd.index >= CLI_BUFF_SIZE - 1) + { + return false; + } if (c == '\b') // handle backspace { - cmd.buff[--cmd.index] = '\0'; + if (cmd.index) + { + cmd.buff[--cmd.index] = '\0'; + } } else { + if (!_active) + { + _active = true; + _interactive = true; + } cmd.buff[cmd.index] = c; cmd.buff[++cmd.index] = '\0'; } @@ -724,29 +928,33 @@ bool Cli::process(const char c, CliCmd& cmd, Stream& stream) void Cli::parse(CliCmd& cmd) { const char* DELIM = " \t"; - char* pch = strtok(cmd.buff, DELIM); + char* pch = std::strtok(cmd.buff, DELIM); size_t count = 0; while (pch) { cmd.args[count++] = pch; - pch = strtok(nullptr, DELIM); + pch = std::strtok(nullptr, DELIM); + if (count >= CLI_ARGS_SIZE) break; } } void Cli::execute(CliCmd& cmd, Stream& s) { - if (cmd.args[0]) s.print("# "); - for (size_t i = 0; i < CLI_ARGS_SIZE; ++i) + if (_interactive) { - if (!cmd.args[i]) break; - s.print(cmd.args[i]); - s.print(' '); + if (cmd.args[0]) s.print("# "); + for (size_t i = 0; i < CLI_ARGS_SIZE; ++i) + { + if (!cmd.args[i]) break; + s.print(cmd.args[i]); + s.print(' '); + } + s.println(); } - s.println(); if (!cmd.args[0]) return; - if (strcmp(cmd.args[0], "help") == 0) + if (std::strcmp(cmd.args[0], "help") == 0) { static const char* const helps[] = {"available commands:", " help", " dump", " get param", " set param value ...", " cal [gyro]", " defaults", " save", " reboot", " scaler", " mixer", " stats", @@ -759,13 +967,13 @@ void Cli::execute(CliCmd& cmd, Stream& s) s.println(*ptr); } } - else if (strcmp(cmd.args[0], "version") == 0) + else if (std::strcmp(cmd.args[0], "version") == 0) { printVersion(s); s.println(); } #if defined(ESPFC_WIFI) || defined(ESPFC_WIFI_ALT) - else if (strcmp(cmd.args[0], "wifi") == 0) + else if (std::strcmp(cmd.args[0], "wifi") == 0) { s.print("ST IP4: tcp://"); s.print(WiFi.localIP()); @@ -789,7 +997,7 @@ void Cli::execute(CliCmd& cmd, Stream& s) } #endif #if defined(ESPFC_FREE_RTOS) - else if (strcmp(cmd.args[0], "tasks") == 0) + else if (std::strcmp(cmd.args[0], "tasks") == 0) { printVersion(s); s.println(); @@ -801,7 +1009,7 @@ void Cli::execute(CliCmd& cmd, Stream& s) s.println(); } #endif - else if (strcmp(cmd.args[0], "devinfo") == 0) + else if (std::strcmp(cmd.args[0], "devinfo") == 0) { printVersion(s); s.println(); @@ -817,8 +1025,20 @@ void Cli::execute(CliCmd& cmd, Stream& s) s.print(", "); s.println(targetFreeHeap()); } - else if (strcmp(cmd.args[0], "get") == 0) + else if (std::strcmp(cmd.args[0], "get") == 0) { + if (cmd.args[1] && std::strcmp(cmd.args[1], "mag_calibration") == 0) + { + // BF specific required by configurator + s.print("mag_calibration = "); + s.print(lrintf(_model.state.mag.calibrationOffset[0] * 10.f)); + s.print(","); + s.print(lrintf(_model.state.mag.calibrationOffset[1] * 10.f)); + s.print(","); + s.print(lrintf(_model.state.mag.calibrationOffset[2] * 10.f)); + s.println(); + return; + } bool found = false; for (size_t i = 0; _params[i].name; ++i) { @@ -836,7 +1056,7 @@ void Cli::execute(CliCmd& cmd, Stream& s) } s.println(); } - else if (strcmp(cmd.args[0], "set") == 0) + else if (std::strcmp(cmd.args[0], "set") == 0) { if (!cmd.args[1]) { @@ -847,7 +1067,7 @@ void Cli::execute(CliCmd& cmd, Stream& s) bool found = false; for (size_t i = 0; _params[i].name; ++i) { - if (strcmp(cmd.args[1], _params[i].name) == 0) + if (std::strcmp(cmd.args[1], _params[i].name) == 0) { _params[i].update(cmd.args); print(_params[i], s); @@ -861,7 +1081,7 @@ void Cli::execute(CliCmd& cmd, Stream& s) s.println(cmd.args[1]); } } - else if (strcmp(cmd.args[0], "dump") == 0) + else if (std::strcmp(cmd.args[0], "dump") == 0) { s.println("defaults"); for (size_t i = 0; _params[i].name; ++i) @@ -870,7 +1090,48 @@ void Cli::execute(CliCmd& cmd, Stream& s) } s.println("save"); } - else if (strcmp(cmd.args[0], "cal") == 0) + else if (std::strcmp(cmd.args[0], "sensor_hardware") == 0) + { + // BF specific required by configurator + s.print("gyro: "); + const auto* gyroAccNames = Device::GyroDevice::getNames(); + for (size_t i = 0; gyroAccNames[i]; ++i) + { + if (i) s.print(','); + s.print(gyroAccNames[i]); + } + s.println(); + + s.print("acc: "); + for (size_t i = 0; gyroAccNames[i]; i++) + { + if (i) s.print(','); + s.print(gyroAccNames[i]); + } + s.println(); + + s.print("baro: "); + const auto* baroNames = Device::BaroDevice::getNames(); + for (size_t i = 0; baroNames[i]; ++i) + { + if (i) s.print(','); + s.print(baroNames[i]); + } + s.println(); + + s.print("mag: "); + const auto* magNames = Device::MagDevice::getNames(); + for (size_t i = 0; magNames[i]; ++i) + { + if (i) s.print(','); + s.print(magNames[i]); + } + s.println(); + + s.println("rangefinder: NONE"); + s.println("opticalflow: NONE"); + } + else if (std::strcmp(cmd.args[0], "cal") == 0) { if (!cmd.args[1]) { @@ -930,29 +1191,29 @@ void Cli::execute(CliCmd& cmd, Stream& s) s.print(_model.state.mag.calibrationScale[2]); s.println("]"); } - else if (strcmp(cmd.args[1], "gyro") == 0) + else if (std::strcmp(cmd.args[1], "gyro") == 0) { if (!_model.isModeActive(MODE_ARMED)) _model.calibrateGyro(); s.println("OK"); } - else if (strcmp(cmd.args[1], "mag") == 0) + else if (std::strcmp(cmd.args[1], "mag") == 0) { if (!_model.isModeActive(MODE_ARMED)) _model.calibrateMag(); s.println("OK"); } else { - if (strcmp(cmd.args[1], "reset_accel") == 0 || strcmp(cmd.args[1], "reset_all") == 0) + if (std::strcmp(cmd.args[1], "reset_accel") == 0 || std::strcmp(cmd.args[1], "reset_all") == 0) { _model.state.accel.bias = {}; s.println("OK"); } - if (strcmp(cmd.args[1], "reset_gyro") == 0 || strcmp(cmd.args[1], "reset_all") == 0) + if (std::strcmp(cmd.args[1], "reset_gyro") == 0 || std::strcmp(cmd.args[1], "reset_all") == 0) { _model.state.gyro.bias = {}; s.println("OK"); } - if (strcmp(cmd.args[1], "reset_mag") == 0 || strcmp(cmd.args[1], "reset_all") == 0) + if (std::strcmp(cmd.args[1], "reset_mag") == 0 || std::strcmp(cmd.args[1], "reset_all") == 0) { _model.state.mag.calibrationOffset = {}; _model.state.mag.calibrationScale = {1.f, 1.f, 1.f}; @@ -960,14 +1221,14 @@ void Cli::execute(CliCmd& cmd, Stream& s) } } } - else if (strcmp(cmd.args[0], "gps") == 0) + else if (std::strcmp(cmd.args[0], "gps") == 0) { - if (cmd.args[1] && strcmp(cmd.args[1], "set_home") == 0) + if (cmd.args[1] && std::strcmp(cmd.args[1], "set_home") == 0) { _model.setGpsHome(true); s.println(_model.state.gps.homeSet ? "Home position set" : "No GPS fix"); } - else if (cmd.args[1] && strcmp(cmd.args[1], "clear_home") == 0) + else if (cmd.args[1] && std::strcmp(cmd.args[1], "clear_home") == 0) { _model.state.gps.homeSet = false; s.println("Home position cleared"); @@ -977,13 +1238,13 @@ void Cli::execute(CliCmd& cmd, Stream& s) printGpsStatus(s, true); } } - else if (strcmp(cmd.args[0], "preset") == 0) + else if (std::strcmp(cmd.args[0], "preset") == 0) { if (!cmd.args[1]) { s.println("Available presets: scaler, modes, micrus, brobot"); } - else if (strcmp(cmd.args[1], "scaler") == 0) + else if (std::strcmp(cmd.args[1], "scaler") == 0) { _model.config.scaler[0].dimension = (ScalerDimension)(ACT_INNER_P | ACT_AXIS_PITCH | ACT_AXIS_ROLL); _model.config.scaler[0].channel = 5; @@ -1002,7 +1263,7 @@ void Cli::execute(CliCmd& cmd, Stream& s) s.println("OK"); } - else if (strcmp(cmd.args[1], "modes") == 0) + else if (std::strcmp(cmd.args[1], "modes") == 0) { _model.config.conditions[0].id = MODE_ARMED; _model.config.conditions[0].ch = AXIS_AUX_1 + 0; @@ -1021,11 +1282,11 @@ void Cli::execute(CliCmd& cmd, Stream& s) s.println("OK"); } - else if (strcmp(cmd.args[1], "micrus") == 0) + else if (std::strcmp(cmd.args[1], "micrus") == 0) { s.println("OK"); } - else if (strcmp(cmd.args[1], "brobot") == 0) + else if (std::strcmp(cmd.args[1], "brobot") == 0) { s.println("OK"); } @@ -1034,18 +1295,18 @@ void Cli::execute(CliCmd& cmd, Stream& s) s.println("NOT OK"); } } - else if (strcmp(cmd.args[0], "load") == 0) + else if (std::strcmp(cmd.args[0], "load") == 0) { _model.load(); s.println("OK"); } - else if (strcmp(cmd.args[0], "save") == 0) + else if (std::strcmp(cmd.args[0], "save") == 0) { _model.save(); s.println("# Saved, type reboot to apply changes"); s.println(); } - else if (strcmp(cmd.args[0], "eeprom") == 0) + else if (std::strcmp(cmd.args[0], "eeprom") == 0) { /* int start = 0; @@ -1071,7 +1332,7 @@ void Cli::execute(CliCmd& cmd, Stream& s) s.println(); */ } - else if (strcmp(cmd.args[0], "scaler") == 0) + else if (std::strcmp(cmd.args[0], "scaler") == 0) { for (size_t i = 0; i < SCALER_COUNT; i++) { @@ -1096,7 +1357,7 @@ void Cli::execute(CliCmd& cmd, Stream& s) s.println(scale); } } - else if (strcmp(cmd.args[0], "mixer") == 0) + else if (std::strcmp(cmd.args[0], "mixer") == 0) { const MixerConfig& mixer = _model.state.currentMixer; s.print("set mix_outputs "); @@ -1112,7 +1373,7 @@ void Cli::execute(CliCmd& cmd, Stream& s) if (mixer.mixes[i].src == MIXER_SOURCE_NULL) break; } } - else if (strcmp(cmd.args[0], "status") == 0) + else if (std::strcmp(cmd.args[0], "status") == 0) { printVersion(s); s.println(); @@ -1185,35 +1446,23 @@ void Cli::execute(CliCmd& cmd, Stream& s) s.print(" Hz, "); s.print(_model.state.input.autoFreq); s.print(" Hz, "); - s.println(_model.state.input.autoFactor); - - static const char* armingDisableNames[] = {"NO_GYRO", - "FAILSAFE", - "RX_FAILSAFE", - "BAD_RX_RECOVERY", - "BOXFAILSAFE", - "RUNAWAY_TAKEOFF", - "CRASH_DETECTED", - "THROTTLE", - "ANGLE", - "BOOT_GRACE_TIME", - "NOPREARM", - "LOAD", - "CALIBRATING", - "CLI", - "CMS_MENU", - "BST", - "MSP", - "PARALYZE", - "GPS", - "RESC", - "RPMFILTER", - "REBOOT_REQUIRED", - "DSHOT_BITBANG", - "ACC_CALIBRATION", - "MOTOR_PROTOCOL", - "ARM_SWITCH"}; - const size_t armingDisableNamesLength = std::size(armingDisableNames); + s.print(_model.state.input.autoFactor); + s.print(", "); + s.print(_model.state.input.autoThrottleFreq); + s.print(" Hz, "); + s.println(_model.state.input.autoThrottleFactor); + + // clang-format off + static const char* armingDisableNames[] = { + "NO_GYRO", "FAILSAFE", "RX_FAILSAFE", "BAD_RX_RECOVERY", "BOXFAILSAFE", "RUNAWAY_TAKEOFF", "CRASH_DETECTED", + "THROTTLE", "ANGLE", "BOOT_GRACE_TIME", "NOPREARM", "LOAD", "CALIBRATING", "CLI", "CMS_MENU", "BST", + "MSP", "PARALYZE", "GPS", "RESC", "RPMFILTER", "REBOOT_REQUIRED", "DSHOT_BITBANG", "ACC_CALIBRATION", + "MOTOR_PROTOCOL", "CRASHFLIP", "ALTHOLD", "POSHOLD", "AUTOPILOT", "ARM_SWITCH" + }; + // clang-format on + constexpr size_t armingDisableNamesLength = std::size(armingDisableNames); + static_assert(armingDisableNamesLength == ARMING_DISABLED_FLAGS_COUNT, + "armingDisableNamesLength != ARMING_DISABLED_FLAGS_COUNT"); s.print(" arm flags:"); for (size_t i = 0; i < armingDisableNamesLength; i++) @@ -1233,7 +1482,7 @@ void Cli::execute(CliCmd& cmd, Stream& s) s.print(millis() * 0.001, 1); s.println(); } - else if (strcmp(cmd.args[0], "stats") == 0) + else if (std::strcmp(cmd.args[0], "stats") == 0) { printVersion(s); s.println(); @@ -1278,16 +1527,17 @@ void Cli::execute(CliCmd& cmd, Stream& s) s.print("%"); s.println(); } - else if (strcmp(cmd.args[0], "reboot") == 0 || strcmp(cmd.args[0], "exit") == 0) + else if (std::strcmp(cmd.args[0], "reboot") == 0 || std::strcmp(cmd.args[0], "exit") == 0) { _active = false; + _interactive = false; Hardware::restart(_model); } - else if (strcmp(cmd.args[0], "defaults") == 0) + else if (std::strcmp(cmd.args[0], "defaults") == 0) { _model.reset(); } - else if (strcmp(cmd.args[0], "motors") == 0) + else if (std::strcmp(cmd.args[0], "motors") == 0) { s.print("count: "); s.println(getMotorCount()); @@ -1309,14 +1559,64 @@ void Cli::execute(CliCmd& cmd, Stream& s) } } } - else if (strcmp(cmd.args[0], "logs") == 0) + else if (std::strcmp(cmd.args[0], "logs") == 0) { s.print(_model.logger.c_str()); s.print("usage: "); s.println(_model.logger.length()); } + else if (std::strcmp(cmd.args[0], "tuning") == 0) + { + const auto& st = _model.config.simplifiedTuning; + const auto& pid = _model.config.pid; + PidConfig res[3] = {pid[0], pid[1], pid[2]}; + _model.calculateSimplifiedPids(st, res); + // clang-format off + s.print("X: "); s.print(st.pidsMode); s.print(", M: "); s.print(st.masterMultiplier); + s.print(", R/P: "); s.println(st.rollPitchRatio); + s.print("PI: "); s.print(st.piGain); s.print(", I: "); s.print(st.iGain); + s.print(", D: "); s.print(st.dGain); s.print(", FF: "); s.print(st.ffGain); + s.print(", DM: "); s.print(st.dMaxGain); s.print(", PPI: "); s.println(st.pitchPiGain); + s.println("X P I D F"); + for (size_t i = 0; i < AXIS_COUNT_RPY; i++) + { + s.print(i); s.print(" "); s.print(pid[i].P); s.print(" "); s.print(pid[i].I); + s.print(" "); s.print(pid[i].D); s.print(" "); s.println(pid[i].F); + s.print(' '); s.print(" "); s.print(res[i].P); s.print(" "); s.print(res[i].I); + s.print(" "); s.print(res[i].D); s.print(" "); s.println(res[i].F); + } + auto [pidOk, gyroOk, dtermOk] = _model.validateSimplifiedTuning(); + s.print("VALID: "); s.println(pidOk ? "OK" : "NOK"); s.println(); + + const auto& gyro = _model.config.gyro; + int16_t glpf1 = gyro.filter.freq; + int16_t glpf2 = gyro.filter2.freq; + int16_t gmin = gyro.dynLpfFilter.cutoff; + int16_t gmax = gyro.dynLpfFilter.freq; + _model.calculateSimplifiedGyroFilters(st.gyroFilterMultiplier, glpf1, glpf2, gmin, gmax); + s.print("Gyro: "); s.print(st.gyroFilter); s.print(", gain: "); s.println(st.gyroFilterMultiplier); + s.println("lpf1 lpf2 dmin dmax"); + s.print(gyro.filter.freq); s.print(" "); s.print(gyro.filter2.freq); s.print(" "); + s.print(gyro.dynLpfFilter.cutoff); s.print(" "); s.println(gyro.dynLpfFilter.freq); + s.print(glpf1); s.print(" "); s.print(glpf2); s.print(" "); s.print(gmin); s.print(" "); s.println(gmax); + s.print("VALID: "); s.println(gyroOk ? "OK" : "NOK"); s.println(); + + const auto& dterm = _model.config.dterm; + int16_t dlpf1 = dterm.filter.freq; + int16_t dlpf2 = dterm.filter2.freq; + int16_t dmin = dterm.dynLpfFilter.cutoff; + int16_t dmax = dterm.dynLpfFilter.freq; + _model.calculateSimplifiedDtermFilters(st.dtermFilterMultiplier, dlpf1, dlpf2, dmin, dmax); + s.print("Dterm: "); s.print(st.dtermFilter); s.print(", gain: "); s.println(st.dtermFilterMultiplier); + s.println("lpf1 lpf2 dmin dmax"); + s.print(dterm.filter.freq); s.print(" "); s.print(dterm.filter2.freq); s.print(" "); + s.print(dterm.dynLpfFilter.cutoff); s.print(" "); s.println(dterm.dynLpfFilter.freq); + s.print(dlpf1); s.print(" "); s.print(dlpf2); s.print(" "); s.print(dmin); s.print(" "); s.println(dmax); + s.print("VALID: "); s.println(dtermOk ? "OK" : "NOK"); + // clang-format on + } #ifdef USE_FLASHFS - else if (strcmp(cmd.args[0], "flash") == 0) + else if (std::strcmp(cmd.args[0], "flash") == 0) { if (!cmd.args[1]) { @@ -1326,11 +1626,11 @@ void Cli::execute(CliCmd& cmd, Stream& s) s.printf(" used: %zu\r\n", used); s.printf(" free: %zu\r\n", total - used); } - else if (strcmp(cmd.args[1], "partitions") == 0) + else if (std::strcmp(cmd.args[1], "partitions") == 0) { Device::FlashDevice::partitions(s); } - else if (strcmp(cmd.args[1], "journal") == 0) + else if (std::strcmp(cmd.args[1], "journal") == 0) { const FlashfsRuntime* flashfs = flashfsGetRuntime(); FlashfsJournalItem journal[16]; @@ -1343,12 +1643,12 @@ void Cli::execute(CliCmd& cmd, Stream& s) } s.printf("current: %u\r\n", flashfs->journalIdx); } - else if (strcmp(cmd.args[1], "erase") == 0) + else if (std::strcmp(cmd.args[1], "erase") == 0) { flashfsEraseCompletely(); s.println("OK"); } - else if (strcmp(cmd.args[1], "test") == 0) + else if (std::strcmp(cmd.args[1], "test") == 0) { const char* data = "flashfs-test"; flashfsWrite((const uint8_t*)data, strlen(data), true); @@ -1356,7 +1656,7 @@ void Cli::execute(CliCmd& cmd, Stream& s) flashfsClose(); s.println("OK"); } - else if (strcmp(cmd.args[1], "print") == 0) + else if (std::strcmp(cmd.args[1], "print") == 0) { size_t addr = 0; if (cmd.args[2]) @@ -1397,7 +1697,7 @@ void Cli::execute(CliCmd& cmd, Stream& s) #endif else { - s.print("unknown command: "); + s.print(_interactive ? "unknown command: " : "ERR_CMD_NA: "); s.println(cmd.args[0]); } s.println(); @@ -1417,6 +1717,7 @@ static constexpr const char* const qualityNames[] = {"no_signal", "searching", "locked", "fully_locked", "fully_locked", "fully_locked"}; static constexpr const char* const usedNames[] = {" No", "Yes"}; +#ifndef UNIT_TEST static const char* const getGnssName(size_t num) { constexpr size_t gnssNamesMax = sizeof(gnssNames) / sizeof(gnssNames[0]); @@ -1437,6 +1738,7 @@ static const char* const getUsedName(size_t num) if (num < usedNamesMax) return usedNames[num]; return "?"; } +#endif void Cli::printGpsStatus(Stream& s, bool full) const { @@ -1610,6 +1912,4 @@ void Cli::printStats(Stream& s) const s.println(" Hz"); } -} // namespace Connect - -} // namespace Espfc +} // namespace Espfc::Connect diff --git a/lib/Espfc/src/Connect/Cli.hpp b/lib/Espfc/src/Connect/Cli.hpp index edcc9ae6..fa33e1f2 100644 --- a/lib/Espfc/src/Connect/Cli.hpp +++ b/lib/Espfc/src/Connect/Cli.hpp @@ -93,7 +93,9 @@ class Cli void parse(CliCmd& cmd); void execute(CliCmd& cmd, Stream& s); +#if !defined(UNIT_TEST) private: +#endif void print(const Param& param, Stream& s) const; void printGpsStatus(Stream& s, bool full) const; void printVersion(Stream& s) const; @@ -103,6 +105,7 @@ class Cli const Param* _params; bool _ignore; bool _active; + bool _interactive; }; } // namespace Espfc::Connect diff --git a/lib/Espfc/src/Connect/Msp.cpp b/lib/Espfc/src/Connect/Msp.cpp index d075df4b..59ab5425 100644 --- a/lib/Espfc/src/Connect/Msp.cpp +++ b/lib/Espfc/src/Connect/Msp.cpp @@ -2,6 +2,7 @@ #include "Utils/Crc.hpp" #include #include +#include namespace Espfc::Connect { @@ -84,10 +85,14 @@ void MspResponse::writeData(const char* v, int size) void MspResponse::writeString(const char* v) { - while (*v) - { - writeU8(*v++); - } + writeData(v, std::clamp(std::strlen(v), 0, 168)); +} + +void MspResponse::writePString(const char* v) +{ + const auto len = std::clamp(std::strlen(v), 0, 168); + writeU8(len); + writeData(v, len); } void MspResponse::writeU8(uint8_t v) diff --git a/lib/Espfc/src/Connect/Msp.hpp b/lib/Espfc/src/Connect/Msp.hpp index dd296ee4..4af51d3d 100644 --- a/lib/Espfc/src/Connect/Msp.hpp +++ b/lib/Espfc/src/Connect/Msp.hpp @@ -95,6 +95,7 @@ class MspResponse void advance(size_t size); void writeData(const char* v, int size); void writeString(const char* v); + void writePString(const char* v); void writeU8(uint8_t v); void writeU16(uint16_t v); void writeU32(uint32_t v); diff --git a/lib/Espfc/src/Connect/MspProcessor.cpp b/lib/Espfc/src/Connect/MspProcessor.cpp index a5fba4e6..0351d13c 100644 --- a/lib/Espfc/src/Connect/MspProcessor.cpp +++ b/lib/Espfc/src/Connect/MspProcessor.cpp @@ -1,5 +1,7 @@ #include "Connect/MspProcessor.hpp" #include "Hardware.h" +#include "Model.h" +#include "ModelConfig.h" #include #include #include @@ -19,6 +21,7 @@ int blackboxCalculatePDenom(int rateNum, int rateDenom); uint8_t blackboxCalculateSampleRate(uint16_t pRatio); uint8_t blackboxGetRateDenom(void); uint16_t blackboxGetPRatio(void); +bool blackboxMayEditConfig(void); } namespace { @@ -93,36 +96,6 @@ static Espfc::SerialSpeed fromBaudIndex(SerialSpeedIndex index) // clang-format on } -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 fromGyroDlpf(uint8_t t) { switch (t) @@ -179,6 +152,32 @@ static uint16_t toIbatCurrent(float current) constexpr uint8_t MSP_PASSTHROUGH_ESC_4WAY = 0xff; +// FC version reported over MSP, mirrors Betaflight 2026.6 CalVer for configurator compatibility +constexpr uint8_t MSP_FC_VERSION_YEAR = 2026 - 2000; +constexpr uint8_t MSP_FC_VERSION_MONTH = 6; +constexpr uint8_t MSP_FC_VERSION_PATCH = 0; +constexpr char MSP_FC_VERSION_STRING[] = "2026.6.0"; + +// MCU type id sentinel telling the configurator the name follows as a string +constexpr uint8_t MCU_TYPE_ID_PROVIDED_BY_NAME = 255; + +// Reported for sensor slots the target does not support at all +constexpr uint8_t SENSOR_NOT_AVAILABLE = 0xff; + +static uint8_t toAccHw(uint8_t dev) +{ + if (dev == 0) return 1; + if (dev == 1) return 0; + return dev; +} + +static uint8_t fromAccHw(uint8_t dev) +{ + if (dev == 0) return 1; + if (dev == 1) return 0; + return dev; +} + } // namespace namespace Espfc::Connect { @@ -215,23 +214,38 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD break; case MSP_FC_VERSION: - r.writeU8(FC_VERSION_MAJOR); - r.writeU8(FC_VERSION_MINOR); - r.writeU8(FC_VERSION_PATCH_LEVEL); + r.writeU8(MSP_FC_VERSION_YEAR); + r.writeU8(MSP_FC_VERSION_MONTH); + r.writeU8(MSP_FC_VERSION_PATCH); + r.writePString(MSP_FC_VERSION_STRING); + break; + + case MSP2_MCU_INFO: + r.writeU8(MCU_TYPE_ID_PROVIDED_BY_NAME); + r.writePString(targetName); 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.writeU16(0); // No other build targets currently have hardware revision detection. + r.writeU8(0); // 0 == FC + { + uint8_t targetCapabilities = 0; +#if defined(ESPFC_SERIAL_USB) + constexpr uint8_t TARGET_HAS_VCP = 0; + targetCapabilities |= 1 << TARGET_HAS_VCP; +#endif + r.writeU8(targetCapabilities); // 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 + } + r.writeU8(MCU_TYPE_ID_PROVIDED_BY_NAME); // mcu id // 1.42 r.writeU8(2); // configuration state: configured // 1.43 @@ -257,6 +271,8 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD r.writeData(buildDate, BUILD_DATE_LENGTH); r.writeData(buildTime, BUILD_TIME_LENGTH); r.writeData(shortGitRevision, GIT_SHORT_REVISION_LENGTH); + // 1.46 + // build info flags - 0 * uint16_t break; case MSP_UID: @@ -281,19 +297,22 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD 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); + else // MSP_STATUS + { + r.writeU16(0); // unused - gyro cycle time } - // flight mode flags (above 32 bits) - r.writeU8(0); // count - + r.writeU8(0); // count + rest of flags // 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 + r.writeU8(_model.getRebootRequired()); // reboot required + // 1.46 + r.writeU16(0); // getCoreTemperatureCelsius() // cpu temperature + r.writeU8(1); // CONTROL_RATE_PROFILE_COUNT + // 1.48 + r.writeU8(1); // BATTERY_PROFILE_COUNT + r.writeU8(0); // getCurrentBatteryProfileIndex break; case MSP_NAME: @@ -308,6 +327,40 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD } break; + case MSP2_GET_TEXT: { + const uint8_t textType = m.remain() ? m.readU8() : 0; + r.writeU8(textType); + switch (textType) + { + case MSP2TEXT_CRAFT_NAME: + r.writePString(_model.config.modelName); + break; + default: + r.writePString(""); // unsupported text types reported as empty + break; + } + break; + } + + case MSP2_SET_TEXT: { + const uint8_t textType = m.readU8(); + const uint8_t textLength = m.readU8(); + if (textType == MSP2TEXT_CRAFT_NAME) + { + memset(&_model.config.modelName, 0, MODEL_NAME_LEN + 1); + for (size_t i = 0; i < textLength; i++) + { + const uint8_t c = m.readU8(); + if (i < MODEL_NAME_LEN) _model.config.modelName[i] = c; + } + } + else + { + m.advance(textLength); // ignore unsupported text types + } + break; + } + case MSP_BOXNAMES: r.writeString("ARM;AIRMODE;ANGLE;ALTHOLD;BEEPER;FAILSAFE;BLACKBOX;BLACKBOXERASE;"); break; @@ -341,7 +394,6 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD r.writeU8(_model.config.conditions[i].logicMode); r.writeU8(_model.config.conditions[i].linkId); } - break; case MSP_SET_MODE_RANGE: { @@ -380,6 +432,7 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD case MSP_SET_FEATURE_CONFIG: _model.config.featureMask = m.readU32(); _model.reload(); + _model.setRebootRequired(); break; case MSP_BATTERY_CONFIG: @@ -547,7 +600,7 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD case MSP_SET_ACC_TRIM: _model.config.accel.trim[0] = std::clamp(m.readU16(), -300, 300); // pitch _model.config.accel.trim[1] = std::clamp(m.readU16(), -300, 300); // roll - _model.onAccChange(); + _model.notifyConfigChange(ModelChangeEvent::MODEL_CHANGE_ACCEL); break; case MSP_MIXER_CONFIG: @@ -561,16 +614,47 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD 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 + r.writeU8(toAccHw(_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 + // 1.46 + r.writeU8(0); // rangefinder 0=none + r.writeU8(0); // opticalflow 0=none 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.config.accel.dev = fromAccHw(m.readU8()); // 3 acc mpu6050 + _model.config.baro.dev = m.readU8(); // 2 baro bmp085 + _model.config.mag.dev = m.readU8(); // 3 mag hmc5883l + // 1.46 + if (m.remain() >= 1) + { + m.readU8(); // rangefinder skip + } + if (m.remain() >= 1) + { + m.readU8(); // opticalflow skip + } _model.reload(); + _model.setRebootRequired(); + break; + + case MSP2_SENSOR_CONFIG_ACTIVE: { + const auto& state = _model.state; + r.writeU8(_model.gyroActive() && state.gyro.dev ? toAccHw(state.gyro.dev->getType()) + : toAccHw(GYRO_NONE)); // gyro + r.writeU8(_model.accelActive() && state.gyro.dev ? toAccHw(state.gyro.dev->getType()) + : toAccHw(GYRO_NONE)); // acc + r.writeU8(_model.baroActive() && state.baro.dev ? state.baro.dev->getType() : BARO_NONE); // baro + r.writeU8(_model.magActive() && state.mag.dev ? state.mag.dev->getType() : MAG_NONE); // mag + r.writeU8(SENSOR_NOT_AVAILABLE); // rangefinder + r.writeU8(SENSOR_NOT_AVAILABLE); // opticalflow + break; + } + + case MSP2_GYRO_SENSOR_ACTIVE: + r.writeU8(1); // gyro count, single gyro only + r.writeU8(_model.gyroActive() ? _model.state.gyro.dev->getType() : GYRO_NONE); // gyro 1 break; case MSP_SENSOR_ALIGNMENT: @@ -579,9 +663,10 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD 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 + r.writeU8(_model.state.gyro.present ? 1 : 0); // gyro_enable_mask, was gyro_to_use + r.writeU16(0); // mag align roll + r.writeU16(0); // mag align pitch + r.writeU16(0); // mag align yaw break; case MSP_SET_SENSOR_ALIGNMENT: { @@ -589,15 +674,20 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD 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() >= 1) + { + uint8_t gyroEnableMask = m.readU8(); // gyro_enable_mask + _model.config.gyro.dev = gyroEnableMask & 1 ? GYRO_AUTO : GYRO_NONE; + } + if (m.remain() >= 6) { - m.readU8(); // gyro_to_use - gyroAlign = m.readU8(); // gyro 1 align - m.readU8(); // gyro 2 align + m.readU16(); // gyro 1 roll + m.readU16(); // gyro 1 pitch + m.readU16(); // gyro 1 yaw } _model.config.gyro.align = gyroAlign; + break; } - break; case MSP_CF_SERIAL_CONFIG: for (int i = 0; i < SERIAL_UART_COUNT; i++) @@ -639,8 +729,8 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD r.writeU8(0); // telemetry_baudrateIndex r.writeU8(toBaudIndex(_model.config.serial[i].blackboxBaud)); // blackbox_baudrateIndex } + break; } - break; case MSP_SET_CF_SERIAL_CONFIG: { const int packetSize = 1 + 2 + 4; @@ -665,9 +755,10 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD m.readU8(); _model.config.serial[k].blackboxBaud = fromBaudIndex((SerialSpeedIndex)m.readU8()); } - } _model.reload(); + _model.setRebootRequired(); break; + } case MSP2_COMMON_SET_SERIAL_CONFIG: { m.readU8(); // was count - ignore @@ -693,23 +784,26 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD m.readU8(); _model.config.serial[k].blackboxBaud = fromBaudIndex((SerialSpeedIndex)m.readU8()); } - } _model.reload(); + _model.setRebootRequired(); 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); + 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(0); // blackboxGetPRatio()); // p_denom + // 1.44 + r.writeU8(_model.config.blackbox.pDenom); // sample_rate + // 1.45 + r.writeU32(~_model.config.blackbox.fieldsMask); // fields mask break; case MSP_SET_BLACKBOX_CONFIG: - // TODO: Don't allow config to be updated while Blackbox is logging - if (true) + // Don't allow config to be updated while Blackbox is logging + if (blackboxMayEditConfig()) { _model.config.blackbox.dev = m.readU8(); const int rateNum = m.readU8(); // was rate_num @@ -717,25 +811,32 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD uint16_t pRatio = 0; if (m.remain() >= 2) { - pRatio = m.readU16(); // p_denom specified, so use it directly + // p_denom specified, so use it directly + pRatio = m.readU16(); } else { // p_denom not specified in MSP, so calculate it from old rateNum and rateDenom - // pRatio = blackboxCalculatePDenom(rateNum, rateDenom); - (void)(rateNum + rateDenom); + pRatio = blackboxCalculatePDenom(rateNum, rateDenom); } - _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; + // 1.44 + if (m.remain() >= 1) + { + // sample_rate specified, so use it directly + _model.config.blackbox.pDenom = m.readU8(); + } + else + { + // sample_rate not specified in MSP, so calculate it from old p_ratio + _model.config.blackbox.pDenom = blackboxCalculateSampleRate(pRatio); } - if (m.remain() >= 4) { + + // 1.45 + if (m.remain() >= 4) + { _model.config.blackbox.fieldsMask = ~m.readU32(); - }*/ + } } break; @@ -745,6 +846,16 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD r.writeU16(lrintf(Utils::toDeg(-_model.state.attitude.euler.z))); // yaw [degrees] break; + case MSP_ATTITUDE_QUATERNION: { + const float scale = 32767.0f; // int16 unit-quaternion scaling + const auto& q = _model.state.attitude.quaternion; + r.writeU16((int16_t)lrintf(q.w * scale)); + r.writeU16((int16_t)lrintf(q.x * scale)); + r.writeU16((int16_t)lrintf(q.y * scale)); + r.writeU16((int16_t)lrintf(q.z * scale)); + 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] @@ -770,6 +881,7 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD _model.config.boardAlignment[0] = m.readU16(); _model.config.boardAlignment[1] = m.readU16(); _model.config.boardAlignment[2] = m.readU16(); + _model.notifyConfigChange(ModelChangeEvent::MODEL_CHANGE_ACCEL); break; case MSP_RX_MAP: @@ -795,7 +907,7 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD break; case MSP_MOTOR_CONFIG: - r.writeU16(_model.config.output.minThrottle); // minthrottle + r.writeU16(0); // minthrottle (dropped in 1.46) r.writeU16(_model.config.output.maxThrottle); // maxthrottle r.writeU16(_model.config.output.minCommand); // mincommand r.writeU8(_model.state.currentMixer.count); // motor count @@ -806,7 +918,7 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD break; case MSP_SET_MOTOR_CONFIG: - _model.config.output.minThrottle = m.readU16(); // minthrottle + m.readU16(); // minthrottle (dropped in 1.46) _model.config.output.maxThrottle = m.readU16(); // maxthrottle _model.config.output.minCommand = m.readU16(); // mincommand if (m.remain() >= 2) @@ -820,6 +932,7 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD #endif } _model.reload(); + _model.setRebootRequired(); break; case MSP_MOTOR_3D_CONFIG: @@ -832,52 +945,63 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD r.writeU8(5); // auto_disarm delay r.writeU8(0); // disarm kill switch r.writeU8(_model.config.arming.smallAngle); // small angle + r.writeU8(0); // gyro_cal_on_first_arm break; case MSP_SET_ARMING_CONFIG: m.readU8(); // auto_disarm delay m.readU8(); // disarm kill switch _model.config.arming.smallAngle = std::min(180, m.readU8()); // small angle + m.readU8(); // gyro_cal_on_first_arm break; case MSP_RC_DEADBAND: r.writeU8(_model.config.input.deadband); r.writeU8(0); // yaw deadband - r.writeU8(0); // alt hold deadband + r.writeU8(0); // pos 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.readU8(); // pos 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 + 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(0); // rc interpolation + r.writeU8(0); // rc interpolation interval + r.writeU16(_model.config.input.airModeActivateThreshold * 10 + 1000); // 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(0); // deprecated: rc_smoothing_type + r.writeU8(_model.config.input.filter.freq); // rc_smoothing_setpoint_cutoff + r.writeU8(_model.config.input.filterThrottle.freq); // rc_smoothing_throtle_cutoff + r.writeU8(_model.config.input.filterAutoThrottleFactor); // rc_smoothing_auto_factor_throttle + r.writeU8(0); // rc_smoothing_derivative_type + r.writeU8(0); // usb type // 1.42+ r.writeU8(_model.config.input.filterAutoFactor); // rc_smoothing_auto_factor + // 1.44 + r.writeU8(_model.config.input.filterEnable); // rc_smoothing + // 1.45 + { + uint8_t uid[6] = {}; + r.writeData((const char*)uid, sizeof(uid)); // elrs uid + } + // 1.47 + r.writeU8(0); // elrs modelId break; case MSP_SET_RX_CONFIG: @@ -890,9 +1014,9 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD _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 + m.readU8(); // rc interpolation + m.readU8(); // rc interpolation interval + _model.config.input.airModeActivateThreshold = (m.readU16() - 1000) / 10; // airmode activate threshold } if (m.remain() >= 6) { @@ -907,13 +1031,12 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD // 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 + m.readU8(); // was rc iterpolation channels + m.readU8(); // was rc_smoothing_type + _model.config.input.filter.freq = m.readU8(); // rc_smoothing_setpoint_cutoff + _model.config.input.filterThrottle.freq = m.readU8(); // rc_smoothing_throttle_cutoff + _model.config.input.filterAutoThrottleFactor = m.readU8(); // rc_smoothing_auto_factor_throttle + m.readU8(); // was rc_smoothing_derivative_type } if (m.remain() >= 1) { @@ -924,8 +1047,22 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD { _model.config.input.filterAutoFactor = m.readU8(); // rc_smoothing_auto_factor } - - _model.reload(); + // 1.44 + if (m.remain() >= 1) + { + _model.config.input.filterEnable = m.readU8(); // rc_smoothing + } + // 1.45 + if (m.remain() >= 6) + { + m.advance(6); // elrs uid + } + // 1.47 + if (m.remain() >= 1) + { + m.readU8(); // elrs modelId + } + _model.notifyConfigChange(MODEL_CHANGE_INPUT); break; case MSP_FAILSAFE_CONFIG: @@ -982,14 +1119,14 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD { 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 + r.writeU8(0); // was tpa scale + r.writeU8(50); // thrMid8 + r.writeU8(0); // thr expo + r.writeU16(0); // was 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%) @@ -999,7 +1136,8 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD r.writeU16(_model.config.input.rateLimit[2]); // rate limit yaw // 1.43+ r.writeU8(_model.config.input.rateType); // rates type - + // 1.47 + r.writeU8(50); // thrHover8 break; case MSP_SET_RC_TUNING: @@ -1023,10 +1161,10 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD { _model.config.input.superRate[i] = m.readU8(); } - _model.config.controller.tpaScale = std::clamp(m.readU8(), 0, 90); // dyn thr pid - m.readU8(); // thrMid8 - m.readU8(); // thr expo - _model.config.controller.tpaBreakpoint = std::clamp(m.readU16(), 1000, 2000); // tpa breakpoint + m.readU8(); // was tpa scale + m.readU8(); // thrMid8 + m.readU8(); // thr expo + m.readU16(); // was tpa breakpoint if (m.remain() >= 1) { _model.config.input.expo[AXIS_YAW] = m.readU8(); // yaw expo @@ -1061,6 +1199,12 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD { _model.config.input.rateType = m.readU8(); } + // 1.47 + if (m.remain() >= 1) + { + m.readU8(); // thrHover8 + } + _model.notifyConfigChange(MODEL_CHANGE_RATES); } else { @@ -1075,7 +1219,7 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD 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.writeU16(_model.config.output.motorIdle); r.writeU8(0); // 32k gyro r.writeU8(0); // PWM inversion r.writeU8(0); // gyro_to_use: {1:0, 2:1. 2:both} @@ -1096,7 +1240,7 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD _model.config.output.rate = m.readU16(); if (m.remain() >= 2) { - _model.config.output.dshotIdle = m.readU16(); // dshot idle + _model.config.output.motorIdle = m.readU16(); // dshot idle } if (m.remain()) { @@ -1120,11 +1264,16 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD _model.config.debug.mode = m.readU8(); } _model.reload(); + _model.setRebootRequired(); break; - // case MSP_COMPASS_CONFIG: - // r.writeU16(0); // mag_declination * 10 - // break; + case MSP_COMPASS_CONFIG: + r.writeU16(_model.config.mag.declination); // mag_declination * 10 + break; + + case MSP_SET_COMPASS_CONFIG: + _model.config.mag.declination = m.readU16(); // mag_declination * 10 + break; case MSP_FILTER_CONFIG: r.writeU8(_model.config.gyro.filter.freq); // gyro lpf @@ -1151,8 +1300,8 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD 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.writeU8(0); // deprecated dyn notch range + r.writeU8(0); // deprecated 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 @@ -1160,6 +1309,16 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD 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 + // 1.44 + r.writeU8(0); // dterm lpf1 dyn expo + r.writeU8(_model.config.gyro.dynamicFilter.count); + // 1.48 + r.writeU16(_model.config.gyro.rpmFilter.fade); // rpm_notch_fade_range_hz + r.writeU16(_model.config.gyro.rpmFilter.q); // rpm_notch_q + for (size_t i = 0; i < RPM_FILTER_HARMONICS_MAX; i++) + { + r.writeU8(_model.config.gyro.rpmFilter.weights[i]); // rpm_notch_harmonic_freq + } break; case MSP_SET_FILTER_CONFIG: @@ -1215,15 +1374,35 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD { _model.config.gyro.dynamicFilter.max_freq = m.readU16(); // dyn_notch_max_hz } - _model.reload(); + // 1.44 + if (m.remain() >= 2) + { + m.readU8(); // dterm lpf1 dyn expo + _model.config.gyro.dynamicFilter.count = m.readU8(); + } + // 1.48 + if (m.remain() >= 7) + { + // TODO: validate + _model.config.gyro.rpmFilter.fade = m.readU16(); // rpm_notch_fade_range_hz + _model.config.gyro.rpmFilter.q = m.readU16(); // rpm_notch_q + for (size_t i = 0; i < RPM_FILTER_HARMONICS_MAX; i++) + { + _model.config.gyro.rpmFilter.weights[i] = m.readU8(); // rpm_notch_harmonic_freq + } + } + _model.notifyConfigChange(MODEL_CHANGE_FILTER); break; case MSP_PID_CONTROLLER: r.writeU8(1); // betaflight controller id break; + case MSP_SET_PID_CONTROLLER: + break; + case MSP_PIDNAMES: - r.writeString("ROLL;PITCH;YAW;ALT;Pos;PosR;NavR;LEVEL;MAG;VEL;"); + r.writeString("ROLL;PITCH;YAW;LEVEL;MAG;ALT;VEL;Pos;PosR;NavR;"); break; case MSP_PID: @@ -1242,28 +1421,27 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD _model.config.pid[i].I = m.readU8(); _model.config.pid[i].D = m.readU8(); } - _model.reload(); + _model.notifyConfigChange(MODEL_CHANGE_PID); break; - case MSP_PID_ADVANCED: /// !!!FINISHED HERE!!! + case MSP_PID_ADVANCED: 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.writeU16(0); // was pidProfile.yaw_p_limit + r.writeU8(0); // reserved + r.writeU8(0); // vbatPidCompensation; + r.writeU8(0); // feedForwardTransition; + r.writeU8(0); // 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(0); // itermAcceleratorGain; anti_gravity_gain, 0 in 1.45+ + r.writeU16(0); r.writeU8(0); // iterm rotation r.writeU8(0); // smart feed forward r.writeU8(_model.config.iterm.relax); // iterm relax @@ -1276,11 +1454,11 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD 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); // d max roll + r.writeU8(0); // d max pitch + r.writeU8(0); // d max yaw + r.writeU8(0); // d max gain + r.writeU8(0); // d max advance r.writeU8(0); // use_integrated_yaw r.writeU8(0); // integrated_yaw_relax // 1.42+ @@ -1289,6 +1467,17 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD r.writeU8(_model.config.output.motorLimit); // motor_output_limit r.writeU8(0); // auto_profile_cell_count r.writeU8(0); // idle_min_rpm + // 1.44 + r.writeU8(0); // ff avg + r.writeU8(0); // ff smooth + r.writeU8(0); // ff boost + r.writeU8(0); // ff max rate limit + r.writeU8(0); // ff jitter factor + r.writeU8(0); // vbat sag compensation + r.writeU8(0); // thrust linearization + r.writeU8(_model.config.controller.tpaMode); // tpa mode + r.writeU8(_model.config.controller.tpaScale); // tpa rate + r.writeU16(_model.config.controller.tpaBreakpoint); // tpa breakpoint break; case MSP_SET_PID_ADVANCED: @@ -1298,7 +1487,7 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD m.readU8(); // reserved m.readU8(); m.readU8(); - _model.config.dterm.setpointWeight = m.readU8(); + m.readU8(); m.readU8(); // reserved m.readU8(); // reserved m.readU8(); // reserved @@ -1316,7 +1505,7 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD } if (m.remain() >= 2) { - _model.config.dterm.setpointWeight = m.readU16(); + m.readU16(); } if (m.remain() >= 14) { @@ -1335,11 +1524,11 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD // 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(); // d max roll + m.readU8(); // d max pitch + m.readU8(); // d max yaw + m.readU8(); // d max gain + m.readU8(); // d max advance m.readU8(); // use_integrated_yaw m.readU8(); // integrated_yaw_relax } @@ -1355,8 +1544,190 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD m.readU8(); // auto_profile_cell_count m.readU8(); // idle_min_rpm } - _model.reload(); + // 1.44 + if (m.remain() >= 5) + { + m.readU8(); // ff avg + m.readU8(); // ff smooth + m.readU8(); // ff boost + m.readU8(); // ff max rate limit + m.readU8(); // ff jitter factor + } + if (m.remain() >= 1) + { + m.readU8(); // vbat sag compensation + } + if (m.remain() >= 1) + { + m.readU8(); // thrust linearization + } + if (m.remain() >= 4) + { + _model.config.controller.tpaMode = m.readU8(); // tpa mode + _model.config.controller.tpaScale = std::clamp(m.readU8(), 0, 100); // tpa rate + _model.config.controller.tpaBreakpoint = std::clamp(m.readU16(), 1000, 2000); // tpa breakpoint + } + _model.notifyConfigChange(MODEL_CHANGE_PID); + break; + + case MSP_SIMPLIFIED_TUNING: { + const auto& s = _model.config.simplifiedTuning; + r.writeU8(s.pidsMode); + r.writeU8(s.masterMultiplier); + r.writeU8(s.rollPitchRatio); + r.writeU8(s.iGain); + r.writeU8(s.dGain); + r.writeU8(s.piGain); + r.writeU8(s.dMaxGain); + r.writeU8(s.ffGain); + r.writeU8(s.pitchPiGain); + r.writeU32(0); + r.writeU32(0); + r.writeU8(s.dtermFilter); + r.writeU8(s.dtermFilterMultiplier); + r.writeU16(_model.config.dterm.filter.freq); + r.writeU16(_model.config.dterm.filter2.freq); + r.writeU16(_model.config.dterm.dynLpfFilter.cutoff); + r.writeU16(_model.config.dterm.dynLpfFilter.freq); + r.writeU32(0); + r.writeU32(0); + r.writeU8(s.gyroFilter); + r.writeU8(s.gyroFilterMultiplier); + r.writeU16(_model.config.gyro.filter.freq); + r.writeU16(_model.config.gyro.filter2.freq); + r.writeU16(_model.config.gyro.dynLpfFilter.cutoff); + r.writeU16(_model.config.gyro.dynLpfFilter.freq); + r.writeU32(0); + r.writeU32(0); + break; + } + + case MSP_SET_SIMPLIFIED_TUNING: { + auto& s = _model.config.simplifiedTuning; + s.pidsMode = m.readU8(); + s.masterMultiplier = m.readU8(); + s.rollPitchRatio = m.readU8(); + s.iGain = m.readU8(); + s.dGain = m.readU8(); + s.piGain = m.readU8(); + s.dMaxGain = m.readU8(); + s.ffGain = m.readU8(); + s.pitchPiGain = m.readU8(); + m.readU32(); + m.readU32(); + s.dtermFilter = m.readU8(); + s.dtermFilterMultiplier = m.readU8(); + _model.config.dterm.filter.freq = m.readU16(); + _model.config.dterm.filter2.freq = m.readU16(); + _model.config.dterm.dynLpfFilter.cutoff = m.readU16(); + _model.config.dterm.dynLpfFilter.freq = m.readU16(); + m.readU32(); + m.readU32(); + s.gyroFilter = m.readU8(); + s.gyroFilterMultiplier = m.readU8(); + _model.config.gyro.filter.freq = m.readU16(); + _model.config.gyro.filter2.freq = m.readU16(); + _model.config.gyro.dynLpfFilter.cutoff = m.readU16(); + _model.config.gyro.dynLpfFilter.freq = m.readU16(); + m.readU32(); + m.readU32(); + _model.calculateSimplifiedPids(s, _model.config.pid); + if (s.dtermFilter) + { + _model.calculateSimplifiedDtermFilters( + s.dtermFilterMultiplier, _model.config.dterm.filter.freq, _model.config.dterm.filter2.freq, + _model.config.dterm.dynLpfFilter.cutoff, _model.config.dterm.dynLpfFilter.freq); + } + if (s.gyroFilter) + { + _model.calculateSimplifiedGyroFilters(s.gyroFilterMultiplier, _model.config.gyro.filter.freq, + _model.config.gyro.filter2.freq, _model.config.gyro.dynLpfFilter.cutoff, + _model.config.gyro.dynLpfFilter.freq); + } + _model.notifyConfigChange(MODEL_CHANGE_PID); + if (s.gyroFilter || s.dtermFilter) + { + _model.notifyConfigChange(MODEL_CHANGE_FILTER); + } + break; + } + + case MSP_CALCULATE_SIMPLIFIED_PID: { + SimplifiedTuningConfig s; + s.pidsMode = m.readU8(); + s.masterMultiplier = m.readU8(); + s.rollPitchRatio = m.readU8(); + s.iGain = m.readU8(); + s.dGain = m.readU8(); + s.piGain = m.readU8(); + s.dMaxGain = m.readU8(); + s.ffGain = m.readU8(); + s.pitchPiGain = m.readU8(); + m.readU32(); + m.readU32(); + PidConfig tmp[3] = {_model.config.pid[FC_PID_ROLL], _model.config.pid[FC_PID_PITCH], + _model.config.pid[FC_PID_YAW]}; + _model.calculateSimplifiedPids(s, tmp); + for (int i = 0; i < 3; i++) + { + r.writeU8(tmp[i].P); + r.writeU8(tmp[i].I); + r.writeU8(tmp[i].D); + r.writeU8(0); // d_max not supported + r.writeU16(tmp[i].F); + } + break; + } + + case MSP_CALCULATE_SIMPLIFIED_GYRO: { + uint8_t filter = m.readU8(); + uint8_t mult = m.readU8(); + int16_t lpf1 = m.readU16(); + int16_t lpf2 = m.readU16(); + int16_t dynMin = m.readU16(); + int16_t dynMax = m.readU16(); + m.readU32(); + m.readU32(); + if (filter) _model.calculateSimplifiedGyroFilters(mult, lpf1, lpf2, dynMin, dynMax); + r.writeU8(filter); + r.writeU8(mult); + r.writeU16(lpf1); + r.writeU16(lpf2); + r.writeU16(dynMin); + r.writeU16(dynMax); + r.writeU32(0); + r.writeU32(0); break; + } + + case MSP_CALCULATE_SIMPLIFIED_DTERM: { + uint8_t filter = m.readU8(); + uint8_t mult = m.readU8(); + int16_t lpf1 = m.readU16(); + int16_t lpf2 = m.readU16(); + int16_t dynMin = m.readU16(); + int16_t dynMax = m.readU16(); + m.readU32(); + m.readU32(); + if (filter) _model.calculateSimplifiedDtermFilters(mult, lpf1, lpf2, dynMin, dynMax); + r.writeU8(filter); + r.writeU8(mult); + r.writeU16(lpf1); + r.writeU16(lpf2); + r.writeU16(dynMin); + r.writeU16(dynMax); + r.writeU32(0); + r.writeU32(0); + break; + } + + case MSP_VALIDATE_SIMPLIFIED_TUNING: { + auto [pidOk, gyroOk, dtermOk] = _model.validateSimplifiedTuning(); + r.writeU8(pidOk); + r.writeU8(gyroOk); + r.writeU8(dtermOk); + break; + } case MSP_RAW_IMU: { auto accel = _model.state.accel.adc.fetch(); @@ -1658,7 +2029,14 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD break; case MSP_EEPROM_WRITE: - _model.save(); + if (!_model.isModeActive(MODE_ARMED)) + { + _model.save(); + } + else + { + r.result = -1; + } break; case MSP_RESET_CONF: @@ -1666,20 +2044,39 @@ void MspProcessor::processCommand(MspMessage& m, MspResponse& r, Device::SerialD { _model.reset(); _model.save(); + _postCommand = [this]() { processRestart(); }; + r.writeU8(1); // success } else { - r.result = -1; // not allowed when armed + r.writeU8(0); // fail } break; - case MSP_REBOOT: - r.writeU8(0); // reboot to firmware + case MSP_REBOOT: { + uint8_t rebootType = 0; // reboot to firmware only + if (m.remain()) + { + // TODO: reboot to bootloader + rebootType = m.readU8(); + if (rebootType != 0) + { + r.result = -1; // fail + break; + } + } + r.writeU8(rebootType); _postCommand = [this]() { processRestart(); }; break; + } + + case MSP_SET_RTC: + m.readU32(); // secs: ignore + m.readU16(); // msecs: ignore + break; // RTC not supported, accept and ignore default: - r.result = 0; + r.result = -1; break; } } diff --git a/lib/Espfc/src/Control/Actuator.cpp b/lib/Espfc/src/Control/Actuator.cpp index 8bb7a91d..68510575 100644 --- a/lib/Espfc/src/Control/Actuator.cpp +++ b/lib/Espfc/src/Control/Actuator.cpp @@ -13,11 +13,11 @@ int Actuator::begin() _model.state.mode.maskPrev = 0; _model.state.mode.maskPresent = 0; _model.state.mode.maskSwitch = 0; - for(size_t i = 0; i < ACTUATOR_CONDITIONS; i++) + for (size_t i = 0; i < ACTUATOR_CONDITIONS; i++) { - const auto &c = _model.config.conditions[i]; - if(!(c.min < c.max)) continue; // inactive - if(c.ch < AXIS_AUX_1 || c.ch >= AXIS_COUNT) continue; // invalid channel + const auto& c = _model.config.conditions[i]; + if (c.min >= c.max) continue; // inactive + if (c.ch < AXIS_AUX_1 || c.ch >= AXIS_COUNT) continue; // invalid channel _model.state.mode.maskPresent |= 1 << c.id; } _model.state.mode.airmodeAllowed = false; @@ -39,7 +39,7 @@ int Actuator::update() updateRescueConfig(); updateLed(); - if(_model.config.debug.mode == DEBUG_PIDLOOP) + if (_model.config.debug.mode == DEBUG_PIDLOOP) { _model.state.debug[4] = micros() - startTime; } @@ -49,38 +49,33 @@ int Actuator::update() void Actuator::updateScaler() { - for(size_t i = 0; i < SCALER_COUNT; i++) + for (size_t i = 0; i < SCALER_COUNT; i++) { uint32_t mode = _model.config.scaler[i].dimension; - if(!mode) continue; + if (!mode) continue; short c = _model.config.scaler[i].channel; - if(c < AXIS_AUX_1) continue; + if (c < AXIS_AUX_1) continue; float v = _model.state.input.ch[c]; float min = _model.config.scaler[i].minScale * 0.01f; float max = _model.config.scaler[i].maxScale * 0.01f; float scale = Utils::map3(v, -1.f, 0.f, 1.f, min, min < 0 ? 0.f : 1.f, max); - for(size_t x = 0; x < AXIS_COUNT_RPYT; x++) + for (size_t x = 0; x < AXIS_COUNT_RPYT; x++) { - if( - (x == AXIS_ROLL && (mode & ACT_AXIS_ROLL)) || - (x == AXIS_PITCH && (mode & ACT_AXIS_PITCH)) || - (x == AXIS_YAW && (mode & ACT_AXIS_YAW)) || - (x == AXIS_THRUST && (mode & ACT_AXIS_THRUST)) - ) + if ((x == AXIS_ROLL && (mode & ACT_AXIS_ROLL)) || (x == AXIS_PITCH && (mode & ACT_AXIS_PITCH)) || + (x == AXIS_YAW && (mode & ACT_AXIS_YAW)) || (x == AXIS_THRUST && (mode & ACT_AXIS_THRUST))) { - if(mode & ACT_INNER_P) _model.state.innerPid[x].pScale = scale; - if(mode & ACT_INNER_I) _model.state.innerPid[x].iScale = scale; - if(mode & ACT_INNER_D) _model.state.innerPid[x].dScale = scale; - if(mode & ACT_INNER_F) _model.state.innerPid[x].fScale = scale; - - if(mode & ACT_OUTER_P) _model.state.outerPid[x].pScale = scale; - if(mode & ACT_OUTER_I) _model.state.outerPid[x].iScale = scale; - if(mode & ACT_OUTER_D) _model.state.outerPid[x].dScale = scale; - if(mode & ACT_OUTER_F) _model.state.outerPid[x].fScale = scale; + if (mode & ACT_INNER_P) _model.state.innerPid[x].pScale = scale; + if (mode & ACT_INNER_I) _model.state.innerPid[x].iScale = scale; + if (mode & ACT_INNER_D) _model.state.innerPid[x].dScale = scale; + if (mode & ACT_INNER_F) _model.state.innerPid[x].fScale = scale; + if (mode & ACT_OUTER_P) _model.state.outerPid[x].pScale = scale; + if (mode & ACT_OUTER_I) _model.state.outerPid[x].iScale = scale; + if (mode & ACT_OUTER_D) _model.state.outerPid[x].dScale = scale; + if (mode & ACT_OUTER_F) _model.state.outerPid[x].fScale = scale; } } } @@ -91,16 +86,16 @@ void Actuator::updateArmingDisabled() int errors = _model.state.i2cErrorDelta; _model.state.i2cErrorDelta = 0; - _model.setArmingDisabled(ARMING_DISABLED_NO_GYRO, !_model.state.gyro.present || errors); - _model.setArmingDisabled(ARMING_DISABLED_FAILSAFE, _model.state.failsafe.phase != FC_FAILSAFE_IDLE); - _model.setArmingDisabled(ARMING_DISABLED_RX_FAILSAFE, _model.state.input.rxLoss || _model.state.input.rxFailSafe); - _model.setArmingDisabled(ARMING_DISABLED_THROTTLE, !_model.isThrottleLow()); - _model.setArmingDisabled(ARMING_DISABLED_CALIBRATING, _model.calibrationActive()); - _model.setArmingDisabled(ARMING_DISABLED_MOTOR_PROTOCOL, _model.config.output.protocol == ESC_PROTOCOL_DISABLED); + _model.setArmingDisabled(ARMING_DISABLED_NO_GYRO, !_model.state.gyro.present || errors); + _model.setArmingDisabled(ARMING_DISABLED_FAILSAFE, _model.state.failsafe.phase != FC_FAILSAFE_IDLE); + _model.setArmingDisabled(ARMING_DISABLED_RX_FAILSAFE, _model.state.input.rxLoss || _model.state.input.rxFailSafe); + _model.setArmingDisabled(ARMING_DISABLED_THROTTLE, !_model.isThrottleLow()); + _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.0f && _model.accelActive()) + if (_model.config.arming.smallAngle < 180.0f && _model.accelActive()) { const float maxTiltRad = Utils::toRad(_model.config.arming.smallAngle); const float roll = _model.state.attitude.euler[AXIS_ROLL]; @@ -112,27 +107,28 @@ void Actuator::updateArmingDisabled() { _model.setArmingDisabled(ARMING_DISABLED_ANGLE, false); } - if(_model.isFeatureActive(FEATURE_GPS)) + if (_model.isFeatureActive(FEATURE_GPS)) { - _model.setArmingDisabled(ARMING_DISABLED_GPS, !_model.state.gps.present || _model.state.gps.numSats < _model.config.gps.minSats); + _model.setArmingDisabled(ARMING_DISABLED_GPS, + !_model.state.gps.present || _model.state.gps.numSats < _model.config.gps.minSats); } } void Actuator::updateModeMask() { uint32_t newMask = 0; - for(size_t i = 0; i < ACTUATOR_CONDITIONS; i++) + for (size_t i = 0; i < ACTUATOR_CONDITIONS; i++) { - ActuatorCondition * c = &_model.config.conditions[i]; - if(!(c->min < c->max)) continue; // inactive + ActuatorCondition* c = &_model.config.conditions[i]; + if (c->min >= c->max) continue; // inactive - int16_t min = c->min; // * 25 + 900; - int16_t max = c->max; // * 25 + 900; - size_t ch = c->ch; // + AXIS_AUX_1; - if(ch < AXIS_AUX_1 || ch >= AXIS_COUNT) continue; // invalid channel + int16_t min = c->min; // * 25 + 900; + int16_t max = c->max; // * 25 + 900; + size_t ch = c->ch; // + AXIS_AUX_1; + if (ch < AXIS_AUX_1 || ch >= AXIS_COUNT) continue; // invalid channel int16_t val = _model.state.input.us[ch]; - if(val > min && val < max) + if (val > min && val < max) { newMask |= 1 << c->id; } @@ -140,21 +136,21 @@ void Actuator::updateModeMask() _model.updateSwitchActive(newMask); - _model.setArmingDisabled(ARMING_DISABLED_FAILSAFE, _model.state.failsafe.phase != FC_FAILSAFE_IDLE); + _model.setArmingDisabled(ARMING_DISABLED_FAILSAFE, _model.state.failsafe.phase != FC_FAILSAFE_IDLE); _model.setArmingDisabled(ARMING_DISABLED_BOXFAILSAFE, _model.isSwitchActive(MODE_FAILSAFE)); - _model.setArmingDisabled(ARMING_DISABLED_ARM_SWITCH, _model.armingDisabled() && _model.isSwitchActive(MODE_ARMED)); + _model.setArmingDisabled(ARMING_DISABLED_ARM_SWITCH, _model.armingDisabled() && _model.isSwitchActive(MODE_ARMED)); - if(_model.state.failsafe.phase != FC_FAILSAFE_IDLE) + if (_model.state.failsafe.phase != FC_FAILSAFE_IDLE) { newMask |= (1 << MODE_FAILSAFE); } - for(size_t i = 0; i < MODE_COUNT; i++) + for (size_t i = 0; i < MODE_COUNT; i++) { bool newVal = newMask & (1 << i); bool oldVal = _model.state.mode.mask & (1 << i); - if(newVal == oldVal) continue; // mode unchanged - if(newVal && !canActivateMode((FlightMode)i)) + if (newVal == oldVal) continue; // mode unchanged + if (newVal && !canActivateMode((FlightMode)i)) { newMask &= ~(1 << i); // block activation, clear bit } @@ -165,7 +161,7 @@ void Actuator::updateModeMask() bool Actuator::canActivateMode(FlightMode mode) { - switch(mode) + switch (mode) { case MODE_ARMED: return !_model.armingDisabled() && _model.isThrottleLow(); @@ -182,30 +178,32 @@ bool Actuator::canActivateMode(FlightMode mode) void Actuator::updateArmed() { - if(_model.hasChanged(MODE_ARMED)) + if (_model.hasChanged(MODE_ARMED)) { bool armed = _model.isModeActive(MODE_ARMED); - if(armed) + if (armed) { _model.state.mode.disarmReason = DISARM_REASON_SYSTEM; _model.state.mode.rescueConfigMode = RESCUE_CONFIG_DISABLED; } - else if(!armed && _model.state.mode.disarmReason == DISARM_REASON_SYSTEM) + else if (!armed && _model.state.mode.disarmReason == DISARM_REASON_SYSTEM) { _model.state.mode.disarmReason = DISARM_REASON_SWITCH; } - if(armed) _model.setGpsHome(); + if (armed) _model.setGpsHome(); } } void Actuator::updateAirMode() { bool armed = _model.isModeActive(MODE_ARMED); - if(!armed) + if (!armed) { _model.state.mode.airmodeAllowed = false; } - if(armed && !_model.state.mode.airmodeAllowed && _model.state.input.us[AXIS_THRUST] > 1400) // activate airmode in the air + const int16_t airModeActivateThreshold = _model.config.input.airModeActivateThreshold * 10 + 1000; + if (armed && !_model.state.mode.airmodeAllowed && + _model.state.input.us[AXIS_THRUST] > airModeActivateThreshold) // activate airmode in the air { _model.state.mode.airmodeAllowed = true; } @@ -213,23 +211,23 @@ void Actuator::updateAirMode() void Actuator::updateBuzzer() { - if(_model.isModeActive(MODE_FAILSAFE)) + if (_model.isModeActive(MODE_FAILSAFE)) { _model.state.buzzer.play(BUZZER_RX_LOST); } - if(_model.state.battery.warn(_model.config.vbat.cellWarning)) + if (_model.state.battery.warn(_model.config.vbat.cellWarning)) { _model.state.buzzer.play(BUZZER_BAT_LOW); } - if(_model.isModeActive(MODE_BUZZER)) + if (_model.isModeActive(MODE_BUZZER)) { _model.state.buzzer.play(BUZZER_RX_SET); } - if((_model.hasChanged(MODE_ARMED))) + if ((_model.hasChanged(MODE_ARMED))) { _model.state.buzzer.push(_model.isModeActive(MODE_ARMED) ? BUZZER_ARMING : BUZZER_DISARMING); } - if(!_model.state.gps.wasLocked && _model.state.gps.numSats >= _model.config.gps.minSats) + if (!_model.state.gps.wasLocked && _model.state.gps.numSats >= _model.config.gps.minSats) { _model.state.buzzer.play(BUZZER_READY_BEEP); _model.state.gps.wasLocked = true; @@ -240,15 +238,21 @@ void Actuator::updateDynLpf() { return; // temporary disable int scale = std::clamp((int)_model.state.input.us[AXIS_THRUST], 1000, 2000); - if(_model.config.gyro.dynLpfFilter.cutoff > 0) { - int gyroFreq = Utils::map(scale, 1000, 2000, _model.config.gyro.dynLpfFilter.cutoff, _model.config.gyro.dynLpfFilter.freq); - for(size_t i = 0; i < AXIS_COUNT_RPY; i++) { + if (_model.config.gyro.dynLpfFilter.cutoff > 0) + { + int gyroFreq = + Utils::map(scale, 1000, 2000, _model.config.gyro.dynLpfFilter.cutoff, _model.config.gyro.dynLpfFilter.freq); + for (size_t i = 0; i < AXIS_COUNT_RPY; i++) + { _model.state.gyro.filter[i].reconfigure(gyroFreq); } } - if(_model.config.dterm.dynLpfFilter.cutoff > 0) { - int dtermFreq = Utils::map(scale, 1000, 2000, _model.config.dterm.dynLpfFilter.cutoff, _model.config.dterm.dynLpfFilter.freq); - for(size_t i = 0; i < AXIS_COUNT_RPY; i++) { + if (_model.config.dterm.dynLpfFilter.cutoff > 0) + { + int dtermFreq = + Utils::map(scale, 1000, 2000, _model.config.dterm.dynLpfFilter.cutoff, _model.config.dterm.dynLpfFilter.freq); + for (size_t i = 0; i < AXIS_COUNT_RPY; i++) + { _model.state.innerPid[i].dtermFilter.reconfigure(dtermFreq); } } @@ -256,15 +260,16 @@ void Actuator::updateDynLpf() void Actuator::updateRescueConfig() { - switch(_model.state.mode.rescueConfigMode) + switch (_model.state.mode.rescueConfigMode) { case RESCUE_CONFIG_PENDING: // if some rc frames are received, disable to prevent activate later - if(_model.state.input.frameCount > 100) + if (_model.state.input.frameCount > 100) { _model.state.mode.rescueConfigMode = RESCUE_CONFIG_DISABLED; } - if(_model.state.failsafe.phase != FC_FAILSAFE_IDLE && _model.config.rescueConfigDelay > 0 && millis() > _model.config.rescueConfigDelay * 1000) + if (_model.state.failsafe.phase != FC_FAILSAFE_IDLE && _model.config.rescueConfigDelay > 0 && + millis() > _model.config.rescueConfigDelay * 1000) { _model.state.mode.rescueConfigMode = RESCUE_CONFIG_ACTIVE; } @@ -278,12 +283,12 @@ void Actuator::updateRescueConfig() void Actuator::updateLed() { - if(_model.isModeActive(MODE_ARMED) || _model.state.mode.isLongClickActive()) + if (_model.isModeActive(MODE_ARMED) || _model.state.mode.isLongClickActive()) { - if(_model.state.mode.isLongClickActive()) _model.setGpsHome(); + if (_model.state.mode.isLongClickActive()) _model.setGpsHome(); _model.state.led.setStatus(Connect::LED_ON); } - else if(_model.armingDisabled()) + else if (_model.armingDisabled()) { _model.state.led.setStatus(Connect::LED_ERROR); } @@ -293,4 +298,4 @@ void Actuator::updateLed() } } -} +} // namespace Espfc::Control diff --git a/lib/Espfc/src/Control/Actuator.h b/lib/Espfc/src/Control/Actuator.h index d8b692eb..0e5422e7 100644 --- a/lib/Espfc/src/Control/Actuator.h +++ b/lib/Espfc/src/Control/Actuator.h @@ -6,28 +6,28 @@ namespace Espfc::Control { class Actuator { - public: - Actuator(Model& model); - - int begin(); - int update(); - - #ifndef UNIT_TEST - private: - #endif - - void updateScaler(); - void updateArmingDisabled(); - void updateModeMask(); - bool canActivateMode(FlightMode mode); - void updateArmed(); - void updateAirMode(); - void updateBuzzer(); - void updateDynLpf(); - void updateRescueConfig(); - void updateLed(); - - Model& _model; +public: + Actuator(Model& model); + + int begin(); + int update(); + +#ifndef UNIT_TEST +private: +#endif + + void updateScaler(); + void updateArmingDisabled(); + void updateModeMask(); + bool canActivateMode(FlightMode mode); + void updateArmed(); + void updateAirMode(); + void updateBuzzer(); + void updateDynLpf(); + void updateRescueConfig(); + void updateLed(); + + Model& _model; }; -} +} // namespace Espfc::Control diff --git a/lib/Espfc/src/Control/Altitude.hpp b/lib/Espfc/src/Control/Altitude.hpp index 86e8110f..dc9243d8 100644 --- a/lib/Espfc/src/Control/Altitude.hpp +++ b/lib/Espfc/src/Control/Altitude.hpp @@ -16,13 +16,26 @@ class Altitude _model.state.altitude.height = 0.0f; _model.state.altitude.vario = 0.0f; - _altitudeFilter.begin(FilterConfig(FILTER_PT3, 5), _model.state.accel.timer.rate); - _varioFilter.begin(FilterConfig(FILTER_PT3, 5), _model.state.accel.timer.rate); - _varioFusion.begin(_model.state.accel.timer.rate, _model.config.altHold.baroTau * 0.1f); + reload(MODEL_CHANGE_FILTER); return 1; } + int reload(ModelChangeEvent event) + { + switch (event) + { + case MODEL_CHANGE_FILTER: + _altitudeFilter.begin(FilterConfig(FILTER_PT3, 5), _model.state.accel.timer.rate); + _varioFilter.begin(FilterConfig(FILTER_PT3, 5), _model.state.accel.timer.rate); + _varioFusion.begin(_model.state.accel.timer.rate, _model.config.altHold.baroTau * 0.1f); + break; + default: + break; + } + return 1; + } + int update() { Utils::Stats::Measure measure(_model.state.stats, COUNTER_IMU_FUSION2); diff --git a/lib/Espfc/src/Control/Controller.cpp b/lib/Espfc/src/Control/Controller.cpp index d4594b50..b81ee9a6 100644 --- a/lib/Espfc/src/Control/Controller.cpp +++ b/lib/Espfc/src/Control/Controller.cpp @@ -8,16 +8,28 @@ Controller::Controller(Model& model): _model(model), _rates{} {} int Controller::begin() { - _rates.begin(_model.config.input); - _speedFilter.begin(FilterConfig(FILTER_BIQUAD, 10), _model.state.loopTimer.rate); - - beginInnerLoop(AXIS_ROLL); - beginInnerLoop(AXIS_PITCH); - beginInnerLoop(AXIS_YAW); - beginOuterLoop(AXIS_ROLL); - beginOuterLoop(AXIS_PITCH); - beginAltHold(); + reload(MODEL_CHANGE_RATES); + reload(MODEL_CHANGE_FILTER); + reload(MODEL_CHANGE_PID); + return 1; +} +int Controller::reload(ModelChangeEvent event) +{ + switch (event) + { + case MODEL_CHANGE_RATES: + _rates.begin(_model.config.input); + break; + case MODEL_CHANGE_FILTER: + reloadFilter(); + break; + case MODEL_CHANGE_PID: + reloadPid(); + break; + default: + break; + } return 1; } @@ -35,9 +47,13 @@ int FAST_CODE_ATTR Controller::update() resetIterm(); switch (_model.config.mixer.type) { - case FC_MIXER_GIMBAL: outerLoopRobot(); break; + case FC_MIXER_GIMBAL: + outerLoopRobot(); + break; - default: outerLoop(); break; + default: + outerLoop(); + break; } } @@ -45,9 +61,13 @@ int FAST_CODE_ATTR Controller::update() Utils::Stats::Measure measure(_model.state.stats, COUNTER_INNER_PID); switch (_model.config.mixer.type) { - case FC_MIXER_GIMBAL: innerLoopRobot(); break; + case FC_MIXER_GIMBAL: + innerLoopRobot(); + break; - default: innerLoop(); break; + default: + innerLoop(); + break; } } @@ -254,13 +274,13 @@ void Controller::resetIterm() float Controller::calculateSetpointRate(int axis, float input) const { - if (axis == AXIS_YAW) input *= -1.f; - return _rates.getSetpoint(axis, input); + return _rates.getSetpoint(axis, axis == AXIS_YAW ? -input : input); } -void Controller::beginInnerLoop(size_t axis) +void Controller::reloadPid() { const int pidFilterRate = _model.state.loopTimer.rate; + float pidScale[] = {1.f, 1.f, 1.f}; if (_model.config.mixer.type == FC_MIXER_GIMBAL) { @@ -268,69 +288,55 @@ void Controller::beginInnerLoop(size_t axis) pidScale[AXIS_PITCH] = 20.f; // ROBOT } - const auto& pc = _model.config.pid[axis]; - const auto& dtermConf = _model.config.dterm; - - auto& pid = _model.state.innerPid[axis]; - pid.Kp = (float)pc.P * PTERM_SCALE * pidScale[axis]; - pid.Ki = (float)pc.I * ITERM_SCALE * pidScale[axis]; - pid.Kd = (float)pc.D * DTERM_SCALE * pidScale[axis]; - pid.Kf = (float)pc.F * FTERM_SCALE * pidScale[axis]; - pid.iLimitLow = -_model.config.iterm.limit * 0.01f; - pid.iLimitHigh = _model.config.iterm.limit * 0.01f; - pid.oLimitLow = -0.66f; - pid.oLimitHigh = 0.66f; - pid.rate = pidFilterRate; - pid.dtermNotchFilter.begin(dtermConf.notchFilter, pidFilterRate); - if (dtermConf.dynLpfFilter.cutoff > 0) - { - pid.dtermFilter.begin(FilterConfig((FilterType)dtermConf.filter.type, dtermConf.dynLpfFilter.cutoff), - pidFilterRate); - } - else + // inner loop + for (size_t axis = 0; axis < AXIS_COUNT_RPY; axis++) { - pid.dtermFilter.begin(dtermConf.filter, pidFilterRate); - } - pid.dtermFilter2.begin(dtermConf.filter2, pidFilterRate); - pid.ftermFilter.begin(_model.config.input.filterDerivative, pidFilterRate); - pid.itermRelaxFilter.begin(FilterConfig(FILTER_PT1, _model.config.iterm.relaxCutoff), pidFilterRate); - if (axis == AXIS_YAW) - { - pid.itermRelax = (_model.config.iterm.relax == ITERM_RELAX_RPY || _model.config.iterm.relax == ITERM_RELAX_RPY_INC) - ? _model.config.iterm.relax - : ITERM_RELAX_OFF; - pid.ptermFilter.begin(_model.config.yaw.filter, pidFilterRate); + const auto& pc = _model.config.pid[axis]; + auto& pid = _model.state.innerPid[axis]; + pid.Kp = (float)pc.P * PTERM_SCALE * pidScale[axis]; + pid.Ki = (float)pc.I * ITERM_SCALE * pidScale[axis]; + pid.Kd = (float)pc.D * DTERM_SCALE * pidScale[axis]; + pid.Kf = (float)pc.F * FTERM_SCALE * pidScale[axis]; + pid.iLimitLow = -_model.config.iterm.limit * 0.01f; + pid.iLimitHigh = _model.config.iterm.limit * 0.01f; + pid.oLimitLow = -0.66f; + pid.oLimitHigh = 0.66f; + pid.rate = pidFilterRate; + if (axis == AXIS_YAW) + { + pid.itermRelax = + (_model.config.iterm.relax == ITERM_RELAX_RPY || _model.config.iterm.relax == ITERM_RELAX_RPY_INC) + ? _model.config.iterm.relax + : ITERM_RELAX_OFF; + } + else + { + pid.itermRelax = _model.config.iterm.relax; + } + pid.begin(); } - else + + // outer loop + for (size_t axis = 0; axis < AXIS_COUNT_RP; axis++) { - pid.itermRelax = _model.config.iterm.relax; + const auto& pc = _model.config.pid[FC_PID_LEVEL]; + + auto& pid = _model.state.outerPid[axis]; + pid.Kp = (float)pc.P * LEVEL_PTERM_SCALE; + pid.Ki = (float)pc.I * LEVEL_ITERM_SCALE; + pid.Kd = (float)pc.D * LEVEL_DTERM_SCALE; + pid.Kf = (float)pc.F * LEVEL_FTERM_SCALE; + pid.iLimitHigh = Utils::toRad(_model.config.level.rateLimit * 0.1f); + pid.iLimitLow = -pid.iLimitHigh; + pid.oLimitHigh = Utils::toRad(_model.config.level.rateLimit); + pid.oLimitLow = -pid.oLimitHigh; + pid.rate = pidFilterRate; + // pid.iLimit = 0.3f; // ROBOT + // pid.oLimit = 1.f; // ROBOT + pid.begin(); } - pid.begin(); -} - -void Controller::beginOuterLoop(size_t axis) -{ - const int pidFilterRate = _model.state.loopTimer.rate; - const auto& pc = _model.config.pid[FC_PID_LEVEL]; - - auto& pid = _model.state.outerPid[axis]; - pid.Kp = (float)pc.P * LEVEL_PTERM_SCALE; - pid.Ki = (float)pc.I * LEVEL_ITERM_SCALE; - pid.Kd = (float)pc.D * LEVEL_DTERM_SCALE; - pid.Kf = (float)pc.F * LEVEL_FTERM_SCALE; - pid.iLimitHigh = Utils::toRad(_model.config.level.rateLimit * 0.1f); - pid.iLimitLow = -pid.iLimitHigh; - pid.oLimitHigh = Utils::toRad(_model.config.level.rateLimit); - pid.oLimitLow = -pid.oLimitHigh; - pid.rate = pidFilterRate; - pid.ptermFilter.begin(_model.config.level.ptermFilter, pidFilterRate); - // pid.iLimit = 0.3f; // ROBOT - // pid.oLimit = 1.f; // ROBOT - pid.begin(); -} -void Controller::beginAltHold() -{ + // alt hold pid float itermCenter = std::clamp((int)_model.config.altHold.itermCenter, 10, 60) * 0.01f; float itermRange = itermCenter * std::clamp((int)_model.config.altHold.itermRange, 10, 60) * 0.01f; const auto& pc = _model.config.pid[FC_PID_VEL]; @@ -344,6 +350,53 @@ void Controller::beginAltHold() pid.iLimitHigh = -1.0f + 2.0f * (itermCenter + itermRange); pid.iReset = pid.iLimitLow; pid.rate = _model.state.loopTimer.rate; + pid.begin(); +} + +void Controller::reloadFilter() +{ + _speedFilter.begin(FilterConfig(FILTER_BIQUAD, 10), _model.state.loopTimer.rate); + + const int pidFilterRate = _model.state.loopTimer.rate; + + // inner loop + const auto& dtermConf = _model.config.dterm; + for (size_t axis = 0; axis < AXIS_COUNT_RPY; axis++) + { + auto& pid = _model.state.innerPid[axis]; + pid.rate = pidFilterRate; + pid.dtermNotchFilter.begin(dtermConf.notchFilter, pidFilterRate); + if (dtermConf.dynLpfFilter.cutoff > 0) + { + pid.dtermFilter.begin(FilterConfig((FilterType)dtermConf.filter.type, dtermConf.dynLpfFilter.cutoff), + pidFilterRate); + } + else + { + pid.dtermFilter.begin(dtermConf.filter, pidFilterRate); + } + pid.dtermFilter2.begin(dtermConf.filter2, pidFilterRate); + pid.ftermFilter.begin(_model.config.input.filterDerivative, pidFilterRate); + pid.itermRelaxFilter.begin(FilterConfig(FILTER_PT1, _model.config.iterm.relaxCutoff), pidFilterRate); + if (axis == AXIS_YAW) + { + pid.ptermFilter.begin(_model.config.yaw.filter, pidFilterRate); + } + pid.begin(); + } + + // outer loop + for (size_t axis = 0; axis < AXIS_COUNT_RP; axis++) + { + auto& pid = _model.state.outerPid[axis]; + pid.rate = pidFilterRate; + pid.ptermFilter.begin(_model.config.level.ptermFilter, pidFilterRate); + pid.begin(); + } + + // alt hold pid + auto& pid = _model.state.innerPid[AXIS_THRUST]; + pid.rate = _model.state.loopTimer.rate; pid.dtermFilter.begin(FilterConfig(FILTER_PT1, 10), _model.state.loopTimer.rate); pid.ftermDerivative = false; pid.begin(); diff --git a/lib/Espfc/src/Control/Controller.h b/lib/Espfc/src/Control/Controller.h index b0d58417..563dcb5f 100644 --- a/lib/Espfc/src/Control/Controller.h +++ b/lib/Espfc/src/Control/Controller.h @@ -1,6 +1,5 @@ #pragma once -#include "Control/Altitude.hpp" #include "Control/Rates.h" #include "Model.h" @@ -11,6 +10,7 @@ class Controller public: Controller(Model& model); int begin(); + int reload(ModelChangeEvent event); int update(); void outerLoopRobot(); @@ -24,9 +24,8 @@ class Controller float calcualteAltHoldSetpoint() const; private: - void beginAltHold(); - void beginInnerLoop(size_t axis); - void beginOuterLoop(size_t axis); + void reloadFilter(); + void reloadPid(); Model& _model; Rates _rates; diff --git a/lib/Espfc/src/Control/Fusion.cpp b/lib/Espfc/src/Control/Fusion.cpp index 55cbcf63..8185bf76 100644 --- a/lib/Espfc/src/Control/Fusion.cpp +++ b/lib/Espfc/src/Control/Fusion.cpp @@ -1,9 +1,7 @@ #include "Control/Fusion.h" #include "Utils/MemoryHelper.h" -namespace Espfc { - -namespace Control { +namespace Espfc::Control { Fusion::Fusion(Model& model): _model(model), _madgwick(), _mahony(), _rtqf(), _useMag(false) {} @@ -21,16 +19,35 @@ int Fusion::begin() _rtqf.begin(_model.state.accel.timer.rate); _rtqf.setKp(_model.config.fusion.gain * 0.0002f); + reload(MODEL_CHANGE_FILTER); + _model.logger.info() .log("FUSION") .log(FusionConfig::getModeName((FusionMode)_model.config.fusion.mode)) .logln(_model.config.fusion.gain); - for (size_t i = 0; i < 4; i++) + return 1; +} + +int Fusion::reload(ModelChangeEvent event) +{ + switch (event) { - _qFilter[i].begin(FilterConfig(FILTER_BIQUAD, 20), _model.state.accel.timer.rate); + case MODEL_CHANGE_FILTER: { + const auto cutoff = _model.state.accel.timer.rate / GYRO_FUSION_LPF_DIV; + for (size_t i = 0; i < AXIS_COUNT_RPY; i++) + { + _model.state.attitude.filter[i].begin(FilterConfig(FILTER_PT1, cutoff), _model.state.loopTimer.rate); + } + for (size_t i = 0; i < 4; i++) + { + _qFilter[i].begin(FilterConfig(FILTER_BIQUAD, 20), _model.state.accel.timer.rate); + } + break; + } + default: + break; } - return 1; } @@ -55,11 +72,18 @@ int FAST_CODE_ATTR Fusion::update() switch (_model.config.fusion.mode) { - case FUSION_MADGWICK: q = madgwickFusion(g, a, m); break; - case FUSION_MAHONY: q = mahonyFusion(g, a, m); break; - case FUSION_RTQF: q = rtqfFusion(g, a, m); break; + case FUSION_MADGWICK: + q = madgwickFusion(g, a, m); + break; + case FUSION_MAHONY: + q = mahonyFusion(g, a, m); + break; + case FUSION_RTQF: + q = rtqfFusion(g, a, m); + break; case FUSION_NONE: - default: break; + default: + break; } _model.state.attitude.quaternion = Quaternion::ensureSign(q, _model.state.attitude.quaternion); @@ -136,6 +160,4 @@ Quaternion FAST_CODE_ATTR Fusion::rtqfFusion(VectorFloat g, VectorFloat a, Vecto return _rtqf.getQuaternion(); } -} // namespace Control - -} // namespace Espfc +} // namespace Espfc::Control diff --git a/lib/Espfc/src/Control/Fusion.h b/lib/Espfc/src/Control/Fusion.h index 65ec667a..9c6f1634 100644 --- a/lib/Espfc/src/Control/Fusion.h +++ b/lib/Espfc/src/Control/Fusion.h @@ -1,37 +1,34 @@ #pragma once #include "Model.h" +#include "Utils/Filter.h" #include #include #include -#include "Utils/Filter.h" - -namespace Espfc { -namespace Control { +namespace Espfc::Control { class Fusion { - public: - Fusion(Model& model); - int begin(); - void restoreGain(); - int update(); - - private: - Quaternion madgwickFusion(VectorFloat g, VectorFloat a, VectorFloat m); - Quaternion mahonyFusion(VectorFloat g, VectorFloat a, VectorFloat m); - Quaternion rtqfFusion(VectorFloat g, VectorFloat a, VectorFloat m); - Quaternion filterQuaternion(const Quaternion& q); - - Model& _model; - Madgwick _madgwick; - Mahony _mahony; - Rtqf _rtqf; - Utils::Filter _qFilter[4]; - bool _useMag; +public: + Fusion(Model& model); + int begin(); + int reload(ModelChangeEvent event); + int update(); + void restoreGain(); + +private: + Quaternion madgwickFusion(VectorFloat g, VectorFloat a, VectorFloat m); + Quaternion mahonyFusion(VectorFloat g, VectorFloat a, VectorFloat m); + Quaternion rtqfFusion(VectorFloat g, VectorFloat a, VectorFloat m); + Quaternion filterQuaternion(const Quaternion& q); + + Model& _model; + Madgwick _madgwick; + Mahony _mahony; + Rtqf _rtqf; + Utils::Filter _qFilter[4]; + bool _useMag; }; -} - -} +} // namespace Espfc::Control diff --git a/lib/Espfc/src/Control/Pid.cpp b/lib/Espfc/src/Control/Pid.cpp index 5067a50e..59a7d5f7 100644 --- a/lib/Espfc/src/Control/Pid.cpp +++ b/lib/Espfc/src/Control/Pid.cpp @@ -3,20 +3,16 @@ #include "Utils/MemoryHelper.h" #include -namespace Espfc { +namespace Espfc::Control { -namespace Control { - -Pid::Pid(): - rate(1.0f), dt(1.0f), Kp(0.1), Ki(0.f), Kd(0.f), Kf(0.0f), - iLimitLow(-0.3f), iLimitHigh(0.3f), iReset(0.0f), oLimitLow(-1.f), oLimitHigh(1.f), - pScale(1.f), iScale(1.f), dScale(1.f), fScale(1.f), - error(0.f), iTermError(0.f), - pTerm(0.f), iTerm(0.f), dTerm(0.f), fTerm(0.f), - prevMeasurement(0.f), prevError(0.f), prevSetpoint(0.f), - ftermDerivative(true), outputSaturated(false), - itermRelax(ITERM_RELAX_OFF), itermRelaxFactor(1.0f), itermRelaxBase(0.f) - {} +Pid::Pid() + : rate(1.0f), dt(1.0f), Kp(0.1), Ki(0.f), Kd(0.f), Kf(0.0f), iLimitLow(-0.3f), iLimitHigh(0.3f), iReset(0.0f), + oLimitLow(-1.f), oLimitHigh(1.f), pScale(1.f), iScale(1.f), dScale(1.f), fScale(1.f), error(0.f), iTermError(0.f), + pTerm(0.f), iTerm(0.f), dTerm(0.f), fTerm(0.f), prevMeasurement(0.f), prevError(0.f), prevSetpoint(0.f), + ftermDerivative(true), outputSaturated(false), itermRelax(ITERM_RELAX_OFF), itermRelaxFactor(1.0f), + itermRelaxBase(0.f) +{ +} void Pid::begin() { @@ -31,25 +27,25 @@ void Pid::resetIterm() float FAST_CODE_ATTR Pid::update(float setpoint, float measurement) { error = setpoint - measurement; - + // P-term pTerm = Kp * error * pScale; pTerm = ptermFilter.update(pTerm); // I-term iTermError = error; - if(Ki > 0.f && iScale > 0.f) + if (Ki > 0.f && iScale > 0.f) { - if(!outputSaturated) + if (!outputSaturated) { // I-term relax - if(itermRelax) + if (itermRelax) { const bool increasing = (iTerm > 0 && iTermError > 0) || (iTerm < 0 && iTermError < 0); const bool incrementOnly = itermRelax == ITERM_RELAX_RP_INC || itermRelax == ITERM_RELAX_RPY_INC; itermRelaxBase = setpoint - itermRelaxFilter.update(setpoint); - itermRelaxFactor = std::max(0.0f, 1.0f - std::abs(Utils::toDeg(itermRelaxBase)) * 0.025f); // (itermRelaxBase / 40) - if(!incrementOnly || increasing) iTermError *= itermRelaxFactor; + itermRelaxFactor = std::max(0.0f, 1.0f - std::abs(Utils::toDeg(itermRelaxBase)) * 0.025f); + if (!incrementOnly || increasing) iTermError *= itermRelaxFactor; } iTerm += Ki * iScale * iTermError * dt; iTerm = std::clamp(iTerm, iLimitLow, iLimitHigh); @@ -61,9 +57,9 @@ float FAST_CODE_ATTR Pid::update(float setpoint, float measurement) } // D-term - if(Kd > 0.f && dScale > 0.f) + if (Kd > 0.f && dScale > 0.f) { - //dTerm = (Kd * dScale * (((error - prevError) * dGamma) + (prevMeasurement - measure) * (1.f - dGamma)) / dt); + // dTerm = (Kd * dScale * (((error - prevError) * dGamma) + (prevMeasurement - measure) * (1.f - dGamma)) / dt); dTerm = Kd * dScale * ((prevMeasurement - measurement) * rate); dTerm = dtermNotchFilter.update(dTerm); dTerm = dtermFilter.update(dTerm); @@ -75,9 +71,9 @@ float FAST_CODE_ATTR Pid::update(float setpoint, float measurement) } // F-term - if(Kf > 0.f && fScale > 0.f) + if (Kf > 0.f && fScale > 0.f) { - if(ftermDerivative) + if (ftermDerivative) { fTerm = Kf * fScale * (setpoint - prevSetpoint) * rate; } @@ -99,6 +95,4 @@ float FAST_CODE_ATTR Pid::update(float setpoint, float measurement) return std::clamp(pTerm + iTerm + dTerm + fTerm, oLimitLow, oLimitHigh); } -} - -} +} // namespace Espfc::Control diff --git a/lib/Espfc/src/Control/Pid.h b/lib/Espfc/src/Control/Pid.h index eb977f18..5d4581d6 100644 --- a/lib/Espfc/src/Control/Pid.h +++ b/lib/Espfc/src/Control/Pid.h @@ -1,8 +1,8 @@ #pragma once -#include #include "Utils/Filter.h" #include "Utils/Math.hpp" +#include namespace Espfc { @@ -12,22 +12,23 @@ constexpr float ITERM_SCALE_BETAFLIGHT = 0.244381f; constexpr float DTERM_SCALE_BETAFLIGHT = 0.000529f; constexpr float FTERM_SCALE_BETAFLIGHT = 0.00013754f; -constexpr float PTERM_SCALE = PTERM_SCALE_BETAFLIGHT * Utils::toDeg(1.0f) * 0.001f; // ~ 0.00183 = 0.032029f * 57.29 / 1000 +constexpr float PTERM_SCALE = PTERM_SCALE_BETAFLIGHT * Utils::toDeg(1.0f) * 0.001f; // ~ 0.00183f constexpr float ITERM_SCALE = ITERM_SCALE_BETAFLIGHT * Utils::toDeg(1.0f) * 0.001f; // ~ 0.014f constexpr float DTERM_SCALE = DTERM_SCALE_BETAFLIGHT * Utils::toDeg(1.0f) * 0.001f; // ~ 0.0000303f constexpr float FTERM_SCALE = FTERM_SCALE_BETAFLIGHT * Utils::toDeg(1.0f) * 0.001f; // ~ 0.00000788f -constexpr float LEVEL_PTERM_SCALE = 0.1f; // 1/10 -constexpr float LEVEL_ITERM_SCALE = 0.1f; // 1/10 -constexpr float LEVEL_DTERM_SCALE = 0.001f; // 1/1000 -constexpr float LEVEL_FTERM_SCALE = 0.001f; // 1/1000 +constexpr float LEVEL_PTERM_SCALE = 0.1f; // 1/10 +constexpr float LEVEL_ITERM_SCALE = 0.1f; // 1/10 +constexpr float LEVEL_DTERM_SCALE = 0.001f; // 1/1000 +constexpr float LEVEL_FTERM_SCALE = 0.001f; // 1/1000 constexpr float VEL_PTERM_SCALE = 0.001f; constexpr float VEL_ITERM_SCALE = 0.0025f; constexpr float VEL_DTERM_SCALE = 0.00005f; constexpr float VEL_FTERM_SCALE = 0.001f; -enum ItermRelaxType { +enum ItermRelaxType +{ ITERM_RELAX_OFF, ITERM_RELAX_RP, ITERM_RELAX_RPY, @@ -91,6 +92,6 @@ class Pid float itermRelaxBase; }; -} +} // namespace Control -} +} // namespace Espfc diff --git a/lib/Espfc/src/Device/BaroDevice.hpp b/lib/Espfc/src/Device/BaroDevice.hpp index da792866..6b50a756 100644 --- a/lib/Espfc/src/Device/BaroDevice.hpp +++ b/lib/Espfc/src/Device/BaroDevice.hpp @@ -5,7 +5,7 @@ namespace Espfc { -enum BaroDeviceType +enum BaroDeviceType : uint8_t { BARO_DEFAULT = 0, BARO_NONE = 1, diff --git a/lib/Espfc/src/Device/GyroDevice.cpp b/lib/Espfc/src/Device/GyroDevice.cpp index 504832ef..4a80ea1c 100644 --- a/lib/Espfc/src/Device/GyroDevice.cpp +++ b/lib/Espfc/src/Device/GyroDevice.cpp @@ -4,7 +4,7 @@ namespace Espfc::Device { const char** GyroDevice::getNames() { - static const char* devChoices[] = {"AUTO", "NONE", "MPU6000", "MPU6050", "MPU6500", "MPU9250", + static const char* devChoices[] = {"NONE", "AUTO", "MPU6000", "MPU6050", "MPU6500", "MPU9250", "LSM6DSO", "ICM20602", "BMI160", "ICM42688", nullptr}; return devChoices; } diff --git a/lib/Espfc/src/Device/GyroDevice.hpp b/lib/Espfc/src/Device/GyroDevice.hpp index 89714f07..a5ed4b7b 100644 --- a/lib/Espfc/src/Device/GyroDevice.hpp +++ b/lib/Espfc/src/Device/GyroDevice.hpp @@ -6,10 +6,10 @@ namespace Espfc { -enum GyroDeviceType +enum GyroDeviceType : uint8_t { - GYRO_AUTO = 0, - GYRO_NONE = 1, + GYRO_NONE = 0, + GYRO_AUTO = 1, GYRO_MPU6000 = 2, GYRO_MPU6050 = 3, GYRO_MPU6500 = 4, diff --git a/lib/Espfc/src/Device/InputIBUS.hpp b/lib/Espfc/src/Device/InputIBUS.hpp index 0fb123fb..ceed43c3 100644 --- a/lib/Espfc/src/Device/InputIBUS.hpp +++ b/lib/Espfc/src/Device/InputIBUS.hpp @@ -1,10 +1,9 @@ #pragma once -#include "Device/SerialDevice.h" #include "Device/InputDevice.h" +#include "Device/SerialDevice.h" -namespace Espfc::Device -{ +namespace Espfc::Device { class InputIBUS : public InputDevice { @@ -19,10 +18,10 @@ class InputIBUS : public InputDevice InputIBUS(); - int begin(Device::SerialDevice *serial); + int begin(Device::SerialDevice* serial); InputStatus update() override; uint16_t get(uint8_t i) const override; - void get(uint16_t *data, size_t len) const override; + void get(uint16_t* data, size_t len) const override; size_t getChannelCount() const override; bool needAverage() const override; @@ -44,7 +43,7 @@ class InputIBUS : public InputDevice static constexpr uint8_t IBUS_COMMAND = 0x40; static constexpr size_t CHANNELS = 14; - Device::SerialDevice *_serial; + Device::SerialDevice* _serial; IbusState _state; uint8_t _idx = 0; bool _new_data; @@ -53,4 +52,4 @@ class InputIBUS : public InputDevice uint16_t _channels[CHANNELS]; }; -} +} // namespace Espfc::Device diff --git a/lib/Espfc/src/Device/Mag/MagQMC5883P.cpp b/lib/Espfc/src/Device/Mag/MagQMC5883P.cpp index cef07b11..f16260bb 100644 --- a/lib/Espfc/src/Device/Mag/MagQMC5883P.cpp +++ b/lib/Espfc/src/Device/Mag/MagQMC5883P.cpp @@ -98,10 +98,18 @@ const VectorFloat MagQMC5883P::convert(const VectorInt16& v) const float lsbPerGauss = 3750.0f; switch (_currentRange) { - case QMC5883P_RANGE_30G: lsbPerGauss = 1000.0f; break; - case QMC5883P_RANGE_12G: lsbPerGauss = 2500.0f; break; - case QMC5883P_RANGE_8G: lsbPerGauss = 3750.0f; break; - case QMC5883P_RANGE_2G: lsbPerGauss = 15000.0f; break; + case QMC5883P_RANGE_30G: + lsbPerGauss = 1000.0f; + break; + case QMC5883P_RANGE_12G: + lsbPerGauss = 2500.0f; + break; + case QMC5883P_RANGE_8G: + lsbPerGauss = 3750.0f; + break; + case QMC5883P_RANGE_2G: + lsbPerGauss = 15000.0f; + break; } return static_cast(v) * (1.0f / lsbPerGauss); @@ -111,11 +119,15 @@ int MagQMC5883P::getRate() const { switch (_currentOdr) { - case QMC5883P_ODR_10HZ: return 10; - case QMC5883P_ODR_50HZ: return 50; - case QMC5883P_ODR_200HZ: return 200; + case QMC5883P_ODR_10HZ: + return 10; + case QMC5883P_ODR_50HZ: + return 50; + case QMC5883P_ODR_200HZ: + return 200; case QMC5883P_ODR_100HZ: - default: return 100; + default: + return 100; } } diff --git a/lib/Espfc/src/Device/MagDevice.hpp b/lib/Espfc/src/Device/MagDevice.hpp index 664f9d93..6abec3b7 100644 --- a/lib/Espfc/src/Device/MagDevice.hpp +++ b/lib/Espfc/src/Device/MagDevice.hpp @@ -5,7 +5,7 @@ namespace Espfc { -enum MagDeviceType +enum MagDeviceType : uint8_t { MAG_DEFAULT = 0, MAG_NONE = 1, diff --git a/lib/Espfc/src/Espfc.cpp b/lib/Espfc/src/Espfc.cpp index 6823fb7b..cd1443a5 100644 --- a/lib/Espfc/src/Espfc.cpp +++ b/lib/Espfc/src/Espfc.cpp @@ -1,13 +1,13 @@ #include "Espfc.h" -#include "Hal/Gpio.h" #include "Debug_Espfc.h" namespace Espfc { -Espfc::Espfc(): - _hardware{_model}, _controller{_model}, _telemetry{_model}, _input{_model, _telemetry}, _actuator{_model}, _sensor{_model}, - _mixer{_model}, _blackbox{_model}, _buzzer{_model}, _serial{_model, _telemetry} - {} +Espfc::Espfc() + : _hardware{_model}, _controller{_model}, _telemetry{_model}, _input{_model, _telemetry}, _actuator{_model}, + _sensor{_model}, _mixer{_model}, _blackbox{_model}, _buzzer{_model}, _serial{_model, _telemetry} +{ +} int Espfc::load() { @@ -21,43 +21,49 @@ int Espfc::begin() { _model.state.led.begin(_model.config.pin[PIN_LED_BLINK], _model.config.led.type, _model.config.led.invert); - _serial.begin(); // requires _model.load() - //_model.logStorageResult(); - _hardware.begin(); // requires _model.load() - _model.begin(); // requires _hardware.begin() + _serial.begin(); // requires _model.load() + _hardware.begin(); // requires _model.load() + _model.begin(); // requires _hardware.begin() _mixer.begin(); - _sensor.begin(); // requires _hardware.begin() - _input.begin(); // requires _serial.begin() - _actuator.begin(); // requires _model.begin() + _sensor.begin(); // requires _hardware.begin() + _input.begin(); // requires _serial.begin() + _actuator.begin(); // requires _model.begin() _controller.begin(); - _blackbox.begin(); // requires _serial.begin(), _actuator.begin() + _blackbox.begin(); // requires _serial.begin(), _actuator.begin() _buzzer.begin(); _model.state.buzzer.push(BUZZER_SYSTEM_INIT); + _model.setConfigChangeListener([this](ModelChangeEvent event) { + _serial.reload(event); + _sensor.reload(event); + _input.reload(event); + _controller.reload(event); + }); + return 1; } int FAST_CODE_ATTR Espfc::update(bool externalTrigger) { - if(externalTrigger) + if (externalTrigger) { _model.state.gyro.timer.update(); } else { - if(!_model.state.gyro.timer.check()) return 0; + if (!_model.state.gyro.timer.check()) return 0; } Utils::Stats::Measure measure(_model.state.stats, COUNTER_CPU_0); #if defined(ESPFC_MULTI_CORE) _sensor.read(); - if(_model.state.input.timer.syncTo(_model.state.gyro.timer, 1u)) + if (_model.state.input.timer.syncTo(_model.state.gyro.timer, 1u)) { _input.update(); } - if(_model.state.actuatorTimer.check()) + if (_model.state.actuatorTimer.check()) { _actuator.update(); } @@ -65,19 +71,19 @@ int FAST_CODE_ATTR Espfc::update(bool externalTrigger) #else _sensor.update(); - if(_model.state.loopTimer.syncTo(_model.state.gyro.timer)) + if (_model.state.loopTimer.syncTo(_model.state.gyro.timer)) { _controller.update(); - if(_model.state.mixer.timer.syncTo(_model.state.loopTimer)) + if (_model.state.mixer.timer.syncTo(_model.state.loopTimer)) { _mixer.update(); } _blackbox.update(); - if(_model.state.input.timer.syncTo(_model.state.gyro.timer, 1u)) + if (_model.state.input.timer.syncTo(_model.state.gyro.timer, 1u)) { _input.update(); } - if(_model.state.actuatorTimer.check()) + if (_model.state.actuatorTimer.check()) { _actuator.update(); } @@ -98,7 +104,7 @@ int FAST_CODE_ATTR Espfc::update(bool externalTrigger) int FAST_CODE_ATTR Espfc::updateOther() { #if defined(ESPFC_MULTI_CORE) - if(_model.state.appQueue.isEmpty()) + if (_model.state.appQueue.isEmpty()) { return 0; } @@ -106,14 +112,14 @@ int FAST_CODE_ATTR Espfc::updateOther() Utils::Stats::Measure measure(_model.state.stats, COUNTER_CPU_1); - switch(e.type) + switch (e.type) { case EVENT_GYRO_READ: _sensor.preLoop(); _controller.update(); - // skip mixer and bb if earlier than half cycle, possible delay in previous iteration, + // skip mixer and bb if earlier than half cycle, possible delay in previous iteration, // to keep space to receive dshot erpm frame, but process rest - if(_loop_next < micros()) + if (_loop_next < micros()) { _loop_next = micros() + _model.state.loopTimer.interval / 2; _mixer.update(); @@ -133,5 +139,4 @@ int FAST_CODE_ATTR Espfc::updateOther() return 1; } -} - +} // namespace Espfc diff --git a/lib/Espfc/src/Espfc.h b/lib/Espfc/src/Espfc.h index 5da2181e..90381f27 100644 --- a/lib/Espfc/src/Espfc.h +++ b/lib/Espfc/src/Espfc.h @@ -1,47 +1,47 @@ #pragma once -#include "Model.h" -#include "Hardware.h" +#include "Blackbox/Blackbox.h" +#include "Connect/Buzzer.hpp" +#include "Control/Actuator.h" #include "Control/Controller.h" +#include "Hardware.h" #include "Input.h" -#include "Control/Actuator.h" +#include "Model.h" +#include "Output/Mixer.h" #include "SensorManager.h" -#include "TelemetryManager.h" #include "SerialManager.h" -#include "Output/Mixer.h" -#include "Blackbox/Blackbox.h" -#include "Connect/Buzzer.hpp" +#include "TelemetryManager.h" namespace Espfc { class Espfc { - public: - Espfc(); +public: + Espfc(); - int load(); - int begin(); - int update(bool externalTrigger = false); - int updateOther(); + int load(); + int begin(); + int update(bool externalTrigger = false); + int updateOther(); - int getGyroInterval() const - { - return _model.state.gyro.timer.interval; - } + int getGyroInterval() const + { + return _model.state.gyro.timer.interval; + } - private: - Model _model; - Hardware _hardware; - Control::Controller _controller; - TelemetryManager _telemetry; - Input _input; - Control::Actuator _actuator; - SensorManager _sensor; - Output::Mixer _mixer; - Blackbox::Blackbox _blackbox; - Connect::Buzzer _buzzer; - SerialManager _serial; - uint32_t _loop_next; +private: + Model _model; + Hardware _hardware; + Control::Controller _controller; + TelemetryManager _telemetry; + Input _input; + Control::Actuator _actuator; + SensorManager _sensor; + Output::Mixer _mixer; + Blackbox::Blackbox _blackbox; + Connect::Buzzer _buzzer; + SerialManager _serial; + uint32_t _loop_next; }; -} +} // namespace Espfc diff --git a/lib/Espfc/src/Input.cpp b/lib/Espfc/src/Input.cpp index f78e920e..d8e450a3 100644 --- a/lib/Espfc/src/Input.cpp +++ b/lib/Espfc/src/Input.cpp @@ -1,5 +1,7 @@ #include "Input.h" +#include "ModelConfig.h" +#include "Utils/Filter.h" #include "Utils/Math.hpp" #include "Utils/MemoryHelper.h" @@ -14,27 +16,12 @@ int Input::begin() _model.state.input.frameDelta = FRAME_TIME_DEFAULT_US; _model.state.input.frameRate = 1000000ul / _model.state.input.frameDelta; _model.state.input.frameCount = 0; - _model.state.input.autoFactor = 1.f / (2.f + _model.config.input.filterAutoFactor * 0.1f); - switch(_model.config.input.interpolationMode) - { - case INPUT_INTERPOLATION_AUTO: - _model.state.input.interpolationDelta = std::clamp(_model.state.input.frameDelta, 4000, 40000) * 0.000001f; // estimate real interval - break; - case INPUT_INTERPOLATION_MANUAL: - _model.state.input.interpolationDelta = _model.config.input.interpolationInterval * 0.001f; // manual interval - break; - case INPUT_INTERPOLATION_DEFAULT: - case INPUT_INTERPOLATION_OFF: - default: - _model.state.input.interpolationDelta = FRAME_TIME_DEFAULT_US * 0.000001f; - break; - } - _model.state.input.interpolationStep = _model.state.loopTimer.intervalf / _model.state.input.interpolationDelta; - _step = 0.0f; - for(size_t c = 0; c < INPUT_CHANNELS; ++c) + + reload(MODEL_CHANGE_INPUT); + + for (size_t c = 0; c < INPUT_CHANNELS; ++c) { - if(_device) _filter[c].begin(FilterConfig(_device->needAverage() ? FILTER_FIR2 : FILTER_NONE, 1), _model.state.loopTimer.rate); - int16_t v = c == AXIS_THRUST ? PWM_RANGE_MIN : PWM_RANGE_MID; + const int16_t v = c == AXIS_THRUST ? PWM_RANGE_MIN : PWM_RANGE_MID; _model.state.input.raw[c] = v; _model.state.input.buffer[c] = v; _model.state.input.bufferPrevious[c] = v; @@ -43,10 +30,42 @@ int Input::begin() return 1; } +int Input::reload(ModelChangeEvent event) +{ + switch (event) + { + case MODEL_CHANGE_INPUT: { + _model.state.input.autoFactor = 1.f / (2.f + _model.config.input.filterAutoFactor * 0.1f); + _model.state.input.autoThrottleFactor = 1.f / (2.f + _model.config.input.filterAutoThrottleFactor * 0.1f); + const FilterConfig rxFilter{_device && _device->needAverage() ? FILTER_FIR2 : FILTER_NONE, 1}; + const FilterConfig inputFilter{_model.config.input.filterEnable ? _model.config.input.filter + : FilterConfig(FILTER_PT3, 25)}; + const FilterConfig throtleFilter{_model.config.input.filterEnable ? _model.config.input.filterThrottle + : FilterConfig(FILTER_PT3, 25)}; + for (size_t i = 0; i < AXIS_COUNT_RPYT; i++) + { + _filter[i].begin(rxFilter, 100); // rx filter uses FIR2 on NONE, sample rate doesn't really matter here + if (i == AXIS_THRUST) + { + _model.state.input.filter[i].begin(throtleFilter, _model.state.input.timer.rate); + } + else + { + _model.state.input.filter[i].begin(inputFilter, _model.state.input.timer.rate); + } + } + break; + } + default: + break; + } + return 1; +} + int16_t FAST_CODE_ATTR Input::getFailsafeValue(uint8_t c) { const InputChannelConfig& ich = _model.config.input.channel[c]; - switch(ich.fsMode) + switch (ich.fsMode) { case FAILSAFE_MODE_AUTO: return c == AXIS_THRUST ? PWM_RANGE_MIN : PWM_RANGE_MID; @@ -62,13 +81,13 @@ int16_t FAST_CODE_ATTR Input::getFailsafeValue(uint8_t c) void FAST_CODE_ATTR Input::setInput(Axis i, float v, bool newFrame, bool noFilter) { const InputChannelConfig& ich = _model.config.input.channel[i]; - if(i <= AXIS_THRUST) + if (i <= AXIS_THRUST) { const float nv = noFilter ? v : _model.state.input.filter[i].update(v); _model.state.input.us[i] = nv; _model.state.input.ch[i] = Utils::map(nv, ich.min, ich.max, -1.f, 1.f); } - else if(newFrame) + else if (newFrame) { _model.state.input.us[i] = v; _model.state.input.ch[i] = Utils::map(v, ich.min, ich.max, -1.f, 1.f); @@ -77,18 +96,18 @@ void FAST_CODE_ATTR Input::setInput(Axis i, float v, bool newFrame, bool noFilte int FAST_CODE_ATTR Input::update() { - if(!_device) return 0; + if (!_device) return 0; uint32_t startTime = micros(); InputStatus status = readInputs(); - if(!failsafe(status)) + if (!failsafe(status)) { filterInputs(status); } - if(_model.config.debug.mode == DEBUG_PIDLOOP) + if (_model.config.debug.mode == DEBUG_PIDLOOP) { _model.state.debug[1] = micros() - startTime; } @@ -103,12 +122,12 @@ InputStatus FAST_CODE_ATTR Input::readInputs() InputStatus status = _device->update(); - if(_model.config.debug.mode == DEBUG_RX_TIMING) + if (_model.config.debug.mode == DEBUG_RX_TIMING) { _model.state.debug[0] = micros() - startTime; } - if(status == INPUT_IDLE) return status; + if (status == INPUT_IDLE) return status; _model.state.input.rxLoss = (status == INPUT_LOST || status == INPUT_FAILSAFE); _model.state.input.rxFailSafe = (status == INPUT_FAILSAFE); @@ -118,7 +137,7 @@ InputStatus FAST_CODE_ATTR Input::readInputs() processInputs(); - if(_model.config.debug.mode == DEBUG_RX_SIGNAL_LOSS) + if (_model.config.debug.mode == DEBUG_RX_SIGNAL_LOSS) { _model.state.debug[0] = !_model.state.input.rxLoss; _model.state.debug[1] = _model.state.input.rxFailSafe; @@ -131,7 +150,7 @@ InputStatus FAST_CODE_ATTR Input::readInputs() void FAST_CODE_ATTR Input::processInputs() { - if(_model.state.input.frameCount < 5) return; // ignore few first frames that might be garbage + if (_model.state.input.frameCount < 5) return; // ignore few first frames that might be garbage uint32_t startTime = micros(); @@ -139,7 +158,7 @@ void FAST_CODE_ATTR Input::processInputs() _device->get(channels, _model.state.input.channelCount); _model.state.input.channelsValid = true; - for(size_t c = 0; c < _model.state.input.channelCount; c++) + for (size_t c = 0; c < _model.state.input.channelCount; c++) { const InputChannelConfig& ich = _model.config.input.channel[c]; @@ -150,7 +169,8 @@ void FAST_CODE_ATTR Input::processInputs() v -= _model.config.input.midRc - PWM_RANGE_MID; // adj range - //float t = Utils::map3((float)v, (float)ich.min, (float)ich.neutral, (float)ich.max, (float)PWM_RANGE_MIN, (float)PWM_RANGE_MID, (float)PWM_RANGE_MAX); + // float t = Utils::map3((float)v, (float)ich.min, (float)ich.neutral, (float)ich.max, (float)PWM_RANGE_MIN, + // (float)PWM_RANGE_MID, (float)PWM_RANGE_MAX); float t = Utils::mapi(v, ich.min, ich.max, PWM_RANGE_MIN, PWM_RANGE_MAX); // filter if required @@ -158,16 +178,16 @@ void FAST_CODE_ATTR Input::processInputs() v = lrintf(t); // apply deadband - if(c < AXIS_THRUST) + if (c < AXIS_THRUST) { v = Utils::deadband(v - PWM_RANGE_MID, (int)_model.config.input.deadband) + PWM_RANGE_MID; } // check if inputs are valid, apply failsafe value otherwise - if(v < _model.config.input.minRc || v > _model.config.input.maxRc) + if (v < _model.config.input.minRc || v > _model.config.input.maxRc) { v = getFailsafeValue(c); - if(c <= AXIS_THRUST) _model.state.input.channelsValid = false; + if (c <= AXIS_THRUST) _model.state.input.channelsValid = false; } // update input buffer @@ -175,7 +195,7 @@ void FAST_CODE_ATTR Input::processInputs() _model.state.input.buffer[c] = v; } - if(_model.config.debug.mode == DEBUG_RX_TIMING) + if (_model.config.debug.mode == DEBUG_RX_TIMING) { _model.state.debug[2] = micros() - startTime; } @@ -185,19 +205,19 @@ bool FAST_CODE_ATTR Input::failsafe(InputStatus status) { Utils::Stats::Measure measure(_model.state.stats, COUNTER_FAILSAFE); - if(_model.isSwitchActive(MODE_FAILSAFE)) + if (_model.isSwitchActive(MODE_FAILSAFE)) { failsafeStage2(); return false; // not real failsafe, rx link is still valid } - if(status == INPUT_RECEIVED) + if (status == INPUT_RECEIVED) { failsafeIdle(); return false; } - if(status == INPUT_FAILSAFE) + if (status == INPUT_FAILSAFE) { failsafeStage2(); return true; @@ -205,14 +225,14 @@ bool FAST_CODE_ATTR Input::failsafe(InputStatus status) // stage 2 timeout _model.state.input.lossTime = micros() - _model.state.input.frameTime; - if(_model.state.input.lossTime > std::clamp(_model.config.failsafe.delay, 2u, 200u) * TENTH_TO_US) + if (_model.state.input.lossTime > std::clamp(_model.config.failsafe.delay, 2u, 200u) * TENTH_TO_US) { failsafeStage2(); return true; } // stage 1 timeout (100ms) - if(_model.state.input.lossTime >= 2 * TENTH_TO_US) + if (_model.state.input.lossTime >= 2 * TENTH_TO_US) { failsafeStage1(); return true; @@ -231,7 +251,7 @@ void FAST_CODE_ATTR Input::failsafeStage1() { _model.state.failsafe.phase = FC_FAILSAFE_RX_LOSS_DETECTED; _model.state.input.rxLoss = true; - for(size_t i = 0; i < _model.state.input.channelCount; i++) + for (size_t i = 0; i < _model.state.input.channelCount; i++) { setInput((Axis)i, getFailsafeValue(i), true, true); } @@ -242,7 +262,7 @@ void FAST_CODE_ATTR Input::failsafeStage2() _model.state.failsafe.phase = FC_FAILSAFE_RX_LOSS_DETECTED; _model.state.input.rxLoss = true; _model.state.input.rxFailSafe = true; - if(_model.isModeActive(MODE_ARMED)) + if (_model.isModeActive(MODE_ARMED)) { _model.state.failsafe.phase = FC_FAILSAFE_LANDED; _model.disarm(DISARM_REASON_FAILSAFE); @@ -255,31 +275,14 @@ void FAST_CODE_ATTR Input::filterInputs(InputStatus status) uint32_t startTime = micros(); const bool newFrame = status != INPUT_IDLE; - const bool interpolation = _model.config.input.interpolationMode != INPUT_INTERPOLATION_OFF && _model.config.input.filterType == INPUT_INTERPOLATION; - - if(interpolation) - { - if(newFrame) - { - _step = 0.0f; - } - if(_step < 1.f) - { - _step += _model.state.input.interpolationStep; - } - } - for(size_t c = 0; c < _model.state.input.channelCount; c++) + for (size_t c = 0; c < _model.state.input.channelCount; c++) { - float v = _model.state.input.buffer[c]; - if(c <= AXIS_THRUST) - { - v = interpolation ? _interpolate(_model.state.input.bufferPrevious[c], v, _step) : v; - } + const float v = _model.state.input.buffer[c]; setInput((Axis)c, v, newFrame); } - if(_model.config.debug.mode == DEBUG_RX_TIMING) + if (_model.config.debug.mode == DEBUG_RX_TIMING) { _model.state.debug[3] = micros() - startTime; } @@ -287,62 +290,71 @@ void FAST_CODE_ATTR Input::filterInputs(InputStatus status) void FAST_CODE_ATTR Input::updateFrameRate() { + auto& input = _model.state.input; const uint32_t now = micros(); - const uint32_t frameDelta = now - _model.state.input.frameTime; + const uint32_t frameDelta = now - input.frameTime; - _model.state.input.frameTime = now; - _model.state.input.frameDelta += (((int)frameDelta - (int)_model.state.input.frameDelta) >> 3); // avg * 0.125 - _model.state.input.frameRate = 1000000ul / _model.state.input.frameDelta; - - if (_model.config.input.interpolationMode == INPUT_INTERPOLATION_AUTO && _model.config.input.filterType == INPUT_INTERPOLATION) - { - _model.state.input.interpolationDelta = std::clamp(_model.state.input.frameDelta, 4000, 40000) * 0.000001f; // estimate real interval - _model.state.input.interpolationStep = _model.state.loopTimer.intervalf / _model.state.input.interpolationDelta; - } + input.frameTime = now; + input.frameDelta += (((int)frameDelta - (int)input.frameDelta) >> 3); // avg * 0.125 + input.frameRate = 1000000ul / input.frameDelta; - if(_model.config.debug.mode == DEBUG_RC_SMOOTHING_RATE) + if (_model.config.debug.mode == DEBUG_RC_SMOOTHING_RATE) { - _model.state.debug[0] = _model.state.input.frameDelta / 10; - _model.state.debug[1] = _model.state.input.frameRate; + _model.state.debug[0] = input.frameDelta / 10; + _model.state.debug[1] = input.frameRate; } // auto cutoff input freq - float freq = std::max(_model.state.input.frameRate * _model.state.input.autoFactor, 15.f); // no lower than 15Hz - if(freq > _model.state.input.autoFreq * 1.1f || freq < _model.state.input.autoFreq * 0.9f) + float freq = std::clamp(input.frameRate * input.autoFactor, 15.f, 500.f); // no lower than 15Hz + float throttleFreq = std::clamp(input.frameRate * input.autoThrottleFactor, 15.f, 500.f); // no lower than 15Hz + if (freq > input.autoFreq * 1.1f || freq < input.autoFreq * 0.9f) { - _model.state.input.autoFreq += 0.25f * (freq - _model.state.input.autoFreq); - if(_model.config.debug.mode == DEBUG_RC_SMOOTHING_RATE) - { - _model.state.debug[2] = lrintf(freq); - _model.state.debug[3] = lrintf(_model.state.input.autoFreq); - } - FilterConfig conf((FilterType)_model.config.input.filter.type, _model.state.input.autoFreq); - FilterConfig confDerivative((FilterType)_model.config.input.filterDerivative.type, _model.state.input.autoFreq); - for(size_t i = 0; i < AXIS_COUNT_RPYT; i++) + input.autoFreq += 0.25f * (freq - input.autoFreq); // lpf + input.autoThrottleFreq += 0.25f * (throttleFreq - input.autoThrottleFreq); // lpf + + FilterConfig conf{(FilterType)_model.config.input.filter.type, std::clamp(input.autoFreq, 15, 500)}; + FilterConfig confThrottle{(FilterType)_model.config.input.filterThrottle.type, + std::clamp(input.autoThrottleFreq, 15, 500)}; + FilterConfig confDerivative{(FilterType)_model.config.input.filterDerivative.type, + std::clamp(input.autoFreq, 15, 500)}; + + for (size_t i = 0; i < AXIS_COUNT_RPY; i++) { - if(_model.config.input.filter.freq == 0) + if (_model.config.input.filter.freq == 0) { _model.state.input.filter[i].reconfigure(conf, _model.state.loopTimer.rate); } - if(_model.config.input.filterDerivative.freq == 0) + if (_model.config.input.filterDerivative.freq == 0) { _model.state.innerPid[i].ftermFilter.reconfigure(confDerivative, _model.state.loopTimer.rate); } } + + if (_model.config.input.filterThrottle.freq == 0) + { + _model.state.input.filter[AXIS_THRUST].reconfigure(confThrottle, _model.state.loopTimer.rate); + } + + if (_model.config.debug.mode == DEBUG_RC_SMOOTHING_RATE) + { + _model.state.debug[2] = lrintf(freq); + _model.state.debug[3] = lrintf(input.autoFreq); + _model.state.debug[4] = lrintf(input.autoThrottleFreq); + } } - if(_model.config.debug.mode == DEBUG_RX_TIMING) + if (_model.config.debug.mode == DEBUG_RX_TIMING) { _model.state.debug[1] = micros() - now; } } -Device::InputDevice * Input::getInputDevice() +Device::InputDevice* Input::getInputDevice() { - Device::SerialDevice * serial = _model.getSerialStream(SERIAL_FUNCTION_RX_SERIAL); - if(serial && _model.isFeatureActive(FEATURE_RX_SERIAL)) + Device::SerialDevice* serial = _model.getSerialStream(SERIAL_FUNCTION_RX_SERIAL); + if (serial && _model.isFeatureActive(FEATURE_RX_SERIAL)) { - switch(_model.config.input.serialRxProvider) + switch (_model.config.input.serialRxProvider) { case SERIALRX_IBUS: _ibus.begin(serial); @@ -360,14 +372,14 @@ Device::InputDevice * Input::getInputDevice() return &_crsf; } } - else if(_model.isFeatureActive(FEATURE_RX_PPM) && _model.config.pin[PIN_INPUT_RX] != -1) + else if (_model.isFeatureActive(FEATURE_RX_PPM) && _model.config.pin[PIN_INPUT_RX] != -1) { _ppm.begin(_model.config.pin[PIN_INPUT_RX], _model.config.input.ppmMode); _model.logger.info().log("RX PPM").log(_model.config.pin[PIN_INPUT_RX]).logln(_model.config.input.ppmMode); return &_ppm; } #if defined(ESPFC_ESPNOW) - else if(_model.isFeatureActive(FEATURE_RX_SPI)) + else if (_model.isFeatureActive(FEATURE_RX_SPI)) { int status = _espnow.begin(); _model.logger.info().log("RX ESPNOW").logln(status); @@ -378,4 +390,4 @@ Device::InputDevice * Input::getInputDevice() return nullptr; } -} +} // namespace Espfc diff --git a/lib/Espfc/src/Input.h b/lib/Espfc/src/Input.h index 11af6b41..61039539 100644 --- a/lib/Espfc/src/Input.h +++ b/lib/Espfc/src/Input.h @@ -1,12 +1,11 @@ #pragma once -#include "Model.h" -#include "Utils/Math.hpp" +#include "Device/InputCRSF.h" #include "Device/InputDevice.h" -#include "Device/InputPPM.h" #include "Device/InputIBUS.hpp" +#include "Device/InputPPM.h" #include "Device/InputSBUS.h" -#include "Device/InputCRSF.h" +#include "Model.h" #include "TelemetryManager.h" #if defined(ESPFC_ESPNOW) #include "Device/InputEspNow.h" @@ -14,14 +13,16 @@ namespace Espfc { -enum FailsafeChannelMode { +enum FailsafeChannelMode +{ FAILSAFE_MODE_AUTO, FAILSAFE_MODE_HOLD, FAILSAFE_MODE_SET, FAILSAFE_MODE_INVALID }; -enum InputPwmRange { +enum InputPwmRange +{ PWM_RANGE_MIN = 1000, PWM_RANGE_MID = 1500, PWM_RANGE_MAX = 2000 @@ -29,48 +30,43 @@ enum InputPwmRange { class Input { - public: - Input(Model& model, TelemetryManager& telemetry); - - int begin(); - int update(); +public: + Input(Model& model, TelemetryManager& telemetry); - int16_t getFailsafeValue(uint8_t c); - void setInput(Axis i, float v, bool newFrame, bool noFilter = false); + int begin(); + int reload(ModelChangeEvent event); + int update(); - InputStatus readInputs(); - void processInputs(); + int16_t getFailsafeValue(uint8_t c); + void setInput(Axis i, float v, bool newFrame, bool noFilter = false); - bool failsafe(InputStatus status); - void failsafeIdle(); - void failsafeStage1(); - void failsafeStage2(); - void filterInputs(InputStatus status); + InputStatus readInputs(); + void processInputs(); - void updateFrameRate(); - Device::InputDevice * getInputDevice(); + bool failsafe(InputStatus status); + void failsafeIdle(); + void failsafeStage1(); + void failsafeStage2(); + void filterInputs(InputStatus status); - private: - inline float _interpolate(float left, float right, float step) - { - return (left * (1.f - step) + right * step); - } + void updateFrameRate(); + Device::InputDevice* getInputDevice(); - Model& _model; - TelemetryManager& _telemetry; - Device::InputDevice * _device; - Utils::Filter _filter[INPUT_CHANNELS]; - float _step; - Device::InputPPM _ppm; - Device::InputIBUS _ibus; - Device::InputSBUS _sbus; - Device::InputCRSF _crsf; +private: + Model& _model; + TelemetryManager& _telemetry; + Device::InputDevice* _device; + Utils::Filter _filter[INPUT_CHANNELS]; + Device::InputPPM _ppm; + Device::InputIBUS _ibus; + Device::InputSBUS _sbus; + Device::InputCRSF _crsf; #if defined(ESPFC_ESPNOW) - Device::InputEspNow _espnow; + Device::InputEspNow _espnow; #endif - static constexpr uint32_t TENTH_TO_US = 100000UL; // 1_000_000 / 10; - static constexpr uint32_t FRAME_TIME_DEFAULT_US = 23000; // 23 ms + static constexpr uint32_t TENTH_TO_US = 100000UL; // 1_000_000 / 10; + static constexpr uint32_t FRAME_TIME_DEFAULT_US = 23000; // 23 ms }; -} +} // namespace Espfc diff --git a/lib/Espfc/src/Model.h b/lib/Espfc/src/Model.h index 6bfd3a85..c91031ed 100644 --- a/lib/Espfc/src/Model.h +++ b/lib/Espfc/src/Model.h @@ -3,8 +3,9 @@ #include #include +#include +#include #include -#include "Debug_Espfc.h" #include "ModelConfig.h" #include "ModelState.h" #include "Utils/Storage.h" @@ -13,6 +14,15 @@ namespace Espfc { +enum ModelChangeEvent +{ + MODEL_CHANGE_FILTER, + MODEL_CHANGE_PID, + MODEL_CHANGE_RATES, + MODEL_CHANGE_ACCEL, + MODEL_CHANGE_INPUT, +}; + class Model { public: @@ -88,7 +98,7 @@ class Model bool blackboxEnabled() const { // serial or flash - return (config.blackbox.dev == BLACKBOX_DEV_SERIAL || config.blackbox.dev == BLACKBOX_DEV_FLASH) && config.blackbox.pDenom > 0; + return (config.blackbox.dev == BLACKBOX_DEV_SERIAL || config.blackbox.dev == BLACKBOX_DEV_FLASH); } bool gyroActive() const /* IRAM_ATTR */ @@ -291,6 +301,100 @@ class Model begin(); } + void setRebootRequired() + { + state.rebootRequired = true; + setArmingDisabled(ARMING_DISABLED_REBOOT_REQUIRED, true); + } + + bool getRebootRequired() const + { + return state.rebootRequired; + } + + void calculateSimplifiedPids(const SimplifiedTuningConfig& s, PidConfig out[3]) const + { + // ESP-FC compile-time PID defaults for roll/pitch/yaw (no D-Max on this target) + static const PidConfig def[3] = { + { 45, 80, 30, 110 }, + { 47, 84, 34, 115 }, + { 45, 80, 0, 110 }, + }; + if (s.pidsMode == SIMPLIFIED_TUNING_OFF) return; + const float master = s.masterMultiplier * 0.01f; + const float pi = s.piGain * 0.01f; + const float d = s.dGain * 0.01f; + const float ff = s.ffGain * 0.01f; + const float ig = s.iGain * 0.01f; + for (int axis = FC_PID_ROLL; axis <= std::clamp(s.pidsMode, FC_PID_ROLL, FC_PID_YAW); axis++) + { + const float pitchD = (axis == FC_PID_PITCH) ? s.rollPitchRatio * 0.01f : 1.0f; + const float pitchPi = (axis == FC_PID_PITCH) ? s.pitchPiGain * 0.01f : 1.0f; + out[axis].P = constrain(lrintf(def[axis].P * master * pi * pitchPi), 0, SIMPLIFIED_PID_GAIN_MAX); + out[axis].I = constrain(lrintf(def[axis].I * master * pi * ig * pitchPi), 0, SIMPLIFIED_PID_GAIN_MAX); + out[axis].D = constrain(lrintf(def[axis].D * master * d * pitchD), 0, SIMPLIFIED_PID_GAIN_MAX); + out[axis].F = constrain(lrintf(def[axis].F * master * pitchPi * ff), 0, SIMPLIFIED_F_GAIN_MAX); + } + } + + void calculateSimplifiedDtermFilters(uint8_t mult, int16_t& lpf1, int16_t& lpf2, int16_t& dynMin, int16_t& dynMax) const + { + if (dynMin) + { + dynMin = constrain(SIMPLIFIED_DTERM_LPF1_DYN_MIN_HZ * mult / 100, 0, SIMPLIFIED_DYN_LPF_MAX_HZ); + dynMax = constrain(SIMPLIFIED_DTERM_LPF1_DYN_MAX_HZ * mult / 100, 0, SIMPLIFIED_DYN_LPF_MAX_HZ); + } + if (lpf1) lpf1 = constrain(SIMPLIFIED_DTERM_LPF1_DYN_MIN_HZ * mult / 100, 0, SIMPLIFIED_DYN_LPF_MAX_HZ); + if (lpf2) lpf2 = constrain(SIMPLIFIED_DTERM_LPF2_HZ * mult / 100, 0, SIMPLIFIED_LPF_MAX_HZ); + } + + void calculateSimplifiedGyroFilters(uint8_t mult, int16_t& lpf1, int16_t& lpf2, int16_t& dynMin, int16_t& dynMax) const + { + if (dynMin) + { + dynMin = constrain(SIMPLIFIED_GYRO_LPF1_DYN_MIN_HZ * mult / 100, 0, SIMPLIFIED_DYN_LPF_MAX_HZ); + dynMax = constrain(SIMPLIFIED_GYRO_LPF1_DYN_MAX_HZ * mult / 100, 0, SIMPLIFIED_DYN_LPF_MAX_HZ); + } + if (lpf1) lpf1 = constrain(SIMPLIFIED_GYRO_LPF1_DYN_MIN_HZ * mult / 100, 0, SIMPLIFIED_DYN_LPF_MAX_HZ); + if (lpf2) lpf2 = constrain(SIMPLIFIED_GYRO_LPF2_HZ * mult / 100, 0, SIMPLIFIED_LPF_MAX_HZ); + } + + std::tuple validateSimplifiedTuning() const + { + const auto& s = config.simplifiedTuning; + + const auto& pids = config.pid; + PidConfig tmp[3] = {pids[0], pids[1], pids[2]}; + + calculateSimplifiedPids(s, tmp); + bool pidOk = tmp[0].P == pids[0].P && tmp[0].I == pids[0].I && + tmp[0].D == pids[0].D && tmp[0].F == pids[0].F && + tmp[1].P == pids[1].P && tmp[1].I == pids[1].I && + tmp[1].D == pids[1].D && tmp[1].F == pids[1].F && + tmp[2].P == pids[2].P && tmp[2].I == pids[2].I && + tmp[2].D == pids[2].D && tmp[2].F == pids[2].F; + + const auto& gyro = config.gyro; + int16_t glpf1 = gyro.filter.freq; + int16_t glpf2 = gyro.filter2.freq; + int16_t gmin = gyro.dynLpfFilter.cutoff; + int16_t gmax = gyro.dynLpfFilter.freq; + if (s.gyroFilter) calculateSimplifiedGyroFilters(s.gyroFilterMultiplier, glpf1, glpf2, gmin, gmax); + bool gyroOk = glpf1 == gyro.filter.freq && glpf2 == gyro.filter2.freq && + gmin == gyro.dynLpfFilter.cutoff && gmax == gyro.dynLpfFilter.freq; + + const auto& dterm = config.dterm; + int16_t dlpf1 = dterm.filter.freq; + int16_t dlpf2 = dterm.filter2.freq; + int16_t dmin = dterm.dynLpfFilter.cutoff; + int16_t dmax = dterm.dynLpfFilter.freq; + if (s.dtermFilter) calculateSimplifiedDtermFilters(s.dtermFilterMultiplier, dlpf1, dlpf2, dmin, dmax); + bool dtermOk = dlpf1 == dterm.filter.freq && dlpf2 == dterm.filter2.freq && + dmin == dterm.dynLpfFilter.cutoff && dmax == dterm.dynLpfFilter.freq; + + return std::make_tuple(pidOk, gyroOk, dtermOk); + } + void reset() { initialize(); @@ -453,64 +557,7 @@ class Model { state.mag.timer.setRate(state.mag.rate); } - - state.boardAlignment.init(VectorFloat(Utils::toRad(config.boardAlignment[0]), Utils::toRad(config.boardAlignment[1]), Utils::toRad(config.boardAlignment[2]))); - onAccChange(); - - const uint32_t gyroPreFilterRate = state.gyro.timer.rate; - const uint32_t gyroFilterRate = state.loopTimer.rate; - const uint32_t inputFilterRate = state.input.timer.rate; - - // configure filters - for(size_t i = 0; i < AXIS_COUNT_RPY; i++) - { - if(isFeatureActive(FEATURE_DYNAMIC_FILTER)) - { - for(size_t p = 0; p < (size_t)config.gyro.dynamicFilter.count; p++) - { - state.gyro.dynNotchFilter[p][i].begin(FilterConfig(FILTER_NOTCH_DF1, 400, 380), gyroFilterRate); - } - } - state.gyro.notch1Filter[i].begin(config.gyro.notch1Filter, gyroFilterRate); - state.gyro.notch2Filter[i].begin(config.gyro.notch2Filter, gyroFilterRate); - if(config.gyro.dynLpfFilter.cutoff > 0) - { - state.gyro.filter[i].begin(FilterConfig((FilterType)config.gyro.filter.type, config.gyro.dynLpfFilter.cutoff), gyroFilterRate); - } - else - { - state.gyro.filter[i].begin(config.gyro.filter, gyroFilterRate); - } - state.gyro.filter2[i].begin(config.gyro.filter2, gyroFilterRate); - state.gyro.filter3[i].begin(config.gyro.filter3, gyroPreFilterRate); - state.attitude.filter[i].begin(FilterConfig(FILTER_PT1, state.accel.timer.rate / GYRO_FUSION_LPF_DIV), gyroFilterRate); - for(size_t m = 0; m < RPM_FILTER_MOTOR_MAX; m++) - { - state.gyro.rpmFreqFilter[m].begin(FilterConfig(FILTER_PT1, config.gyro.rpmFilter.freqLpf), gyroFilterRate); - for(size_t n = 0; n < config.gyro.rpmFilter.harmonics; n++) - { - int center = Utils::mapi(m * RPM_FILTER_HARMONICS_MAX + n, 0, RPM_FILTER_MOTOR_MAX * config.gyro.rpmFilter.harmonics, config.gyro.rpmFilter.minFreq, gyroFilterRate / 2); - state.gyro.rpmFilter[m][n][i].begin(FilterConfig(FILTER_NOTCH_DF1, center, center * 0.98f), gyroFilterRate); - } - } - if(magActive()) - { - state.mag.filter[i].begin(config.mag.filter, state.mag.timer.rate); - } - } - - for(size_t i = 0; i < 4; i++) - { - if (config.input.filterType == INPUT_FILTER) - { - state.input.filter[i].begin(config.input.filter, inputFilterRate); - } - else - { - state.input.filter[i].begin(FilterConfig(FILTER_PT3, 25), inputFilterRate); - } - } - + // ensure disarmed pulses for(size_t i = 0; i < OUTPUT_CHANNELS; i++) { @@ -525,11 +572,6 @@ class Model //state.telemetryTimer.setRate(100); } - void onAccChange() - { - state.trimRotation.init(VectorFloat{Utils::toRad(config.accel.trim[1]) * 0.1f, Utils::toRad(config.accel.trim[0]) * 0.1f, 0.0f}); - } - void postLoad() { // load current sensor calibration @@ -576,11 +618,23 @@ class Model #endif } + void notifyConfigChange(ModelChangeEvent event) + { + if (_onConfigChange) _onConfigChange(event); + } + + void setConfigChangeListener(std::function listener) + { + _onConfigChange = listener; + } + private: #ifndef UNIT_TEST Utils::Storage _storage; #endif StorageResult _storageResult; + + std::function _onConfigChange{}; }; } diff --git a/lib/Espfc/src/ModelConfig.h b/lib/Espfc/src/ModelConfig.h index c09985ff..ca6b3195 100644 --- a/lib/Espfc/src/ModelConfig.h +++ b/lib/Espfc/src/ModelConfig.h @@ -26,11 +26,6 @@ enum GyroDlpf { GYRO_DLPF_EX = 0x07, }; -enum AccelMode { - ACCEL_DELAYED = 0x00, - ACCEL_GYRO = 0x01, -}; - enum SensorAlign { ALIGN_DEFAULT = 0, ALIGN_CW0_DEG = 1, @@ -116,7 +111,6 @@ enum DebugMode { DEBUG_GYRO_FILTERED, DEBUG_ACCELEROMETER, DEBUG_PIDLOOP, - DEBUG_GYRO_SCALED, DEBUG_RC_INTERPOLATION, DEBUG_ANGLERATE, DEBUG_ESC_SENSOR, @@ -131,14 +125,15 @@ enum DebugMode { DEBUG_RX_FRSKY_SPI, DEBUG_RX_SFHSS_SPI, DEBUG_GYRO_RAW, - DEBUG_DUAL_GYRO_RAW, - DEBUG_DUAL_GYRO_DIFF, + DEBUG_MULTI_GYRO_RAW, + DEBUG_MULTI_GYRO_DIFF, DEBUG_MAX7456_SIGNAL, DEBUG_MAX7456_SPICLOCK, DEBUG_SBUS, DEBUG_FPORT, DEBUG_RANGEFINDER, DEBUG_RANGEFINDER_QUALITY, + DEBUG_OPTICALFLOW, DEBUG_LIDAR_TF, DEBUG_ADC_INTERNAL, DEBUG_RUNAWAY_TAKEOFF, @@ -157,22 +152,62 @@ enum DebugMode { DEBUG_RX_SPEKTRUM_SPI, DEBUG_DSHOT_RPM_TELEMETRY, DEBUG_RPM_FILTER, - DEBUG_D_MIN, + DEBUG_D_MAX, DEBUG_AC_CORRECTION, DEBUG_AC_ERROR, - DEBUG_DUAL_GYRO_SCALED, + DEBUG_MULTI_GYRO_SCALED, DEBUG_DSHOT_RPM_ERRORS, DEBUG_CRSF_LINK_STATISTICS_UPLINK, DEBUG_CRSF_LINK_STATISTICS_PWR, DEBUG_CRSF_LINK_STATISTICS_DOWN, DEBUG_BARO, - DEBUG_GPS_RESCUE_THROTTLE_PID, + DEBUG_AUTOPILOT_ALTITUDE, DEBUG_DYN_IDLE, - DEBUG_FF_LIMIT, - DEBUG_FF_INTERPOLATED, + DEBUG_FEEDFORWARD_LIMIT, + DEBUG_FEEDFORWARD, DEBUG_BLACKBOX_OUTPUT, DEBUG_GYRO_SAMPLE, DEBUG_RX_TIMING, + DEBUG_D_LPF, + DEBUG_VTX_TRAMP, + DEBUG_GHST, + DEBUG_GHST_MSP, + DEBUG_SCHEDULER_DETERMINISM, + DEBUG_TIMING_ACCURACY, + DEBUG_RX_EXPRESSLRS_SPI, + DEBUG_RX_EXPRESSLRS_PHASELOCK, + DEBUG_RX_STATE_TIME, + DEBUG_GPS_RESCUE_VELOCITY, + DEBUG_GPS_RESCUE_HEADING, + DEBUG_GPS_RESCUE_TRACKING, + DEBUG_GPS_CONNECTION, + DEBUG_ATTITUDE, + DEBUG_VTX_MSP, + DEBUG_GPS_DOP, + DEBUG_FAILSAFE, + DEBUG_GYRO_CALIBRATION, + DEBUG_ANGLE_MODE, + DEBUG_ANGLE_TARGET, + DEBUG_CURRENT_ANGLE, + DEBUG_DSHOT_TELEMETRY_COUNTS, + DEBUG_RPM_LIMIT, + DEBUG_RC_STATS, + DEBUG_MAG_CALIB, + DEBUG_MAG_TASK_RATE, + DEBUG_EZLANDING, + DEBUG_TPA, + DEBUG_S_TERM, + DEBUG_SPA, + DEBUG_TASK, + DEBUG_GIMBAL, + DEBUG_WING_SETPOINT, + DEBUG_CHIRP, + DEBUG_FLASH_TEST_PRBS, + DEBUG_MAVLINK_TELEMETRY, + DEBUG_AUTOPILOT_PID, + DEBUG_POSITION_NAV, + DEBUG_AUTOPILOT_STOP, + DEBUG_PITOT, DEBUG_COUNT, }; @@ -211,18 +246,6 @@ enum Feature { FEATURE_DYNAMIC_FILTER = 1 << 29, }; -enum InputInterpolation { - INPUT_INTERPOLATION_OFF, - INPUT_INTERPOLATION_DEFAULT, - INPUT_INTERPOLATION_AUTO, - INPUT_INTERPOLATION_MANUAL, -}; - -enum InputFilterType : uint8_t { - INPUT_INTERPOLATION, - INPUT_FILTER, -}; - constexpr size_t MODEL_NAME_LEN = 16; constexpr size_t INPUT_CHANNELS = AXIS_COUNT; constexpr size_t OUTPUT_CHANNELS = ESC_CHANNEL_COUNT; @@ -350,13 +373,13 @@ enum PidIndex { FC_PID_ROLL, FC_PID_PITCH, FC_PID_YAW, + FC_PID_LEVEL, + FC_PID_MAG, FC_PID_ALT, + FC_PID_VEL, FC_PID_POS, FC_PID_POSR, FC_PID_NAVR, - FC_PID_LEVEL, - FC_PID_MAG, - FC_PID_VEL, FC_PID_ITEM_COUNT, }; @@ -414,14 +437,15 @@ struct InputConfig int16_t midRc = 1500; int16_t maxRc = 2115; - int8_t interpolationMode = INPUT_INTERPOLATION_AUTO; - int8_t interpolationInterval = 26; int8_t deadband = 3; + int8_t airModeActivateThreshold = 40; - int8_t filterType = INPUT_FILTER; - int8_t filterAutoFactor = 50; - FilterConfig filter{FILTER_PT3, 0}; - FilterConfig filterDerivative{FILTER_PT3, 0}; + bool filterEnable = true; + int8_t filterAutoFactor = 50; // RPY factor + int8_t filterAutoThrottleFactor = 50; // Throttle factor + FilterConfig filter{FILTER_PT3, 0}; // autoFactor if freq=0 + FilterConfig filterThrottle{FILTER_PT3, 0}; // autoFactor if freq=0 + FilterConfig filterDerivative{FILTER_PT3, 0}; // autoFactor if freq=0 uint8_t expo[3] = { 0, 0, 0 }; uint8_t rate[3] = { 20, 20, 30 }; @@ -453,9 +477,8 @@ struct OutputConfig int16_t servoRate = 0; int16_t minCommand = 1000; - int16_t minThrottle = 1070; int16_t maxThrottle = 2000; - int16_t dshotIdle = 550; + int16_t motorIdle = 550; int8_t throttleLimitType = 0; int8_t throttleLimitPercent = 100; @@ -503,10 +526,14 @@ enum ArmingDisabledFlags { ARMING_DISABLED_DSHOT_BITBANG = (1 << 22), ARMING_DISABLED_ACC_CALIBRATION = (1 << 23), ARMING_DISABLED_MOTOR_PROTOCOL = (1 << 24), - ARMING_DISABLED_ARM_SWITCH = (1 << 25), // Needs to be the last element, since it's always activated if one of the others is active when arming + ARMING_DISABLED_CRASHFLIP = (1 << 25), + ARMING_DISABLED_ALTHOLD = (1 << 26), + ARMING_DISABLED_POSHOLD = (1 << 27), + ARMING_DISABLED_AUTOPILOT = (1 << 28), + ARMING_DISABLED_ARM_SWITCH = (1 << 29), // Needs to be the last element, since it's always activated if one of the others is active when arming }; -static constexpr size_t ARMING_DISABLED_FLAGS_COUNT = 25; +static constexpr size_t ARMING_DISABLED_FLAGS_COUNT = 30; struct WirelessConfig { @@ -525,7 +552,7 @@ struct FailsafeConfig struct BlackboxConfig { int8_t dev = 0; - int16_t pDenom = 32; // 1k + int16_t pDenom = 1; // loop / 2 int32_t fieldsMask = 0xffff; int8_t mode = 0; }; @@ -569,12 +596,12 @@ struct GyroConfig int8_t dlpf = GYRO_DLPF_256; int8_t align = ALIGN_DEFAULT; int16_t bias[3] = { 0, 0, 0 }; - FilterConfig filter{FILTER_PT1, 100}; - FilterConfig filter2{FILTER_PT1, 213}; + FilterConfig filter{FILTER_PT1, 150}; + FilterConfig filter2{FILTER_PT1, 400}; + FilterConfig dynLpfFilter{FILTER_PT1, 400, 150}; FilterConfig filter3{FILTER_FO, 150}; FilterConfig notch1Filter{FILTER_NOTCH, 0, 0}; FilterConfig notch2Filter{FILTER_NOTCH, 0, 0}; - FilterConfig dynLpfFilter{FILTER_PT1, 425, 170}; DynamicFilterConfig dynamicFilter; RpmFilterConfig rpmFilter; }; @@ -605,6 +632,7 @@ struct MagConfig int16_t offset[3] = { 0, 0, 0 }; int16_t scale[3] = { 1000, 1000, 1000 }; FilterConfig filter{FILTER_BIQUAD, 10}; + int16_t declination = 0; }; struct YawConfig @@ -614,11 +642,10 @@ struct YawConfig struct DtermConfig { - FilterConfig filter{FILTER_PT1, 128}; - FilterConfig filter2{FILTER_PT1, 128}; + FilterConfig filter{FILTER_PT1, 75}; + FilterConfig filter2{FILTER_PT1, 150}; + FilterConfig dynLpfFilter{FILTER_PT1, 150, 75}; FilterConfig notchFilter{FILTER_NOTCH, 0, 0}; - FilterConfig dynLpfFilter{FILTER_PT1, 145, 60}; - int16_t setpointWeight = 30; }; struct ItermConfig @@ -651,6 +678,7 @@ struct MixerConfiguration struct ControllerConfig { + int8_t tpaMode = 0; int8_t tpaScale = 10; int16_t tpaBreakpoint = 1650; }; @@ -690,6 +718,42 @@ struct ArmingConfig uint8_t smallAngle = 25; }; +enum SimplifiedTuningMode: uint8_t +{ + SIMPLIFIED_TUNING_OFF = 0, + SIMPLIFIED_TUNING_RP = FC_PID_PITCH, // roll + pitch + SIMPLIFIED_TUNING_RPY = FC_PID_YAW, // roll + pitch + yaw +}; + +// Betaflight simplified-tuning slider baselines and limits +static constexpr int SIMPLIFIED_PID_GAIN_MAX = 250; +static constexpr int SIMPLIFIED_F_GAIN_MAX = 2000; +static constexpr int SIMPLIFIED_DYN_LPF_MAX_HZ = 500; +static constexpr int SIMPLIFIED_LPF_MAX_HZ = 500; +static constexpr int SIMPLIFIED_GYRO_LPF1_DYN_MIN_HZ = 150; +static constexpr int SIMPLIFIED_GYRO_LPF1_DYN_MAX_HZ = 400; +static constexpr int SIMPLIFIED_GYRO_LPF2_HZ = 400; +static constexpr int SIMPLIFIED_DTERM_LPF1_DYN_MIN_HZ = 75; +static constexpr int SIMPLIFIED_DTERM_LPF1_DYN_MAX_HZ = 150; +static constexpr int SIMPLIFIED_DTERM_LPF2_HZ = 150; + +struct SimplifiedTuningConfig +{ + int8_t pidsMode = SIMPLIFIED_TUNING_RPY; + uint8_t masterMultiplier = 100; + uint8_t rollPitchRatio = 100; + uint8_t iGain = 100; + uint8_t dGain = 80; + uint8_t piGain = 100; + uint8_t dMaxGain = 0; // unused, ESP-FC has no D-Max + uint8_t ffGain = 80; + uint8_t pitchPiGain = 100; + uint8_t dtermFilter = 1; + uint8_t dtermFilterMultiplier = 100; + uint8_t gyroFilter = 1; + uint8_t gyroFilterMultiplier = 100; +}; + // persistent data class ModelConfig { @@ -713,16 +777,16 @@ class ModelConfig // pid controller PidConfig pid[FC_PID_ITEM_COUNT] = { - [FC_PID_ROLL] = { .P = 42, .I = 85, .D = 24, .F = 72 }, // ROLL - [FC_PID_PITCH] = { .P = 46, .I = 90, .D = 26, .F = 76 }, // PITCH - [FC_PID_YAW] = { .P = 45, .I = 90, .D = 0, .F = 72 }, // YAW + [FC_PID_ROLL] = { .P = 45, .I = 80, .D = 24, .F = 88 }, // ROLL + [FC_PID_PITCH] = { .P = 47, .I = 84, .D = 27, .F = 92 }, // PITCH + [FC_PID_YAW] = { .P = 45, .I = 80, .D = 0, .F = 88 }, // YAW + [FC_PID_LEVEL] = { .P = 45, .I = 0, .D = 0, .F = 0 }, // ANGLE/LEVEL + [FC_PID_MAG] = { .P = 0, .I = 0, .D = 0, .F = 0 }, // MAG [FC_PID_ALT] = { .P = 0, .I = 0, .D = 0, .F = 0 }, // ALTHOLD POS + [FC_PID_VEL] = { .P = 80, .I = 60, .D = 40, .F = 20 }, // ALTHOLD VEL [FC_PID_POS] = { .P = 0, .I = 0, .D = 0, .F = 0 }, // POSHOLD_P * 100, POSHOLD_I * 100, [FC_PID_POSR] = { .P = 0, .I = 0, .D = 0, .F = 0 }, // POSHOLD_RATE_P * 10, POSHOLD_RATE_I * 100, POSHOLD_RATE_D * 1000, [FC_PID_NAVR] = { .P = 0, .I = 0, .D = 0, .F = 0 }, // NAV_P * 10, NAV_I * 100, NAV_D * 1000 - [FC_PID_LEVEL] = { .P = 45, .I = 0, .D = 0, .F = 0 }, // ANGLE/LEVEL - [FC_PID_MAG] = { .P = 0, .I = 0, .D = 0, .F = 0 }, // MAG - [FC_PID_VEL] = { .P = 80, .I = 60, .D = 40, .F = 20 }, // ALTHOLD VEL }; YawConfig yaw; LevelConfig level; @@ -730,6 +794,7 @@ class ModelConfig ItermConfig iterm; AltHoldConfig altHold; ControllerConfig controller; + SimplifiedTuningConfig simplifiedTuning; // hardware int8_t pin[PIN_COUNT] = { #ifdef ESPFC_INPUT @@ -859,7 +924,7 @@ class ModelConfig { #ifdef ESPFC_DEV_PRESET_BLACKBOX_SERIAL blackbox.dev = BLACKBOX_DEV_SERIAL; // serial - debug.mode = DEBUG_GYRO_SCALED; + debug.mode = DEBUG_GYRO_SAMPLE; serial[ESPFC_DEV_PRESET_BLACKBOX_SERIAL].functionMask |= SERIAL_FUNCTION_BLACKBOX; serial[ESPFC_DEV_PRESET_BLACKBOX_SERIAL].blackboxBaud = SERIAL_SPEED_250000; serial[ESPFC_DEV_PRESET_BLACKBOX_SERIAL].baud = SERIAL_SPEED_250000; @@ -867,7 +932,8 @@ class ModelConfig #ifdef ESPFC_DEV_PRESET_BLACKBOX_FLASH blackbox.dev = BLACKBOX_DEV_FLASH; // flash - blackbox.pDenom = 16; // 500Hz + debug.mode = DEBUG_GYRO_SAMPLE; + blackbox.pDenom = 1; // 500Hz #endif #ifdef ESPFC_DEV_PRESET_MODES @@ -935,7 +1001,6 @@ class ModelConfig iterm.lowThrottleZeroIterm = false; // ROBOT iterm.limit = 10; // ROBOT - dterm.setpointWeight = 0; // ROBOT level.angleLimit = 10; // deg // ROBOT output.protocol = ESC_PROTOCOL_PWM; // ROBOT diff --git a/lib/Espfc/src/ModelState.h b/lib/Espfc/src/ModelState.h index d8dc392e..df6939dc 100644 --- a/lib/Espfc/src/ModelState.h +++ b/lib/Espfc/src/ModelState.h @@ -24,15 +24,12 @@ constexpr size_t DEBUG_VALUE_COUNT = 8; constexpr size_t CLI_BUFF_SIZE = 128; constexpr size_t CLI_ARGS_SIZE = 12; -class CliCmd +struct CliCmd { - public: - CliCmd(): buff{0}, index{0} { - std::fill_n(args, CLI_ARGS_SIZE, nullptr); - } - const char * args[CLI_ARGS_SIZE]; - char buff[CLI_BUFF_SIZE]; - size_t index; + CliCmd(): args{}, buff{}, index{0} {} + const char * args[CLI_ARGS_SIZE]; + char buff[CLI_BUFF_SIZE]; + size_t index; }; class SerialPortState @@ -182,10 +179,10 @@ struct InputState uint32_t frameCount; uint32_t lossTime; - float interpolationDelta; - float interpolationStep; float autoFactor; float autoFreq; + float autoThrottleFactor; + float autoThrottleFreq; int16_t raw[INPUT_CHANNELS]; int16_t buffer[INPUT_CHANNELS]; @@ -524,6 +521,7 @@ struct ModelState Utils::Timer serialTimer; Target::Queue appQueue; + bool rebootRequired = false; }; } diff --git a/lib/Espfc/src/Output/Mixer.cpp b/lib/Espfc/src/Output/Mixer.cpp index 663b90db..d420467e 100644 --- a/lib/Espfc/src/Output/Mixer.cpp +++ b/lib/Espfc/src/Output/Mixer.cpp @@ -77,12 +77,15 @@ int Mixer::begin() } motorInitEscDevice(_motor); - _model.state.mixer.minThrottle = _model.config.output.minThrottle; + _model.state.mixer.minThrottle = _model.config.output.minCommand + _model.config.output.motorIdle * 0.1f; _model.state.mixer.maxThrottle = _model.config.output.maxThrottle; _model.state.mixer.digitalOutput = _model.config.output.protocol >= ESC_PROTOCOL_DSHOT150; if (_model.state.mixer.digitalOutput) { - _model.state.mixer.minThrottle = (_model.config.output.dshotIdle * 0.1f) + 1001.f; + // pwm to dshot mapping: (pwm - 1000) * 2 + 47 + // so we want to skip command range + // 1000->0(disarmed), 1001->49, 2000->2047 + _model.state.mixer.minThrottle = (_model.config.output.motorIdle * 0.1f) + 1001.f; _model.state.mixer.maxThrottle = 2000.f; } _model.state.currentMixer = Mixers::getMixer((MixerType)_model.config.mixer.type, _model.state.customMixer); diff --git a/lib/Espfc/src/Output/OutputIBUS.hpp b/lib/Espfc/src/Output/OutputIBUS.hpp index 9da49d9b..584644b6 100644 --- a/lib/Espfc/src/Output/OutputIBUS.hpp +++ b/lib/Espfc/src/Output/OutputIBUS.hpp @@ -20,7 +20,7 @@ class OutputIBUS int update() { - if(!_timer.check()) return 0; + if (!_timer.check()) return 0; // const uint8_t data[] = { // 0x20, 0x40, // preambule (len, cmd) @@ -31,13 +31,14 @@ class OutputIBUS // 0x83, 0xF3 // checksum // }; + // clang-format off const uint8_t data[] = { 0x20, 0x40, 0xDB, 0x05, 0xDC, 0x05, 0x54, 0x05, 0xDC, 0x05, 0xE8, 0x03, 0xD0, 0x07, 0xD2, 0x05, 0xE8, 0x03, 0xDC, 0x05, 0xDC, 0x05, 0xDC, 0x05, 0xDC, 0x05, 0xDC, 0x05, 0xDC, 0x05, 0xDA, 0xF3, }; - + // clang-format on _serial->write(data, sizeof(data)); return 1; @@ -48,4 +49,4 @@ class OutputIBUS Utils::Timer _timer; }; -} +} // namespace Espfc::Output diff --git a/lib/Espfc/src/Sensor/AccelSensor.cpp b/lib/Espfc/src/Sensor/AccelSensor.cpp index e2f8b7d0..0883ebd5 100644 --- a/lib/Espfc/src/Sensor/AccelSensor.cpp +++ b/lib/Espfc/src/Sensor/AccelSensor.cpp @@ -17,18 +17,14 @@ int AccelSensor::begin() return 0; } - _model.state.accel.scale = 16.f * ACCEL_G / 32768.f; const auto& timer = _model.state.accel.timer; - - for (size_t i = 0; i < AXIS_COUNT_RPY; i++) - { - _filter[i].begin(FilterConfig(FILTER_FIR2, 1), timer.rate); - _model.state.accel.filter[i].begin(_model.config.accel.filter, timer.rate); - } - + _model.state.accel.scale = 16.f * ACCEL_G / 32768.f; _model.state.accel.biasAlpha = 5.0f / timer.rate; _model.state.accel.calibrationState = CALIBRATION_IDLE; + reload(MODEL_CHANGE_FILTER); + reload(MODEL_CHANGE_ACCEL); + _model.logger.info() .log("ACCEL INIT") .log(Device::GyroDevice::getName(_gyro->getType())) @@ -40,6 +36,30 @@ int AccelSensor::begin() return 1; } +int AccelSensor::reload(ModelChangeEvent event) +{ + switch (event) + { + case MODEL_CHANGE_FILTER: + 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); + } + break; + case MODEL_CHANGE_ACCEL: + _model.state.boardAlignment.init(VectorFloat(Utils::toRad(_model.config.boardAlignment[0]), + Utils::toRad(_model.config.boardAlignment[1]), + Utils::toRad(_model.config.boardAlignment[2]))); + _model.state.trimRotation.init(VectorFloat{Utils::toRad(_model.config.accel.trim[1]) * 0.1f, + Utils::toRad(_model.config.accel.trim[0]) * 0.1f, 0.0f}); + break; + default: + break; + } + return 1; +} + int FAST_CODE_ATTR AccelSensor::update() { int status = read(); diff --git a/lib/Espfc/src/Sensor/AccelSensor.hpp b/lib/Espfc/src/Sensor/AccelSensor.hpp index b5d8cf71..d3a925b1 100644 --- a/lib/Espfc/src/Sensor/AccelSensor.hpp +++ b/lib/Espfc/src/Sensor/AccelSensor.hpp @@ -12,6 +12,7 @@ class AccelSensor : public BaseSensor AccelSensor(Model& model); int begin(); + int reload(ModelChangeEvent event); int update(); int read(); int filter(); diff --git a/lib/Espfc/src/Sensor/BaroSensor.cpp b/lib/Espfc/src/Sensor/BaroSensor.cpp index 45ffbfd1..5c78c23e 100644 --- a/lib/Espfc/src/Sensor/BaroSensor.cpp +++ b/lib/Espfc/src/Sensor/BaroSensor.cpp @@ -21,23 +21,34 @@ int BaroSensor::begin() _biasAlpha = 1.0f - expf(-dt / tau); _model.state.baro.altitudeBiasSamples = 3 * rate; - const auto internalFilter = FILTER_PT1; - const auto internalCutoff = std::max((rate + 2) / 4, 1); - _temperatureFilter.begin(FilterConfig(internalFilter, internalCutoff), rate); - _pressureFilter.begin(FilterConfig(internalFilter, internalCutoff), rate); - _varioFilter.begin(FilterConfig(internalFilter, internalCutoff), rate); - - _model.logger.info() - .log("BARO INIT") - .log(Device::BaroDevice::getName(_baro->getType())) - .log(rate) - .logln(internalCutoff); + reload(MODEL_CHANGE_FILTER); + + _model.logger.info().log("BARO INIT").log(Device::BaroDevice::getName(_baro->getType())).logln(rate); _baro->setMode(BARO_MODE_TEMP); return 1; } +int BaroSensor::reload(ModelChangeEvent event) +{ + switch (event) + { + case MODEL_CHANGE_FILTER: { + const int rate = _model.state.baro.rate; + const auto internalFilter = FILTER_PT1; + const auto internalCutoff = std::max((rate + 2) / 4, 1); + _temperatureFilter.begin(FilterConfig(internalFilter, internalCutoff), rate); + _pressureFilter.begin(FilterConfig(internalFilter, internalCutoff), rate); + _varioFilter.begin(FilterConfig(internalFilter, internalCutoff), rate); + break; + } + default: + break; + } + return 1; +} + int BaroSensor::update() { int status = read(); diff --git a/lib/Espfc/src/Sensor/BaroSensor.hpp b/lib/Espfc/src/Sensor/BaroSensor.hpp index abea85fd..be6be839 100644 --- a/lib/Espfc/src/Sensor/BaroSensor.hpp +++ b/lib/Espfc/src/Sensor/BaroSensor.hpp @@ -22,6 +22,7 @@ class BaroSensor : public BaseSensor int begin(); int update(); int read(); + int reload(ModelChangeEvent event); private: void readTemperature(); diff --git a/lib/Espfc/src/Sensor/GpsSensor.cpp b/lib/Espfc/src/Sensor/GpsSensor.cpp index 884a8346..6f6b5097 100644 --- a/lib/Espfc/src/Sensor/GpsSensor.cpp +++ b/lib/Espfc/src/Sensor/GpsSensor.cpp @@ -36,6 +36,11 @@ int GpsSensor::begin(Device::SerialDevice* port, int baud) return 1; } +int GpsSensor::reload(ModelChangeEvent event) +{ + return 1; +} + int GpsSensor::update() { if (!_port) return 0; diff --git a/lib/Espfc/src/Sensor/GpsSensor.hpp b/lib/Espfc/src/Sensor/GpsSensor.hpp index e87644b2..3eddca9f 100644 --- a/lib/Espfc/src/Sensor/GpsSensor.hpp +++ b/lib/Espfc/src/Sensor/GpsSensor.hpp @@ -14,7 +14,7 @@ class GpsSensor GpsSensor(Model& model); int begin(Device::SerialDevice* port, int baud); - + int reload(ModelChangeEvent event); int update(); private: diff --git a/lib/Espfc/src/Sensor/GyroSensor.cpp b/lib/Espfc/src/Sensor/GyroSensor.cpp index 773ebd3b..eb97e2b3 100644 --- a/lib/Espfc/src/Sensor/GyroSensor.cpp +++ b/lib/Espfc/src/Sensor/GyroSensor.cpp @@ -22,40 +22,13 @@ int GyroSensor::begin() _gyro->setDLPFMode(_model.config.gyro.dlpf); _gyro->setRate(_gyro->getRate()); - _model.state.gyro.scale = Utils::toRad(2000.f) / 32768.f; + + reload(MODEL_CHANGE_FILTER); _model.state.gyro.calibrationState = CALIBRATION_START; // calibrate gyro on start _model.state.gyro.calibrationRate = _model.state.loopTimer.rate; _model.state.gyro.biasAlpha = 5.0f / _model.state.gyro.calibrationRate; - _sma.begin(_model.config.loopSync); - _dyn_notch_denom = std::max((uint32_t)1, _model.state.loopTimer.rate / 1000); - _dyn_notch_sma.begin(_dyn_notch_denom); - _dyn_notch_count = std::min((size_t)_model.config.gyro.dynamicFilter.count, DYN_NOTCH_COUNT_MAX); - _dyn_notch_enabled = _model.isFeatureActive(FEATURE_DYNAMIC_FILTER) && _dyn_notch_count > 0 && - _model.state.loopTimer.rate >= DynamicFilterConfig::MIN_FREQ; - _dyn_notch_debug = _model.config.debug.mode == DEBUG_FFT_FREQ || _model.config.debug.mode == DEBUG_FFT_TIME; - - _rpm_enabled = _model.config.gyro.rpmFilter.harmonics > 0 && _model.config.output.dshotTelemetry; - _rpm_motor_index = 0; - _rpm_fade_inv = 1.0f / _model.config.gyro.rpmFilter.fade; - _rpm_min_freq = _model.config.gyro.rpmFilter.minFreq; - _rpm_max_freq = 0.48f * _model.state.loopTimer.rate; - _rpm_q = _model.config.gyro.rpmFilter.q * 0.01f; - - for (size_t i = 0; i < RPM_FILTER_HARMONICS_MAX; i++) - { - _rpm_weights[i] = std::clamp(0.01f * _model.config.gyro.rpmFilter.weights[i], 0.0f, 1.0f); - } - for (size_t i = 0; i < AXIS_COUNT_RPY; i++) - { -#ifdef ESPFC_DSP - _fft[i].begin(_model.state.loopTimer.rate / _dyn_notch_denom, _model.config.gyro.dynamicFilter, i); -#else - _freqAnalyzer[i].begin(_model.state.loopTimer.rate / _dyn_notch_denom, _model.config.gyro.dynamicFilter); -#endif - } - _model.logger.info() .log("GYRO INIT") .log(Device::GyroDevice::getName(_gyro->getType())) @@ -68,6 +41,89 @@ int GyroSensor::begin() return 1; } +int GyroSensor::reload(ModelChangeEvent event) +{ + const uint32_t gyroFilterRate = _model.state.gyro.timer.rate; + const uint32_t loopFilterRate = _model.state.loopTimer.rate; + auto& gyroState = _model.state.gyro; + + switch (event) + { + case MODEL_CHANGE_FILTER: + _model.state.gyro.scale = Utils::toRad(2000.f) / 32768.f; + + _sma.begin(_model.config.loopSync); + _dyn_notch_denom = std::max((uint32_t)1, _model.state.loopTimer.rate / 1000); + _dyn_notch_sma.begin(_dyn_notch_denom); + _dyn_notch_count = std::min((size_t)_model.config.gyro.dynamicFilter.count, DYN_NOTCH_COUNT_MAX); + _dyn_notch_enabled = _model.isFeatureActive(FEATURE_DYNAMIC_FILTER) && _dyn_notch_count > 0 && + _model.state.loopTimer.rate >= DynamicFilterConfig::MIN_FREQ; + _dyn_notch_debug = _model.config.debug.mode == DEBUG_FFT_FREQ || _model.config.debug.mode == DEBUG_FFT_TIME; + + _rpm_enabled = _model.config.gyro.rpmFilter.harmonics > 0 && _model.config.output.dshotTelemetry; + _rpm_motor_index = 0; + _rpm_fade_inv = 1.0f / _model.config.gyro.rpmFilter.fade; + _rpm_min_freq = _model.config.gyro.rpmFilter.minFreq; + _rpm_max_freq = 0.48f * _model.state.loopTimer.rate; + _rpm_q = _model.config.gyro.rpmFilter.q * 0.01f; + + for (size_t i = 0; i < RPM_FILTER_HARMONICS_MAX; i++) + { + _rpm_weights[i] = std::clamp(0.01f * _model.config.gyro.rpmFilter.weights[i], 0.0f, 1.0f); + } + + for (size_t i = 0; i < AXIS_COUNT_RPY; i++) + { + // lpf filters + if (_model.config.gyro.dynLpfFilter.cutoff > 0) + { + gyroState.filter[i].begin( + {(FilterType)_model.config.gyro.filter.type, _model.config.gyro.dynLpfFilter.cutoff}, loopFilterRate); + } + else + { + gyroState.filter[i].begin(_model.config.gyro.filter, loopFilterRate); + } + gyroState.filter2[i].begin(_model.config.gyro.filter2, loopFilterRate); + gyroState.filter3[i].begin(_model.config.gyro.filter3, gyroFilterRate); + // rpm filters + for (size_t m = 0; m < RPM_FILTER_MOTOR_MAX; m++) + { + gyroState.rpmFreqFilter[m].begin({FILTER_PT1, _model.config.gyro.rpmFilter.freqLpf}, loopFilterRate); + for (size_t n = 0; n < _model.config.gyro.rpmFilter.harmonics; n++) + { + int center = Utils::mapi(m * RPM_FILTER_HARMONICS_MAX + n, 0, + RPM_FILTER_MOTOR_MAX * _model.config.gyro.rpmFilter.harmonics, + _model.config.gyro.rpmFilter.minFreq, loopFilterRate / 2); + gyroState.rpmFilter[m][n][i].begin(FilterConfig(FILTER_NOTCH_DF1, center, center * 0.98f), loopFilterRate); + } + } + // dynamic notch filters + if (_model.isFeatureActive(FEATURE_DYNAMIC_FILTER)) + { + for (size_t p = 0; p < _dyn_notch_count; p++) + { + gyroState.dynNotchFilter[p][i].begin(FilterConfig(FILTER_NOTCH_DF1, 400, 380), gyroFilterRate); + } + } + // static notches + gyroState.notch1Filter[i].begin(_model.config.gyro.notch1Filter, gyroFilterRate); + gyroState.notch2Filter[i].begin(_model.config.gyro.notch2Filter, gyroFilterRate); + +#ifdef ESPFC_DSP + _fft[i].begin(_model.state.loopTimer.rate / _dyn_notch_denom, _model.config.gyro.dynamicFilter, i); +#else + _freqAnalyzer[i].begin(_model.state.loopTimer.rate / _dyn_notch_denom, _model.config.gyro.dynamicFilter); +#endif + } + break; + default: + break; + } + + return 1; +} + int FAST_CODE_ATTR GyroSensor::read() { if (!_model.gyroActive()) return 0; @@ -107,7 +163,6 @@ int FAST_CODE_ATTR GyroSensor::filter() for (size_t i = 0; i < AXIS_COUNT_RPY; ++i) { _model.setDebug(DEBUG_GYRO_RAW, i, _model.state.gyro.raw[i]); - _model.setDebug(DEBUG_GYRO_SCALED, i, lrintf(Utils::toDeg(_model.state.gyro.scaled[i]))); } _model.setDebug(DEBUG_GYRO_SAMPLE, 0, lrintf(Utils::toDeg(_model.state.gyro.adc[_model.config.debug.axis]))); @@ -274,7 +329,7 @@ void FAST_CODE_ATTR GyroSensor::dynNotchFilterUpdate() size_t x = (p + i) % 3; int harmonic = (p / 3) + 1; int16_t f = std::clamp((int16_t)lrintf(freq * harmonic), _model.config.gyro.dynamicFilter.min_freq, - _model.config.gyro.dynamicFilter.max_freq); + _model.config.gyro.dynamicFilter.max_freq); _model.state.gyro.dynNotchFilter[p][x].reconfigure(f, f, q); } } diff --git a/lib/Espfc/src/Sensor/GyroSensor.hpp b/lib/Espfc/src/Sensor/GyroSensor.hpp index 4c232612..20036384 100644 --- a/lib/Espfc/src/Sensor/GyroSensor.hpp +++ b/lib/Espfc/src/Sensor/GyroSensor.hpp @@ -21,6 +21,8 @@ class GyroSensor : public BaseSensor ~GyroSensor(); int begin(); + int reload(ModelChangeEvent event); + int read(); int filter(); void postLoop(); diff --git a/lib/Espfc/src/Sensor/MagSensor.cpp b/lib/Espfc/src/Sensor/MagSensor.cpp index ac5f358d..3fafcc55 100644 --- a/lib/Espfc/src/Sensor/MagSensor.cpp +++ b/lib/Espfc/src/Sensor/MagSensor.cpp @@ -22,6 +22,8 @@ int MagSensor::begin() _model.state.mag.calibrationState = CALIBRATION_IDLE; _model.state.mag.calibrationValid = true; + reload(MODEL_CHANGE_FILTER); + _model.logger.info() .log("MAG INIT") .log(Device::MagDevice::getName(_mag->getType())) @@ -31,6 +33,25 @@ int MagSensor::begin() return 1; } +int MagSensor::reload(ModelChangeEvent event) +{ + switch (event) + { + case MODEL_CHANGE_FILTER: + if (_model.magActive()) + { + for (size_t i = 0; i < AXIS_COUNT_RPY; i++) + { + _model.state.mag.filter[i].begin(_model.config.mag.filter, _model.state.mag.timer.rate); + } + } + break; + default: + break; + } + return 1; +} + int MagSensor::update() { int status = read(); diff --git a/lib/Espfc/src/Sensor/MagSensor.hpp b/lib/Espfc/src/Sensor/MagSensor.hpp index 67b69356..91c4ff55 100644 --- a/lib/Espfc/src/Sensor/MagSensor.hpp +++ b/lib/Espfc/src/Sensor/MagSensor.hpp @@ -12,6 +12,7 @@ class MagSensor : public BaseSensor MagSensor(Model& model); int begin(); + int reload(ModelChangeEvent event); int update(); int read(); int filter(); diff --git a/lib/Espfc/src/Sensor/VoltageSensor.cpp b/lib/Espfc/src/Sensor/VoltageSensor.cpp index 8708254c..d270f8dd 100644 --- a/lib/Espfc/src/Sensor/VoltageSensor.cpp +++ b/lib/Espfc/src/Sensor/VoltageSensor.cpp @@ -11,17 +11,29 @@ int VoltageSensor::begin() _model.state.battery.timer.setRate(100); _model.state.battery.samples = 50; - _vFilterFast.begin(FilterConfig(FILTER_PT1, 20), _model.state.battery.timer.rate); - _vFilter.begin(FilterConfig(FILTER_PT2, 2), _model.state.battery.timer.rate); - - _iFilterFast.begin(FilterConfig(FILTER_PT1, 20), _model.state.battery.timer.rate); - _iFilter.begin(FilterConfig(FILTER_PT2, 2), _model.state.battery.timer.rate); + reload(MODEL_CHANGE_FILTER); _state = VBAT; return 1; } +int VoltageSensor::reload(ModelChangeEvent event) +{ + switch (event) + { + case MODEL_CHANGE_FILTER: + _vFilterFast.begin(FilterConfig(FILTER_PT1, 20), _model.state.battery.timer.rate); + _vFilter.begin(FilterConfig(FILTER_PT2, 2), _model.state.battery.timer.rate); + _iFilterFast.begin(FilterConfig(FILTER_PT1, 20), _model.state.battery.timer.rate); + _iFilter.begin(FilterConfig(FILTER_PT2, 2), _model.state.battery.timer.rate); + break; + default: + break; + } + return 1; +} + int VoltageSensor::update() { if (!_model.state.battery.timer.check()) return 0; diff --git a/lib/Espfc/src/Sensor/VoltageSensor.hpp b/lib/Espfc/src/Sensor/VoltageSensor.hpp index f67d1d22..bad2de6c 100644 --- a/lib/Espfc/src/Sensor/VoltageSensor.hpp +++ b/lib/Espfc/src/Sensor/VoltageSensor.hpp @@ -16,6 +16,7 @@ class VoltageSensor : public BaseSensor }; VoltageSensor(Model& model); int begin(); + int reload(ModelChangeEvent event); int update(); int readVbat(); int readIbat(); diff --git a/lib/Espfc/src/SensorManager.cpp b/lib/Espfc/src/SensorManager.cpp index 6d06ace8..2073db4d 100644 --- a/lib/Espfc/src/SensorManager.cpp +++ b/lib/Espfc/src/SensorManager.cpp @@ -2,9 +2,9 @@ namespace Espfc { -SensorManager::SensorManager(Model& model): - _model(model), _gyro(model), _accel(model), _mag(model), _baro(model), - _voltage(model), _fusion(model), _altitude(model), _fusionUpdate(false) +SensorManager::SensorManager(Model& model) + : _model(model), _gyro(model), _accel(model), _mag(model), _baro(model), _voltage(model), _fusion(model), + _altitude(model), _fusionUpdate(false) { } @@ -22,16 +22,28 @@ int SensorManager::begin() return 1; } +int SensorManager::reload(ModelChangeEvent event) +{ + _gyro.reload(event); + _accel.reload(event); + _fusion.reload(event); + _baro.reload(event); + _mag.reload(event); + _altitude.reload(event); + _voltage.reload(event); + return 1; +} + int FAST_CODE_ATTR SensorManager::read() { _gyro.read(); - if(_model.state.loopTimer.syncTo(_model.state.gyro.timer)) + if (_model.state.loopTimer.syncTo(_model.state.gyro.timer)) { _model.state.appQueue.send(Event(EVENT_GYRO_READ)); } - if(_model.state.accel.timer.syncTo(_model.state.gyro.timer)) + if (_model.state.accel.timer.syncTo(_model.state.gyro.timer)) { _accel.update(); _model.state.appQueue.send(Event(EVENT_ACCEL_READ)); @@ -39,11 +51,11 @@ int FAST_CODE_ATTR SensorManager::read() return 1; } - if(_mag.update()) return 1; + if (_mag.update()) return 1; - if(_baro.update()) return 1; + if (_baro.update()) return 1; - if(_voltage.update()) return 1; + if (_voltage.update()) return 1; return 0; } @@ -51,7 +63,7 @@ int FAST_CODE_ATTR SensorManager::read() int FAST_CODE_ATTR SensorManager::preLoop() { _gyro.filter(); - if(_model.state.gyro.biasSamples == 0) + if (_model.state.gyro.biasSamples == 0) { _model.state.gyro.biasSamples = -1; _fusion.restoreGain(); @@ -86,7 +98,7 @@ int SensorManager::updateDelayed() // update at most one sensor besides gyro int status = 0; - if(_model.state.accel.timer.syncTo(_model.state.gyro.timer)) + if (_model.state.accel.timer.syncTo(_model.state.gyro.timer)) { _accel.update(); _model.state.mode.button = _button.update(); @@ -94,22 +106,22 @@ int SensorManager::updateDelayed() } // delay imu update to next cycle - if(_fusionUpdate) + if (_fusionUpdate) { _fusionUpdate = false; fusion(); } _fusionUpdate = status; - if(status) return 1; + if (status) return 1; - if(_mag.update()) return 1; + if (_mag.update()) return 1; - if(_baro.update()) return 1; + if (_baro.update()) return 1; - if(_voltage.update()) return 0; + if (_voltage.update()) return 0; return 0; } -} +} // namespace Espfc diff --git a/lib/Espfc/src/SensorManager.h b/lib/Espfc/src/SensorManager.h index 0f62f0ff..f01cec2b 100644 --- a/lib/Espfc/src/SensorManager.h +++ b/lib/Espfc/src/SensorManager.h @@ -1,43 +1,45 @@ #pragma once -#include "Model.h" -#include "Control/Fusion.h" #include "Control/Altitude.hpp" -#include "Sensor/GyroSensor.hpp" +#include "Control/Fusion.h" +#include "Device/Input/InputButton.hpp" +#include "Model.h" #include "Sensor/AccelSensor.hpp" -#include "Sensor/MagSensor.hpp" #include "Sensor/BaroSensor.hpp" +#include "Sensor/GyroSensor.hpp" +#include "Sensor/MagSensor.hpp" #include "Sensor/VoltageSensor.hpp" -#include "Device/Input/InputButton.hpp" namespace Espfc { class SensorManager { - public: - SensorManager(Model& model); +public: + SensorManager(Model& model); + + int begin(); + int read(); + int preLoop(); + int postLoop(); + int fusion(); + // main task + int update(); + // sub task + int updateDelayed(); - int begin(); - int read(); - int preLoop(); - int postLoop(); - int fusion(); - // main task - int update(); - // sub task - int updateDelayed(); + int reload(ModelChangeEvent event); - private: - Model& _model; - Sensor::GyroSensor _gyro; - Sensor::AccelSensor _accel; - Sensor::MagSensor _mag; - Sensor::BaroSensor _baro; - Sensor::VoltageSensor _voltage; - Control::Fusion _fusion; - Control::Altitude _altitude; - bool _fusionUpdate; - Device::Input::InputButton _button; +private: + Model& _model; + Sensor::GyroSensor _gyro; + Sensor::AccelSensor _accel; + Sensor::MagSensor _mag; + Sensor::BaroSensor _baro; + Sensor::VoltageSensor _voltage; + Control::Fusion _fusion; + Control::Altitude _altitude; + bool _fusionUpdate; + Device::Input::InputButton _button; }; -} +} // namespace Espfc diff --git a/lib/Espfc/src/SerialManager.cpp b/lib/Espfc/src/SerialManager.cpp index 37fb9348..ea810250 100644 --- a/lib/Espfc/src/SerialManager.cpp +++ b/lib/Espfc/src/SerialManager.cpp @@ -1,6 +1,11 @@ #include "SerialManager.h" #include "Device/SerialDeviceAdapter.h" #include "Debug_Espfc.h" +#if defined(ESPFC_SERIAL_USB_REENUMERATE) +#include "Hal/Gpio.h" +#include +#include +#endif // TODO: move to target #ifdef ESPFC_SERIAL_0 @@ -21,6 +26,33 @@ namespace Espfc { +static void reenumerateUsb() +{ +#if defined(ESPFC_SERIAL_USB_REENUMERATE) + + esp_reset_reason_t reason = esp_reset_reason(); + + // Reenumerate USB only after crash / WDT / soft reset + if (reason == ESP_RST_WDT || reason == ESP_RST_INT_WDT || reason == ESP_RST_PANIC || reason == ESP_RST_TASK_WDT || + reason == ESP_RST_SW) + { + + // Disconnect pull-up and pull D+ line to ground + CLEAR_PERI_REG_MASK(USB_SERIAL_JTAG_CONF0_REG, USB_SERIAL_JTAG_DP_PULLUP); + Hal::Gpio::pinMode(ESPFC_SERIAL_USB_DP, OUTPUT); + Hal::Gpio::digitalWrite(ESPFC_SERIAL_USB_DP, LOW); + + // Sufficient delay for USB host controller + delay(200); + + // Restore USB state + Hal::Gpio::digitalWrite(ESPFC_SERIAL_USB_DP, HIGH); + Hal::Gpio::pinMode(ESPFC_SERIAL_USB_DP, INPUT); + SET_PERI_REG_MASK(USB_SERIAL_JTAG_CONF0_REG, USB_SERIAL_JTAG_DP_PULLUP); + } +#endif +} + SerialManager::SerialManager(Model& model, TelemetryManager& telemetry): _model(model), _current(0), _msp(model), _cli(model), _vtx(model), _telemetry(telemetry), _gps(model) #ifdef ESPFC_SERIAL_SOFT_0_WIFI @@ -30,6 +62,8 @@ SerialManager::SerialManager(Model& model, TelemetryManager& telemetry): _model( int SerialManager::begin() { + reenumerateUsb(); + for(int i = 0; i < SERIAL_UART_COUNT; i++) { Device::SerialDevice * port = getSerialPortById((SerialPort)i); @@ -141,6 +175,12 @@ int SerialManager::begin() return 1; } +int SerialManager::reload(ModelChangeEvent event) +{ + _gps.reload(event); + return 1; +} + int FAST_CODE_ATTR SerialManager::update() { const SerialPortConfig& sc = _model.config.serial[_current]; diff --git a/lib/Espfc/src/SerialManager.h b/lib/Espfc/src/SerialManager.h index f839928a..e9f96eb7 100644 --- a/lib/Espfc/src/SerialManager.h +++ b/lib/Espfc/src/SerialManager.h @@ -1,13 +1,13 @@ #pragma once -#include "Model.h" -#include "Device/SerialDevice.h" +#include "Connect/Cli.hpp" #include "Connect/MspProcessor.hpp" #include "Connect/Vtx.hpp" -#include "Connect/Cli.hpp" -#include "TelemetryManager.h" +#include "Device/SerialDevice.h" +#include "Model.h" #include "Output/OutputIBUS.hpp" #include "Sensor/GpsSensor.hpp" +#include "TelemetryManager.h" #ifdef ESPFC_SERIAL_SOFT_0_WIFI #include "Wireless.h" #endif @@ -20,16 +20,17 @@ class SerialManager SerialManager(Model& model, TelemetryManager& telemetry); int begin(); + int reload(ModelChangeEvent event); int update(); private: - static Device::SerialDevice * getSerialPortById(SerialPort portId); + static Device::SerialDevice* getSerialPortById(SerialPort portId); void processMsp(SerialPortState& ss); void next() { _current++; - if(_current >= SERIAL_UART_COUNT) _current = 0; + if (_current >= SERIAL_UART_COUNT) _current = 0; } Model& _model; @@ -46,4 +47,4 @@ class SerialManager #endif }; -} +} // namespace Espfc diff --git a/lib/Espfc/src/Target/TargetESP32s3.h b/lib/Espfc/src/Target/TargetESP32s3.h index 3fd8fe29..58d56e20 100644 --- a/lib/Espfc/src/Target/TargetESP32s3.h +++ b/lib/Espfc/src/Target/TargetESP32s3.h @@ -44,7 +44,10 @@ #define ESPFC_SERIAL_USB #define ESPFC_SERIAL_USB_DEV Serial #define ESPFC_SERIAL_USB_DEV_T HWCDC +#define ESPFC_SERIAL_USB_DM 19 +#define ESPFC_SERIAL_USB_DP 20 #define ESPFC_SERIAL_USB_FN (SERIAL_FUNCTION_MSP) +#define ESPFC_SERIAL_USB_REENUMERATE #define ESPFC_SERIAL_SOFT_0 #define ESPFC_SERIAL_SOFT_0_FN (SERIAL_FUNCTION_MSP) diff --git a/lib/betaflight/src/blackbox/blackbox.c b/lib/betaflight/src/blackbox/blackbox.c index d77bb11b..caea1dfe 100644 --- a/lib/betaflight/src/blackbox/blackbox.c +++ b/lib/betaflight/src/blackbox/blackbox.c @@ -2066,7 +2066,8 @@ uint16_t blackboxGetPRatio(void) uint8_t blackboxCalculateSampleRate(uint16_t pRatio) { - return LOG2(32000 / (targetPidLooptime * pRatio)); + pRatio = pRatio ? pRatio : 1; + return llog2(32000 / (targetPidLooptime * pRatio)); } /** diff --git a/lib/betaflight/src/msp/msp_protocol.h b/lib/betaflight/src/msp/msp_protocol.h index 15156e5e..203bb5b4 100644 --- a/lib/betaflight/src/msp/msp_protocol.h +++ b/lib/betaflight/src/msp/msp_protocol.h @@ -55,15 +55,11 @@ #pragma once -/* Protocol numbers used both by the wire format, config system, and - field setters. -*/ - +// Protocol numbers used both by the wire format, config system, and field setters. #define MSP_PROTOCOL_VERSION 0 -#define API_VERSION_MAJOR 1 // increment when major changes are made -#define API_VERSION_MINOR 43 // increment after a release, to set the version for all changes to go into the following release (if no changes to MSP are made between the releases, this can be reverted before the release) - +#define API_VERSION_MAJOR 1 +#define API_VERSION_MINOR 48 #define API_VERSION_LENGTH 2 #define MULTIWII_IDENTIFIER "MWII"; @@ -95,249 +91,189 @@ #define CAP_NAVCAP ((uint32_t)1 << 4) #define CAP_EXTAUX ((uint32_t)1 << 5) -#define MSP_API_VERSION 1 //out message -#define MSP_FC_VARIANT 2 //out message -#define MSP_FC_VERSION 3 //out message -#define MSP_BOARD_INFO 4 //out message -#define MSP_BUILD_INFO 5 //out message - -#define MSP_NAME 10 //out message Returns user set board name - betaflight -#define MSP_SET_NAME 11 //in message Sets board name - betaflight - -// -// MSP commands for Cleanflight original features -// -#define MSP_BATTERY_CONFIG 32 -#define MSP_SET_BATTERY_CONFIG 33 - -#define MSP_MODE_RANGES 34 //out message Returns all mode ranges -#define MSP_SET_MODE_RANGE 35 //in message Sets a single mode range - -#define MSP_FEATURE_CONFIG 36 -#define MSP_SET_FEATURE_CONFIG 37 - -#define MSP_BOARD_ALIGNMENT_CONFIG 38 -#define MSP_SET_BOARD_ALIGNMENT_CONFIG 39 - -#define MSP_CURRENT_METER_CONFIG 40 -#define MSP_SET_CURRENT_METER_CONFIG 41 - -#define MSP_MIXER_CONFIG 42 -#define MSP_SET_MIXER_CONFIG 43 - -#define MSP_RX_CONFIG 44 -#define MSP_SET_RX_CONFIG 45 - -#define MSP_LED_COLORS 46 -#define MSP_SET_LED_COLORS 47 - -#define MSP_LED_STRIP_CONFIG 48 -#define MSP_SET_LED_STRIP_CONFIG 49 - -#define MSP_RSSI_CONFIG 50 -#define MSP_SET_RSSI_CONFIG 51 - -#define MSP_ADJUSTMENT_RANGES 52 -#define MSP_SET_ADJUSTMENT_RANGE 53 - -// private - only to be used by the configurator, the commands are likely to change -#define MSP_CF_SERIAL_CONFIG 54 -#define MSP_SET_CF_SERIAL_CONFIG 55 - -#define MSP_VOLTAGE_METER_CONFIG 56 -#define MSP_SET_VOLTAGE_METER_CONFIG 57 - -#define MSP_SONAR_ALTITUDE 58 //out message get sonar altitude [cm] - -#define MSP_PID_CONTROLLER 59 -#define MSP_SET_PID_CONTROLLER 60 - -#define MSP_ARMING_CONFIG 61 -#define MSP_SET_ARMING_CONFIG 62 - -// -// Baseflight MSP commands (if enabled they exist in Cleanflight) -// -#define MSP_RX_MAP 64 //out message get channel map (also returns number of channels total) -#define MSP_SET_RX_MAP 65 //in message set rx map, numchannels to set comes from MSP_RX_MAP - -// DEPRECATED - DO NOT USE "MSP_BF_CONFIG" and MSP_SET_BF_CONFIG. In Cleanflight, isolated commands already exist and should be used instead. -// DEPRECATED - #define MSP_BF_CONFIG 66 //out message baseflight-specific settings that aren't covered elsewhere -// DEPRECATED - #define MSP_SET_BF_CONFIG 67 //in message baseflight-specific settings save - -#define MSP_REBOOT 68 //in message reboot settings - -// Use MSP_BUILD_INFO instead -// DEPRECATED - #define MSP_BF_BUILD_INFO 69 //out message build date as well as some space for future expansion - -#define MSP_DATAFLASH_SUMMARY 70 //out message - get description of dataflash chip -#define MSP_DATAFLASH_READ 71 //out message - get content of dataflash chip -#define MSP_DATAFLASH_ERASE 72 //in message - erase dataflash chip - -// No-longer needed -// DEPRECATED - #define MSP_LOOP_TIME 73 //out message Returns FC cycle time i.e looptime parameter // DEPRECATED -// DEPRECATED - #define MSP_SET_LOOP_TIME 74 //in message Sets FC cycle time i.e looptime parameter // DEPRECATED - -#define MSP_FAILSAFE_CONFIG 75 //out message Returns FC Fail-Safe settings -#define MSP_SET_FAILSAFE_CONFIG 76 //in message Sets FC Fail-Safe settings - -#define MSP_RXFAIL_CONFIG 77 //out message Returns RXFAIL settings -#define MSP_SET_RXFAIL_CONFIG 78 //in message Sets RXFAIL settings - -#define MSP_SDCARD_SUMMARY 79 //out message Get the state of the SD card - -#define MSP_BLACKBOX_CONFIG 80 //out message Get blackbox settings -#define MSP_SET_BLACKBOX_CONFIG 81 //in message Set blackbox settings - -#define MSP_TRANSPONDER_CONFIG 82 //out message Get transponder settings -#define MSP_SET_TRANSPONDER_CONFIG 83 //in message Set transponder settings - -#define MSP_OSD_CONFIG 84 //out message Get osd settings - betaflight -#define MSP_SET_OSD_CONFIG 85 //in message Set osd settings - betaflight - -#define MSP_OSD_CHAR_READ 86 //out message Get osd settings - betaflight -#define MSP_OSD_CHAR_WRITE 87 //in message Set osd settings - betaflight - -#define MSP_VTX_CONFIG 88 //out message Get vtx settings - betaflight -#define MSP_SET_VTX_CONFIG 89 //in message Set vtx settings - betaflight - -// Betaflight Additional Commands -#define MSP_ADVANCED_CONFIG 90 -#define MSP_SET_ADVANCED_CONFIG 91 - -#define MSP_FILTER_CONFIG 92 -#define MSP_SET_FILTER_CONFIG 93 - -#define MSP_PID_ADVANCED 94 -#define MSP_SET_PID_ADVANCED 95 - -#define MSP_SENSOR_CONFIG 96 -#define MSP_SET_SENSOR_CONFIG 97 - -#define MSP_CAMERA_CONTROL 98 - -#define MSP_SET_ARMING_DISABLED 99 - -// -// OSD specific -// -#define MSP_OSD_VIDEO_CONFIG 180 -#define MSP_SET_OSD_VIDEO_CONFIG 181 - -// External OSD displayport mode messages -#define MSP_DISPLAYPORT 182 - -#define MSP_COPY_PROFILE 183 - -#define MSP_BEEPER_CONFIG 184 -#define MSP_SET_BEEPER_CONFIG 185 - -#define MSP_SET_TX_INFO 186 // in message Used to send runtime information from TX lua scripts to the firmware -#define MSP_TX_INFO 187 // out message Used by TX lua scripts to read information from the firmware - -// -// Multwii original MSP commands -// - -// See MSP_API_VERSION and MSP_MIXER_CONFIG -//DEPRECATED - #define MSP_IDENT 100 //out message mixerMode + multiwii version + protocol version + capability variable - - -#define MSP_STATUS 101 //out message cycletime & errors_count & sensor present & box activation & current setting number -#define MSP_RAW_IMU 102 //out message 9 DOF -#define MSP_SERVO 103 //out message servos -#define MSP_MOTOR 104 //out message motors -#define MSP_RC 105 //out message rc channels and more -#define MSP_RAW_GPS 106 //out message fix, numsat, lat, lon, alt, speed, ground course -#define MSP_COMP_GPS 107 //out message distance home, direction home -#define MSP_ATTITUDE 108 //out message 2 angles 1 heading -#define MSP_ALTITUDE 109 //out message altitude, variometer -#define MSP_ANALOG 110 //out message vbat, powermetersum, rssi if available on RX -#define MSP_RC_TUNING 111 //out message rc rate, rc expo, rollpitch rate, yaw rate, dyn throttle PID -#define MSP_PID 112 //out message P I D coeff (9 are used currently) -// Legacy Multiicommand that was never used. -//DEPRECATED - #define MSP_BOX 113 //out message BOX setup (number is dependant of your setup) -// Legacy command that was under constant change due to the naming vagueness, avoid at all costs - use more specific commands instead. -//DEPRECATED - #define MSP_MISC 114 //out message powermeter trig -// Legacy Multiicommand that was never used and always wrong -//DEPRECATED - #define MSP_MOTOR_PINS 115 //out message which pins are in use for motors & servos, for GUI -#define MSP_BOXNAMES 116 //out message the aux switch names -#define MSP_PIDNAMES 117 //out message the PID names -#define MSP_WP 118 //out message get a WP, WP# is in the payload, returns (WP#, lat, lon, alt, flags) WP#0-home, WP#16-poshold -#define MSP_BOXIDS 119 //out message get the permanent IDs associated to BOXes -#define MSP_SERVO_CONFIGURATIONS 120 //out message All servo configurations. -#define MSP_NAV_STATUS 121 //out message Returns navigation status -#define MSP_NAV_CONFIG 122 //out message Returns navigation parameters -#define MSP_MOTOR_3D_CONFIG 124 //out message Settings needed for reversible ESCs -#define MSP_RC_DEADBAND 125 //out message deadbands for yaw alt pitch roll -#define MSP_SENSOR_ALIGNMENT 126 //out message orientation of acc,gyro,mag -#define MSP_LED_STRIP_MODECOLOR 127 //out message Get LED strip mode_color settings -#define MSP_VOLTAGE_METERS 128 //out message Voltage (per meter) -#define MSP_CURRENT_METERS 129 //out message Amperage (per meter) -#define MSP_BATTERY_STATE 130 //out message Connected/Disconnected, Voltage, Current Used -#define MSP_MOTOR_CONFIG 131 //out message Motor configuration (min/max throttle, etc) -#define MSP_GPS_CONFIG 132 //out message GPS configuration -//DEPRECATED - #define MSP_COMPASS_CONFIG 133 //out message Compass configuration -#define MSP_ESC_SENSOR_DATA 134 //out message Extra ESC data from 32-Bit ESCs (Temperature, RPM) -#define MSP_GPS_RESCUE 135 //out message GPS Rescues's angle, initialAltitude, descentDistance, rescueGroundSpeed, sanityChecks and minSats -#define MSP_GPS_RESCUE_PIDS 136 //out message GPS Rescues's throttleP and velocity PIDS + yaw P -#define MSP_VTXTABLE_BAND 137 //out message vtxTable band/channel data -#define MSP_VTXTABLE_POWERLEVEL 138 //out message vtxTable powerLevel data -#define MSP_MOTOR_TELEMETRY 139 //out message Per-motor telemetry data (RPM, packet stats, ESC temp, etc.) - -#define MSP_SET_RAW_RC 200 //in message 8 rc chan -#define MSP_SET_RAW_GPS 201 //in message fix, numsat, lat, lon, alt, speed -#define MSP_SET_PID 202 //in message P I D coeff (9 are used currently) -// Legacy multiiwii command that was never used. -//DEPRECATED - #define MSP_SET_BOX 203 //in message BOX setup (number is dependant of your setup) -#define MSP_SET_RC_TUNING 204 //in message rc rate, rc expo, rollpitch rate, yaw rate, dyn throttle PID, yaw expo -#define MSP_ACC_CALIBRATION 205 //in message no param -#define MSP_MAG_CALIBRATION 206 //in message no param -// Legacy command that was under constant change due to the naming vagueness, avoid at all costs - use more specific commands instead. -//DEPRECATED - #define MSP_SET_MISC 207 //in message powermeter trig + 8 free for future use -#define MSP_RESET_CONF 208 //in message no param -#define MSP_SET_WP 209 //in message sets a given WP (WP#,lat, lon, alt, flags) -#define MSP_SELECT_SETTING 210 //in message Select Setting Number (0-2) -#define MSP_SET_HEADING 211 //in message define a new heading hold direction -#define MSP_SET_SERVO_CONFIGURATION 212 //in message Servo settings -#define MSP_SET_MOTOR 214 //in message PropBalance function -#define MSP_SET_NAV_CONFIG 215 //in message Sets nav config parameters - write to the eeprom -#define MSP_SET_MOTOR_3D_CONFIG 217 //in message Settings needed for reversible ESCs -#define MSP_SET_RC_DEADBAND 218 //in message deadbands for yaw alt pitch roll -#define MSP_SET_RESET_CURR_PID 219 //in message resetting the current pid profile to defaults -#define MSP_SET_SENSOR_ALIGNMENT 220 //in message set the orientation of the acc,gyro,mag -#define MSP_SET_LED_STRIP_MODECOLOR 221 //in message Set LED strip mode_color settings -#define MSP_SET_MOTOR_CONFIG 222 //out message Motor configuration (min/max throttle, etc) -#define MSP_SET_GPS_CONFIG 223 //out message GPS configuration -//DEPRECATED - #define MSP_SET_COMPASS_CONFIG 224 //out message Compass configuration -#define MSP_SET_GPS_RESCUE 225 //in message GPS Rescues's angle, initialAltitude, descentDistance, rescueGroundSpeed, sanityChecks and minSats -#define MSP_SET_GPS_RESCUE_PIDS 226 //in message GPS Rescues's throttleP and velocity PIDS + yaw P -#define MSP_SET_VTXTABLE_BAND 227 //in message set vtxTable band/channel data (one band at a time) -#define MSP_SET_VTXTABLE_POWERLEVEL 228 //in message set vtxTable powerLevel data (one powerLevel at a time) - -// #define MSP_BIND 240 //in message no param -// #define MSP_ALARMS 242 - -#define MSP_EEPROM_WRITE 250 //in message no param -#define MSP_RESERVE_1 251 //reserved for system usage -#define MSP_RESERVE_2 252 //reserved for system usage -#define MSP_DEBUGMSG 253 //out message debug string buffer -#define MSP_DEBUG 254 //out message debug1,debug2,debug3,debug4 -#define MSP_V2_FRAME 255 //MSPv2 payload indicator - -// Additional commands that are not compatible with MultiWii -#define MSP_STATUS_EX 150 //out message cycletime, errors_count, CPU load, sensor present etc -#define MSP_UID 160 //out message Unique device ID -#define MSP_GPSSVINFO 164 //out message get Signal Strength (only U-Blox) -#define MSP_GPSSTATISTICS 166 //out message get GPS debugging data -#define MSP_MULTIPLE_MSP 230 //out message request multiple MSPs in one request - limit is the TX buffer; returns each MSP in the order they were requested starting with length of MSP; MSPs with input arguments are not supported -#define MSP_MODE_RANGES_EXTRA 238 //out message Reads the extra mode range data -#define MSP_ACC_TRIM 240 //out message get acc angle trim values -#define MSP_SET_ACC_TRIM 239 //in message set acc angle trim values -#define MSP_SERVO_MIX_RULES 241 //out message Returns servo mixer configuration -#define MSP_SET_SERVO_MIX_RULE 242 //in message Sets servo mixer configuration -#define MSP_SET_PASSTHROUGH 245 //in message Sets up passthrough to different peripherals (4way interface, uart, etc...) -#define MSP_SET_RTC 246 //in message Sets the RTC clock -#define MSP_RTC 247 //out message Gets the RTC clock -#define MSP_SET_BOARD_INFO 248 //in message Sets the board information for this board -#define MSP_SET_SIGNATURE 249 //in message Sets the signature of the board and serial number +#define MSP_API_VERSION 1 // out message: Get API version +#define MSP_FC_VARIANT 2 // out message: Get flight controller variant +#define MSP_FC_VERSION 3 // out message: Get flight controller version +#define MSP_BOARD_INFO 4 // out message: Get board information +#define MSP_BUILD_INFO 5 // out message: Get build information + +#define MSP_NAME 10 // out message: Returns user set board name - betaflight +#define MSP_SET_NAME 11 // in message: Sets board name - betaflight + +// Cleanflight original features (32-62) +#define MSP_BATTERY_CONFIG 32 // out message: Get battery configuration +#define MSP_SET_BATTERY_CONFIG 33 // in message: Set battery configuration +#define MSP_MODE_RANGES 34 // out message: Returns all mode ranges +#define MSP_SET_MODE_RANGE 35 // in message: Sets a single mode range +#define MSP_FEATURE_CONFIG 36 // out message: Get feature configuration +#define MSP_SET_FEATURE_CONFIG 37 // in message: Set feature configuration +#define MSP_BOARD_ALIGNMENT_CONFIG 38 // out message: Get board alignment configuration +#define MSP_SET_BOARD_ALIGNMENT_CONFIG 39 // in message: Set board alignment configuration +#define MSP_CURRENT_METER_CONFIG 40 // out message: Get current meter configuration +#define MSP_SET_CURRENT_METER_CONFIG 41 // in message: Set current meter configuration +#define MSP_MIXER_CONFIG 42 // out message: Get mixer configuration +#define MSP_SET_MIXER_CONFIG 43 // in message: Set mixer configuration +#define MSP_RX_CONFIG 44 // out message: Get RX configuration +#define MSP_SET_RX_CONFIG 45 // in message: Set RX configuration +#define MSP_LED_COLORS 46 // out message: Get LED colors +#define MSP_SET_LED_COLORS 47 // in message: Set LED colors +#define MSP_LED_STRIP_CONFIG 48 // out message: Get LED strip configuration +#define MSP_SET_LED_STRIP_CONFIG 49 // in message: Set LED strip configuration +#define MSP_RSSI_CONFIG 50 // out message: Get RSSI configuration +#define MSP_SET_RSSI_CONFIG 51 // in message: Set RSSI configuration +#define MSP_ADJUSTMENT_RANGES 52 // out message: Get adjustment ranges +#define MSP_SET_ADJUSTMENT_RANGE 53 // in message: Set adjustment range +#define MSP_CF_SERIAL_CONFIG 54 // out message: Get Cleanflight serial configuration +#define MSP_SET_CF_SERIAL_CONFIG 55 // in message: Set Cleanflight serial configuration +#define MSP_VOLTAGE_METER_CONFIG 56 // out message: Get voltage meter configuration +#define MSP_SET_VOLTAGE_METER_CONFIG 57 // in message: Set voltage meter configuration +#define MSP_SONAR_ALTITUDE 58 // out message: Get sonar altitude [cm] +#define MSP_PID_CONTROLLER 59 // out message: Get PID controller +#define MSP_SET_PID_CONTROLLER 60 // in message: Set PID controller +#define MSP_ARMING_CONFIG 61 // out message: Get arming configuration +#define MSP_SET_ARMING_CONFIG 62 // in message: Set arming configuration + +// Baseflight MSP commands (64-89) +#define MSP_RX_MAP 64 // out message: Get RX map (also returns number of channels total) +#define MSP_SET_RX_MAP 65 // in message: Set RX map, numchannels to set comes from MSP_RX_MAP +#define MSP_REBOOT 68 // in message: Reboot settings +#define MSP_DATAFLASH_SUMMARY 70 // out message: Get description of dataflash chip +#define MSP_DATAFLASH_READ 71 // out message: Get content of dataflash chip +#define MSP_DATAFLASH_ERASE 72 // in message: Erase dataflash chip +#define MSP_FAILSAFE_CONFIG 75 // out message: Get failsafe settings +#define MSP_SET_FAILSAFE_CONFIG 76 // in message: Set failsafe settings +#define MSP_RXFAIL_CONFIG 77 // out message: Get RX failsafe settings +#define MSP_SET_RXFAIL_CONFIG 78 // in message: Set RX failsafe settings +#define MSP_SDCARD_SUMMARY 79 // out message: Get SD card state +#define MSP_BLACKBOX_CONFIG 80 // out message: Get blackbox settings +#define MSP_SET_BLACKBOX_CONFIG 81 // in message: Set blackbox settings +#define MSP_TRANSPONDER_CONFIG 82 // out message: Get transponder settings +#define MSP_SET_TRANSPONDER_CONFIG 83 // in message: Set transponder settings +#define MSP_OSD_CONFIG 84 // out message: Get OSD settings +#define MSP_SET_OSD_CONFIG 85 // in message: Set OSD settings +#define MSP_OSD_CHAR_READ 86 // out message: Get OSD characters +#define MSP_OSD_CHAR_WRITE 87 // in message: Set OSD characters +#define MSP_VTX_CONFIG 88 // out message: Get VTX settings +#define MSP_SET_VTX_CONFIG 89 // in message: Set VTX settings + +// Betaflight Additional Commands (90-99) +#define MSP_ADVANCED_CONFIG 90 // out message: Get advanced configuration +#define MSP_SET_ADVANCED_CONFIG 91 // in message: Set advanced configuration +#define MSP_FILTER_CONFIG 92 // out message: Get filter configuration +#define MSP_SET_FILTER_CONFIG 93 // in message: Set filter configuration +#define MSP_PID_ADVANCED 94 // out message: Get advanced PID settings +#define MSP_SET_PID_ADVANCED 95 // in message: Set advanced PID settings +#define MSP_SENSOR_CONFIG 96 // out message: Get sensor configuration +#define MSP_SET_SENSOR_CONFIG 97 // in message: Set sensor configuration +#define MSP_CAMERA_CONTROL 98 // in/out message: Camera control +#define MSP_SET_ARMING_DISABLED 99 // in message: Enable/disable arming + +// Multiwii original MSP commands (101-139) +#define MSP_STATUS 101 // out message: Cycletime & errors_count & sensor present & box activation & current setting number +#define MSP_RAW_IMU 102 // out message: 9 DOF +#define MSP_SERVO 103 // out message: Servos +#define MSP_MOTOR 104 // out message: Motors +#define MSP_RC 105 // out message: RC channels and more +#define MSP_RAW_GPS 106 // out message: Fix, numsat, lat, lon, alt, speed, ground course +#define MSP_COMP_GPS 107 // out message: Distance home, direction home +#define MSP_ATTITUDE 108 // out message: 2 angles 1 heading +#define MSP_ALTITUDE 109 // out message: Altitude, variometer +#define MSP_ANALOG 110 // out message: Vbat, powermetersum, rssi if available on RX +#define MSP_RC_TUNING 111 // out message: RC rate, rc expo, rollpitch rate, yaw rate, dyn throttle PID +#define MSP_PID 112 // out message: P I D coeff (9 are used currently) +#define MSP_BOXNAMES 116 // out message: The aux switch names +#define MSP_PIDNAMES 117 // out message: The PID names +#define MSP_WP 118 // out message: Get a WP, WP# is in the payload, returns (WP#, lat, lon, alt, flags) WP#0-home, WP#16-poshold +#define MSP_BOXIDS 119 // out message: Get the permanent IDs associated to BOXes +#define MSP_SERVO_CONFIGURATIONS 120 // out message: All servo configurations +#define MSP_NAV_STATUS 121 // out message: Returns navigation status +#define MSP_NAV_CONFIG 122 // out message: Returns navigation parameters +#define MSP_MOTOR_3D_CONFIG 124 // out message: Settings needed for reversible ESCs +#define MSP_RC_DEADBAND 125 // out message: Deadbands for yaw alt pitch roll +#define MSP_SENSOR_ALIGNMENT 126 // out message: Orientation of acc,gyro,mag +#define MSP_LED_STRIP_MODECOLOR 127 // out message: Get LED strip mode_color settings +#define MSP_VOLTAGE_METERS 128 // out message: Voltage (per meter) +#define MSP_CURRENT_METERS 129 // out message: Amperage (per meter) +#define MSP_BATTERY_STATE 130 // out message: Connected/Disconnected, Voltage, Current Used +#define MSP_MOTOR_CONFIG 131 // out message: Motor configuration (min/max throttle, etc) +#define MSP_GPS_CONFIG 132 // out message: GPS configuration +#define MSP_COMPASS_CONFIG 133 // out message: Compass configuration +#define MSP_ESC_SENSOR_DATA 134 // out message: Extra ESC data from 32-Bit ESCs (Temperature, RPM) +#define MSP_GPS_RESCUE 135 // out message: GPS Rescue angle, returnAltitude, descentDistance, groundSpeed, sanityChecks and minSats +#define MSP_GPS_RESCUE_PIDS 136 // out message: GPS Rescue throttleP and velocity PIDS + yaw P +#define MSP_VTXTABLE_BAND 137 // out message: VTX table band/channel data +#define MSP_VTXTABLE_POWERLEVEL 138 // out message: VTX table powerLevel data +#define MSP_MOTOR_TELEMETRY 139 // out message: Per-motor telemetry data (RPM, packet stats, ESC temp, etc.) + +// Simplified tuning commands (140-145) +#define MSP_SIMPLIFIED_TUNING 140 // out message: Get simplified tuning values and enabled state +#define MSP_SET_SIMPLIFIED_TUNING 141 // in message: Set simplified tuning positions and apply calculated tuning +#define MSP_CALCULATE_SIMPLIFIED_PID 142 // out message: Calculate PID values based on sliders without saving +#define MSP_CALCULATE_SIMPLIFIED_GYRO 143 // out message: Calculate gyro filter values based on sliders without saving +#define MSP_CALCULATE_SIMPLIFIED_DTERM 144 // out message: Calculate D term filter values based on sliders without saving +#define MSP_VALIDATE_SIMPLIFIED_TUNING 145 // out message: Returns array of true/false showing which simplified tuning groups match values + +// Additional non-MultiWii commands (150-166) +#define MSP_STATUS_EX 150 // out message: Cycletime, errors_count, CPU load, sensor present etc +#define MSP_UID 160 // out message: Unique device ID +#define MSP_GPSSVINFO 164 // out message: Get Signal Strength (only U-Blox) +#define MSP_GPSSTATISTICS 166 // out message: Get GPS debugging data +#define MSP_ATTITUDE_QUATERNION 167 // out message: Orientation quaternion components (w, x, y, z) + +// OSD specific commands (180-189) +#define MSP_OSD_VIDEO_CONFIG 180 // out message: Get OSD video settings +#define MSP_SET_OSD_VIDEO_CONFIG 181 // in message: Set OSD video settings +#define MSP_DISPLAYPORT 182 // out message: External OSD displayport mode +#define MSP_COPY_PROFILE 183 // in message: Copy settings between profiles +#define MSP_BEEPER_CONFIG 184 // out message: Get beeper configuration +#define MSP_SET_BEEPER_CONFIG 185 // in message: Set beeper configuration +#define MSP_SET_TX_INFO 186 // in message: Set runtime information from TX lua scripts +#define MSP_TX_INFO 187 // out message: Get runtime information for TX lua scripts +#define MSP_SET_OSD_CANVAS 188 // in message: Set OSD canvas size COLSxROWS +#define MSP_OSD_CANVAS 189 // out message: Get OSD canvas size COLSxROWS + +// Set commands (200-229) +#define MSP_SET_RAW_RC 200 // in message: 8 rc chan +#define MSP_SET_RAW_GPS 201 // in message: Fix, numsat, lat, lon, alt, speed +#define MSP_SET_PID 202 // in message: P I D coeff (9 are used currently) +#define MSP_SET_RC_TUNING 204 // in message: RC rate, rc expo, rollpitch rate, yaw rate, dyn throttle PID, yaw expo +#define MSP_ACC_CALIBRATION 205 // in message: No param - calibrate accelerometer +#define MSP_MAG_CALIBRATION 206 // in message: No param - calibrate magnetometer +#define MSP_RESET_CONF 208 // in message: No param - reset settings +#define MSP_SET_WP 209 // in message: Sets a given WP (WP#,lat, lon, alt, flags) +#define MSP_SELECT_SETTING 210 // in message: Select setting number (0-2) +#define MSP_SET_HEADING 211 // in message: Define a new heading hold direction +#define MSP_SET_SERVO_CONFIGURATION 212 // in message: Servo settings +#define MSP_SET_MOTOR 214 // in message: PropBalance function +#define MSP_SET_NAV_CONFIG 215 // in message: Sets nav config parameters +#define MSP_SET_MOTOR_3D_CONFIG 217 // in message: Settings needed for reversible ESCs +#define MSP_SET_RC_DEADBAND 218 // in message: Deadbands for yaw alt pitch roll +#define MSP_SET_RESET_CURR_PID 219 // in message: Reset current PID profile to defaults +#define MSP_SET_SENSOR_ALIGNMENT 220 // in message: Set the orientation of acc,gyro,mag +#define MSP_SET_LED_STRIP_MODECOLOR 221 // in message: Set LED strip mode_color settings +#define MSP_SET_MOTOR_CONFIG 222 // in message: Motor configuration (min/max throttle, etc) +#define MSP_SET_GPS_CONFIG 223 // in message: GPS configuration +#define MSP_SET_COMPASS_CONFIG 224 // in message: Compass configuration +#define MSP_SET_GPS_RESCUE 225 // in message: Set GPS Rescue parameters +#define MSP_SET_GPS_RESCUE_PIDS 226 // in message: Set GPS Rescue PID values +#define MSP_SET_VTXTABLE_BAND 227 // in message: Set vtxTable band/channel data +#define MSP_SET_VTXTABLE_POWERLEVEL 228 // in message: Set vtxTable powerLevel data + +// Multiple MSP and special commands (230-255) +#define MSP_MULTIPLE_MSP 230 // out message: Request multiple MSPs in one request +#define MSP_MODE_RANGES_EXTRA 238 // out message: Extra mode range data +#define MSP_SET_ACC_TRIM 239 // in message: Set acc angle trim values +#define MSP_ACC_TRIM 240 // out message: Get acc angle trim values +#define MSP_SERVO_MIX_RULES 241 // out message: Get servo mixer configuration +#define MSP_SET_SERVO_MIX_RULE 242 // in message: Set servo mixer configuration +#define MSP_SET_PASSTHROUGH 245 // in message: Set passthrough to peripherals +#define MSP_SET_RTC 246 // in message: Set the RTC clock +#define MSP_RTC 247 // out message: Get the RTC clock +#define MSP_SET_BOARD_INFO 248 // in message: Set the board information +#define MSP_SET_SIGNATURE 249 // in message: Set the signature of the board and serial number +#define MSP_EEPROM_WRITE 250 // in message: Write settings to EEPROM +#define MSP_RESERVE_1 251 // reserved for system usage +#define MSP_RESERVE_2 252 // reserved for system usage +#define MSP_DEBUGMSG 253 // out message: debug string buffer +#define MSP_DEBUG 254 // out message: debug1,debug2,debug3,debug4 +#define MSP_V2_FRAME 255 // MSPv2 payload indicator diff --git a/lib/betaflight/src/msp/msp_protocol_v2_betaflight.h b/lib/betaflight/src/msp/msp_protocol_v2_betaflight.h index ba07b5b7..7ed9dd27 100644 --- a/lib/betaflight/src/msp/msp_protocol_v2_betaflight.h +++ b/lib/betaflight/src/msp/msp_protocol_v2_betaflight.h @@ -21,3 +21,30 @@ #define MSP2_BETAFLIGHT_BIND 0x3000 #define MSP2_MOTOR_OUTPUT_REORDERING 0x3001 #define MSP2_SET_MOTOR_OUTPUT_REORDERING 0x3002 +#define MSP2_SEND_DSHOT_COMMAND 0x3003 +#define MSP2_GET_VTX_DEVICE_STATUS 0x3004 +#define MSP2_GET_OSD_WARNINGS 0x3005 // returns active OSD warning message text +#define MSP2_GET_TEXT 0x3006 +#define MSP2_SET_TEXT 0x3007 +#define MSP2_GET_LED_STRIP_CONFIG_VALUES 0x3008 +#define MSP2_SET_LED_STRIP_CONFIG_VALUES 0x3009 +#define MSP2_SENSOR_CONFIG_ACTIVE 0x300A +#define MSP2_SENSOR_OPTICALFLOW 0x300B +#define MSP2_MCU_INFO 0x300C +#define MSP2_GYRO_SENSOR_ACTIVE 0x300D +#define MSP2_BATTERY_PROFILE 0x300E +#define MSP2_SET_BATTERY_PROFILE 0x300F +#define MSP2_CLI_SETTING 0x3010 +#define MSP2_CLI_SETTING_INFO 0x3011 + +// MSP2_SET_TEXT and MSP2_GET_TEXT variable types +#define MSP2TEXT_PILOT_NAME 1 +#define MSP2TEXT_CRAFT_NAME 2 +#define MSP2TEXT_PID_PROFILE_NAME 3 +#define MSP2TEXT_RATE_PROFILE_NAME 4 +#define MSP2TEXT_BUILDKEY 5 +#define MSP2TEXT_RELEASENAME 6 +#define MSP2TEXT_CUSTOM_MSG_0 7 // CUSTOM_MSG_MAX_NUM entries are allocated +#define CUSTOM_MSG_MAX_NUM 4 +#define MSP2TEXT_BATTERY_PROFILE_NAME 11 +// next new variable type must be >= MSP2TEXT_BATTERY_PROFILE_NAME + 1 (12) diff --git a/lib/betaflight/src/msp/msp_protocol_v2_common.h b/lib/betaflight/src/msp/msp_protocol_v2_common.h index 2031df2b..28e14691 100644 --- a/lib/betaflight/src/msp/msp_protocol_v2_common.h +++ b/lib/betaflight/src/msp/msp_protocol_v2_common.h @@ -20,3 +20,9 @@ #define MSP2_COMMON_SERIAL_CONFIG 0x1009 #define MSP2_COMMON_SET_SERIAL_CONFIG 0x100A + +// Sensors +#define MSP2_SENSOR_GPS 0x1F03 +// TODO: implement new, extensible rangefinder protocol +#define MSP2_SENSOR_RANGEFINDER_LIDARMT 0x1F01 +#define MSP2_SENSOR_OPTICALFLOW_MT 0x1F02 diff --git a/lib/betaflight/src/platform.h b/lib/betaflight/src/platform.h index 92c91f47..9955f8f1 100644 --- a/lib/betaflight/src/platform.h +++ b/lib/betaflight/src/platform.h @@ -19,7 +19,6 @@ #if defined(ESP32) #define USE_FLASHFS -#include "esp_partition.h" #endif #if defined(ESP8266) @@ -55,9 +54,9 @@ #define PID_PROCESS_DENOM_DEFAULT 1 #define FC_FIRMWARE_NAME "Betaflight" -#define FC_VERSION_MAJOR 4 // increment when a major release is made (big new feature, etc) -#define FC_VERSION_MINOR 4 // increment when a minor release is made (small new feature, change etc) -#define FC_VERSION_PATCH_LEVEL 0 // increment when a bug is fixed +#define FC_VERSION_MAJOR 2026 // increment when a major release is made (big new feature, etc) +#define FC_VERSION_MINOR 6 // increment when a minor release is made (small new feature, change etc) +#define FC_VERSION_PATCH_LEVEL 1 // increment when a bug is fixed #define STR_HELPER(x) #x #define STR(x) STR_HELPER(x) @@ -101,6 +100,7 @@ extern const char * boardIdentifier; #define LOG2_32BIT(v) (16*((v)>65535L) + LOG2_16BIT((v)*1L >>16*((v)>65535L))) #define LOG2_64BIT(v) (32*((v)/2L>>31 > 0) + LOG2_32BIT((v)*1L >>16*((v)/2L>>31 > 0) >>16*((v)/2L>>31 > 0))) #define LOG2(v) LOG2_64BIT(v) +static inline uint32_t llog2(uint32_t n) { return 31 - __builtin_clz(n | 1); } #ifdef UNIT_TEST #define STATIC_UNIT_TESTED @@ -108,7 +108,9 @@ extern const char * boardIdentifier; #define STATIC_UNIT_TESTED #endif +#ifndef offsetof #define offsetof(TYPE, MEMBER) __builtin_offsetof (TYPE, MEMBER) +#endif #define UNUSED(v) ((void)v) #define ARRAYLEN(x) (sizeof(x) / sizeof((x)[0])) #define STATIC_ASSERT(condition, name) \ @@ -728,7 +730,6 @@ typedef enum { DEBUG_GYRO_FILTERED, DEBUG_ACCELEROMETER, DEBUG_PIDLOOP, - DEBUG_GYRO_SCALED, DEBUG_RC_INTERPOLATION, DEBUG_ANGLERATE, DEBUG_ESC_SENSOR, @@ -743,14 +744,15 @@ typedef enum { DEBUG_RX_FRSKY_SPI, DEBUG_RX_SFHSS_SPI, DEBUG_GYRO_RAW, - DEBUG_DUAL_GYRO_RAW, - DEBUG_DUAL_GYRO_DIFF, + DEBUG_MULTI_GYRO_RAW, + DEBUG_MULTI_GYRO_DIFF, DEBUG_MAX7456_SIGNAL, DEBUG_MAX7456_SPICLOCK, DEBUG_SBUS, DEBUG_FPORT, DEBUG_RANGEFINDER, DEBUG_RANGEFINDER_QUALITY, + DEBUG_OPTICALFLOW, DEBUG_LIDAR_TF, DEBUG_ADC_INTERNAL, DEBUG_RUNAWAY_TAKEOFF, @@ -769,22 +771,62 @@ typedef enum { DEBUG_RX_SPEKTRUM_SPI, DEBUG_DSHOT_RPM_TELEMETRY, DEBUG_RPM_FILTER, - DEBUG_D_MIN, + DEBUG_D_MAX, DEBUG_AC_CORRECTION, DEBUG_AC_ERROR, - DEBUG_DUAL_GYRO_SCALED, + DEBUG_MULTI_GYRO_SCALED, DEBUG_DSHOT_RPM_ERRORS, DEBUG_CRSF_LINK_STATISTICS_UPLINK, DEBUG_CRSF_LINK_STATISTICS_PWR, DEBUG_CRSF_LINK_STATISTICS_DOWN, DEBUG_BARO, - DEBUG_GPS_RESCUE_THROTTLE_PID, + DEBUG_AUTOPILOT_ALTITUDE, DEBUG_DYN_IDLE, - DEBUG_FF_LIMIT, - DEBUG_FF_INTERPOLATED, + DEBUG_FEEDFORWARD_LIMIT, + DEBUG_FEEDFORWARD, DEBUG_BLACKBOX_OUTPUT, DEBUG_GYRO_SAMPLE, DEBUG_RX_TIMING, + DEBUG_D_LPF, + DEBUG_VTX_TRAMP, + DEBUG_GHST, + DEBUG_GHST_MSP, + DEBUG_SCHEDULER_DETERMINISM, + DEBUG_TIMING_ACCURACY, + DEBUG_RX_EXPRESSLRS_SPI, + DEBUG_RX_EXPRESSLRS_PHASELOCK, + DEBUG_RX_STATE_TIME, + DEBUG_GPS_RESCUE_VELOCITY, + DEBUG_GPS_RESCUE_HEADING, + DEBUG_GPS_RESCUE_TRACKING, + DEBUG_GPS_CONNECTION, + DEBUG_ATTITUDE, + DEBUG_VTX_MSP, + DEBUG_GPS_DOP, + DEBUG_FAILSAFE, + DEBUG_GYRO_CALIBRATION, + DEBUG_ANGLE_MODE, + DEBUG_ANGLE_TARGET, + DEBUG_CURRENT_ANGLE, + DEBUG_DSHOT_TELEMETRY_COUNTS, + DEBUG_RPM_LIMIT, + DEBUG_RC_STATS, + DEBUG_MAG_CALIB, + DEBUG_MAG_TASK_RATE, + DEBUG_EZLANDING, + DEBUG_TPA, + DEBUG_S_TERM, + DEBUG_SPA, + DEBUG_TASK, + DEBUG_GIMBAL, + DEBUG_WING_SETPOINT, + DEBUG_CHIRP, + DEBUG_FLASH_TEST_PRBS, + DEBUG_MAVLINK_TELEMETRY, + DEBUG_AUTOPILOT_PID, + DEBUG_POSITION_NAV, + DEBUG_AUTOPILOT_STOP, + DEBUG_PITOT, DEBUG_COUNT } debugType_e; @@ -959,20 +1001,16 @@ int32_t getAmperageLatest(void); /* SENSOR END */ /* RX START */ -#define RX_MAPPABLE_CHANNEL_COUNT 8 - typedef struct rxConfig_s { uint8_t serialrx_provider; // type of UART-based receiver (0 = spek 10, 1 = spek 11, 2 = sbus). Must be enabled by FEATURE_RX_SERIAL first. uint8_t rssi_channel; - uint8_t rcInterpolation; - uint8_t rcInterpolationChannels; - uint8_t rcInterpolationInterval; - uint16_t airModeActivateThreshold; // Throttle setpoint where airmode gets activated + uint8_t airModeActivateThreshold; // Throttle setpoint where airmode gets activated } rxConfig_t; PG_DECLARE(rxConfig_t, rxConfig); uint16_t getRssi(void); +/* RX END */ /* FAILSAFE START */ typedef enum { @@ -987,13 +1025,14 @@ typedef enum { failsafePhase_e failsafePhase(); bool rxIsReceivingSignal(void); bool rxAreFlightChannelsValid(void); +/* FAILSAFE END */ + float pidGetPreviousSetpoint(int axis); float mixerGetThrottle(void); bool isRssiConfigured(void); int16_t getMotorOutputLow(); int16_t getMotorOutputHigh(); uint16_t getDshotErpm(uint8_t i); -/* FAILSAFE END */ typedef enum { GPS_LATITUDE, diff --git a/platformio.ini b/platformio.ini index 192fb35f..57350b73 100644 --- a/platformio.ini +++ b/platformio.ini @@ -44,8 +44,9 @@ build_unflags = monitor_speed = 115200 upload_speed = 921600 ; upload_speed = 460800 -; monitor_filters = esp8266_exception_decoder -; monitor_filters = esp32_exception_decoder +monitor_filters = send_on_enter +; monitor_filters = send_on_enter,esp32_exception_decoder +monitor_echo = yes lib_deps = yoursunny/WifiEspNow @ ^0.0.20230713 @@ -197,8 +198,12 @@ lib_deps = build_unflags = -std=gnu++11 build_flags = + -std=c++17 + -Wall + -I.pio/build/native/unity_config -DIRAM_ATTR="" -DUNIT_TEST - -std=c++17 -DNO_GLOBAL_INSTANCES ; -DUNITY_INCLUDE_PRINT_FORMATTED +#build_type = debug +#debug_test = test_cli diff --git a/src/main.cpp b/src/main.cpp index c8241404..0d80be9e 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -19,7 +19,6 @@ #elif defined(ESPFC_WIFI) #include #endif -#include "Debug_Espfc.h" #ifdef ESP32 void IRAM_ATTR serialEventRun(void) {} diff --git a/test/test_cli/test_cli.cpp b/test/test_cli/test_cli.cpp new file mode 100644 index 00000000..5bb44e79 --- /dev/null +++ b/test/test_cli/test_cli.cpp @@ -0,0 +1,363 @@ +#include +#include +#include +#include + +using Cli = Espfc::Connect::Cli; +using CliCmd = Espfc::CliCmd; +using Model = Espfc::Model; +using namespace fakeit; + +static size_t appendText(Stream& stream, const char* text) +{ + if (!text) return 0; + const size_t len = std::strlen(text); + stream.write(reinterpret_cast(text), len); + return len; +} + +static size_t appendChar(Stream& stream, char c) +{ + return stream.write(static_cast(c)); +} + +static void setupPrintMockForStream(Stream& stream) +{ + // Route Print method calls made on this Stream instance to ArduinoFake(Print). + getArduinoFakeContext()->Mapping[&stream] = getArduinoFakeContext()->Print(); + + When(OverloadedMethod(ArduinoFake(Print), print, size_t(const char*))).AlwaysDo([&stream](const char* s) -> size_t { + return appendText(stream, s); + }); + When(OverloadedMethod(ArduinoFake(Print), print, size_t(char))).AlwaysDo([&stream](char c) -> size_t { + return appendChar(stream, c); + }); + When(OverloadedMethod(ArduinoFake(Print), print, size_t(int, int))).AlwaysDo([&stream](int value, int) -> size_t { + return appendText(stream, std::to_string(value).c_str()); + }); + When(OverloadedMethod(ArduinoFake(Print), print, size_t(unsigned int, int))) + .AlwaysDo( + [&stream](unsigned int value, int) -> size_t { return appendText(stream, std::to_string(value).c_str()); }); + When(OverloadedMethod(ArduinoFake(Print), print, size_t(long, int))).AlwaysDo([&stream](long value, int) -> size_t { + return appendText(stream, std::to_string(value).c_str()); + }); + When(OverloadedMethod(ArduinoFake(Print), print, size_t(unsigned long, int))) + .AlwaysDo( + [&stream](unsigned long value, int) -> size_t { return appendText(stream, std::to_string(value).c_str()); }); + + When(OverloadedMethod(ArduinoFake(Print), println, size_t())).AlwaysDo([&stream]() -> size_t { + return appendChar(stream, '\n'); + }); + When(OverloadedMethod(ArduinoFake(Print), println, size_t(const char*))).AlwaysDo([&stream](const char* s) -> size_t { + const size_t n = appendText(stream, s); + return n + appendChar(stream, '\n'); + }); +} + +class StreamMock : public Stream +{ +public: + StreamMock(): _buffer("") {} + size_t write(uint8_t c) override + { + _buffer += static_cast(c); + return 1; + } + size_t write(const uint8_t* buffer, size_t size) override + { + _buffer.append(reinterpret_cast(buffer), size); + return size; + } + int available() override + { + return _buffer.length(); + } + int read() override + { + if (_buffer.empty()) return -1; + char c = _buffer[0]; + _buffer.erase(0, 1); + return static_cast(c); + } + int peek() override + { + if (_buffer.empty()) return -1; + return static_cast(_buffer[0]); + } + void flush() override + { + _buffer.clear(); + } + const char* c_str() const + { + return _buffer.c_str(); + } + std::string str() const + { + return _buffer; + } + +private: + std::string _buffer; +}; + +static StreamMock stream; + +void setUp(void) +{ + ArduinoFakeReset(); + stream.flush(); + setupPrintMockForStream(stream); +} + +void tearDown(void) +{ + stream.flush(); +} + +void test_cli_init() +{ + Model model; + Cli cli{model}; + TEST_ASSERT_FALSE(cli._active); + TEST_ASSERT_FALSE(cli._interactive); + TEST_ASSERT_FALSE(cli._ignore); + TEST_ASSERT_NOT_NULL(cli._params); +} + +void test_cli_enter_interactive() +{ + Model model; + Cli cli{model}; + CliCmd cmd; + + cli.process('h', cmd, stream); + TEST_ASSERT_TRUE(cli._active); + TEST_ASSERT_TRUE(cli._interactive); + TEST_ASSERT_FALSE(cli._ignore); +} + +void test_cli_enter_leave_non_interactive() +{ + Model model; + Cli cli{model}; + CliCmd cmd; + + cli.process(0x02, cmd, stream); + TEST_ASSERT_TRUE(cli._active); + TEST_ASSERT_FALSE(cli._interactive); + TEST_ASSERT_FALSE(cli._ignore); + + cli.process(0x03, cmd, stream); + TEST_ASSERT_FALSE(cli._active); + TEST_ASSERT_FALSE(cli._interactive); + TEST_ASSERT_FALSE(cli._ignore); + + auto result = stream.str(); + TEST_ASSERT_EQUAL(2, result.length()); + TEST_ASSERT_EQUAL(0x02, result[0]); + TEST_ASSERT_EQUAL(0x03, result[1]); +} + +void test_cli_configurator_handshake() +{ + Model model; + Cli cli{model}; + CliCmd cmd; + + cli.process('#', cmd, stream); + TEST_ASSERT_TRUE(cli._active); + TEST_ASSERT_TRUE(cli._interactive); + TEST_ASSERT_FALSE(cli._ignore); + + auto result = stream.str(); + TEST_ASSERT_NOT_EQUAL(0, result.length()); + TEST_ASSERT_TRUE(result.find("Entering CLI Mode") != std::string::npos); + + stream.flush(); + + // CTRL-D (0x04) is used to exit CLI mode + cli.process(0x04, cmd, stream); + TEST_ASSERT_FALSE(cli._active); + TEST_ASSERT_FALSE(cli._interactive); + TEST_ASSERT_FALSE(cli._ignore); + + result = stream.str(); + TEST_ASSERT_NOT_EQUAL(0, result.length()); + TEST_ASSERT_NOT_EQUAL(std::string::npos, result.find("leaving CLI mode")); +} + +void test_cli_process_comment() +{ + Model model; + Cli cli{model}; + CliCmd cmd; + + for (char c : std::string("command # comment")) + { + cli.process(c, cmd, stream); + } + + TEST_ASSERT_EQUAL(8, cmd.index); + TEST_ASSERT_EQUAL_STRING("command ", cmd.buff); + TEST_ASSERT_EQUAL(0, cmd.args[cmd.index]); + TEST_ASSERT_EQUAL(std::string::npos, std::string{cmd.buff}.find("# comment")); + + TEST_ASSERT_TRUE(cli._active); + TEST_ASSERT_TRUE(cli._interactive); + TEST_ASSERT_TRUE(cli._ignore); + + cli.process('\n', cmd, stream); + + auto result = stream.str(); + TEST_ASSERT_NOT_EQUAL(0, result.length()); + TEST_ASSERT_NOT_EQUAL(std::string::npos, result.find("# command")); + TEST_ASSERT_NOT_EQUAL(std::string::npos, result.find("unknown command: command")); +} + +void test_cli_overflow() +{ + Model model; + Cli cli{model}; + CliCmd cmd; + + for (size_t i = 0; i < sizeof(cmd.buff) + 2; ++i) + { + cli.process('a', cmd, stream); + } + + TEST_ASSERT_EQUAL(sizeof(cmd.buff) - 1, cmd.index); + TEST_ASSERT_EQUAL_STRING(std::string(sizeof(cmd.buff) - 1, 'a').c_str(), cmd.buff); + TEST_ASSERT_EQUAL(0, cmd.args[cmd.index]); +} + +void test_cli_process_help() +{ + Model model; + Cli cli{model}; + CliCmd cmd; + + for (char c : std::string("help")) + { + cli.process(c, cmd, stream); + } + + TEST_ASSERT_EQUAL(4, cmd.index); + TEST_ASSERT_EQUAL_STRING("help", cmd.buff); + TEST_ASSERT_EQUAL(0, cmd.args[cmd.index]); + + cli.process('\n', cmd, stream); + + auto result = stream.str(); + TEST_ASSERT_NOT_EQUAL(0, result.length()); + TEST_ASSERT_NOT_EQUAL(std::string::npos, result.find("# help")); + TEST_ASSERT_NOT_EQUAL(std::string::npos, result.find("available commands")); + TEST_ASSERT_NOT_EQUAL(std::string::npos, result.find("defaults")); + TEST_ASSERT_NOT_EQUAL(std::string::npos, result.find("reboot")); +} + +void test_cli_process_help_non_interactive() +{ + Model model; + Cli cli{model}; + CliCmd cmd; + + for (char c : std::string("\02help\n\03")) + { + cli.process(c, cmd, stream); + } + + auto result = stream.str(); + TEST_ASSERT_NOT_EQUAL(0, result.length()); + TEST_ASSERT_EQUAL(0x02, result[0]); + TEST_ASSERT_EQUAL(0x03, result[result.length() - 1]); + TEST_ASSERT_EQUAL(std::string::npos, result.find("# help")); + TEST_ASSERT_NOT_EQUAL(std::string::npos, result.find("available commands")); + TEST_ASSERT_NOT_EQUAL(std::string::npos, result.find("defaults")); + TEST_ASSERT_NOT_EQUAL(std::string::npos, result.find("reboot")); +} + +void test_cli_get_mixer_type() +{ + Model model; + model.config.mixer.type = 0; + Cli cli{model}; + CliCmd cmd; + + for (char c : std::string("get mixer_type\n")) + { + cli.process(c, cmd, stream); + } + + auto result = stream.str(); + TEST_ASSERT_NOT_EQUAL(0, result.length()); + TEST_ASSERT_NOT_EQUAL(std::string::npos, result.find("set mixer_type NONE")); +} + +void test_cli_set_mixer_type() +{ + Model model; + model.config.mixer.type = 0; + Cli cli{model}; + CliCmd cmd; + + for (char c : std::string("set mixer_type QUADX\n")) + { + cli.process(c, cmd, stream); + } + + TEST_ASSERT_EQUAL(Espfc::FC_MIXER_QUADX, model.config.mixer.type); +} + +void test_cli_bf_get_mag_calibration() +{ + Model model; + Cli cli{model}; + CliCmd cmd; + + for (char c : std::string("get mag_calibration\n")) + { + cli.process(c, cmd, stream); + } + + auto result = stream.str(); + TEST_ASSERT_NOT_EQUAL(0, result.length()); + TEST_ASSERT_NOT_EQUAL(std::string::npos, result.find("mag_calibration =")); +} + +void test_cli_bf_sensor_hardware() +{ + Model model; + Cli cli{model}; + CliCmd cmd; + + for (char c : std::string("sensor_hardware\n")) + { + cli.process(c, cmd, stream); + } + + auto result = stream.str(); + TEST_ASSERT_NOT_EQUAL(0, result.length()); + TEST_ASSERT_NOT_EQUAL(std::string::npos, result.find("gyro: NONE,")); + TEST_ASSERT_NOT_EQUAL(std::string::npos, result.find("acc: NONE,")); + TEST_ASSERT_NOT_EQUAL(std::string::npos, result.find("baro: AUTO,")); + TEST_ASSERT_NOT_EQUAL(std::string::npos, result.find("mag: AUTO,")); +} + +int main(int argc, char** argv) +{ + UNITY_BEGIN(); + RUN_TEST(test_cli_init); + RUN_TEST(test_cli_enter_interactive); + RUN_TEST(test_cli_enter_leave_non_interactive); + RUN_TEST(test_cli_configurator_handshake); + RUN_TEST(test_cli_process_comment); + RUN_TEST(test_cli_overflow); + RUN_TEST(test_cli_process_help); + RUN_TEST(test_cli_process_help_non_interactive); + RUN_TEST(test_cli_get_mixer_type); + RUN_TEST(test_cli_set_mixer_type); + RUN_TEST(test_cli_bf_get_mag_calibration); + RUN_TEST(test_cli_bf_sensor_hardware); + return UNITY_END(); +} \ No newline at end of file diff --git a/test/test_input_crsf/test_input_crsf.cpp b/test/test_input_crsf/test_input_crsf.cpp index 4d46f71b..b93a5bf6 100644 --- a/test/test_input_crsf/test_input_crsf.cpp +++ b/test/test_input_crsf/test_input_crsf.cpp @@ -1,9 +1,10 @@ -#include -#include #include "Device/InputCRSF.h" #include "Device/InputIBUS.hpp" #include "msp/msp_protocol.h" +#include #include +#include +#include using namespace Espfc; using namespace Espfc::Device; @@ -15,21 +16,21 @@ void test_input_crsf_rc_valid() InputCRSF input; CrsfMessage frame; memset(&frame, 0, sizeof(frame)); - uint8_t * frame_data = reinterpret_cast(&frame); + uint8_t* frame_data = reinterpret_cast(&frame); When(Method(ArduinoFake(), micros)).Return(0); input.begin(nullptr, nullptr); - const uint8_t data[] = { - 0xC8, 0x18, 0x16, 0xE0, 0x03, 0xDF, 0xD9, 0xC0, 0xF7, 0x8B, 0x5F, 0x94, 0xAF, - 0x7C, 0xE5, 0x2B, 0x5F, 0xF9, 0xCA, 0x07, 0x00, 0x00, 0x4C, 0x7C, 0xE2, 0x23 - }; - for (size_t i = 0; i < sizeof(data); i++) { + const uint8_t data[] = {0xC8, 0x18, 0x16, 0xE0, 0x03, 0xDF, 0xD9, 0xC0, 0xF7, 0x8B, 0x5F, 0x94, 0xAF, + 0x7C, 0xE5, 0x2B, 0x5F, 0xF9, 0xCA, 0x07, 0x00, 0x00, 0x4C, 0x7C, 0xE2, 0x23}; + for (size_t i = 0; i < sizeof(data); i++) + { input.parse(frame, data[i]); } - for (size_t i = 0; i < sizeof(data); i++) { + for (size_t i = 0; i < sizeof(data); i++) + { TEST_ASSERT_EQUAL_UINT8(data[i], frame_data[i]); } @@ -54,20 +55,20 @@ void test_input_crsf_rc_valid_no_payload() InputCRSF input; CrsfMessage frame; memset(&frame, 0, sizeof(frame)); - uint8_t * frame_data = reinterpret_cast(&frame); + uint8_t* frame_data = reinterpret_cast(&frame); When(Method(ArduinoFake(), micros)).Return(0); input.begin(nullptr, nullptr); - const uint8_t data[] = { - 0xC8, 0x02, 0x16, 0xD3 - }; - for (size_t i = 0; i < sizeof(data); i++) { + const uint8_t data[] = {0xC8, 0x02, 0x16, 0xD3}; + for (size_t i = 0; i < sizeof(data); i++) + { input.parse(frame, data[i]); } - for (size_t i = 0; i < sizeof(data); i++) { + for (size_t i = 0; i < sizeof(data); i++) + { TEST_ASSERT_EQUAL_UINT8(data[i], frame_data[i]); } @@ -91,12 +92,10 @@ void test_input_crsf_rc_prefix() input.begin(nullptr, nullptr); // prefix with few random bytes - const uint8_t data[] = { - 0xA1, 0x04, 0xC5, 0x09, - 0xC8, 0x18, 0x16, 0xE0, 0x03, 0xDF, 0xD9, 0xC0, 0xF7, 0x8B, 0x5F, 0x94, 0xAF, - 0x7C, 0xE5, 0x2B, 0x5F, 0xF9, 0xCA, 0x07, 0x00, 0x00, 0x4C, 0x7C, 0xE2, 0x23 - }; - for (size_t i = 0; i < sizeof(data); i++) { + const uint8_t data[] = {0xA1, 0x04, 0xC5, 0x09, 0xC8, 0x18, 0x16, 0xE0, 0x03, 0xDF, 0xD9, 0xC0, 0xF7, 0x8B, 0x5F, + 0x94, 0xAF, 0x7C, 0xE5, 0x2B, 0x5F, 0xF9, 0xCA, 0x07, 0x00, 0x00, 0x4C, 0x7C, 0xE2, 0x23}; + for (size_t i = 0; i < sizeof(data); i++) + { input.parse(frame, data[i]); } @@ -136,12 +135,10 @@ void test_crsf_encode_rc() Crsf::encodeRcData(frame, data); - const uint8_t expected[] = { - 0xC8, 0x18, 0x16, 0xE0, 0x03, 0x1F, 0x2B, 0xC0, 0x07, 0x3E, 0xF0, 0x81, 0x0F, - 0x7C, 0xE0, 0x03, 0x1F, 0xF8, 0xC0, 0x07, 0x3E, 0xF0, 0x81, 0x0F, 0x7C, 0xDB - }; + const uint8_t expected[] = {0xC8, 0x18, 0x16, 0xE0, 0x03, 0x1F, 0x2B, 0xC0, 0x07, 0x3E, 0xF0, 0x81, 0x0F, + 0x7C, 0xE0, 0x03, 0x1F, 0xF8, 0xC0, 0x07, 0x3E, 0xF0, 0x81, 0x0F, 0x7C, 0xDB}; - uint8_t * frame_data = reinterpret_cast(&frame); + uint8_t* frame_data = reinterpret_cast(&frame); TEST_ASSERT_EQUAL_UINT8(expected[0], frame_data[0]); // addr TEST_ASSERT_EQUAL_UINT8(expected[1], frame_data[1]); // size @@ -203,13 +200,13 @@ void test_crsf_decode_rc_struct() Crsf::encodeRcData(frame, data); - uint16_t channels[16] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; + uint16_t channels[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; Crsf::decodeRcData(channels, (const CrsfData*)frame.payload); TEST_ASSERT_EQUAL_UINT16(1500, channels[0]); TEST_ASSERT_EQUAL_UINT16(1500, channels[1]); - TEST_ASSERT_EQUAL_UINT16( 988, channels[2]); + TEST_ASSERT_EQUAL_UINT16(988, channels[2]); TEST_ASSERT_EQUAL_UINT16(1500, channels[3]); TEST_ASSERT_EQUAL_UINT16(1500, channels[4]); @@ -253,13 +250,13 @@ void test_crsf_decode_rc_shift8() Crsf::encodeRcData(frame, data); - uint16_t channels[16] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; + uint16_t channels[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; Crsf::decodeRcDataShift8(channels, (const CrsfData*)frame.payload); TEST_ASSERT_EQUAL_UINT16(1500, channels[0]); TEST_ASSERT_EQUAL_UINT16(1500, channels[1]); - TEST_ASSERT_EQUAL_UINT16( 988, channels[2]); + TEST_ASSERT_EQUAL_UINT16(988, channels[2]); TEST_ASSERT_EQUAL_UINT16(1500, channels[3]); TEST_ASSERT_EQUAL_UINT16(1500, channels[4]); @@ -337,9 +334,9 @@ void test_crsf_encode_tlm() frame.writeU8(0x01); frame.finalize(); - TEST_ASSERT_EQUAL_UINT8(0xC8, frame.addr); // addr - TEST_ASSERT_EQUAL_UINT8(0x03, frame.size); // size - TEST_ASSERT_EQUAL_UINT8(0x0B, frame.type); // type: heartbeat + TEST_ASSERT_EQUAL_UINT8(0xC8, frame.addr); // addr + TEST_ASSERT_EQUAL_UINT8(0x03, frame.size); // size + TEST_ASSERT_EQUAL_UINT8(0x0B, frame.type); // type: heartbeat TEST_ASSERT_EQUAL_UINT8(0x01, frame.payload[0]); // payload TEST_ASSERT_EQUAL_UINT8(0x90, frame.payload[1]); // crc @@ -401,7 +398,7 @@ void test_crsf_encode_msp_v1_fragmented() resp.version = Connect::MSP_V1; resp.cmd = MSP_API_VERSION; resp.result = 0; - for(size_t i = 0; i < 64; i++) + for (size_t i = 0; i < 64; i++) { resp.writeU8(i); } @@ -427,7 +424,7 @@ void test_crsf_encode_msp_v1_fragmented() // ext msp v1 header TEST_ASSERT_EQUAL_UINT8(64, frame.payload[3]); // size - TEST_ASSERT_EQUAL_UINT8( 1, frame.payload[4]); // type // api_version(1) + TEST_ASSERT_EQUAL_UINT8(1, frame.payload[4]); // type // api_version(1) // ext msp payload TEST_ASSERT_EQUAL_UINT8(0, frame.payload[5]); // param0 @@ -499,8 +496,8 @@ void test_crsf_encode_msp_v2() TEST_ASSERT_EQUAL_UINT8(0, frame.payload[7]); // size (hi) // ext msp payload - TEST_ASSERT_EQUAL_UINT8(1, frame.payload[8]); // param1 - TEST_ASSERT_EQUAL_UINT8(2, frame.payload[9]); // param2 + TEST_ASSERT_EQUAL_UINT8(1, frame.payload[8]); // param1 + TEST_ASSERT_EQUAL_UINT8(2, frame.payload[9]); // param2 TEST_ASSERT_EQUAL_UINT8(3, frame.payload[10]); // param3 // crsf crc @@ -509,10 +506,8 @@ void test_crsf_encode_msp_v2() void test_crsf_decode_msp_v1() { - const uint8_t data[] = { - 0xc8, 0x08, 0x7a, 0xc8, 0xea, 0x32, 0x00, 0x70, 0x70, 0x4b - }; - CrsfMessage frame; + const uint8_t data[] = {0xc8, 0x08, 0x7a, 0xc8, 0xea, 0x32, 0x00, 0x70, 0x70, 0x4b}; + CrsfMessage frame; std::copy_n(data, sizeof(data), (uint8_t*)&frame); Connect::MspMessage m; @@ -563,7 +558,7 @@ void test_csrf_decode_msp_v1_fragmented() // we need only range from 3 to 37 const size_t dataLen = 34; - const uint8_t *dataPtr = buff + 3; // skip msp header ($M<) + const uint8_t* dataPtr = buff + 3; // skip msp header ($M<) const uint8_t flags1 = (1 << 5) | (1 << 4) | 0x00; // v(1) + start(1) + sequence(0) const uint8_t flags2 = (1 << 5) | (0 << 4) | 0x01; // v(1) + start(0) + sequence(1) const uint8_t dst = CRSF_ADDRESS_FLIGHT_CONTROLLER; @@ -628,7 +623,7 @@ void test_input_ibus_rc_valid() InputIBUS input; InputIBUS::IBusData frame; memset(&frame, 0, sizeof(frame)); - uint8_t * frame_data = reinterpret_cast(&frame); + uint8_t* frame_data = reinterpret_cast(&frame); When(Method(ArduinoFake(), micros)).Return(0); @@ -644,16 +639,16 @@ void test_input_ibus_rc_valid() // }; const uint8_t data[] = { - 0x20, 0x40, - 0xDB, 0x05, 0xDC, 0x05, 0x54, 0x05, 0xDC, 0x05, 0xE8, 0x03, 0xD0, 0x07, 0xD2, 0x05, 0xE8, 0x03, - 0xDC, 0x05, 0xDC, 0x05, 0xDC, 0x05, 0xDC, 0x05, 0xDC, 0x05, 0xDC, 0x05, - 0xDA, 0xF3, + 0x20, 0x40, 0xDB, 0x05, 0xDC, 0x05, 0x54, 0x05, 0xDC, 0x05, 0xE8, 0x03, 0xD0, 0x07, 0xD2, 0x05, + 0xE8, 0x03, 0xDC, 0x05, 0xDC, 0x05, 0xDC, 0x05, 0xDC, 0x05, 0xDC, 0x05, 0xDC, 0x05, 0xDA, 0xF3, }; - for (size_t i = 0; i < sizeof(data); i++) { + for (size_t i = 0; i < sizeof(data); i++) + { input.parse(frame, data[i]); } - for (size_t i = 0; i < sizeof(data); i++) { + for (size_t i = 0; i < sizeof(data); i++) + { TEST_ASSERT_EQUAL_HEX8(data[i], frame_data[i]); } @@ -693,7 +688,7 @@ void test_input_ibus_rc_valid() TEST_ASSERT_EQUAL_UINT16(1500, input.get(13)); } -int main(int argc, char **argv) +int main(int argc, char** argv) { UNITY_BEGIN(); RUN_TEST(test_input_crsf_rc_valid); @@ -702,7 +697,7 @@ int main(int argc, char **argv) RUN_TEST(test_crsf_encode_rc); RUN_TEST(test_crsf_decode_rc_struct); RUN_TEST(test_crsf_decode_rc_shift8); - //RUN_TEST(test_crsf_decode_rc_shift32); + // RUN_TEST(test_crsf_decode_rc_shift32); RUN_TEST(test_crsf_encode_tlm); RUN_TEST(test_crsf_encode_msp_v1); RUN_TEST(test_crsf_encode_msp_v1_fragmented); diff --git a/test/test_msp/test_msp.cpp b/test/test_msp/test_msp.cpp index f5f7d36f..285996ae 100644 --- a/test/test_msp/test_msp.cpp +++ b/test/test_msp/test_msp.cpp @@ -1,5 +1,4 @@ #include -#include #include #include #include @@ -9,7 +8,6 @@ #include "msp/msp_protocol.h" #include -using namespace fakeit; using namespace Espfc; using namespace Espfc::Connect;