From 5a2a0b87a5a496e87b432bbe04ca8ae3a1156db5 Mon Sep 17 00:00:00 2001 From: gismo2004 Date: Sun, 14 Sep 2025 16:57:22 +0200 Subject: [PATCH 01/48] [crsf] add temperature and RPM telemetry --- src/main/rx/crsf.h | 2 + src/main/telemetry/crsf.c | 97 +++++++++++++++++++++++++++++++++++++++ 2 files changed, 99 insertions(+) diff --git a/src/main/rx/crsf.h b/src/main/rx/crsf.h index 8d84d52b8e5..02ca7ddac42 100755 --- a/src/main/rx/crsf.h +++ b/src/main/rx/crsf.h @@ -88,6 +88,8 @@ typedef enum { CRSF_FRAMETYPE_VARIO_SENSOR = 0x07, CRSF_FRAMETYPE_BATTERY_SENSOR = 0x08, CRSF_FRAMETYPE_BAROMETER_ALTITUDE = 0x09, + CRSF_FRAMETYPE_RPM = 0x0C, + CRSF_FRAMETYPE_TEMP = 0x0D, CRSF_FRAMETYPE_LINK_STATISTICS = 0x14, CRSF_FRAMETYPE_RC_CHANNELS_PACKED = 0x16, CRSF_FRAMETYPE_ATTITUDE = 0x1E, diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index d86822ada7a..c6ef8a53a35 100755 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -46,6 +46,7 @@ #include "fc/runtime_config.h" #include "flight/imu.h" +#include "flight/mixer.h" #include "io/gps.h" #include "io/serial.h" @@ -55,8 +56,11 @@ #include "rx/crsf.h" #include "rx/rx.h" + #include "sensors/battery.h" +#include "sensors/esc_sensor.h" #include "sensors/sensors.h" +#include "sensors/temperature.h" #include "telemetry/crsf.h" #include "telemetry/telemetry.h" @@ -162,6 +166,14 @@ static void crsfSerialize16(sbuf_t *dst, uint16_t v) crsfSerialize8(dst, (uint8_t)v); } +static void crsfSerialize24(sbuf_t *dst, uint32_t v) +{ + // Use BigEndian format + crsfSerialize8(dst, (v >> 16)); + crsfSerialize8(dst, (v >> 8)); + crsfSerialize8(dst, (uint8_t)v); +} + static void crsfSerialize32(sbuf_t *dst, uint32_t v) { // Use BigEndian format @@ -296,6 +308,69 @@ static void crsfBarometerAltitude(sbuf_t *dst) crsfSerialize16(dst, altitude_packed); } +/* +0x0C RPM +Payload: +uint8_t rpm_source_id; // Identifies the source of the RPM data (e.g., 0 = Motor 1, 1 = Motor 2, etc.) +int24_t rpm_value[]; // 1 - 19 RPM values with negative ones representing the motor spinning in reverse +*/ +static void crsfRpm(sbuf_t *dst) +{ + uint8_t motorCount = getMotorCount(); + + if (STATE(ESC_SENSOR_ENABLED) && motorCount > 0) { + sbufWriteU8(dst, 1 + (motorCount * 3) + CRSF_FRAME_LENGTH_TYPE_CRC); + crsfSerialize8(dst, CRSF_FRAMETYPE_RPM); + // 0 = FC including all ESCs + crsfSerialize8(dst, 0); + + for (uint8_t i = 0; i < motorCount; i++) { + const escSensorData_t *escState = getEscTelemetry(i); + crsfSerialize24(dst, (escState) ? escState->rpm : 0); + } + } +} + +/* +0x0D TEMP +Payload: +uint8_t temp_source_id; // Identifies the source of the temperature data (e.g., 0 = FC including all ESCs, 1 = Ambient, etc.) +int16_t temperature[]; // up to 20 temperature values in deci-degree (tenths of a degree) Celsius (e.g., 250 = 25.0°C, -50 = -5.0°C) +*/ +static void crsfTemperature(sbuf_t *dst) +{ + + uint8_t tempCount = 0; + int16_t temperatures[20]; + +#ifdef USE_ESC_SENSOR + uint8_t motorCount = getMotorCount(); + if (STATE(ESC_SENSOR_ENABLED) && motorCount > 0) { + for (uint8_t i = 0; i < motorCount; i++) { + const escSensorData_t *escState = getEscTelemetry(i); + temperatures[tempCount++] = (escState) ? escState->temperature * 10 : TEMPERATURE_INVALID_VALUE; + } + } +#endif + +#ifdef USE_TEMPERATURE_SENSOR + for (uint8_t i = 0; i < MAX_TEMP_SENSORS; i++) { + int16_t value; + if (getSensorTemperature(i, &value)) + temperatures[tempCount++] = value; + } +#endif + + if (tempCount > 0) { + sbufWriteU8(dst, 1 + (tempCount * 2) + CRSF_FRAME_LENGTH_TYPE_CRC); + crsfSerialize8(dst, CRSF_FRAMETYPE_TEMP); + // 0 = FC including all ESCs + crsfSerialize8(dst, 0); + for (uint8_t i = 0; i < tempCount; i++) + crsfSerialize16(dst, temperatures[i]); + } +} + typedef enum { CRSF_ACTIVE_ANTENNA1 = 0, CRSF_ACTIVE_ANTENNA2 = 1 @@ -465,6 +540,8 @@ typedef enum { CRSF_FRAME_GPS_INDEX, CRSF_FRAME_VARIO_SENSOR_INDEX, CRSF_FRAME_BAROMETER_ALTITUDE_INDEX, + CRSF_FRAME_TEMP_INDEX, + CRSF_FRAME_RPM_INDEX, CRSF_SCHEDULE_COUNT_MAX } crsfFrameTypeIndex_e; @@ -526,6 +603,20 @@ static void processCrsf(void) crsfFrameFlightMode(dst); crsfFinalize(dst); } +#ifdef USE_ESC_SENSOR + if (currentSchedule & BV(CRSF_FRAME_RPM_INDEX)) { + crsfInitializeFrame(dst); + crsfRpm(dst); + crsfFinalize(dst); + } +#endif +#if defined(USE_ESC_SENSOR) || defined(USE_TEMPERATURE_SENSOR) + if (currentSchedule & BV(CRSF_FRAME_TEMP_INDEX)) { + crsfInitializeFrame(dst); + crsfTemperature(dst); + crsfFinalize(dst); + } +#endif #ifdef USE_GPS if (currentSchedule & BV(CRSF_FRAME_GPS_INDEX)) { crsfInitializeFrame(dst); @@ -582,6 +673,12 @@ void initCrsfTelemetry(void) if (sensors(SENSOR_BARO)) { crsfSchedule[index++] = BV(CRSF_FRAME_BAROMETER_ALTITUDE_INDEX); } +#endif +#ifdef USE_ESC_SENSOR + crsfSchedule[index++] = BV(CRSF_FRAME_RPM_INDEX); +#endif +#if defined(USE_ESC_SENSOR) || defined(USE_TEMPERATURE_SENSOR) + crsfSchedule[index++] = BV(CRSF_FRAME_TEMP_INDEX); #endif crsfScheduleCount = (uint8_t)index; } From 710b36e46d56b827456ac80caa5a658d60e4ad55 Mon Sep 17 00:00:00 2001 From: gismo2004 Date: Sun, 14 Sep 2025 17:29:00 +0200 Subject: [PATCH 02/48] make the compiler happy --- src/main/telemetry/crsf.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index c6ef8a53a35..afbd1438cb6 100755 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -308,6 +308,7 @@ static void crsfBarometerAltitude(sbuf_t *dst) crsfSerialize16(dst, altitude_packed); } +#ifdef USE_ESC_SENSOR /* 0x0C RPM Payload: @@ -330,6 +331,7 @@ static void crsfRpm(sbuf_t *dst) } } } +#endif /* 0x0D TEMP From b39cc96f274fb3e554c2cbe197cd64a3d7f6c1cb Mon Sep 17 00:00:00 2001 From: gismo2004 Date: Mon, 15 Sep 2025 06:44:52 +0200 Subject: [PATCH 03/48] more compiler warnings --- src/main/telemetry/crsf.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index afbd1438cb6..33cd8253f27 100755 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -56,7 +56,6 @@ #include "rx/crsf.h" #include "rx/rx.h" - #include "sensors/battery.h" #include "sensors/esc_sensor.h" #include "sensors/sensors.h" @@ -166,6 +165,7 @@ static void crsfSerialize16(sbuf_t *dst, uint16_t v) crsfSerialize8(dst, (uint8_t)v); } +#ifdef USE_ESC_SENSOR static void crsfSerialize24(sbuf_t *dst, uint32_t v) { // Use BigEndian format @@ -173,6 +173,7 @@ static void crsfSerialize24(sbuf_t *dst, uint32_t v) crsfSerialize8(dst, (v >> 8)); crsfSerialize8(dst, (uint8_t)v); } +#endif static void crsfSerialize32(sbuf_t *dst, uint32_t v) { From f8dc0bce93c8f4bebf92656942d1354ddef96331 Mon Sep 17 00:00:00 2001 From: gismo2004 Date: Tue, 23 Sep 2025 12:06:05 +0200 Subject: [PATCH 04/48] add AirSpeed --- src/main/rx/crsf.h | 2 ++ src/main/telemetry/crsf.c | 33 +++++++++++++++++++++++++++++++-- 2 files changed, 33 insertions(+), 2 deletions(-) diff --git a/src/main/rx/crsf.h b/src/main/rx/crsf.h index 02ca7ddac42..d5c0219f106 100755 --- a/src/main/rx/crsf.h +++ b/src/main/rx/crsf.h @@ -46,6 +46,7 @@ enum { CRSF_FRAME_VARIO_SENSOR_PAYLOAD_SIZE = 2, CRSF_FRAME_BATTERY_SENSOR_PAYLOAD_SIZE = 8, CRSF_FRAME_BAROMETER_ALTITUDE_PAYLOAD_SIZE = 2, + CRSF_FRAME_AIRSPEED_PAYLOAD_SIZE = 2, CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE = 10, CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE = 22, // 11 bits per channel * 16 channels = 22 bytes. CRSF_FRAME_ATTITUDE_PAYLOAD_SIZE = 6, @@ -88,6 +89,7 @@ typedef enum { CRSF_FRAMETYPE_VARIO_SENSOR = 0x07, CRSF_FRAMETYPE_BATTERY_SENSOR = 0x08, CRSF_FRAMETYPE_BAROMETER_ALTITUDE = 0x09, + CRSF_FRAMETYPE_AIRSPEED_SENSOR = 0x0A, CRSF_FRAMETYPE_RPM = 0x0C, CRSF_FRAMETYPE_TEMP = 0x0D, CRSF_FRAMETYPE_LINK_STATISTICS = 0x14, diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index 33cd8253f27..3007742df5c 100755 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -58,6 +58,7 @@ #include "sensors/battery.h" #include "sensors/esc_sensor.h" +#include "sensors/pitotmeter.h" #include "sensors/sensors.h" #include "sensors/temperature.h" @@ -309,6 +310,21 @@ static void crsfBarometerAltitude(sbuf_t *dst) crsfSerialize16(dst, altitude_packed); } +#ifdef USE_PITOT +/* +0x0A Airspeed sensor +Payload: +int16 Air speed ( dm/s ) +*/ +static void crsfFrameAirSpeedSensor(sbuf_t *dst) +{ + // use sbufWrite since CRC does not include frame length + sbufWriteU8(dst, CRSF_FRAME_AIRSPEED_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC); + crsfSerialize8(dst, CRSF_FRAMETYPE_AIRSPEED_SENSOR); + crsfSerialize16(dst, (uint16_t)(getAirspeedEstimate() * 36 / 100)); +} +#endif + #ifdef USE_ESC_SENSOR /* 0x0C RPM @@ -545,11 +561,12 @@ typedef enum { CRSF_FRAME_BAROMETER_ALTITUDE_INDEX, CRSF_FRAME_TEMP_INDEX, CRSF_FRAME_RPM_INDEX, + CRSF_FRAME_AIRSPEED_INDEX, CRSF_SCHEDULE_COUNT_MAX } crsfFrameTypeIndex_e; static uint8_t crsfScheduleCount; -static uint8_t crsfSchedule[CRSF_SCHEDULE_COUNT_MAX]; +static uint16_t crsfSchedule[CRSF_SCHEDULE_COUNT_MAX]; #if defined(USE_MSP_OVER_TELEMETRY) @@ -586,7 +603,7 @@ static void processCrsf(void) } static uint8_t crsfScheduleIndex = 0; - const uint8_t currentSchedule = crsfSchedule[crsfScheduleIndex]; + const uint16_t currentSchedule = crsfSchedule[crsfScheduleIndex]; sbuf_t crsfPayloadBuf; sbuf_t *dst = &crsfPayloadBuf; @@ -638,6 +655,13 @@ static void processCrsf(void) crsfBarometerAltitude(dst); crsfFinalize(dst); } +#endif +#ifdef USE_PITOT + if (currentSchedule & BV(CRSF_FRAME_AIRSPEED_INDEX)) { + crsfInitializeFrame(dst); + crsfFrameAirSpeedSensor(dst); + crsfFinalize(dst); + } #endif crsfScheduleIndex = (crsfScheduleIndex + 1) % crsfScheduleCount; } @@ -682,6 +706,11 @@ void initCrsfTelemetry(void) #endif #if defined(USE_ESC_SENSOR) || defined(USE_TEMPERATURE_SENSOR) crsfSchedule[index++] = BV(CRSF_FRAME_TEMP_INDEX); +#endif +#ifdef USE_PITOT + if (sensors(SENSOR_PITOT)) { + crsfSchedule[index++] = BV(CRSF_FRAME_AIRSPEED_INDEX); + } #endif crsfScheduleCount = (uint8_t)index; } From e5bfe799c3d56fc95a7573b27bfec77ca8044249 Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Thu, 18 Dec 2025 12:14:41 -0600 Subject: [PATCH 05/48] Fix CRSF telemetry corruption from PR #11025 MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit This commit fixes the critical bugs that caused PR #11025 to be reverted (PR #11139). The original implementation caused telemetry stream corruption by emitting malformed frames when sensors were unavailable. Root Cause: The original PR scheduled telemetry frames unconditionally (if feature compiled in) but frame functions only wrote data when sensors were available. This resulted in frames containing only [SYNC][CRC] instead of the proper [SYNC][LENGTH][TYPE][PAYLOAD][CRC] structure, corrupting the CRSF protocol stream and breaking ALL telemetry (not just new frames). Fixes Implemented: 1. RPM Frame (Bug #1 - CRITICAL): - Added conditional scheduling: only schedule if ESC_SENSOR_ENABLED and motorCount > 0 - Added protocol limit enforcement: max 19 RPM values per CRSF spec - Location: src/main/telemetry/crsf.c:705-707, 337-343 2. Temperature Frame (Bug #2 - CRITICAL): - Added conditional scheduling: only schedule if temperature sources are actually available (ESC sensors OR temperature sensors) - Added bounds checking: prevent buffer overflow beyond 20 temperatures - Location: src/main/telemetry/crsf.c:709-732, 361-381 3. Buffer Overflow Protection (Bug #4): - Added MAX_CRSF_TEMPS constant and bounds checks in loops - Prevents array overflow if >20 temperature sources configured - Location: src/main/telemetry/crsf.c:361, 368, 376 4. Protocol Limit Enforcement (Bug #5): - Added MAX_CRSF_RPM_VALUES constant (19 per CRSF spec) - Clamp motorCount to protocol limit before sending - Location: src/main/telemetry/crsf.c:337, 342-344 Implementation Pattern: Follows the GPS frame pattern: ✓ Conditional scheduling - only schedule if sensor available ✓ Unconditional writing - always write complete frame data when called Testing: - Code compiles successfully (verified with SITL build) - No syntax errors or warnings - All fixes follow existing code patterns in crsf.c Related Issues: - Original PR: #11025 (merged Nov 28, 2025, reverted same day) - Revert PR: #11139 - Investigation: claude/developer/sent/pr11025-root-cause-analysis.md Co-Authored-By: Claude Sonnet 4.5 --- src/main/telemetry/crsf.c | 39 ++++++++++++++++++++++++++++++++++----- 1 file changed, 34 insertions(+), 5 deletions(-) diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index 3007742df5c..159a8c16ce2 100755 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -334,9 +334,15 @@ int24_t rpm_value[]; // 1 - 19 RPM values with negative ones representing */ static void crsfRpm(sbuf_t *dst) { + const uint8_t MAX_CRSF_RPM_VALUES = 19; // CRSF protocol limit: 1-19 RPM values uint8_t motorCount = getMotorCount(); if (STATE(ESC_SENSOR_ENABLED) && motorCount > 0) { + // Enforce protocol limit + if (motorCount > MAX_CRSF_RPM_VALUES) { + motorCount = MAX_CRSF_RPM_VALUES; + } + sbufWriteU8(dst, 1 + (motorCount * 3) + CRSF_FRAME_LENGTH_TYPE_CRC); crsfSerialize8(dst, CRSF_FRAMETYPE_RPM); // 0 = FC including all ESCs @@ -358,14 +364,14 @@ int16_t temperature[]; // up to 20 temperature values in deci-degree (tenths of */ static void crsfTemperature(sbuf_t *dst) { - + const uint8_t MAX_CRSF_TEMPS = 20; // Maximum temperatures per CRSF frame uint8_t tempCount = 0; int16_t temperatures[20]; #ifdef USE_ESC_SENSOR uint8_t motorCount = getMotorCount(); if (STATE(ESC_SENSOR_ENABLED) && motorCount > 0) { - for (uint8_t i = 0; i < motorCount; i++) { + for (uint8_t i = 0; i < motorCount && tempCount < MAX_CRSF_TEMPS; i++) { const escSensorData_t *escState = getEscTelemetry(i); temperatures[tempCount++] = (escState) ? escState->temperature * 10 : TEMPERATURE_INVALID_VALUE; } @@ -373,7 +379,7 @@ static void crsfTemperature(sbuf_t *dst) #endif #ifdef USE_TEMPERATURE_SENSOR - for (uint8_t i = 0; i < MAX_TEMP_SENSORS; i++) { + for (uint8_t i = 0; i < MAX_TEMP_SENSORS && tempCount < MAX_CRSF_TEMPS; i++) { int16_t value; if (getSensorTemperature(i, &value)) temperatures[tempCount++] = value; @@ -702,10 +708,33 @@ void initCrsfTelemetry(void) } #endif #ifdef USE_ESC_SENSOR - crsfSchedule[index++] = BV(CRSF_FRAME_RPM_INDEX); + if (STATE(ESC_SENSOR_ENABLED) && getMotorCount() > 0) { + crsfSchedule[index++] = BV(CRSF_FRAME_RPM_INDEX); + } #endif #if defined(USE_ESC_SENSOR) || defined(USE_TEMPERATURE_SENSOR) - crsfSchedule[index++] = BV(CRSF_FRAME_TEMP_INDEX); + // Only schedule temperature frame if we have temperature sources available + bool hasTemperatureSources = false; +#ifdef USE_ESC_SENSOR + if (STATE(ESC_SENSOR_ENABLED) && getMotorCount() > 0) { + hasTemperatureSources = true; + } +#endif +#ifdef USE_TEMPERATURE_SENSOR + if (!hasTemperatureSources) { + // Check if any temperature sensors are configured + for (uint8_t i = 0; i < MAX_TEMP_SENSORS; i++) { + int16_t value; + if (getSensorTemperature(i, &value)) { + hasTemperatureSources = true; + break; + } + } + } +#endif + if (hasTemperatureSources) { + crsfSchedule[index++] = BV(CRSF_FRAME_TEMP_INDEX); + } #endif #ifdef USE_PITOT if (sensors(SENSOR_PITOT)) { From 7b391b3ba1e910f2d54d74dac108d1c331def56b Mon Sep 17 00:00:00 2001 From: meng9633 <3330224355@qq.com> Date: Mon, 5 Jan 2026 14:34:41 +0800 Subject: [PATCH 06/48] FIX AT32 LED blink BUG --- src/main/drivers/io.c | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/main/drivers/io.c b/src/main/drivers/io.c index b13b73cbfd2..c602a2df492 100644 --- a/src/main/drivers/io.c +++ b/src/main/drivers/io.c @@ -244,10 +244,11 @@ void IOToggle(IO_t io) IO_GPIO(io)->BSRRL = mask; } #elif defined(AT32F43x) - if (IO_GPIO(io)->odt & mask) - mask <<= 16; // bit is set, shift mask to reset half - - IO_GPIO(io)->scr = IO_Pin(io); + if (IO_GPIO(io)->odt & mask) { + IO_GPIO(io)->clr = mask; + } else { + IO_GPIO(io)->scr = mask; + } #else if (IO_GPIO(io)->ODR & mask) mask <<= 16; // bit is set, shift mask to reset half From 380fccff2a34919e5c5ac6cd902c3ddadab10b3a Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Sun, 11 Jan 2026 14:36:14 -0600 Subject: [PATCH 07/48] Fix SPI register masking in busWriteBuf Correct the SPI register address masking in busWriteBuf() to clear the MSB instead of setting it. For SPI protocol, the MSB indicates read (1) or write (0) operations. Write operations should use (reg & 0x7F) to clear the MSB, not (reg | 0x80) which sets it. This matches the correct implementation in busWrite() at line 318 which uses (reg & 0x7F) for write operations. Note: This bug affects theoretical future SPI devices using buffer writes. Current devices using busWriteBuf() are all I2C-based (VL53L0X, VL53L1X, MLX90393, TERARANGER_EVO, US42 rangefinders) and are unaffected. Fixes #10674 --- src/main/drivers/bus.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/drivers/bus.c b/src/main/drivers/bus.c index 0da3f76d307..71410fb2748 100644 --- a/src/main/drivers/bus.c +++ b/src/main/drivers/bus.c @@ -283,7 +283,7 @@ bool busWriteBuf(const busDevice_t * dev, uint8_t reg, const uint8_t * data, uin return spiBusWriteBuffer(dev, reg, data, length); } else { - return spiBusWriteBuffer(dev, reg | 0x80, data, length); + return spiBusWriteBuffer(dev, reg & 0x7F, data, length); } #else return false; From bd45290a09a729d98bc0f32916e62dcecac585f9 Mon Sep 17 00:00:00 2001 From: Radek Domin Date: Fri, 16 Jan 2026 12:35:57 +0100 Subject: [PATCH 08/48] Adding support for switchable 9v BEC for FLYWOOF405HD and FLYWOOF405PRO targets. --- src/main/target/FLYWOOF405PRO/config.c | 4 ++++ src/main/target/FLYWOOF405PRO/target.h | 6 ++++++ 2 files changed, 10 insertions(+) diff --git a/src/main/target/FLYWOOF405PRO/config.c b/src/main/target/FLYWOOF405PRO/config.c index dfe6c0ce100..6366994c25c 100644 --- a/src/main/target/FLYWOOF405PRO/config.c +++ b/src/main/target/FLYWOOF405PRO/config.c @@ -31,4 +31,8 @@ void targetConfiguration(void) timerOverridesMutable(timer2id(TIM3))->outputMode = OUTPUT_MODE_MOTORS; timerOverridesMutable(timer2id(TIM2))->outputMode = OUTPUT_MODE_MOTORS; + + // Pinio Box params as per manufacturer specification + pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1; + pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER2; } diff --git a/src/main/target/FLYWOOF405PRO/target.h b/src/main/target/FLYWOOF405PRO/target.h index a5a02c5fe84..22ddd0a6fd2 100644 --- a/src/main/target/FLYWOOF405PRO/target.h +++ b/src/main/target/FLYWOOF405PRO/target.h @@ -152,6 +152,12 @@ #define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 #define RSSI_ADC_CHANNEL ADC_CHN_3 +// *************** PINIO *********************** +#define USE_PINIO +#define USE_PINIOBOX +#define PINIO1_PIN PD6 +#define PINIO2_PIN PC8 + // *************** LED2812 ************************ #define USE_LED_STRIP #define WS2811_PIN PA9 From 0f45b6b2f061d4e352d46c45d1ab5eb936e9453b Mon Sep 17 00:00:00 2001 From: meng9633 <3330224355@qq.com> Date: Fri, 23 Jan 2026 17:21:39 +0800 Subject: [PATCH 09/48] Add target BLUEBERRYH743 --- src/main/target/BLUEBERRYH743/CMakeLists.txt | 2 + src/main/target/BLUEBERRYH743/config.c | 34 +++ src/main/target/BLUEBERRYH743/target.c | 60 ++++++ src/main/target/BLUEBERRYH743/target.h | 213 +++++++++++++++++++ 4 files changed, 309 insertions(+) create mode 100644 src/main/target/BLUEBERRYH743/CMakeLists.txt create mode 100644 src/main/target/BLUEBERRYH743/config.c create mode 100644 src/main/target/BLUEBERRYH743/target.c create mode 100644 src/main/target/BLUEBERRYH743/target.h diff --git a/src/main/target/BLUEBERRYH743/CMakeLists.txt b/src/main/target/BLUEBERRYH743/CMakeLists.txt new file mode 100644 index 00000000000..99d8aeeda11 --- /dev/null +++ b/src/main/target/BLUEBERRYH743/CMakeLists.txt @@ -0,0 +1,2 @@ +target_stm32h743xi(BLUEBERRYH743) +target_stm32h743xi(BLUEBERRYH743HD) diff --git a/src/main/target/BLUEBERRYH743/config.c b/src/main/target/BLUEBERRYH743/config.c new file mode 100644 index 00000000000..bcc1e5c91fd --- /dev/null +++ b/src/main/target/BLUEBERRYH743/config.c @@ -0,0 +1,34 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include + +#include "platform.h" + +#include "fc/fc_msp_box.h" +#include "fc/config.h" +#include "io/serial.h" +#include "io/piniobox.h" + +void targetConfiguration(void) +{ + pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1; + pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER2; + beeperConfigMutable()->pwmMode = true; + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART1)].functionMask = FUNCTION_MSP; + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART1)].msp_baudrateIndex = BAUD_115200; +} diff --git a/src/main/target/BLUEBERRYH743/target.c b/src/main/target/BLUEBERRYH743/target.c new file mode 100644 index 00000000000..bc2fb01558a --- /dev/null +++ b/src/main/target/BLUEBERRYH743/target.c @@ -0,0 +1,60 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#include + +#include "platform.h" + +#include "drivers/bus.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" +#include "drivers/pinio.h" +#include "drivers/sensor.h" + +BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000, DEVHW_MPU6000, MPU6000_SPI_BUS, MPU6000_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_MPU6000_ALIGN); +BUSDEV_REGISTER_SPI_TAG(busdev_icm42688, DEVHW_ICM42605, MPU6000_SPI_BUS, MPU6000_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_MPU6000_ALIGN); +BUSDEV_REGISTER_SPI_TAG(busdev_icm20602, DEVHW_MPU6500, MPU6500_SPI_BUS, MPU6500_CS_PIN, NONE, 1, DEVFLAGS_NONE, IMU_MPU6500_ALIGN); +BUSDEV_REGISTER_SPI_TAG(busdev_icm42605, DEVHW_ICM42605, ICM42605_SPI_BUS, ICM42605_CS_PIN, NONE, 2, DEVFLAGS_NONE, IMU_ICM42605_ALIGN); + +timerHardware_t timerHardware[] = { + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 1), // S2 + + DEF_TIM(TIM5, CH1, PA0, TIM_USE_OUTPUT_AUTO, 0, 2), // S3 + DEF_TIM(TIM5, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 3), // S4 + DEF_TIM(TIM5, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 4), // S5 + DEF_TIM(TIM5, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 5), // S6 + + DEF_TIM(TIM4, CH1, PD12, TIM_USE_OUTPUT_AUTO, 0, 6), // S7 + DEF_TIM(TIM4, CH2, PD13, TIM_USE_OUTPUT_AUTO, 0, 7), // S8 + DEF_TIM(TIM4, CH3, PD14, TIM_USE_OUTPUT_AUTO, 0, 0), // S9 + DEF_TIM(TIM4, CH4, PD15, TIM_USE_OUTPUT_AUTO, 0, 0), // S10 DMA_NONE + + DEF_TIM(TIM15, CH1, PE5, TIM_USE_OUTPUT_AUTO, 0, 0), // S11 + DEF_TIM(TIM15, CH2, PE6, TIM_USE_OUTPUT_AUTO, 0, 0), // S12 DMA_NONE + + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 9), // LED_2812 + DEF_TIM(TIM2, CH1, PA15, TIM_USE_BEEPER, 0, 0), // BEEPER PWM + + // DEF_TIM(TIM8, CH2, PC7, TIM_USE_PPM, 0, 0), // RX6 PPM + DEF_TIM(TIM8, CH1, PC6, TIM_USE_ANY, 0, 0), // TX6 SoftwareSerial + // DEF_TIM(TIM16, CH1, PB8, TIM_USE_ANY, 0, 0), // RX4 + // DEF_TIM(TIM17, CH1, PB9, TIM_USE_ANY, 0, 0), // TX4 +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/BLUEBERRYH743/target.h b/src/main/target/BLUEBERRYH743/target.h new file mode 100644 index 00000000000..f11e822d633 --- /dev/null +++ b/src/main/target/BLUEBERRYH743/target.h @@ -0,0 +1,213 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "H743" + +#if defined(BLUEBERRYH743HD) + #define USBD_PRODUCT_STRING "BLUEBERRYH743HD" +#else + #define USBD_PRODUCT_STRING "BLUEBERRYH743" +#endif + +#define USE_TARGET_CONFIG + +#define LED0 PE3 +#define LED1 PE4 + +#define BEEPER PA15 +#define BEEPER_INVERTED +#define BEEPER_PWM_FREQUENCY 2500 + +// *************** IMU generic *********************** +#define USE_DUAL_GYRO +#define USE_TARGET_IMU_HARDWARE_DESCRIPTORS + +// *************** SPI1 IMU0 MPU6000 **************** +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PD7 + +#define USE_IMU_MPU6000 + +#define IMU_MPU6000_ALIGN CW0_DEG_FLIP +#define MPU6000_SPI_BUS BUS_SPI1 +#define MPU6000_CS_PIN PC15 + +// *************** SPI4 IMU1 ICM20602 ************** +#define USE_SPI_DEVICE_4 +#define SPI4_SCK_PIN PE12 +#define SPI4_MISO_PIN PE13 +#define SPI4_MOSI_PIN PE14 + +#define USE_IMU_MPU6500 + +#define IMU_MPU6500_ALIGN CW0_DEG_FLIP +#define MPU6500_SPI_BUS BUS_SPI4 +#define MPU6500_CS_PIN PE11 + +// *************** SPI4 IMU2 ICM42605 ************** +#define USE_IMU_ICM42605 + +#define IMU_ICM42605_ALIGN CW90_DEG_FLIP +#define ICM42605_SPI_BUS BUS_SPI4 +#define ICM42605_CS_PIN PC13 + +// *************** SPI2 OSD *********************** + #define USE_SPI_DEVICE_2 + #define SPI2_SCK_PIN PB13 + #define SPI2_MISO_PIN PB14 + #define SPI2_MOSI_PIN PB15 + +#if defined(BLUEBERRYH743) + #define USE_MAX7456 + #define MAX7456_SPI_BUS BUS_SPI2 + #define MAX7456_CS_PIN PB12 +#endif + +// *************** SPI3 SPARE for external RM3100 *********** +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PB3 +#define SPI3_MISO_PIN PB4 +#define SPI3_MOSI_PIN PB5 + +#define USE_MAG_RM3100 +#define RM3100_CS_PIN PE2 //CS2 pad +// PD4 //CS1 pad +#define RM3100_SPI_BUS BUS_SPI3 + +// *************** I2C /Baro/Mag ********************* +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB6 +#define I2C1_SDA PB7 + +#define USE_I2C_DEVICE_2 +#define I2C2_SCL PB10 +#define I2C2_SDA PB11 + +#define USE_BARO +#define BARO_I2C_BUS BUS_I2C2 +#define USE_BARO_BMP280 +#define USE_BARO_MS5611 +#define USE_BARO_DPS310 +#define USE_BARO_SPL06 + +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C1 +#define USE_MAG_ALL + +#define TEMPERATURE_I2C_BUS BUS_I2C2 +#define PITOT_I2C_BUS BUS_I2C2 + +#define USE_RANGEFINDER +#define RANGEFINDER_I2C_BUS BUS_I2C1 + +// *************** UART ***************************** +#define USE_VCP + +#define USE_UART1 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 + +#define USE_UART2 +#define UART2_TX_PIN PD5 +#define UART2_RX_PIN PD6 + +#define USE_UART3 +#define UART3_TX_PIN PD8 +#define UART3_RX_PIN PD9 + +#define USE_UART4 +#define UART4_TX_PIN PB9 +#define UART4_RX_PIN PB8 + +#define USE_UART6 +#define UART6_TX_PIN PC6 +#define UART6_RX_PIN PC7 + +#define USE_UART7 +#define UART7_TX_PIN PE8 +#define UART7_RX_PIN PE7 + +#define USE_UART8 +#define UART8_TX_PIN PE1 +#define UART8_RX_PIN PE0 + +#define USE_SOFTSERIAL1 +#define SOFTSERIAL_1_TX_PIN PC6 //TX6 pad +#define SOFTSERIAL_1_RX_PIN PC6 //TX6 pad + +#define SERIAL_PORT_COUNT 9 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_CRSF +#define SERIALRX_UART SERIAL_PORT_USART6 + +// *************** SDIO SD BLACKBOX******************* +#define USE_SDCARD +#define USE_SDCARD_SDIO +#define SDCARD_SDIO_DEVICE SDIODEV_1 +#define SDCARD_SDIO_4BIT + +#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT + +// *************** ADC ***************************** +#define USE_ADC +#define ADC_INSTANCE ADC1 + +#define ADC_CHANNEL_1_PIN PC0 //ADC123 VBAT1 +#define ADC_CHANNEL_2_PIN PC1 //ADC123 CURR1 +#define ADC_CHANNEL_3_PIN PC5 //ADC12 RSSI +#define ADC_CHANNEL_4_PIN PC4 //ADC12 AirS +#define ADC_CHANNEL_5_PIN PA4 //ADC12 VB2 +#define ADC_CHANNEL_6_PIN PA7 //ADC12 CU2 + +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 +#define RSSI_ADC_CHANNEL ADC_CHN_3 +#define AIRSPEED_ADC_CHANNEL ADC_CHN_4 + +// *************** PINIO *************************** +#define USE_PINIO +#define USE_PINIOBOX +#define PINIO1_PIN PD10 // VTX power switcher +#define PINIO2_PIN PD11 // 2xCamera switcher + +// *************** LEDSTRIP ************************ +#define USE_LED_STRIP +#define WS2811_PIN PA8 + +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX) +#define CURRENT_METER_SCALE 250 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA (0xffff & ~(BIT(14) | BIT(13))) +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTE 0xffff + +#define MAX_PWM_OUTPUT_PORTS 15 +#define USE_DSHOT +#define USE_ESC_SENSOR + From cdb29763c2c2e8b8394d05a4a13bb2c623fab773 Mon Sep 17 00:00:00 2001 From: Ada Date: Wed, 11 Feb 2026 13:22:27 -0700 Subject: [PATCH 10/48] Add NEXUS target for RadioMaster Nexus (Original) flight controller STM32F722-based target for the original (discontinued) Nexus, distinct from the Nexus-X/XR (NEXUSX target). Pin mapping derived from Rotorflight NEXUS_F7 dump and verified on hardware. Key hardware support: - ICM-42688-P IMU on SPI1 (CW90 alignment, EXTI PA15) - SPL06 barometer on I2C1 (PB8/PB9, separate from external I2C2) - W25N01G 128MB blackbox flash on SPI2 - 5 servo/motor outputs (S1-S4 + M1), expandable to 7 - UART4 with TX/RX swap for CRSF receiver on Port A - UART1 on PA9/PA10 (not PB6/PB7 as on Nexus-X/XR) Also adds USE_UART4_SWAP support in serial_uart_hal.c for the STM32 HAL advanced UART pin swap feature. --- src/main/drivers/serial_uart_hal.c | 7 ++ src/main/target/NEXUS/CMakeLists.txt | 1 + src/main/target/NEXUS/README.md | 144 ++++++++++++++++++++++ src/main/target/NEXUS/config.c | 19 +++ src/main/target/NEXUS/target.c | 29 +++++ src/main/target/NEXUS/target.h | 173 +++++++++++++++++++++++++++ 6 files changed, 373 insertions(+) create mode 100644 src/main/target/NEXUS/CMakeLists.txt create mode 100644 src/main/target/NEXUS/README.md create mode 100644 src/main/target/NEXUS/config.c create mode 100644 src/main/target/NEXUS/target.c create mode 100644 src/main/target/NEXUS/target.h diff --git a/src/main/drivers/serial_uart_hal.c b/src/main/drivers/serial_uart_hal.c index cce38422848..f25b2cbafb6 100644 --- a/src/main/drivers/serial_uart_hal.c +++ b/src/main/drivers/serial_uart_hal.c @@ -52,6 +52,13 @@ static void usartConfigurePinInversion(uartPort_t *uartPort) { uartPort->Handle.AdvancedInit.TxPinLevelInvert = UART_ADVFEATURE_TXINV_ENABLE; } } + +#ifdef USE_UART4_SWAP + if (uartPort->Handle.Instance == UART4) { + uartPort->Handle.AdvancedInit.AdvFeatureInit |= UART_ADVFEATURE_SWAP_INIT; + uartPort->Handle.AdvancedInit.Swap = UART_ADVFEATURE_SWAP_ENABLE; + } +#endif } static void uartReconfigure(uartPort_t *uartPort) diff --git a/src/main/target/NEXUS/CMakeLists.txt b/src/main/target/NEXUS/CMakeLists.txt new file mode 100644 index 00000000000..a9afac49839 --- /dev/null +++ b/src/main/target/NEXUS/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f722xe(NEXUS) diff --git a/src/main/target/NEXUS/README.md b/src/main/target/NEXUS/README.md new file mode 100644 index 00000000000..a6f74074303 --- /dev/null +++ b/src/main/target/NEXUS/README.md @@ -0,0 +1,144 @@ +# RadioMaster Nexus (Original) + +Flight controller originally designed for helicopters using Rotorflight. +Based on STM32F722RET6. This is the **original** (discontinued) Nexus, +not the Nexus-X or Nexus-XR. + +For the Nexus-X and Nexus-XR, use the `NEXUSX` target instead. + +## Hardware + +| Component | Spec | +|-----------|------| +| MCU | STM32F722RET6 (216MHz, 256KB RAM, 512KB flash) | +| IMU | ICM-42688-P (SPI1), CW90 alignment | +| Barometer | SPL06-001 (I2C1, internal) | +| Blackbox | W25N01G 128MB (SPI2) | +| Input voltage | 5 - 12.6V | +| BEC output | 5V / 2A on Port A, B, C | +| DSM power | 3.3V / 0.5A | +| Dimensions | 41.3 x 25.4 x 13.1mm | +| Weight | 16.8g | + +## Differences from Nexus-X/XR + +| | OG Nexus | Nexus-X/XR | +|---|----------|------------| +| IMU EXTI | PA15 | PB8 | +| IMU alignment | CW90 | CW180 | +| Baro I2C | I2C1 (PB8/PB9) | I2C2 (PB10/PB11) | +| Flash | W25N01G (128MB) | W25N02K (256MB) | +| Internal ELRS RX | None | RP4TD-M on UART5 | +| PINIO1 (RX power) | None | PC8 | +| UART1 pins | PA9/PA10 | PB6/PB7 | +| Voltage input | 5-12.6V | 3.6-70V | +| Servo outputs | 5 | 7 default (9 max) | +| Rotorflight target | NEXUS_F7 | NEXUSX | + +## Pin Functions + +### Default Output Assignment + +| Output | Pin | Timer | Connector | +|--------|-----|-------|-----------| +| S1 | PB4 | TIM3_CH1 | Servo header | +| S2 | PB5 | TIM3_CH2 | Servo header | +| S3 | PB0 | TIM3_CH3 | Servo header | +| S4 | PB3 | TIM2_CH2 | Servo header (Tail) | +| M1 | PB6 | TIM4_CH1 | ESC header | + +### UART Ports + +| UART | Label | TX | RX | Notes | +|------|-------|----|----|-------| +| UART1 | DSM | PA9 | PA10 | | +| UART2 | SBUS | PA2 | PA3 | Shared with FREQ/PPM | +| UART3 | Port C | PB11 | PB10 | Shared with I2C2 | +| UART4 | Port A | PA1 | PA0 | Primary receiver (CRSF) | +| UART6 | Port B | PC7 | PC6 | | + +### I2C Buses + +| Bus | SCL | SDA | Usage | +|-----|-----|-----|-------| +| I2C1 | PB8 | PB9 | Internal barometer (SPL06) | +| I2C2 | PB10 | PB11 | External sensors via Port C | + +### ADC Channels + +| Channel | Pin | Divider | Usage | +|---------|-----|---------|-------| +| BUS | PC2 | 320 | External battery voltage (mapped as VBAT) | +| BEC | PC1 | 160 | 5V BEC rail monitoring | + +### Connector Pinouts + +**Port A (UART4 - CRSF receiver):** +1. GND +2. 5V +3. RX (PA0, connect to receiver TX) +4. TX (PA1, connect to receiver RX / telemetry) + +**Port B (UART6):** +1. GND +2. 5V +3. TX (PC7) +4. RX (PC6) + +**Port C (UART3 / I2C2):** +1. GND +2. 5V +3. SCL/TX (PB10) +4. SDA/RX (PB11) + +**ESC Header:** +- Signal: PB6 (TIM4_CH1 PWM) + +## Typical Glider Setup (Elevon / Flying Wing) + +For a 5-channel elevon glider like the Kunai: + +1. Flash NEXUS target via DFU +2. Set aircraft type: **Flying Wing** +3. Connect RP3-H receiver to **Port A** (CRSF on UART4) +4. Connect ESC to **ESC header** (M1) +5. Connect left elevon servo to **S1** +6. Connect right elevon servo to **S2** +7. Configure elevon mixing in the Mixer tab +8. Vario: SPL06 baro provides altitude-based vario out of the box +9. GPS (Phase 2): Connect to **Port B** (UART6) + +## Verified on Hardware + +- [x] MCU boots, LEDs active (PC14, PC15) +- [x] USB CDC enumeration and iNAV CLI +- [x] IMU (gyro + accel) detected and responding +- [x] iNAV Configurator 9.0 connects, 3D model tracks board movement +- [ ] Barometer (SPL06 on I2C1) — fixed bus assignment, needs retest +- [ ] VBAT ADC (PC2) — fixed pin assignment, needs retest +- [ ] BEC ADC (PC1) — needs test +- [ ] Gyro alignment (CW90) — fixed, needs retest +- [ ] All servo outputs — need test +- [ ] CRSF receiver on UART4 — needs test +- [ ] Blackbox logging — needs test +- [ ] VBAT scale calibration with multimeter + +## Building + +Requires NixOS flake (included in repo root) or standard iNAV build deps. + +```sh +nix develop --impure # or set up arm-none-eabi-gcc manually +mkdir -p build && cd build +cmake -GNinja -DCOMPILER_VERSION_CHECK=OFF .. +ninja NEXUS +``` + +## Flashing via DFU + +Hold button while connecting USB, then: + +```sh +arm-none-eabi-objcopy -O binary build/bin/NEXUS.elf build/NEXUS.bin +dfu-util -a 0 -s 0x08000000:leave -D build/NEXUS.bin +``` diff --git a/src/main/target/NEXUS/config.c b/src/main/target/NEXUS/config.c new file mode 100644 index 00000000000..ebbdf786f1c --- /dev/null +++ b/src/main/target/NEXUS/config.c @@ -0,0 +1,19 @@ +/* + * RadioMaster Nexus (Original) - Target configuration defaults + * + * Unlike the Nexus-X/XR, the OG Nexus has no internal ELRS receiver, + * so there is no PINIO / USER1 box configuration needed here. + */ + +#include +#include "platform.h" +#include "drivers/pwm_mapping.h" + +void targetConfiguration(void) +{ + // Force M1/ESC (PB6/TIM4) to motor mode so the PWM auto-allocator + // picks it as the motor output first. Without this, it walks the + // timer table in order, converts TIM3 (S1/S2/S3) to motors via + // pwmClaimTimer(), and leaves no timers available for servos. + timerOverridesMutable(timer2id(TIM4))->outputMode = OUTPUT_MODE_MOTORS; +} diff --git a/src/main/target/NEXUS/target.c b/src/main/target/NEXUS/target.c new file mode 100644 index 00000000000..a903264269e --- /dev/null +++ b/src/main/target/NEXUS/target.c @@ -0,0 +1,29 @@ +/* + * RadioMaster Nexus (Original) - Timer/PWM hardware configuration + * + * Timer allocation (from Rotorflight NEXUS_F7 dump): + * TIM3: S1 (PB4/CH1), S2 (PB5/CH2), S3 (PB0/CH3) + * TIM2: S4 (PB3/CH2) + * TIM4: M1 (PB6/CH1) - ESC output + * + * Note: PA2/PA3 are FREQ/PPM inputs in Rotorflight. In iNAV they + * can be repurposed as servo outputs when UART2 is not assigned. + */ + +#include +#include "platform.h" +#include "drivers/io.h" +#include "drivers/timer.h" +#include "drivers/pwm_mapping.h" + +timerHardware_t timerHardware[] = { + DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 + DEF_TIM(TIM3, CH2, PB5, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 + DEF_TIM(TIM2, CH2, PB3, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 + DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), // M1/ESC + DEF_TIM(TIM5, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 0), // shared w/ UART2 TX + DEF_TIM(TIM9, CH2, PA3, TIM_USE_OUTPUT_AUTO, 0, 0), // shared w/ UART2 RX +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/NEXUS/target.h b/src/main/target/NEXUS/target.h new file mode 100644 index 00000000000..8f0510e5981 --- /dev/null +++ b/src/main/target/NEXUS/target.h @@ -0,0 +1,173 @@ +/* + * RadioMaster Nexus (Original) - iNAV target + * + * Based on the NEXUSX target by functionpointer, adapted for the + * original (discontinued) Nexus hardware. + * + * Key differences from Nexus-X/XR: + * - IMU EXTI on PA15 (X/XR uses PB8) + * - IMU alignment CW90 (X/XR uses CW180) + * - Flash is W25N01G 128MB (X/XR uses W25N02K 256MB) + * - No internal ELRS receiver (X/XR has RP4TD-M on UART5) + * - No PINIO1 receiver power gate + * - Voltage input 5-12.6V (X/XR supports 3.6-70V) + * - 5 servo/motor outputs vs 7-9 on X/XR + * - Baro on I2C1 (PB8/PB9), not I2C2 + * - UART1 on PA9/PA10, not PB6/PB7 + * + * Pin mapping derived from: + * - Rotorflight NEXUS_F7 target (authoritative) + * - groundflight project (joshperry/groundflight) + * - RadioMaster documentation + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "NEXS" +#define USBD_PRODUCT_STRING "RadioMaster Nexus" + +/* ---- LEDs ---- */ +#define LED0 PC14 // active low +#define LED1 PC15 // active low + +/* ---- Beeper ---- */ +// No dedicated beeper pin on Nexus hardware + +/* ---- SPI ---- */ +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +/* ---- IMU: ICM-42688-P ---- */ +// iNAV uses ICM42605 driver which is register-compatible with ICM42688P +#define USE_IMU_ICM42605 +#define IMU_ICM42605_ALIGN CW90_DEG +#define ICM42605_CS_PIN PA4 +#define ICM42605_SPI_BUS BUS_SPI1 +#define ICM42605_EXTI_PIN PA15 + +/* ---- I2C ---- */ +// I2C1: PB8/PB9 - internal, used for barometer +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + +// I2C2: PB10/PB11 - external via Port C connector (shared with UART3) +// Available for external sensors (magnetometer, rangefinder, etc.) +#define USE_I2C_DEVICE_2 +#define I2C2_SCL PB10 +#define I2C2_SDA PB11 +#define I2C_DEVICE_2_SHARES_UART3 + +/* ---- Barometer: SPL06-001 ---- */ +// Confirmed on I2C1 per Rotorflight NEXUS_F7 dump +#define USE_BARO +#define USE_BARO_SPL06 +#define BARO_I2C_BUS BUS_I2C1 + +/* ---- Magnetometer (external, optional via Port C / I2C2) ---- */ +#define USE_MAG +#define USE_MAG_ALL +#define MAG_I2C_BUS BUS_I2C2 + +/* ---- Flash: W25N01G (128MB) ---- */ +#define USE_FLASHFS +#define USE_FLASH_W25N01G +#define W25N01G_SPI_BUS BUS_SPI2 +#define W25N01G_CS_PIN PB12 +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + +/* ---- UARTs ---- */ +// OG Nexus UART layout (confirmed from Rotorflight NEXUS_F7 dump): +// UART1 [DSM port] : PA9 (TX) / PA10 (RX) +// UART2 [SBUS/FREQ] : PA2 (TX) / PA3 (RX) - shared with RPM/TLM pins +// UART3 [Port C] : PB11 (TX) / PB10 (RX) - shared with I2C2 +// UART4 [Port A] : PA1 (TX) / PA0 (RX) - primary CRSF receiver +// UART6 [Port B] : PC7 (TX) / PC6 (RX) +// +// NOTE: No UART5 on OG Nexus (X/XR uses UART5 for internal ELRS) + +#define USE_VCP +#define USE_USB_DETECT +#define USB_DETECT_PIN NONE + +#define USE_UART1 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 + +#define USE_UART2 +#define UART2_TX_PIN PA2 +#define UART2_RX_PIN PA3 + +#define USE_UART3 +#define UART3_TX_PIN PB11 +#define UART3_RX_PIN PB10 + +#define USE_UART4 +#define USE_UART4_SWAP +#define UART4_TX_PIN PA1 +#define UART4_RX_PIN PA0 + +#define USE_UART6 +#define UART6_TX_PIN PC7 +#define UART6_RX_PIN PC6 + +#define SERIAL_PORT_COUNT 6 // VCP + UART1-4 + UART6 + +/* ---- Default serial receiver ---- */ +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_CRSF +#define SERIALRX_UART SERIAL_PORT_USART4 + + +/* ---- ADC ---- */ +// From Rotorflight dump: +// ADC_BEC = PC1, divider 160 (BEC 5V rail monitoring) +// ADC_BUS = PC2, divider 320 (external battery voltage) +// Map BUS voltage as VBAT for iNAV since that's the flight battery +#define USE_ADC +#define ADC_INSTANCE ADC1 +#define ADC_CHANNEL_1_PIN PC2 // BUS voltage (external battery) +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define ADC_CHANNEL_2_PIN PC1 // BEC voltage (5V rail) +// VBAT scale: Rotorflight vbus_divider = 320 +// iNAV scale = divider * ~3.44 (ADC ref / resolution factor) +// Start with 1100, calibrate with multimeter +#define VBAT_SCALE_DEFAULT 1100 + +/* ---- Sensors ---- */ +#define SENSORS_SET (SENSOR_ACC | SENSOR_BARO) + +/* ---- PWM / Servo / Motor outputs ---- */ +// OG Nexus outputs (from Rotorflight dump): +// S1: PB4 (TIM3_CH1) - Servo header +// S2: PB5 (TIM3_CH2) - Servo header +// S3: PB0 (TIM3_CH3) - Servo header +// S4: PB3 (TIM2_CH2) - Servo header (Tail) +// M1: PB6 (TIM4_CH1) - ESC header (motor only, NOT UART1) +// +// Pin multiplexing when UARTs freed: +// PA2 (TIM5_CH3) - shared with UART2 TX / FREQ input +// PA3 (TIM9_CH2) - shared with UART2 RX / PPM input + +#define MAX_PWM_OUTPUT_PORTS 7 + +/* ---- No internal receiver ---- */ +// OG Nexus has no internal ELRS receiver. +// No UART5, no PINIO1 power gate. + +/* ---- Platform ---- */ +#define USE_SERIAL_4WAY_BLHELI_INTERFACE +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff + +#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX) From 66c87bf6be663de02242b2c60a3b8612280ef597 Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Wed, 18 Feb 2026 13:57:39 -0600 Subject: [PATCH 11/48] imu: compute GPS 3D speed only on new GPS data GPS3DspeedFiltered was computed every PID cycle (1 kHz) from gpsSol.velNED[], which only changes when the GPS task delivers a new fix (~10 Hz). Move the sqrt and pt1 filter update into imuCalculateTurnRateacceleration(), gate them on gpsHeartbeat change, and make the filter state a static local. The variable is only consumed in that function (fixed-wing + GPS-trustworthy path), so the module-level FASTRAM variables and the imuInit() reset are also removed. Co-Authored-By: Claude Sonnet 4.6 --- src/main/flight/imu.c | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index b012c040d27..d4ac5cf1a85 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -113,8 +113,6 @@ STATIC_FASTRAM pt1Filter_t HeadVecEFFilterY; STATIC_FASTRAM pt1Filter_t HeadVecEFFilterZ; FASTRAM fpVector3_t HeadVecEFFiltered = {.v = {0.0f, 0.0f, 0.0f}}; -STATIC_FASTRAM float GPS3DspeedFiltered=0.0f; -STATIC_FASTRAM pt1Filter_t GPS3DspeedFilter; FASTRAM bool gpsHeadingInitialized; @@ -210,8 +208,6 @@ void imuInit(void) pt1FilterReset(&HeadVecEFFilterX, 0); pt1FilterReset(&HeadVecEFFilterY, 0); pt1FilterReset(&HeadVecEFFilterZ, 0); - // Initialize 3d speed filter - pt1FilterReset(&GPS3DspeedFilter, 0); } void imuSetMagneticDeclination(float declinationDeg) @@ -667,9 +663,6 @@ static void imuCalculateFilters(float dT) HeadVecEFFiltered.y = pt1FilterApply4(&HeadVecEFFilterY, rMat[1][0], IMU_ROTATION_LPF, dT); HeadVecEFFiltered.z = pt1FilterApply4(&HeadVecEFFilterZ, rMat[2][0], IMU_ROTATION_LPF, dT); - //anti aliasing - float GPS3Dspeed = calc_length_pythagorean_3D(gpsSol.velNED[X],gpsSol.velNED[Y],gpsSol.velNED[Z]); - GPS3DspeedFiltered = pt1FilterApply4(&GPS3DspeedFilter, GPS3Dspeed, IMU_ROTATION_LPF, dT); } static void imuCalculateGPSacceleration(fpVector3_t *vEstAccelEF,fpVector3_t *vEstcentrifugalAccelBF, float *acc_ignore_slope_multipiler) @@ -705,6 +698,14 @@ static void imuCalculateTurnRateacceleration(fpVector3_t *vEstcentrifugalAccelBF float currentspeed = 0; if (isGPSTrustworthy()){ //first speed choice is gps + static bool lastGPSHeartbeat; + static pt1Filter_t GPS3DspeedFilter; + static float GPS3DspeedFiltered = 0.0f; + if (gpsSol.flags.gpsHeartbeat != lastGPSHeartbeat) { + lastGPSHeartbeat = gpsSol.flags.gpsHeartbeat; + float GPS3Dspeed = calc_length_pythagorean_3D(gpsSol.velNED[X], gpsSol.velNED[Y], gpsSol.velNED[Z]); + GPS3DspeedFiltered = pt1FilterApply4(&GPS3DspeedFilter, GPS3Dspeed, IMU_ROTATION_LPF, dT); + } currentspeed = GPS3DspeedFiltered; *acc_ignore_slope_multipiler = 4.0f; } From 2ce3f0ae00e41a55a827938dfb7cc1927d44fc9f Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Wed, 18 Feb 2026 17:35:33 -0600 Subject: [PATCH 12/48] imu: optimize imuMahonyAHRSupdate() hot path (5 micro-opts) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Five cycle-saving changes to imuMahonyAHRSupdate(), which accounts for ~205 µs (30%) of the PID loop on RP2350. Each change is safe on all targets; the gains are proportionally smaller on F7/H7 where the function already lives in ITCM RAM. 1. Replace quaternionRotateVector({0,0,1}) with rMat[2][*] reads. imuComputeRotationMatrix() is called at the end of every invocation and keeps rMat in sync with orientation. Rotating the constant gravity vector EF→BF yields exactly the third row of rMat, so three float loads replace ~56 floating-point multiply/add operations. 2. Eliminate sqrt() from the Taylor-series threshold. Original: thetaMagnitudeSq < sqrt(24e-6). Squaring both sides (both non-negative) gives the equivalent condition thetaMagnitudeSq² < 24e-6 with no sqrt call. 3. First-order Newton quaternion renormalization replaces quaternionNormalize() (sqrt + 4 divides, ~35 cycles) with scale = (3 - normSq) * 0.5 (~14 cycles). At 1 kHz the per-step drift |ε| < 1e-6, making the O(ε²) error < 1e-12. imuCheckAndResetOrientationQuaternion() remains as the catastrophic-failure safety net. 4. Precompute the anti-windup i_limit in imuConfigure() (called only on settings save). The hot path now reads a single float instead of performing an add, a multiply, and a divide every PID cycle. Adds dcm_i_limit to imuRuntimeConfig_t and imuConfigure(). 5. Reduce prevOrientation snapshot frequency from every PID cycle to every 100 cycles (~100 ms at 1 kHz). The snapshot is only used by the fault-recovery path which should never fire in normal flight; 16 bytes of unnecessary SRAM write every ms is not justified. Co-Authored-By: Claude Sonnet 4.6 --- src/main/flight/imu.c | 56 +++++++++++++++++++++++++++++++++++++------ src/main/flight/imu.h | 5 ++++ 2 files changed, 54 insertions(+), 7 deletions(-) diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index b012c040d27..2c129ac27d0 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -171,6 +171,12 @@ void imuConfigure(void) imuRuntimeConfig.dcm_kp_mag = imuConfig()->dcm_kp_mag / 10000.0f; imuRuntimeConfig.dcm_ki_mag = imuConfig()->dcm_ki_mag / 10000.0f; imuRuntimeConfig.small_angle = imuConfig()->small_angle; + /* Precompute the Mahony anti-windup clamp. The PID loop reads this value + * 1000× per second; the kP gains only change when the user saves settings, + * so computing it here (called once per save) avoids one add, one multiply, + * and one divide on every PID cycle. */ + imuRuntimeConfig.dcm_i_limit = DEGREES_TO_RADIANS(2.0f) + * (imuRuntimeConfig.dcm_kp_acc + imuRuntimeConfig.dcm_kp_mag) * 0.5f; } void imuInit(void) @@ -368,7 +374,19 @@ static float imuCalculateMcCogAccWeight(void) static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVector3_t * accBF, const fpVector3_t * magBF, const fpVector3_t * vCOG, const fpVector3_t * vCOGAcc, float accWScaler, float magWScaler) { STATIC_FASTRAM fpVector3_t vGyroDriftEstimate = { 0 }; - fpQuaternion_t prevOrientation = orientation; + + /* Opt 5: snapshot prevOrientation every 100 PID cycles instead of every cycle. + * The snapshot is only used by the fault-recovery path in + * imuCheckAndResetOrientationQuaternion(), which should never fire in normal + * flight. Copying 4 floats 1000×/s just to support a near-zero-probability + * reset path is wasteful; 100 ms staleness is a safe recovery point. */ + static uint8_t prevOrientationSnapshotCount = 0; + static fpQuaternion_t prevOrientation = { .q0 = 1.0f }; // identity quaternion safe default + if (++prevOrientationSnapshotCount >= 100) { + prevOrientationSnapshotCount = 0; + prevOrientation = orientation; + } + fpVector3_t vRotation = *gyroBF; /* Calculate general spin rate (rad/s) */ @@ -501,11 +519,17 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe /* Step 2: Roll and pitch correction - use measured acceleration vector */ if (accBF) { - static const fpVector3_t vGravity = { .v = { 0.0f, 0.0f, 1.0f } }; fpVector3_t vEstGravity, vAcc, vErr; - // Calculate estimated gravity vector in body frame - quaternionRotateVector(&vEstGravity, &vGravity, &orientation); // EF -> BF + /* Opt 1: imuComputeRotationMatrix() is called at the END of every + * imuMahonyAHRSupdate() and keeps rMat in sync with orientation. + * Rotating the constant unit-gravity vector {0,0,1} from EF to BF by + * the current orientation quaternion yields exactly the third row of rMat + * (rMat[2][0..2]). Reading those three floats replaces a quaternionRotateVector() + * call that costs 2× quaternionMultiply = ~32 multiplies + 24 adds. */ + vEstGravity.x = rMat[2][0]; + vEstGravity.y = rMat[2][1]; + vEstGravity.z = rMat[2][2]; // Error is sum of cross product between estimated direction and measured direction of gravity vectorNormalize(&vAcc, accBF); @@ -528,7 +552,9 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe vectorAdd(&vRotation, &vRotation, &vErr); } // Anti wind-up - float i_limit = DEGREES_TO_RADIANS(2.0f) * (imuRuntimeConfig.dcm_kp_acc + imuRuntimeConfig.dcm_kp_mag) / 2.0f; + /* Opt 4: dcm_i_limit is computed once in imuConfigure() (called on settings save), + * not recomputed each PID cycle. The kP gains do not change at runtime. */ + const float i_limit = imuRuntimeConfig.dcm_i_limit; vGyroDriftEstimate.x = constrainf(vGyroDriftEstimate.x, -i_limit, i_limit); vGyroDriftEstimate.y = constrainf(vGyroDriftEstimate.y, -i_limit, i_limit); vGyroDriftEstimate.z = constrainf(vGyroDriftEstimate.z, -i_limit, i_limit); @@ -551,7 +577,10 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe // Proper quaternion from axis/angle involves computing sin/cos, but the formula becomes numerically unstable as Theta approaches zero. // For near-zero cases we use the first 3 terms of the Taylor series expansion for sin/cos. We check if fourth term is less than machine precision - // then we can safely use the "low angle" approximated version without loss of accuracy. - if (thetaMagnitudeSq < fast_fsqrtf(24.0f * 1e-6f)) { + /* Opt 2: original condition was "thetaMagnitudeSq < sqrt(24e-6)". + * Squaring both sides (both are non-negative) gives an equivalent + * condition without a sqrt() call: thetaMagnitudeSq² < 24e-6. */ + if (thetaMagnitudeSq * thetaMagnitudeSq < 24.0e-6f) { quaternionScale(&deltaQ, &deltaQ, 1.0f - thetaMagnitudeSq / 6.0f); deltaQ.q0 = 1.0f - thetaMagnitudeSq / 2.0f; } @@ -563,7 +592,20 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe // Calculate final orientation and renormalize quaternionMultiply(&orientation, &orientation, &deltaQ); - quaternionNormalize(&orientation, &orientation); + /* Opt 3: first-order Newton renormalization avoids sqrt() and 4 divides. + * At 1 kHz the quaternion norm drifts by < 1e-6 per step, so normSq = 1 + ε + * with |ε| ≪ 1. The identity 1/sqrt(x) ≈ (3-x)/2 is accurate to O(ε²) ≈ 1e-12 + * — well within float precision. imuCheckAndResetOrientationQuaternion() below + * catches any catastrophic norm deviation that this approximation cannot correct. */ + { + const float normSq = orientation.q0 * orientation.q0 + orientation.q1 * orientation.q1 + + orientation.q2 * orientation.q2 + orientation.q3 * orientation.q3; + const float scale = (3.0f - normSq) * 0.5f; + orientation.q0 *= scale; + orientation.q1 *= scale; + orientation.q2 *= scale; + orientation.q3 *= scale; + } } // Check for invalid quaternion and reset to previous known good one diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h index 6d285d0f794..14244c9cefc 100644 --- a/src/main/flight/imu.h +++ b/src/main/flight/imu.h @@ -65,6 +65,11 @@ typedef struct imuRuntimeConfig_s { float dcm_ki_acc; float dcm_kp_mag; float dcm_ki_mag; + /* Precomputed anti-windup limit for imuMahonyAHRSupdate(): equals + * DEGREES_TO_RADIANS(2) * (dcm_kp_acc + dcm_kp_mag) / 2. + * Updated once by imuConfigure() whenever settings are saved, so the + * hot PID path reads a single float instead of doing arithmetic. */ + float dcm_i_limit; uint8_t small_angle; } imuRuntimeConfig_t; From 2b8d57141d01c152985145e3a740fcd07b03da19 Mon Sep 17 00:00:00 2001 From: Ada Date: Tue, 24 Feb 2026 09:56:03 -0700 Subject: [PATCH 13/48] NEXUS: fix VBAT scale, Port C pinout, and ADC docs MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Address hardware testing feedback from daijoubu: - VBAT_SCALE_DEFAULT 1100 → 320 (hardware-verified) - Port C connector pinout: pin 3 is SDA/TX (PB11), pin 4 is SCL/RX (PB10) - Clarify OG Nexus has no EXT-V input (Vin ADC only, unlike X/XR) - Update verification checklist with confirmed test results Co-Authored-By: Joshua Perry --- src/main/target/NEXUS/README.md | 31 ++++++++++++++++++------------- src/main/target/NEXUS/target.h | 19 +++++++++---------- 2 files changed, 27 insertions(+), 23 deletions(-) diff --git a/src/main/target/NEXUS/README.md b/src/main/target/NEXUS/README.md index a6f74074303..0edf84a7ac3 100644 --- a/src/main/target/NEXUS/README.md +++ b/src/main/target/NEXUS/README.md @@ -31,7 +31,7 @@ For the Nexus-X and Nexus-XR, use the `NEXUSX` target instead. | Internal ELRS RX | None | RP4TD-M on UART5 | | PINIO1 (RX power) | None | PC8 | | UART1 pins | PA9/PA10 | PB6/PB7 | -| Voltage input | 5-12.6V | 3.6-70V | +| Voltage sense | Vin ADC only (5-12.6V) | EXT-V input (3.6-70V) | | Servo outputs | 5 | 7 default (9 max) | | Rotorflight target | NEXUS_F7 | NEXUSX | @@ -68,9 +68,12 @@ For the Nexus-X and Nexus-XR, use the `NEXUSX` target instead. | Channel | Pin | Divider | Usage | |---------|-----|---------|-------| -| BUS | PC2 | 320 | External battery voltage (mapped as VBAT) | +| BUS | PC2 | 320 | Vin rail, 5-12.6V (mapped as VBAT) | | BEC | PC1 | 160 | 5V BEC rail monitoring | +Note: Unlike the Nexus-X/XR, the OG Nexus has no dedicated EXT-V +high-voltage battery sense input. VBAT monitors the board input power. + ### Connector Pinouts **Port A (UART4 - CRSF receiver):** @@ -88,8 +91,8 @@ For the Nexus-X and Nexus-XR, use the `NEXUSX` target instead. **Port C (UART3 / I2C2):** 1. GND 2. 5V -3. SCL/TX (PB10) -4. SDA/RX (PB11) +3. SDA/TX (PB11) +4. SCL/RX (PB10) **ESC Header:** - Signal: PB6 (TIM4_CH1 PWM) @@ -113,15 +116,17 @@ For a 5-channel elevon glider like the Kunai: - [x] MCU boots, LEDs active (PC14, PC15) - [x] USB CDC enumeration and iNAV CLI - [x] IMU (gyro + accel) detected and responding -- [x] iNAV Configurator 9.0 connects, 3D model tracks board movement -- [ ] Barometer (SPL06 on I2C1) — fixed bus assignment, needs retest -- [ ] VBAT ADC (PC2) — fixed pin assignment, needs retest -- [ ] BEC ADC (PC1) — needs test -- [ ] Gyro alignment (CW90) — fixed, needs retest -- [ ] All servo outputs — need test -- [ ] CRSF receiver on UART4 — needs test -- [ ] Blackbox logging — needs test -- [ ] VBAT scale calibration with multimeter +- [x] Gyro alignment (CW90) confirmed +- [x] Accelerometer working +- [x] Barometer (SPL06 on I2C1) working +- [x] VBAT ADC (PC2) working (scale 320) +- [x] All UART ports (1-4, 6) verified +- [x] UART4 CRSF receiver working (TX/RX swap confirmed) +- [x] DShot on all motor outputs +- [x] Servo outputs working +- [x] Blackbox logging working +- [x] LEDs working +- [x] I2C2 bus working ## Building diff --git a/src/main/target/NEXUS/target.h b/src/main/target/NEXUS/target.h index 8f0510e5981..84fde23a95e 100644 --- a/src/main/target/NEXUS/target.h +++ b/src/main/target/NEXUS/target.h @@ -129,19 +129,18 @@ /* ---- ADC ---- */ -// From Rotorflight dump: -// ADC_BEC = PC1, divider 160 (BEC 5V rail monitoring) -// ADC_BUS = PC2, divider 320 (external battery voltage) -// Map BUS voltage as VBAT for iNAV since that's the flight battery +// OG Nexus has no EXT-V input (unlike X/XR which has a dedicated +// high-voltage sense on PC0). Only two ADC channels: +// ADC_BUS = PC2, divider 320 (Vin rail, 5-12.6V) +// ADC_BEC = PC1, divider 160 (BEC 5V rail) +// Map Vin as VBAT since it's the primary power input #define USE_ADC #define ADC_INSTANCE ADC1 -#define ADC_CHANNEL_1_PIN PC2 // BUS voltage (external battery) +#define ADC_CHANNEL_1_PIN PC2 // Vin (input power rail) #define VBAT_ADC_CHANNEL ADC_CHN_1 -#define ADC_CHANNEL_2_PIN PC1 // BEC voltage (5V rail) -// VBAT scale: Rotorflight vbus_divider = 320 -// iNAV scale = divider * ~3.44 (ADC ref / resolution factor) -// Start with 1100, calibrate with multimeter -#define VBAT_SCALE_DEFAULT 1100 +#define ADC_CHANNEL_2_PIN PC1 // BEC 5V rail +// VBAT scale: hardware-verified value (divider ratio ~320) +#define VBAT_SCALE_DEFAULT 320 /* ---- Sensors ---- */ #define SENSORS_SET (SENSOR_ACC | SENSOR_BARO) From 38da5a7202ec6824d091fb8971b37517856033fd Mon Sep 17 00:00:00 2001 From: Ada Date: Wed, 25 Feb 2026 08:55:33 -0700 Subject: [PATCH 14/48] NEXUS: fix IMU alignment to match board orientation arrow The CW90 value was taken directly from the Rotorflight NEXUS_F7 target, but Rotorflight does not enforce arrow-forward as the board reference orientation. The Rotorflight CW90 was calibrated to an arbitrary mounting, not the case arrow. iNAV convention is arrow = forward, which requires CW180 on this hardware. Same root cause as the NEXUSX fix in #11338 / #11325. Co-Authored-By: Joshua Perry --- src/main/target/NEXUS/target.h | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/target/NEXUS/target.h b/src/main/target/NEXUS/target.h index 84fde23a95e..63c08049931 100644 --- a/src/main/target/NEXUS/target.h +++ b/src/main/target/NEXUS/target.h @@ -6,7 +6,8 @@ * * Key differences from Nexus-X/XR: * - IMU EXTI on PA15 (X/XR uses PB8) - * - IMU alignment CW90 (X/XR uses CW180) + * - IMU alignment CW180 (Rotorflight uses CW90 with a different + * board orientation reference; iNAV uses the arrow on the case) * - Flash is W25N01G 128MB (X/XR uses W25N02K 256MB) * - No internal ELRS receiver (X/XR has RP4TD-M on UART5) * - No PINIO1 receiver power gate @@ -48,7 +49,7 @@ /* ---- IMU: ICM-42688-P ---- */ // iNAV uses ICM42605 driver which is register-compatible with ICM42688P #define USE_IMU_ICM42605 -#define IMU_ICM42605_ALIGN CW90_DEG +#define IMU_ICM42605_ALIGN CW180_DEG #define ICM42605_CS_PIN PA4 #define ICM42605_SPI_BUS BUS_SPI1 #define ICM42605_EXTI_PIN PA15 From 7b0bd005adc7d2afaccbae5b630cb7ebcfd1b725 Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Wed, 25 Feb 2026 21:18:11 -0600 Subject: [PATCH 15/48] New target: FRSKYF405 FrSky F405 flight controller based on STM32F405RGT6. Hardware: IIM-42688P gyro (SPI1), AT7456E OSD (SPI2), SD card (SPI3), SPL06 barometer (I2C1), 9 motor outputs, hardware SBUS inverter on UART2, LED strip on PA15. S7/S8 use TIM12 (PWM only, no DShot). --- src/main/target/FRSKYF405/CMakeLists.txt | 1 + src/main/target/FRSKYF405/config.c | 32 ++++ src/main/target/FRSKYF405/target.c | 70 ++++++++ src/main/target/FRSKYF405/target.h | 210 +++++++++++++++++++++++ 4 files changed, 313 insertions(+) create mode 100644 src/main/target/FRSKYF405/CMakeLists.txt create mode 100644 src/main/target/FRSKYF405/config.c create mode 100644 src/main/target/FRSKYF405/target.c create mode 100644 src/main/target/FRSKYF405/target.h diff --git a/src/main/target/FRSKYF405/CMakeLists.txt b/src/main/target/FRSKYF405/CMakeLists.txt new file mode 100644 index 00000000000..fc8eb478bcc --- /dev/null +++ b/src/main/target/FRSKYF405/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f405xg(FRSKYF405) diff --git a/src/main/target/FRSKYF405/config.c b/src/main/target/FRSKYF405/config.c new file mode 100644 index 00000000000..0aed59803e0 --- /dev/null +++ b/src/main/target/FRSKYF405/config.c @@ -0,0 +1,32 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#include +#include + +#include + +#include "rx/rx.h" +#include "fc/config.h" + +void targetConfiguration(void) +{ + // SBUS is received via a hardware inverter (2N7002E MOSFET, Q1). + // The signal arrives at UART2 RX already de-inverted (active high), + // so the UART must NOT apply additional software inversion. + rxConfigMutable()->serialrx_inverted = 1; +} diff --git a/src/main/target/FRSKYF405/target.c b/src/main/target/FRSKYF405/target.c new file mode 100644 index 00000000000..89f3ee73a0e --- /dev/null +++ b/src/main/target/FRSKYF405/target.c @@ -0,0 +1,70 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#include + +#include "platform.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" + +/* + * Timer Allocation for FrSky F405 + * + * Connector mapping from schematic (CONN.SchDoc): + * - CON1 (S1): T4_2 = TIM4_CH2 = PB7 + * - CON2 (S2): T4_1 = TIM4_CH1 = PB6 + * - CON3 (S3): T3_3 = TIM3_CH3 = PB0 + * - CON4 (S4): T3_4 = TIM3_CH4 = PB1 + * - CON5 (S5): T8_3 = TIM8_CH3 = PC8 + * - CON6 (S6): T8_4 = TIM8_CH4 = PC9 + * - CON7 (S7): T12_1 = TIM12_CH1 = PB14 (no DMA - no DShot) + * - CON8 (S8): T12_2 = TIM12_CH2 = PB15 (no DMA - no DShot) + * - CON9 (S9): T1_1 = TIM1_CH1 = PA8 + * - CON23 (LED): T2_1 = TIM2_CH1 = PA15 + */ + +timerHardware_t timerHardware[] = { + // Motor outputs S1-S9 (in physical connector order from schematic) + DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 (CON1) - Motor 1 + DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 (CON2) - Motor 2 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 (CON3) - Motor 3 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 (CON4) - Motor 4 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 1), // S5 (CON5) - Motor 5 UP(2,1) + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 (CON6) - Motor 6 + DEF_TIM(TIM12, CH1, PB14, TIM_USE_OUTPUT_AUTO, 0, 0), // S7 (CON7) - Motor 7 NOTE: TIM12 has no DMA - DShot not supported + DEF_TIM(TIM12, CH2, PB15, TIM_USE_OUTPUT_AUTO, 0, 0), // S8 (CON8) - Motor 8 NOTE: TIM12 has no DMA - DShot not supported + DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 0), // S9 (CON9) - Motor 9 UP(2,5) + + // LED Strip on CON23 - PA15 (T2_1 signal on schematic) + DEF_TIM(TIM2, CH1, PA15, TIM_USE_LED, 0, 0), // LED strip (CON23) +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); + +/* + * IMPLEMENTATION NOTES: + * + * 1. S1/S2 order matches physical board labels (CON1=S1, CON2=S2 from schematic) + * + * 2. TIM12 (S7, S8): No DMA support on STM32F405. + * Works: Standard PWM, OneShot125, OneShot42, MultiShot + * Does NOT work: DShot (all variants), ProShot + * Use S7/S8 for non-DShot ESCs or servos + * + * 3. LED strip output: PA15 (T2_1) on CON23 with TIM2_CH1 + */ diff --git a/src/main/target/FRSKYF405/target.h b/src/main/target/FRSKYF405/target.h new file mode 100644 index 00000000000..e3eafdb51da --- /dev/null +++ b/src/main/target/FRSKYF405/target.h @@ -0,0 +1,210 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "FRF4" + +#define USBD_PRODUCT_STRING "FrSkyF405" + +// *************** LED & BEEPER ********************** +// Status LEDs share SWD debug pins - only available in release builds +// In debug builds, these pins must remain in SWD mode for debugging +// Red LED is likely a power indicator (always-on, no GPIO control) +#ifndef DEBUG_BUILD + #define LED0 PA14 // Blue LED (shares SWCLK) via R34 1K + #define LED1 PA13 // Green LED (shares SWDIO) via R35 1K +#endif +// Red LED appears to be power indicator (connected to VCC, not GPIO) + +#define BEEPER PC15 +#define BEEPER_INVERTED // INAV beeper driver writes !onoff; BEEPER_INVERTED makes it write HIGH to activate the N-channel MOSFET (Q2) + +// *************** Gyro & ACC ********************** +#define USE_SPI +#define USE_SPI_DEVICE_1 + +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +// IIM-42688P Gyro on SPI1 +#define USE_IMU_ICM42605 // IIM-42688P is compatible with ICM42605 driver +#define ICM42605_CS_PIN PA4 +#define ICM42605_SPI_BUS BUS_SPI1 +#define IMU_ICM42605_ALIGN CW0_DEG // TODO: Verify orientation from board layout + + +// *************** OSD ***************************** +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PC2 +#define SPI2_MOSI_PIN PC3 + +// AT7456E OSD on SPI2 +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI2 +#define MAX7456_CS_PIN PB12 + +// *************** SD Card ************************* +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PC12 + +// SD Card on SPI3 +// NOTE: PC14 used for CS (Chip Select) - OSC32_IN pin used as GPIO for slow CS signal +#define USE_SDCARD +#define USE_SDCARD_SPI +#define SDCARD_SPI_BUS BUS_SPI3 +#define SDCARD_CS_PIN PC14 + +#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT + +// *************** UART ***************************** +#define USE_VCP +// Note: USB VBUS sensing not clearly defined in schematic + +#define USE_UART1 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 + +#define USE_UART2 +#define UART2_TX_PIN PA2 +#define UART2_RX_PIN PA3 + +// USART2 has SBUS inverter circuit +#define SERIALRX_UART SERIAL_PORT_USART2 +#define SERIALRX_PROVIDER SERIALRX_SBUS +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL + +// UART3 conflicts with I2C2 (shares PB10/PB11) +// User must choose: enable UART3 OR I2C2, not both +#define USE_UART3 +#define UART3_TX_PIN PB10 // Conflicts with I2C2_SCL +#define UART3_RX_PIN PB11 // Conflicts with I2C2_SDA + +#define USE_UART4 +#define UART4_TX_PIN PA0 +#define UART4_RX_PIN PC5 // Shared with RSSI_ADC + +// UART5 TX conflicts with SPI3_MOSI (PC12) +// User must choose: enable UART5 OR SD card, not both +#define USE_UART5 +#define UART5_TX_PIN PC12 // Conflicts with SPI3_MOSI +#define UART5_RX_PIN PD2 + +#define USE_UART6 +#define UART6_TX_PIN PC6 +#define UART6_RX_PIN PC7 + +#define SERIAL_PORT_COUNT 7 // VCP + UART1-6 + +// *************** I2C **************************** +#define USE_I2C + +// I2C1 for barometer (SPL06) +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + +// I2C2 conflicts with UART3 (shares PB10/PB11) +// Enable one or the other, not both +// #define USE_I2C_DEVICE_2 +// #define I2C2_SCL PB10 // Conflicts with UART3_TX +// #define I2C2_SDA PB11 // Conflicts with UART3_RX + +#define DEFAULT_I2C_BUS BUS_I2C1 + +// SPL06 Barometer on I2C1 @ address 0x76 +#define USE_BARO +#define BARO_I2C_BUS BUS_I2C1 +#define USE_BARO_SPL06 + +// Magnetometer on I2C (external via connector) +#define USE_MAG +#define MAG_I2C_BUS DEFAULT_I2C_BUS +#define USE_MAG_ALL + +#define PITOT_I2C_BUS DEFAULT_I2C_BUS +#define TEMPERATURE_I2C_BUS DEFAULT_I2C_BUS +#define USE_RANGEFINDER +#define RANGEFINDER_I2C_BUS DEFAULT_I2C_BUS + +// *************** ADC ***************************** +#define USE_ADC +#define ADC_INSTANCE ADC1 +#define ADC1_DMA_STREAM DMA2_Stream0 + +#define ADC_CHANNEL_1_PIN PC0 // VBAT_ADC +#define ADC_CHANNEL_2_PIN PC1 // CURR_ADC +#define ADC_CHANNEL_3_PIN PC5 // RSSI_IN (shared with UART4_RX) + +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 +#define RSSI_ADC_CHANNEL ADC_CHN_3 + +// Current sensor: INA139 with 0.25mΩ shunt +#define CURRENT_METER_SCALE 250 // TODO: Calibrate based on INA139 + shunt values + +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TELEMETRY | FEATURE_BLACKBOX) + +// *************** LED STRIP *********************** +// CON23 LED output - PA15 (T2_1 signal on schematic) +#define USE_LED_STRIP +#define WS2811_PIN PA15 // TIM2_CH1, labeled as T2_1 on schematic + +// *************** PWM OUTPUTS ********************* +// 9 motor outputs (S1-S9) +// Note: S7/S8 on TIM12 do not support DShot (no DMA on TIM12 for STM32F405) +#define MAX_PWM_OUTPUT_PORTS 9 + +// *************** Other *************************** +#define USE_DSHOT +#define USE_ESC_SENSOR +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)) // PD2 used for UART5_RX + +// *************** NOTES & ISSUES ******************* +// +// DESIGN NOTES: +// 1. PC14 used for SPI3_NSS (SD card CS) - RTC clock domain, should be fine for slow CS signal +// +// 2. Status LEDs share SWD debug pins (PA13/PA14) - HANDLED WITH #ifndef DEBUG_BUILD +// Release builds: Pins configured as GPIO outputs for status LEDs +// Debug builds: Pins remain in SWD mode for debugging (LEDs disabled) +// Red LED appears to be power indicator (always-on, no GPIO control needed) +// +// PIN CONFLICTS (target or user must choose): +// 3. UART3 vs I2C2: PB10/PB11 shared +// - Enable UART3 for telemetry/GPS, OR +// - Enable I2C2 for additional sensors +// +// 4. UART5 TX vs SD Card: PC12 shared (SPI3_MOSI) +// - Enable UART5 for telemetry, OR +// - Enable SD card for blackbox logging +// +// 5. UART4_RX vs RSSI ADC: PC5 shared +// - May be intentional if RSSI comes via UART protocol +// +// MISSING INFORMATION: +// 6. Gyro orientation (IMU_ALIGN) unknown - needs physical board inspection +// 7. USB VBUS sensing pin not clearly defined in schematic (PC13 may be used) From 623fe7ae38a7b1c7ee7723f12a0e1af0a1996eb6 Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Sun, 1 Mar 2026 12:08:06 -0600 Subject: [PATCH 16/48] FRSKYF405: fix pin assignments and clean up target MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Fix UART5_RX: PD12 → PD2 (only valid pin for UART5_RX on STM32F405) - Move SPI3 to PB3/PB4/PB5, freeing PC10/PC11/PC12 for UART3 and UART5_TX - Move UART3 to PC10/PC11 (was conflicting with I2C2 on PB10/PB11) - Fix UART4_RX: PC5 → PA1 (PC5 is shared with RSSI ADC) - Re-enable I2C2 on PB10/PB11 (now free after UART3 move) - Add TARGET_IO_PORTD for PD2 (UART5_RX) - Update SERIAL_PORT_COUNT to 7 (includes UART5) - Remove config.c: UART2 has both standard and hardware-inverted SBUS pads --- src/main/target/FRSKYF405/config.c | 32 ---------- src/main/target/FRSKYF405/target.c | 51 +++------------ src/main/target/FRSKYF405/target.h | 99 ++++++++---------------------- 3 files changed, 36 insertions(+), 146 deletions(-) delete mode 100644 src/main/target/FRSKYF405/config.c diff --git a/src/main/target/FRSKYF405/config.c b/src/main/target/FRSKYF405/config.c deleted file mode 100644 index 0aed59803e0..00000000000 --- a/src/main/target/FRSKYF405/config.c +++ /dev/null @@ -1,32 +0,0 @@ -/* - * This file is part of INAV. - * - * INAV is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * INAV is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with INAV. If not, see . - */ - -#include -#include - -#include - -#include "rx/rx.h" -#include "fc/config.h" - -void targetConfiguration(void) -{ - // SBUS is received via a hardware inverter (2N7002E MOSFET, Q1). - // The signal arrives at UART2 RX already de-inverted (active high), - // so the UART must NOT apply additional software inversion. - rxConfigMutable()->serialrx_inverted = 1; -} diff --git a/src/main/target/FRSKYF405/target.c b/src/main/target/FRSKYF405/target.c index 89f3ee73a0e..0708a3981e4 100644 --- a/src/main/target/FRSKYF405/target.c +++ b/src/main/target/FRSKYF405/target.c @@ -22,49 +22,18 @@ #include "drivers/pwm_mapping.h" #include "drivers/timer.h" -/* - * Timer Allocation for FrSky F405 - * - * Connector mapping from schematic (CONN.SchDoc): - * - CON1 (S1): T4_2 = TIM4_CH2 = PB7 - * - CON2 (S2): T4_1 = TIM4_CH1 = PB6 - * - CON3 (S3): T3_3 = TIM3_CH3 = PB0 - * - CON4 (S4): T3_4 = TIM3_CH4 = PB1 - * - CON5 (S5): T8_3 = TIM8_CH3 = PC8 - * - CON6 (S6): T8_4 = TIM8_CH4 = PC9 - * - CON7 (S7): T12_1 = TIM12_CH1 = PB14 (no DMA - no DShot) - * - CON8 (S8): T12_2 = TIM12_CH2 = PB15 (no DMA - no DShot) - * - CON9 (S9): T1_1 = TIM1_CH1 = PA8 - * - CON23 (LED): T2_1 = TIM2_CH1 = PA15 - */ - timerHardware_t timerHardware[] = { - // Motor outputs S1-S9 (in physical connector order from schematic) - DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 (CON1) - Motor 1 - DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 (CON2) - Motor 2 - DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 (CON3) - Motor 3 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 (CON4) - Motor 4 - DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 1), // S5 (CON5) - Motor 5 UP(2,1) - DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 (CON6) - Motor 6 - DEF_TIM(TIM12, CH1, PB14, TIM_USE_OUTPUT_AUTO, 0, 0), // S7 (CON7) - Motor 7 NOTE: TIM12 has no DMA - DShot not supported - DEF_TIM(TIM12, CH2, PB15, TIM_USE_OUTPUT_AUTO, 0, 0), // S8 (CON8) - Motor 8 NOTE: TIM12 has no DMA - DShot not supported - DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 0), // S9 (CON9) - Motor 9 UP(2,5) + DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 + DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 1), // S5 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 + DEF_TIM(TIM12, CH1, PB14, TIM_USE_OUTPUT_AUTO, 0, 0), // S7 - no DShot (TIM12 has no DMA) + DEF_TIM(TIM12, CH2, PB15, TIM_USE_OUTPUT_AUTO, 0, 0), // S8 - no DShot (TIM12 has no DMA) + DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 0), // S9 - // LED Strip on CON23 - PA15 (T2_1 signal on schematic) - DEF_TIM(TIM2, CH1, PA15, TIM_USE_LED, 0, 0), // LED strip (CON23) + DEF_TIM(TIM2, CH1, PA15, TIM_USE_LED, 0, 0), // LED strip }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); - -/* - * IMPLEMENTATION NOTES: - * - * 1. S1/S2 order matches physical board labels (CON1=S1, CON2=S2 from schematic) - * - * 2. TIM12 (S7, S8): No DMA support on STM32F405. - * Works: Standard PWM, OneShot125, OneShot42, MultiShot - * Does NOT work: DShot (all variants), ProShot - * Use S7/S8 for non-DShot ESCs or servos - * - * 3. LED strip output: PA15 (T2_1) on CON23 with TIM2_CH1 - */ diff --git a/src/main/target/FRSKYF405/target.h b/src/main/target/FRSKYF405/target.h index e3eafdb51da..146519a99b8 100644 --- a/src/main/target/FRSKYF405/target.h +++ b/src/main/target/FRSKYF405/target.h @@ -22,17 +22,11 @@ #define USBD_PRODUCT_STRING "FrSkyF405" // *************** LED & BEEPER ********************** -// Status LEDs share SWD debug pins - only available in release builds -// In debug builds, these pins must remain in SWD mode for debugging -// Red LED is likely a power indicator (always-on, no GPIO control) -#ifndef DEBUG_BUILD - #define LED0 PA14 // Blue LED (shares SWCLK) via R34 1K - #define LED1 PA13 // Green LED (shares SWDIO) via R35 1K -#endif -// Red LED appears to be power indicator (connected to VCC, not GPIO) +#define LED0 PA14 // shares SWCLK +#define LED1 PA13 // shares SWDIO #define BEEPER PC15 -#define BEEPER_INVERTED // INAV beeper driver writes !onoff; BEEPER_INVERTED makes it write HIGH to activate the N-channel MOSFET (Q2) +#define BEEPER_INVERTED // *************** Gyro & ACC ********************** #define USE_SPI @@ -42,11 +36,11 @@ #define SPI1_MISO_PIN PA6 #define SPI1_MOSI_PIN PA7 -// IIM-42688P Gyro on SPI1 -#define USE_IMU_ICM42605 // IIM-42688P is compatible with ICM42605 driver +// IIM-42688P is compatible with ICM42605 driver +#define USE_IMU_ICM42605 #define ICM42605_CS_PIN PA4 #define ICM42605_SPI_BUS BUS_SPI1 -#define IMU_ICM42605_ALIGN CW0_DEG // TODO: Verify orientation from board layout +#define IMU_ICM42605_ALIGN CW0_DEG // *************** OSD ***************************** @@ -55,19 +49,16 @@ #define SPI2_MISO_PIN PC2 #define SPI2_MOSI_PIN PC3 -// AT7456E OSD on SPI2 #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI2 #define MAX7456_CS_PIN PB12 // *************** SD Card ************************* #define USE_SPI_DEVICE_3 -#define SPI3_SCK_PIN PC10 -#define SPI3_MISO_PIN PC11 -#define SPI3_MOSI_PIN PC12 +#define SPI3_SCK_PIN PB3 +#define SPI3_MISO_PIN PB4 +#define SPI3_MOSI_PIN PB5 -// SD Card on SPI3 -// NOTE: PC14 used for CS (Chip Select) - OSC32_IN pin used as GPIO for slow CS signal #define USE_SDCARD #define USE_SDCARD_SPI #define SDCARD_SPI_BUS BUS_SPI3 @@ -77,7 +68,6 @@ // *************** UART ***************************** #define USE_VCP -// Note: USB VBUS sensing not clearly defined in schematic #define USE_UART1 #define UART1_TX_PIN PA9 @@ -87,55 +77,47 @@ #define UART2_TX_PIN PA2 #define UART2_RX_PIN PA3 -// USART2 has SBUS inverter circuit +// UART2 has two pads: standard (for CRSF etc) and hardware-inverted SBUS #define SERIALRX_UART SERIAL_PORT_USART2 #define SERIALRX_PROVIDER SERIALRX_SBUS #define DEFAULT_RX_TYPE RX_TYPE_SERIAL -// UART3 conflicts with I2C2 (shares PB10/PB11) -// User must choose: enable UART3 OR I2C2, not both #define USE_UART3 -#define UART3_TX_PIN PB10 // Conflicts with I2C2_SCL -#define UART3_RX_PIN PB11 // Conflicts with I2C2_SDA +#define UART3_TX_PIN PC10 +#define UART3_RX_PIN PC11 #define USE_UART4 #define UART4_TX_PIN PA0 -#define UART4_RX_PIN PC5 // Shared with RSSI_ADC +#define UART4_RX_PIN PA1 + -// UART5 TX conflicts with SPI3_MOSI (PC12) -// User must choose: enable UART5 OR SD card, not both #define USE_UART5 -#define UART5_TX_PIN PC12 // Conflicts with SPI3_MOSI +#define UART5_TX_PIN PC12 #define UART5_RX_PIN PD2 #define USE_UART6 #define UART6_TX_PIN PC6 #define UART6_RX_PIN PC7 -#define SERIAL_PORT_COUNT 7 // VCP + UART1-6 +#define SERIAL_PORT_COUNT 7 // VCP + UART1-5, UART6 // *************** I2C **************************** #define USE_I2C -// I2C1 for barometer (SPL06) #define USE_I2C_DEVICE_1 #define I2C1_SCL PB8 #define I2C1_SDA PB9 -// I2C2 conflicts with UART3 (shares PB10/PB11) -// Enable one or the other, not both -// #define USE_I2C_DEVICE_2 -// #define I2C2_SCL PB10 // Conflicts with UART3_TX -// #define I2C2_SDA PB11 // Conflicts with UART3_RX +#define USE_I2C_DEVICE_2 +#define I2C2_SCL PB10 +#define I2C2_SDA PB11 #define DEFAULT_I2C_BUS BUS_I2C1 -// SPL06 Barometer on I2C1 @ address 0x76 #define USE_BARO #define BARO_I2C_BUS BUS_I2C1 #define USE_BARO_SPL06 -// Magnetometer on I2C (external via connector) #define USE_MAG #define MAG_I2C_BUS DEFAULT_I2C_BUS #define USE_MAG_ALL @@ -150,27 +132,24 @@ #define ADC_INSTANCE ADC1 #define ADC1_DMA_STREAM DMA2_Stream0 -#define ADC_CHANNEL_1_PIN PC0 // VBAT_ADC -#define ADC_CHANNEL_2_PIN PC1 // CURR_ADC -#define ADC_CHANNEL_3_PIN PC5 // RSSI_IN (shared with UART4_RX) +#define ADC_CHANNEL_1_PIN PC0 +#define ADC_CHANNEL_2_PIN PC1 +#define ADC_CHANNEL_3_PIN PC5 #define VBAT_ADC_CHANNEL ADC_CHN_1 #define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 #define RSSI_ADC_CHANNEL ADC_CHN_3 -// Current sensor: INA139 with 0.25mΩ shunt -#define CURRENT_METER_SCALE 250 // TODO: Calibrate based on INA139 + shunt values +#define CURRENT_METER_SCALE 250 #define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TELEMETRY | FEATURE_BLACKBOX) // *************** LED STRIP *********************** -// CON23 LED output - PA15 (T2_1 signal on schematic) #define USE_LED_STRIP -#define WS2811_PIN PA15 // TIM2_CH1, labeled as T2_1 on schematic +#define WS2811_PIN PA15 // *************** PWM OUTPUTS ********************* -// 9 motor outputs (S1-S9) -// Note: S7/S8 on TIM12 do not support DShot (no DMA on TIM12 for STM32F405) +// S7/S8 on TIM12 do not support DShot #define MAX_PWM_OUTPUT_PORTS 9 // *************** Other *************************** @@ -181,30 +160,4 @@ #define TARGET_IO_PORTA 0xffff #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC 0xffff -#define TARGET_IO_PORTD (BIT(2)) // PD2 used for UART5_RX - -// *************** NOTES & ISSUES ******************* -// -// DESIGN NOTES: -// 1. PC14 used for SPI3_NSS (SD card CS) - RTC clock domain, should be fine for slow CS signal -// -// 2. Status LEDs share SWD debug pins (PA13/PA14) - HANDLED WITH #ifndef DEBUG_BUILD -// Release builds: Pins configured as GPIO outputs for status LEDs -// Debug builds: Pins remain in SWD mode for debugging (LEDs disabled) -// Red LED appears to be power indicator (always-on, no GPIO control needed) -// -// PIN CONFLICTS (target or user must choose): -// 3. UART3 vs I2C2: PB10/PB11 shared -// - Enable UART3 for telemetry/GPS, OR -// - Enable I2C2 for additional sensors -// -// 4. UART5 TX vs SD Card: PC12 shared (SPI3_MOSI) -// - Enable UART5 for telemetry, OR -// - Enable SD card for blackbox logging -// -// 5. UART4_RX vs RSSI ADC: PC5 shared -// - May be intentional if RSSI comes via UART protocol -// -// MISSING INFORMATION: -// 6. Gyro orientation (IMU_ALIGN) unknown - needs physical board inspection -// 7. USB VBUS sensing pin not clearly defined in schematic (PC13 may be used) +#define TARGET_IO_PORTD (BIT(2)) // PD2 - UART5_RX From 06e7ef2c2d81e3ec77998e5baf7039659618f648 Mon Sep 17 00:00:00 2001 From: jp39 Date: Wed, 8 Oct 2025 12:50:28 +0200 Subject: [PATCH 17/48] AIKONF4V2 support. --- src/main/target/AIKONF4V2/CMakeLists.txt | 1 + src/main/target/AIKONF4V2/config.c | 27 +++++ src/main/target/AIKONF4V2/target.c | 39 +++++++ src/main/target/AIKONF4V2/target.h | 134 +++++++++++++++++++++++ 4 files changed, 201 insertions(+) create mode 100644 src/main/target/AIKONF4V2/CMakeLists.txt create mode 100644 src/main/target/AIKONF4V2/config.c create mode 100644 src/main/target/AIKONF4V2/target.c create mode 100644 src/main/target/AIKONF4V2/target.h diff --git a/src/main/target/AIKONF4V2/CMakeLists.txt b/src/main/target/AIKONF4V2/CMakeLists.txt new file mode 100644 index 00000000000..a9393354aea --- /dev/null +++ b/src/main/target/AIKONF4V2/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f405xg(AIKONF4V2) diff --git a/src/main/target/AIKONF4V2/config.c b/src/main/target/AIKONF4V2/config.c new file mode 100644 index 00000000000..fcb850b3f53 --- /dev/null +++ b/src/main/target/AIKONF4V2/config.c @@ -0,0 +1,27 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#include +#include + +#include + +#include "drivers/pwm_mapping.h" + +void targetConfiguration(void) +{ +} diff --git a/src/main/target/AIKONF4V2/target.c b/src/main/target/AIKONF4V2/target.c new file mode 100644 index 00000000000..54777bc509c --- /dev/null +++ b/src/main/target/AIKONF4V2/target.c @@ -0,0 +1,39 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#include + +#include "platform.h" +#include "drivers/io.h" + +#include "drivers/dma.h" +#include "drivers/timer.h" +#include "drivers/timer_def.h" + +timerHardware_t timerHardware[] = { + // DEF_TIM(TIM2, CH2, PB3, TIM_USE_CAMERA_CONTROL, 0, 0), // There is not camera control in INAV + DEF_TIM(TIM8, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 1), // S1 + DEF_TIM(TIM8, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0, 1), // S2 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 1), // S3 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 + DEF_TIM(TIM4, CH1, PB6, TIM_USE_LED, 0, 0), // LED +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/AIKONF4V2/target.h b/src/main/target/AIKONF4V2/target.h new file mode 100644 index 00000000000..a93415371bd --- /dev/null +++ b/src/main/target/AIKONF4V2/target.h @@ -0,0 +1,134 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#pragma once + +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_BLACKBOX | FEATURE_SOFTSERIAL) + +#define TARGET_BOARD_IDENTIFIER "AIK4" +#define USBD_PRODUCT_STRING "AIKONF4V2" + +// Beeper +#define USE_BEEPER +#define BEEPER PB5 +#define BEEPER_INVERTED +// Leds +#define USE_LED_STRIP +#define WS2811_PIN PB6 +#define LED0 PB4 +// UARTs +#define USB_IO +#define USE_VCP +#define USE_UART1 +#define UART1_RX_PIN PA10 +#define UART1_TX_PIN PA9 +#define USE_UART2 +#define UART2_TX_PIN PA2 +#define UART2_RX_PIN PA3 +#define USE_UART3 +#define UART3_RX_PIN PB11 +#define UART3_TX_PIN PB10 +#define USE_UART4 +#define UART4_TX_PIN PA0 +#define UART4_RX_PIN PA1 +#define SERIAL_PORT_COUNT 5 //VCP, UART1, UART2, UART3, UART4 +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS +#define SERIALRX_UART SERIAL_PORT_USART1 +// SPI +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define SPI1_NSS_PIN PA4 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 +#define USE_SPI_DEVICE_2 +#define SPI2_NSS_PIN PB12 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 +#define USE_SPI_DEVICE_3 +#define SPI3_NSS_PIN PA15 +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PC12 +// I2C +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 +#define DEFAULT_I2C_BUS BUS_I2C1 +#define EXTERNAL_I2C_BUS DEFAULT_I2C_BUS +// MAG +#define USE_MAG +#define MAG_I2C_BUS DEFAULT_I2C_BUS +#define USE_MAG_ALL +// ADC +#define USE_ADC +#define ADC_CHANNEL_1_PIN PC1 +#define ADC_CHANNEL_2_PIN PC2 +#define ADC_CHANNEL_3_PIN PC3 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_1 +#define VBAT_ADC_CHANNEL ADC_CHN_2 +#define RSSI_ADC_CHANNEL ADC_CHN_3 +#define ADC_INSTANCE ADC1 +// Gyro & ACC +#define USE_IMU_MPU6000 +#define MPU6000_CS_PIN SPI1_NSS_PIN +#define MPU6000_SPI_BUS BUS_SPI1 +#define IMU_MPU6000_ALIGN CW0_DEG +#define USE_IMU_ICM42605 +#define ICM42605_CS_PIN SPI1_NSS_PIN +#define ICM42605_SPI_BUS BUS_SPI1 +#define IMU_ICM42605_ALIGN CW90_DEG +// OSD +#define USE_MAX7456 +#define MAX7456_CS_PIN SPI3_NSS_PIN +#define MAX7456_SPI_BUS BUS_SPI3 +// Baro +#define USE_BARO +#define BARO_I2C_BUS DEFAULT_I2C_BUS +#define USE_BARO_ALL +// Blackbox +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT +#define M25P16_CS_PIN SPI2_NSS_PIN +#define M25P16_SPI_BUS BUS_SPI2 +#define USE_FLASHFS +#define USE_FLASH_M25P16 +// Others +#define TEMPERATURE_I2C_BUS DEFAULT_I2C_BUS +#define PITOT_I2C_BUS DEFAULT_I2C_BUS +#define RANGEFINDER_I2C_BUS DEFAULT_I2C_BUS + +#define MAX_PWM_OUTPUT_PORTS 4 +#define USE_SERIAL_4WAY_BLHELI_INTERFACE +#define USE_SERIALSHOT +#define USE_DSHOT +#define USE_ESC_SENSOR +#define VOLTAGE_METER_SCALE 110 +#define CURRENT_METER_SCALE 400 + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTE 0xffff +#define TARGET_IO_PORTF 0xffff + From 8ba58ccc5f4b5f820760baa0a666ce2b17f43524 Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Wed, 4 Mar 2026 12:05:51 -0600 Subject: [PATCH 18/48] frskyf405: Fix crystal frequency and accel alignment --- src/main/target/FRSKYF405/CMakeLists.txt | 2 +- src/main/target/FRSKYF405/target.h | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/target/FRSKYF405/CMakeLists.txt b/src/main/target/FRSKYF405/CMakeLists.txt index fc8eb478bcc..0e382d8acb7 100644 --- a/src/main/target/FRSKYF405/CMakeLists.txt +++ b/src/main/target/FRSKYF405/CMakeLists.txt @@ -1 +1 @@ -target_stm32f405xg(FRSKYF405) +target_stm32f405xg(FRSKYF405 HSE_MHZ 24) diff --git a/src/main/target/FRSKYF405/target.h b/src/main/target/FRSKYF405/target.h index 146519a99b8..12e61c5f29a 100644 --- a/src/main/target/FRSKYF405/target.h +++ b/src/main/target/FRSKYF405/target.h @@ -17,7 +17,7 @@ #pragma once -#define TARGET_BOARD_IDENTIFIER "FRF4" +#define TARGET_BOARD_IDENTIFIER "FR45" #define USBD_PRODUCT_STRING "FrSkyF405" @@ -40,7 +40,7 @@ #define USE_IMU_ICM42605 #define ICM42605_CS_PIN PA4 #define ICM42605_SPI_BUS BUS_SPI1 -#define IMU_ICM42605_ALIGN CW0_DEG +#define IMU_ICM42605_ALIGN CW180_DEG_FLIP // *************** OSD ***************************** From 8a475617c479a0fad7b3369ccea6c930f571eab4 Mon Sep 17 00:00:00 2001 From: Scavanger Date: Fri, 13 Mar 2026 21:30:03 -0300 Subject: [PATCH 19/48] SITL Tweaks --- cmake/main.cmake | 6 +- cmake/sitl.cmake | 10 +- src/main/config/config_streamer_file.c | 1 - src/main/scheduler/scheduler.c | 50 ++ src/main/target/SITL/sim/realFlight.c | 217 ++++---- src/main/target/SITL/sim/realFlight.h | 1 + src/main/target/SITL/sim/simple_soap_client.c | 172 ------ src/main/target/SITL/sim/soap_client.c | 516 ++++++++++++++++++ .../{simple_soap_client.h => soap_client.h} | 34 +- src/main/target/SITL/sim/xplane.c | 118 ++-- src/main/target/SITL/sim/xplane.h | 1 + src/main/target/SITL/target.c | 29 +- 12 files changed, 822 insertions(+), 333 deletions(-) delete mode 100644 src/main/target/SITL/sim/simple_soap_client.c create mode 100644 src/main/target/SITL/sim/soap_client.c rename src/main/target/SITL/sim/{simple_soap_client.h => soap_client.h} (60%) diff --git a/cmake/main.cmake b/cmake/main.cmake index d8e2dddf3ec..1b987204b25 100644 --- a/cmake/main.cmake +++ b/cmake/main.cmake @@ -108,10 +108,10 @@ function(setup_firmware_target exe name) endfunction() function(exclude_from_all target) - set_property(TARGET ${target} PROPERTY + set_target_properties(${target} PROPERTIES TARGET_MESSAGES OFF - EXCLUDE_FROM_ALL 1 - EXCLUDE_FROM_DEFAULT_BUILD 1) + EXCLUDE_FROM_ALL ON + EXCLUDE_FROM_DEFAULT_BUILD ON) endfunction() function(collect_targets) diff --git a/cmake/sitl.cmake b/cmake/sitl.cmake index 39e6456830a..d93f0b89d7d 100644 --- a/cmake/sitl.cmake +++ b/cmake/sitl.cmake @@ -19,8 +19,8 @@ main_sources(SITL_SRC target/SITL/sim/realFlight.h target/SITL/sim/simHelper.c target/SITL/sim/simHelper.h - target/SITL/sim/simple_soap_client.c - target/SITL/sim/simple_soap_client.h + target/SITL/sim/soap_client.c + target/SITL/sim/soap_client.h target/SITL/sim/xplane.c target/SITL/sim/xplane.h ) @@ -159,8 +159,8 @@ function (target_sitl name) WORKING_DIRECTORY ${CMAKE_BINARY_DIR} COMMAND ${generator_cmd} clean COMMENT "Removing intermediate files for ${name}") - set_property(TARGET ${clean_target} PROPERTY - EXCLUDE_FROM_ALL 1 - EXCLUDE_FROM_DEFAULT_BUILD 1) + set_target_properties(${clean_target} PROPERTIES + EXCLUDE_FROM_ALL ON + EXCLUDE_FROM_DEFAULT_BUILD ON) endif() endfunction() diff --git a/src/main/config/config_streamer_file.c b/src/main/config/config_streamer_file.c index 379155c22e1..86d677498c1 100644 --- a/src/main/config/config_streamer_file.c +++ b/src/main/config/config_streamer_file.c @@ -103,7 +103,6 @@ int config_streamer_impl_write_word(config_streamer_t *c, config_streamer_buffer if ((c->address >= (uintptr_t)eepromData) && (c->address < (uintptr_t)ARRAYEND(eepromData))) { *((uint32_t*)c->address) = *buffer; - fprintf(stderr, "[EEPROM] Program word %p = %08x\n", (void*)c->address, *((uint32_t*)c->address)); } else { fprintf(stderr, "[EEPROM] Program word %p out of range!\n", (void*)c->address); } diff --git a/src/main/scheduler/scheduler.c b/src/main/scheduler/scheduler.c index e04a92a8993..ad21c32359e 100755 --- a/src/main/scheduler/scheduler.c +++ b/src/main/scheduler/scheduler.c @@ -19,6 +19,10 @@ #include #include +#if defined(SITL_BUILD) +#include +#endif + #include "platform.h" #include "scheduler.h" @@ -215,11 +219,22 @@ void FAST_CODE NOINLINE scheduler(void) uint16_t selectedTaskDynamicPriority = 0; bool forcedRealTimeTask = false; +#if defined(SITL_BUILD) + // Track the earliest time at which the next task will become due so we can + // sleep until then instead of busy-waiting. Cap at 1 ms so event-driven + // tasks (checkFunc) are still polled frequently enough. + timeUs_t sitlEarliestNextTaskAt = currentTimeUs + 1000; + bool sitlHasCheckFuncTask = false; +#endif + // Update task dynamic priorities uint16_t waitingTasks = 0; for (cfTask_t *task = queueFirst(); task != NULL; task = queueNext()) { // Task has checkFunc - event driven if (task->checkFunc) { +#if defined(SITL_BUILD) + sitlHasCheckFuncTask = true; +#endif const timeUs_t currentTimeBeforeCheckFuncCallUs = micros(); // Increase priority for event driven tasks @@ -248,7 +263,19 @@ void FAST_CODE NOINLINE scheduler(void) waitingTasks++; forcedRealTimeTask = true; } +#if defined(SITL_BUILD) + const timeUs_t taskNextAt = task->lastExecutedAt + (timeUs_t)task->desiredPeriod; + if (taskNextAt < sitlEarliestNextTaskAt) { + sitlEarliestNextTaskAt = taskNextAt; + } +#endif } else { +#if defined(SITL_BUILD) + const timeUs_t taskNextAt = task->lastExecutedAt + (timeUs_t)task->desiredPeriod; + if (taskNextAt < sitlEarliestNextTaskAt) { + sitlEarliestNextTaskAt = taskNextAt; + } +#endif // Task is time-driven, dynamicPriority is last execution age (measured in desiredPeriods) // Task age is calculated from last execution task->taskAgeCycles = ((timeDelta_t)(currentTimeUs - task->lastExecutedAt)) / task->desiredPeriod; @@ -294,4 +321,27 @@ void FAST_CODE NOINLINE scheduler(void) selectedTask->totalExecutionTime += taskExecutionTime; // time consumed by scheduler + task selectedTask->maxExecutionTime = MAX(selectedTask->maxExecutionTime, taskExecutionTime); } + +#if defined(SITL_BUILD) + { + // Avoid busy-waiting and burning 100% CPU in SITL. After executing the + // current task (or finding nothing to do), sleep until just before the + // next task is due. For event-driven tasks (checkFunc) we limit the + // sleep so the check function is still called frequently. + if (sitlHasCheckFuncTask) { + // Poll event-driven tasks at least every 500 µs + const timeUs_t eventCap = micros() + 500; + if (eventCap < sitlEarliestNextTaskAt) { + sitlEarliestNextTaskAt = eventCap; + } + } + const timeUs_t nowUs = micros(); + if (sitlEarliestNextTaskAt > nowUs + 50) { + const timeDelta_t sleepUs = (timeDelta_t)(sitlEarliestNextTaskAt - nowUs) - 50; + if (sleepUs > 0) { + usleep((useconds_t)sleepUs); + } + } + } +#endif } diff --git a/src/main/target/SITL/sim/realFlight.c b/src/main/target/SITL/sim/realFlight.c index a8d241425d1..6477de23f42 100644 --- a/src/main/target/SITL/sim/realFlight.c +++ b/src/main/target/SITL/sim/realFlight.c @@ -28,14 +28,14 @@ #include #include #include +#include #include #include "platform.h" #include "target.h" #include "target/SITL/sim/realFlight.h" -#include "target/SITL/sim/simple_soap_client.h" -#include "target/SITL/sim/xplane.h" +#include "target/SITL/sim/soap_client.h" #include "target/SITL/sim/simHelper.h" #include "fc/runtime_config.h" #include "drivers/time.h" @@ -55,8 +55,9 @@ #include "flight/imu.h" #include "io/gps.h" #include "rx/sim.h" +#include "realFlight.h" -#define RF_PORT 18083 +#define RF_PORT "18083" #define RF_MAX_CHANNEL_COUNT 12 // "RealFlight Ranch" is located in Sierra Nevada, southern Spain @@ -67,17 +68,14 @@ static uint8_t pwmMapping[RF_MAX_PWM_OUTS]; static uint8_t mappingCount; -static pthread_cond_t sockcond1 = PTHREAD_COND_INITIALIZER; -static pthread_cond_t sockcond2 = PTHREAD_COND_INITIALIZER; -static pthread_mutex_t sockmtx = PTHREAD_MUTEX_INITIALIZER; +static pthread_mutex_t initMutex = PTHREAD_MUTEX_INITIALIZER; +static pthread_cond_t initCond = PTHREAD_COND_INITIALIZER; +static atomic_bool shouldStopSoapThread = false; -static soap_client_t *client = NULL; -static soap_client_t *clientNext = NULL; +static soap_client_t client; +static pthread_t soapThread = 0; -static pthread_t soapThread; -static pthread_t creationThread; - -static bool isInitalised = false; +static atomic_bool isInitalised = false; static bool useImu = false; typedef struct @@ -133,38 +131,6 @@ typedef struct rfValues_t rfValues; -static void deleteClient(soap_client_t *client) -{ - soapClientClose(client); - free(client); - client = NULL; -} - -static void startRequest(char* action, const char* fmt, ...) -{ - pthread_mutex_lock(&sockmtx); - while (clientNext == NULL) { - pthread_cond_wait(&sockcond1, &sockmtx); - } - - client = clientNext; - clientNext = NULL; - - pthread_cond_broadcast(&sockcond2); - pthread_mutex_unlock(&sockmtx); - - va_list va; - va_start(va, fmt); - soapClientSendRequestVa(client, action, fmt, va); - va_end(va); -} - -static char* endRequest(void) -{ - char* ret = soapClientReceive(client); - deleteClient(client); - return ret; -} // Simple, but fast ;) static double getDoubleFromResponse(const char* response, const char* elementName) @@ -261,6 +227,7 @@ static float convertAzimuth(float azimuth) return 360 - fmodf(azimuth + 90, 360.0f); } + static void exchangeData(void) { double servoValues[RF_MAX_PWM_OUTS] = { }; @@ -272,14 +239,32 @@ static void exchangeData(void) } } - startRequest("ExchangeData", "%u" - "%.4f%.4f%.4f%.4f%.4f%.4f%.4f%.4f" - "%.4f%.4f%.4f%.4f", - 0xFFF, - servoValues[0], servoValues[1], servoValues[2], servoValues[3], servoValues[4], servoValues[5], servoValues[6], servoValues[7], - servoValues[8], servoValues[9], servoValues[10], servoValues[11]); - char* response = endRequest(); + char requestBody[1024] = "12"; + + for (int i = 0; i < RF_MAX_CHANNEL_COUNT; i++) { + char value[32]; + snprintf(value, sizeof(value), "%.4f", servoValues[i]); + strncat(requestBody, value, sizeof(requestBody) - strlen(requestBody) - 1); + } + strncat(requestBody, "", sizeof(requestBody) - strlen(requestBody) - 1); + + char *response = NULL; + int http_status = 0; + int ret = soap_client_call_raw_body( + &client, + "ExchangeData", + requestBody, + &response, + &http_status + ); + + if (ret < 0 || http_status != 200 || !response) { + fprintf(stderr, "[SIM] Data exchange with RealFlight failed.\n"); + free(response); + return; + } + //rfValues.m_currentPhysicsTime_SEC = getDoubleFromResponse(response, "m-currentPhysicsTime-SEC"); //rfValues.m_currentPhysicsSpeedMultiplier = getDoubleFromResponse(response, "m-currentPhysicsSpeedMultiplier"); rfValues.m_airspeed_MPS = getDoubleFromResponse(response, "m-airspeed-MPS"); @@ -410,50 +395,71 @@ static void exchangeData(void) free(response); } -static void* soapWorker(void* arg) +static bool restoreOriginalControllerDevice(void) { - UNUSED(arg); - while(true) - { - if (!isInitalised) { - startRequest("RestoreOriginalControllerDevice", "12"); - free(endRequest()); - startRequest("InjectUAVControllerInterface", "12"); - free(endRequest()); - exchangeData(); - ENABLE_ARMING_FLAG(SIMULATOR_MODE_SITL); - - isInitalised = true; - } + char* response = NULL; + int http_status = 0; + const int ret = soap_client_call_raw_body( + &client, + "RestoreOriginalControllerDevice", + "12", + &response, + &http_status + ); - exchangeData(); - unlockMainPID(); + if (ret < 0 || (http_status != 200 && http_status != 500) || !response) { + free(response); + return false; } - return NULL; + free(response); + return true; } - -static void* creationWorker(void* arg) +static void* soapWorker(void* arg) { - char* ip = (char*)arg; - - while (true) { - pthread_mutex_lock(&sockmtx); - while (clientNext != NULL) { - pthread_cond_wait(&sockcond2, &sockmtx); - } - pthread_mutex_unlock(&sockmtx); - - soap_client_t *cli = malloc(sizeof(soap_client_t)); - if (!soapClientConnect(cli, ip, RF_PORT)) { - continue; + UNUSED(arg); + while(!atomic_load(&shouldStopSoapThread)) { + + if (!atomic_load(&isInitalised)) { + // Initialize RealFlight + + // Alway try to restore the original controller device first to avoid problems with broken connections and the interface being stuck in a half-initialised state. + // RealFlight seems to not properly close the connection on its side if the connection is interrupted, but only after a timeout of about 30 seconds. + // During this time the interface is not usable, but without this step it would be stuck in an unusable state until the next restart of the SITL. + if (!restoreOriginalControllerDevice()) { + fprintf(stderr, "[SIM] Failed to restore original controller device in RealFlight. RealFlight might be in a broken state. Retrying...\n"); + delay(1000); + continue; + } + + char* response = NULL; + int http_status = 0; + const int ret = soap_client_call_raw_body( + &client, + "InjectUAVControllerInterface", + "12", + &response, + &http_status + ); + + if (ret < 0 || http_status != 200 || !response) { + free(response); + delay(1000); + continue; + } + + exchangeData(); // Get initial data and set initial state in RealFlight + + ENABLE_ARMING_FLAG(SIMULATOR_MODE_SITL); + atomic_store(&isInitalised, true); + pthread_mutex_lock(&initMutex); + pthread_cond_signal(&initCond); + pthread_mutex_unlock(&initMutex); } - - clientNext = cli; - pthread_mutex_lock(&sockmtx); - pthread_cond_broadcast(&sockcond1); - pthread_mutex_unlock(&sockmtx); + + exchangeData(); + unlockMainPID(); } return NULL; @@ -465,19 +471,46 @@ bool simRealFlightInit(char* ip, uint8_t* mapping, uint8_t mapCount, bool imu) mappingCount = mapCount; useImu = imu; - if (pthread_create(&soapThread, NULL, soapWorker, NULL) < 0) { + if (soap_client_init(&client, ip, RF_PORT, "/", 1000) < 0) { return false; } - if (pthread_create(&creationThread, NULL, creationWorker, (void*)ip) < 0) { + atomic_store(&isInitalised, false); + atomic_store(&shouldStopSoapThread, false); + if (pthread_create(&soapThread, NULL, soapWorker, &client) < 0) { return false; } // Wait until the connection is established, the interface has been initialised - // and the first valid packet has been received to avoid problems with the startup calibration. - while (!isInitalised) { - delay(250); + // and the first valid packet has been received to avoid problems with the startup calibration. + pthread_mutex_lock(&initMutex); + while (!atomic_load(&isInitalised)) { + pthread_cond_wait(&initCond, &initMutex); } + pthread_mutex_unlock(&initMutex); return true; } + +void simRealFlightClose(void) +{ + atomic_store(&shouldStopSoapThread, true); + if (soapThread) { + pthread_join(soapThread, NULL); + } + + if (atomic_load(&isInitalised)) { + + if (!restoreOriginalControllerDevice( )) { + fprintf(stderr, "[SIM] Failed to restore original controller device in RealFlight.\n"); + } else { + fprintf(stderr, "[SIM] Restored original controller device in RealFlight.\n"); + } + } + + DISABLE_ARMING_FLAG(SIMULATOR_MODE_SITL); + atomic_store(&isInitalised, false); + pthread_mutex_destroy(&initMutex); + pthread_cond_destroy(&initCond); + soap_client_destroy(&client); +} diff --git a/src/main/target/SITL/sim/realFlight.h b/src/main/target/SITL/sim/realFlight.h index af34c2c524e..80e27c9087c 100644 --- a/src/main/target/SITL/sim/realFlight.h +++ b/src/main/target/SITL/sim/realFlight.h @@ -27,3 +27,4 @@ #define RF_MAX_PWM_OUTS 12 bool simRealFlightInit(char* ip, uint8_t* mapping, uint8_t mapCount, bool imu); +void simRealFlightClose(void); diff --git a/src/main/target/SITL/sim/simple_soap_client.c b/src/main/target/SITL/sim/simple_soap_client.c deleted file mode 100644 index e6400ae3b14..00000000000 --- a/src/main/target/SITL/sim/simple_soap_client.c +++ /dev/null @@ -1,172 +0,0 @@ -/* - * This file is part of INAV Project. - * - * This Source Code Form is subject to the terms of the Mozilla Public - * License, v. 2.0. If a copy of the MPL was not distributed with this file, - * You can obtain one at http://mozilla.org/MPL/2.0/. - * - * Alternatively, the contents of this file may be used under the terms - * of the GNU General Public License Version 3, as described below: - * - * This file is free software: you may copy, redistribute and/or modify - * it under the terms of the GNU General Public License as published by the - * Free Software Foundation, either version 3 of the License, or (at your - * option) any later version. - * - * This file is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General - * Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program. If not, see http://www.gnu.org/licenses/. - */ - -#define _GNU_SOURCE - -#include -#include -#include -#include -#include -#include -#include -# include -# include -#include -#include - -#include "simple_soap_client.h" - -#define REC_BUF_SIZE 6000 -char recBuffer[REC_BUF_SIZE]; - -bool soapClientConnect(soap_client_t *client, const char *address, int port) -{ - client->sockedFd = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP); - if (client->sockedFd < 0) { - return false; - } - - int one = 1; - if (setsockopt(client->sockedFd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one)) < 0) { - return false; - } - - client->socketAddr.sin_family = AF_INET; - client->socketAddr.sin_port = htons(port); - client->socketAddr.sin_addr.s_addr = inet_addr(address); - - if (connect(client->sockedFd, (struct sockaddr*)&client->socketAddr, sizeof(client->socketAddr)) < 0) { - return false; - } - - client->isConnected = true; - client->isInitalised = true; - - return true; -} - -void soapClientClose(soap_client_t *client) -{ - close(client->sockedFd); - memset(client, 0, sizeof(soap_client_t)); - client->isConnected = false; - client->isInitalised = false; -} - -void soapClientSendRequestVa(soap_client_t *client, const char* action, const char *fmt, va_list va) -{ - if (!client->isConnected) { - return; - } - - char* requestBody; - if (vasprintf(&requestBody, fmt, va) < 0) { - return; - } - - char* request; - if (asprintf(&request, "POST / HTTP/1.1\r\nsoapaction: %s\r\ncontent-length: %u\r\ncontent-type: text/xml;charset='UTF-8'\r\nConnection: Keep-Alive\r\n\r\n%s", - action, (unsigned)strlen(requestBody), requestBody) < 0) { - return; - } - - send(client->sockedFd, request, strlen(request), 0); - - free(requestBody); - free(request); -} - -void soapClientSendRequest(soap_client_t *client, const char* action, const char *fmt, ...) -{ - va_list va; - - va_start(va, fmt); - soapClientSendRequestVa(client, action, fmt, va); - va_end(va); -} - -static bool soapClientPoll(soap_client_t *client, uint32_t timeout_ms) -{ - fd_set fds; - struct timeval tv; - - FD_ZERO(&fds); - FD_SET(client->sockedFd, &fds); - - tv.tv_sec = timeout_ms / 1000; - tv.tv_usec = (timeout_ms % 1000) * 1000UL; - - if (select(client->sockedFd + 1, &fds, NULL, NULL, &tv) != 1) { - return false; - } - return true; -} - - -char* soapClientReceive(soap_client_t *client) -{ - if (!client->isInitalised){ - return false; - } - - if (!soapClientPoll(client, 1000)) { - return false; - } - - ssize_t size = recv(client->sockedFd, recBuffer, REC_BUF_SIZE, 0); - - if (size <= 0) { - return NULL; - } - - char* pos = strstr(recBuffer, "Content-Length: "); - if (!pos) { - return NULL; - } - - uint32_t contentLength = strtoul(pos + 16, NULL, 10); - char *body = strstr(pos, "\r\n\r\n"); - if (!body) { - return NULL; - } - - body += 4; - - ssize_t expectedLength = contentLength + body - recBuffer; - if ((unsigned)expectedLength >= sizeof(recBuffer)) { - return NULL; - } - - while (size < expectedLength){ - ssize_t size2 = recv(client->sockedFd, &recBuffer[size], sizeof(recBuffer - size + 1), 0); - if (size2 <= 0) { - return NULL; - } - size += size2; - } - - recBuffer[size] = '\0'; - return strdup(body); -} diff --git a/src/main/target/SITL/sim/soap_client.c b/src/main/target/SITL/sim/soap_client.c new file mode 100644 index 00000000000..6ac5f65826a --- /dev/null +++ b/src/main/target/SITL/sim/soap_client.c @@ -0,0 +1,516 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +#define _POSIX_C_SOURCE 200809L + +#include "soap_client.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +static int set_socket_timeout(int fd, int timeout_ms) { + struct timeval tv; + tv.tv_sec = timeout_ms / 1000; + tv.tv_usec = (timeout_ms % 1000) * 1000; + + if (setsockopt(fd, SOL_SOCKET, SO_SNDTIMEO, &tv, sizeof(tv)) != 0) { + return -1; + } + if (setsockopt(fd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv)) != 0) { + return -1; + } + return 0; +} + +static int connect_with_timeout(const struct addrinfo* ai, int timeout_ms) { + int fd = socket(ai->ai_family, ai->ai_socktype, ai->ai_protocol); + if (fd < 0) { + return -1; + } + + int flags = fcntl(fd, F_GETFL, 0); + if (flags < 0) { + close(fd); + return -1; + } + if (fcntl(fd, F_SETFL, flags | O_NONBLOCK) < 0) { + close(fd); + return -1; + } + + int one = 1; + if (setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one)) != 0) { + close(fd); + return -1; + } + + int rc = connect(fd, ai->ai_addr, ai->ai_addrlen); + if (rc == 0) { + (void)fcntl(fd, F_SETFL, flags); + return fd; + } + if (errno != EINPROGRESS) { + close(fd); + return -1; + } + + fd_set wfds; + FD_ZERO(&wfds); + FD_SET(fd, &wfds); + struct timeval tv; + tv.tv_sec = timeout_ms / 1000; + tv.tv_usec = (timeout_ms % 1000) * 1000; + + rc = select(fd + 1, NULL, &wfds, NULL, &tv); + if (rc <= 0) { + close(fd); + errno = (rc == 0) ? ETIMEDOUT : errno; + return -1; + } + + int so_error = 0; + socklen_t so_error_len = sizeof(so_error); + if (getsockopt(fd, SOL_SOCKET, SO_ERROR, &so_error, &so_error_len) != 0 || so_error != 0) { + close(fd); + errno = (so_error != 0) ? so_error : errno; + return -1; + } + + if (fcntl(fd, F_SETFL, flags) < 0) { + close(fd); + return -1; + } + + return fd; +} + +static int open_tcp_connection(const char* host, const char* port, int timeout_ms) { + struct addrinfo hints; + memset(&hints, 0, sizeof(hints)); + hints.ai_family = AF_UNSPEC; + hints.ai_socktype = SOCK_STREAM; + + struct addrinfo* result = NULL; + if (getaddrinfo(host, port, &hints, &result) != 0) { + return -1; + } + + int fd = -1; + for (const struct addrinfo* ai = result; ai != NULL; ai = ai->ai_next) { + fd = connect_with_timeout(ai, timeout_ms); + if (fd >= 0) { + if (set_socket_timeout(fd, timeout_ms) == 0) { + break; + } + close(fd); + fd = -1; + } + } + + freeaddrinfo(result); + return fd; +} + +static int send_all(int fd, const char* data, size_t len) { + size_t total = 0; + while (total < len) { + ssize_t n = send(fd, data + total, len - total, 0); + if (n < 0) { + if (errno == EINTR) { + continue; + } + return -1; + } + if (n == 0) { + return -1; + } + total += (size_t)n; + } + return 0; +} + +static char* recv_all(int fd, size_t* out_len) { + size_t cap = 8192; + size_t len = 0; + char* buf = (char*)malloc(cap); + if (!buf) { + return NULL; + } + + while (1) { + if (len == cap) { + size_t new_cap = cap * 2; + char* next = (char*)realloc(buf, new_cap); + if (!next) { + free(buf); + return NULL; + } + buf = next; + cap = new_cap; + } + + ssize_t n = recv(fd, buf + len, cap - len, 0); + if (n < 0) { + if (errno == EINTR) { + continue; + } + free(buf); + return NULL; + } + if (n == 0) { + break; + } + len += (size_t)n; + } + + char* out = (char*)realloc(buf, len + 1); + if (!out) { + free(buf); + return NULL; + } + out[len] = '\0'; + *out_len = len; + return out; +} + +static int parse_http_status(const char* response) { + int status = 0; + if (sscanf(response, "HTTP/%*d.%*d %d", &status) == 1) { + return status; + } + return 0; +} + +static char* dechunk_http_body(const char* body, size_t body_len, size_t* out_len) { + size_t cap = body_len + 1; + char* out = (char*)malloc(cap); + if (!out) { + return NULL; + } + size_t out_pos = 0; + size_t pos = 0; + + while (pos < body_len) { + const char* line_end = strstr(body + pos, "\r\n"); + if (!line_end) { + free(out); + return NULL; + } + + char size_buf[32]; + size_t size_len = (size_t)(line_end - (body + pos)); + if (size_len == 0 || size_len >= sizeof(size_buf)) { + free(out); + return NULL; + } + memcpy(size_buf, body + pos, size_len); + size_buf[size_len] = '\0'; + + char* endptr = NULL; + unsigned long chunk_size = strtoul(size_buf, &endptr, 16); + if (endptr == size_buf) { + free(out); + return NULL; + } + + pos = (size_t)(line_end - body) + 2; + if (chunk_size == 0) { + break; + } + if (pos + chunk_size + 2 > body_len) { + free(out); + return NULL; + } + + if (out_pos + chunk_size + 1 > cap) { + size_t new_cap = cap; + while (out_pos + chunk_size + 1 > new_cap) { + new_cap *= 2; + } + char* next = (char*)realloc(out, new_cap); + if (!next) { + free(out); + return NULL; + } + out = next; + cap = new_cap; + } + + memcpy(out + out_pos, body + pos, chunk_size); + out_pos += chunk_size; + pos += chunk_size; + + if (!(body[pos] == '\r' && body[pos + 1] == '\n')) { + free(out); + return NULL; + } + pos += 2; + } + + out[out_pos] = '\0'; + *out_len = out_pos; + return out; +} + +static char* extract_soap_body_raw(const char* xml, size_t xml_len) { + const char* p = xml; + const char* end = xml + xml_len; + + const char* body_open = NULL; + const char* body_name_start = NULL; + const char* body_name_end = NULL; + + while (p < end) { + const char* lt = strchr(p, '<'); + if (!lt || lt >= end) { + break; + } + if (lt + 1 < end && (lt[1] == '/' || lt[1] == '?' || lt[1] == '!')) { + p = lt + 1; + continue; + } + const char* gt = strchr(lt, '>'); + if (!gt || gt >= end) { + break; + } + + const char* name_start = lt + 1; + const char* name_end = name_start; + while (name_end < gt && *name_end != ' ' && *name_end != '\t' && *name_end != '\r' && *name_end != '\n' && *name_end != '/') { + name_end++; + } + + size_t name_len = (size_t)(name_end - name_start); + if (name_len >= 4 && strncmp(name_end - 4, "Body", 4) == 0) { + body_open = gt + 1; + body_name_start = name_start; + body_name_end = name_end; + break; + } + p = gt + 1; + } + + if (!body_open || !body_name_start || !body_name_end) { + return NULL; + } + + char close_tag[128]; + size_t tag_len = (size_t)(body_name_end - body_name_start); + if (tag_len + 4 >= sizeof(close_tag)) { + return NULL; + } + close_tag[0] = '<'; + close_tag[1] = '/'; + memcpy(close_tag + 2, body_name_start, tag_len); + close_tag[tag_len + 2] = '>'; + close_tag[tag_len + 3] = '\0'; + + const char* close = strstr(body_open, close_tag); + if (!close) { + return NULL; + } + + size_t inner_len = (size_t)(close - body_open); + char* inner = (char*)malloc(inner_len + 1); + if (!inner) { + return NULL; + } + memcpy(inner, body_open, inner_len); + inner[inner_len] = '\0'; + return inner; +} + +int soap_client_init(soap_client_t* c, const char* host, const char* port, const char* path, int timeout_ms) { + if (!c || !host || !port || !path) { + return -1; + } + memset(c, 0, sizeof(*c)); + if (snprintf(c->host, sizeof(c->host), "%s", host) >= (int)sizeof(c->host)) { + return -1; + } + if (snprintf(c->port, sizeof(c->port), "%s", port) >= (int)sizeof(c->port)) { + return -1; + } + if (snprintf(c->path, sizeof(c->path), "%s", path) >= (int)sizeof(c->path)) { + return -1; + } + c->timeout_ms = timeout_ms; + return pthread_mutex_init(&c->lock, NULL); +} + +void soap_client_destroy(soap_client_t* c) { + if (c) { + pthread_mutex_destroy(&c->lock); + } +} + +int soap_client_call_raw_body(soap_client_t* c, + const char* soap_action, + const char* request_body_xml, + char** response_body_xml, + int* http_status) { + if (!c || !request_body_xml || !response_body_xml || !http_status) { + return -1; + } + + char host[256]; + char port[16]; + char path[256]; + int timeout_ms = 0; + + pthread_mutex_lock(&c->lock); + snprintf(host, sizeof(host), "%s", c->host); + snprintf(port, sizeof(port), "%s", c->port); + snprintf(path, sizeof(path), "%s", c->path); + timeout_ms = c->timeout_ms; + pthread_mutex_unlock(&c->lock); + + const char* envelope_prefix = + "" + "" + ""; + const char* envelope_suffix = ""; + + size_t body_len = strlen(request_body_xml); + size_t envelope_len = strlen(envelope_prefix) + body_len + strlen(envelope_suffix); + char* envelope = (char*)malloc(envelope_len + 1); + if (!envelope) { + return -1; + } + snprintf(envelope, envelope_len + 1, "%s%s%s", envelope_prefix, request_body_xml, envelope_suffix); + + const char* action_header = (soap_action && soap_action[0] != '\0') ? soap_action : ""; + int req_len = snprintf(NULL, + 0, + "POST %s HTTP/1.1\r\n" + "Host: %s:%s\r\n" + "Content-Type: text/xml; charset=utf-8\r\n" + "SOAPAction: \"%s\"\r\n" + "Content-Length: %zu\r\n" + "Connection: close\r\n\r\n%s", + path, + host, + port, + action_header, + envelope_len, + envelope); + if (req_len < 0) { + free(envelope); + return -1; + } + + char* request = (char*)malloc((size_t)req_len + 1); + if (!request) { + free(envelope); + return -1; + } + snprintf(request, + (size_t)req_len + 1, + "POST %s HTTP/1.1\r\n" + "Host: %s:%s\r\n" + "Content-Type: text/xml; charset=utf-8\r\n" + "SOAPAction: \"%s\"\r\n" + "Content-Length: %zu\r\n" + "Connection: close\r\n\r\n%s", + path, + host, + port, + action_header, + envelope_len, + envelope); + + int fd = open_tcp_connection(host, port, timeout_ms); + if (fd < 0) { + free(request); + free(envelope); + return -1; + } + + int rc = 0; + size_t resp_len = 0; + char* response = NULL; + char* body = NULL; + char* soap_body = NULL; + + if (send_all(fd, request, (size_t)req_len) != 0) { + rc = -1; + goto cleanup; + } + + response = recv_all(fd, &resp_len); + if (!response || resp_len == 0) { + rc = -1; + goto cleanup; + } + + *http_status = parse_http_status(response); + + char* headers_end = strstr(response, "\r\n\r\n"); + if (!headers_end) { + rc = -1; + goto cleanup; + } + char* raw_body = headers_end + 4; + size_t raw_body_len = resp_len - (size_t)(raw_body - response); + + if (strstr(response, "Transfer-Encoding: chunked") || strstr(response, "transfer-encoding: chunked")) { + size_t decoded_len = 0; + body = dechunk_http_body(raw_body, raw_body_len, &decoded_len); + if (!body) { + rc = -1; + goto cleanup; + } + soap_body = extract_soap_body_raw(body, decoded_len); + } else { + soap_body = extract_soap_body_raw(raw_body, raw_body_len); + } + + if (!soap_body) { + rc = -1; + goto cleanup; + } + + *response_body_xml = soap_body; + soap_body = NULL; + +cleanup: + if (fd >= 0) { + close(fd); + } + free(request); + free(envelope); + free(response); + free(body); + free(soap_body); + return rc; +} diff --git a/src/main/target/SITL/sim/simple_soap_client.h b/src/main/target/SITL/sim/soap_client.h similarity index 60% rename from src/main/target/SITL/sim/simple_soap_client.h rename to src/main/target/SITL/sim/soap_client.h index 8522b28859a..ae8cd8cedcf 100644 --- a/src/main/target/SITL/sim/simple_soap_client.h +++ b/src/main/target/SITL/sim/soap_client.h @@ -22,27 +22,25 @@ * along with this program. If not, see http://www.gnu.org/licenses/. */ -#include -#include -#include +#pragma once -#define SOAP_REC_BUF_SIZE 256 * 1024 +#include typedef struct { - int sockedFd; - struct sockaddr_in socketAddr; - bool isInitalised; - bool isConnected; + char host[256]; + char port[16]; + char path[256]; + int timeout_ms; + pthread_mutex_t lock; } soap_client_t; -typedef struct { - soap_client_t client; - char* content; -} send_info_t; - +int soap_client_init(soap_client_t* c, const char* host, const char* port, const char* path, int timeout_ms); +void soap_client_destroy(soap_client_t* c); -bool soapClientConnect(soap_client_t *client, const char *address, int port); -void soapClientClose(soap_client_t *client); -void soapClientSendRequestVa(soap_client_t *client, const char* action, const char *fmt, va_list va); -void soapClientSendRequest(soap_client_t *client, const char* action, const char *fmt, ...); -char* soapClientReceive(soap_client_t *client); +// Sends raw XML as content of and returns raw XML content of response . +// Caller must free(*response_body_xml) on success. +int soap_client_call_raw_body(soap_client_t* c, + const char* soap_action, + const char* request_body_xml, + char** response_body_xml, + int* http_status); \ No newline at end of file diff --git a/src/main/target/SITL/sim/xplane.c b/src/main/target/SITL/sim/xplane.c index d629c2130e3..bf0349c47d1 100644 --- a/src/main/target/SITL/sim/xplane.c +++ b/src/main/target/SITL/sim/xplane.c @@ -33,8 +33,10 @@ #include #include #include +#include #include #include +#include #include "platform.h" @@ -58,6 +60,7 @@ #include "flight/imu.h" #include "io/gps.h" #include "rx/sim.h" +#include "xplane.h" #define XP_PORT 49000 #define XPLANE_JOYSTICK_AXIS_COUNT 8 @@ -66,11 +69,15 @@ static uint8_t pwmMapping[XP_MAX_PWM_OUTS]; static uint8_t mappingCount; +static pthread_mutex_t initMutex = PTHREAD_MUTEX_INITIALIZER; +static pthread_cond_t initCond = PTHREAD_COND_INITIALIZER; +static atomic_bool shouldStopListenThread = false; + static struct sockaddr_storage serverAddr; static socklen_t serverAddrLen; static int sockFd; -static pthread_t listenThread; -static bool initalized = false; +static pthread_t listenThread = 0; +static atomic_bool initalized = false; static bool useImu = false; static float lattitude = 0; @@ -170,6 +177,40 @@ static void sendDref(char* dref, float value) sendto(sockFd, (void*)buf, sizeof(buf), 0, (struct sockaddr*)&serverAddr, serverAddrLen); } +static void tryRegisterDrefs(void) +{ + registerDref(DREF_LATITUDE, "sim/flightmodel/position/latitude", 100); + registerDref(DREF_LONGITUDE, "sim/flightmodel/position/longitude", 100); + registerDref(DREF_ELEVATION, "sim/flightmodel/position/elevation", 100); + registerDref(DREF_AGL, "sim/flightmodel/position/y_agl", 100); + registerDref(DREF_LOCAL_VX, "sim/flightmodel/position/local_vx", 100); + registerDref(DREF_LOCAL_VY, "sim/flightmodel/position/local_vy", 100); + registerDref(DREF_LOCAL_VZ, "sim/flightmodel/position/local_vz", 100); + registerDref(DREF_GROUNDSPEED, "sim/flightmodel/position/groundspeed", 100); + registerDref(DREF_TRUE_AIRSPEED, "sim/flightmodel/position/true_airspeed", 100); + registerDref(DREF_POS_PHI, "sim/flightmodel/position/phi", 100); + registerDref(DREF_POS_THETA, "sim/flightmodel/position/theta", 100); + registerDref(DREF_POS_PSI, "sim/flightmodel/position/psi", 100); + registerDref(DREF_POS_HPATH, "sim/flightmodel/position/hpath", 100); + registerDref(DREF_FORCE_G_AXI1, "sim/flightmodel/forces/g_axil", 100); + registerDref(DREF_FORCE_G_SIDE, "sim/flightmodel/forces/g_side", 100); + registerDref(DREF_FORCE_G_NRML, "sim/flightmodel/forces/g_nrml", 100); + registerDref(DREF_POS_P, "sim/flightmodel/position/P", 100); + registerDref(DREF_POS_Q, "sim/flightmodel/position/Q", 100); + registerDref(DREF_POS_R, "sim/flightmodel/position/R", 100); + registerDref(DREF_POS_BARO_CURRENT_INHG, "sim/weather/barometer_current_inhg", 100); + registerDref(DREF_HAS_JOYSTICK, "sim/joystick/has_joystick", 100); + registerDref(DREF_JOYSTICK_VALUES_PITCH, "sim/joystick/joy_mapped_axis_value[1]", 100); + registerDref(DREF_JOYSTICK_VALUES_ROll, "sim/joystick/joy_mapped_axis_value[2]", 100); + registerDref(DREF_JOYSTICK_VALUES_YAW, "sim/joystick/joy_mapped_axis_value[3]", 100); + // Abusing cowl flaps for other channels + registerDref(DREF_JOYSTICK_VALUES_THROTTLE, "sim/joystick/joy_mapped_axis_value[57]", 100); + registerDref(DREF_JOYSTICK_VALUES_CH5, "sim/joystick/joy_mapped_axis_value[58]", 100); + registerDref(DREF_JOYSTICK_VALUES_CH6, "sim/joystick/joy_mapped_axis_value[59]", 100); + registerDref(DREF_JOYSTICK_VALUES_CH7, "sim/joystick/joy_mapped_axis_value[60]", 100); + registerDref(DREF_JOYSTICK_VALUES_CH8, "sim/joystick/joy_mapped_axis_value[61]", 100); +} + static void* listenWorker(void* arg) { UNUSED(arg); @@ -179,8 +220,11 @@ static void* listenWorker(void* arg) socklen_t slen = sizeof(remoteAddr); int recvLen; - while (true) + while (!atomic_load(&shouldStopListenThread)) { + if (!atomic_load(&initalized)) { + tryRegisterDrefs(); + } float motorValue = 0; float yokeValues[3] = { 0 }; @@ -210,10 +254,12 @@ static void* listenWorker(void* arg) recvLen = recvfrom(sockFd, buf, sizeof(buf), 0, (struct sockaddr*)&remoteAddr, &slen); if (recvLen < 0 && errno != EWOULDBLOCK) { + delay(250); continue; } if (strncmp((char*)buf, "RREF", 4) != 0) { + delay(250); continue; } @@ -426,11 +472,14 @@ static void* listenWorker(void* arg) constrainToInt16(north.z * 1024.0f) ); - if (!initalized) { + if (!atomic_load(&initalized)) { ENABLE_ARMING_FLAG(SIMULATOR_MODE_SITL); // Aircraft can wobble on the runway and prevents calibration of the accelerometer ENABLE_STATE(ACCELEROMETER_CALIBRATED); - initalized = true; + atomic_store(&initalized, true); + pthread_mutex_lock(&initMutex); + pthread_cond_signal(&initCond); + pthread_mutex_unlock(&initMutex); } unlockMainPID(); @@ -458,7 +507,7 @@ bool simXPlaneInit(char* ip, int port, uint8_t* mapping, uint8_t mapCount, bool if (sockFd < 0) { return false; } else { - char addrbuf[IPADDRESS_PRINT_BUFLEN]; + char addrbuf[IPADDRESS_PRINT_BUFLEN]; char *nptr = prettyPrintAddress((struct sockaddr *)&serverAddr, addrbuf, IPADDRESS_PRINT_BUFLEN ); if (nptr != NULL) { fprintf(stderr, "[SOCKET] xplane address = %s, fd=%d\n", nptr, sockFd); @@ -466,9 +515,9 @@ bool simXPlaneInit(char* ip, int port, uint8_t* mapping, uint8_t mapCount, bool } struct timeval tv; - tv.tv_sec = 1; - tv.tv_usec = 0; - if (setsockopt(sockFd, SOL_SOCKET, SO_RCVTIMEO, (struct timeval *) &tv,sizeof(struct timeval))) { + tv.tv_sec = 1; + tv.tv_usec = 0; + if (setsockopt(sockFd, SOL_SOCKET, SO_RCVTIMEO, (struct timeval *) &tv,sizeof(struct timeval))) { return false; } @@ -476,43 +525,30 @@ bool simXPlaneInit(char* ip, int port, uint8_t* mapping, uint8_t mapCount, bool return false; } + atomic_store(&initalized, false); + atomic_store(&shouldStopListenThread, false); if (pthread_create(&listenThread, NULL, listenWorker, NULL) < 0) { return false; } - while (!initalized) { - registerDref(DREF_LATITUDE, "sim/flightmodel/position/latitude", 100); - registerDref(DREF_LONGITUDE, "sim/flightmodel/position/longitude", 100); - registerDref(DREF_ELEVATION, "sim/flightmodel/position/elevation", 100); - registerDref(DREF_AGL, "sim/flightmodel/position/y_agl", 100); - registerDref(DREF_LOCAL_VX, "sim/flightmodel/position/local_vx", 100); - registerDref(DREF_LOCAL_VY, "sim/flightmodel/position/local_vy", 100); - registerDref(DREF_LOCAL_VZ, "sim/flightmodel/position/local_vz", 100); - registerDref(DREF_GROUNDSPEED, "sim/flightmodel/position/groundspeed", 100); - registerDref(DREF_TRUE_AIRSPEED, "sim/flightmodel/position/true_airspeed", 100); - registerDref(DREF_POS_PHI, "sim/flightmodel/position/phi", 100); - registerDref(DREF_POS_THETA, "sim/flightmodel/position/theta", 100); - registerDref(DREF_POS_PSI, "sim/flightmodel/position/psi", 100); - registerDref(DREF_POS_HPATH, "sim/flightmodel/position/hpath", 100); - registerDref(DREF_FORCE_G_AXI1, "sim/flightmodel/forces/g_axil", 100); - registerDref(DREF_FORCE_G_SIDE, "sim/flightmodel/forces/g_side", 100); - registerDref(DREF_FORCE_G_NRML, "sim/flightmodel/forces/g_nrml", 100); - registerDref(DREF_POS_P, "sim/flightmodel/position/P", 100); - registerDref(DREF_POS_Q, "sim/flightmodel/position/Q", 100); - registerDref(DREF_POS_R, "sim/flightmodel/position/R", 100); - registerDref(DREF_POS_BARO_CURRENT_INHG, "sim/weather/barometer_current_inhg", 100); - registerDref(DREF_HAS_JOYSTICK, "sim/joystick/has_joystick", 100); - registerDref(DREF_JOYSTICK_VALUES_PITCH, "sim/joystick/joy_mapped_axis_value[1]", 100); - registerDref(DREF_JOYSTICK_VALUES_ROll, "sim/joystick/joy_mapped_axis_value[2]", 100); - registerDref(DREF_JOYSTICK_VALUES_YAW, "sim/joystick/joy_mapped_axis_value[3]", 100); - // Abusing cowl flaps for other channels - registerDref(DREF_JOYSTICK_VALUES_THROTTLE, "sim/joystick/joy_mapped_axis_value[57]", 100); - registerDref(DREF_JOYSTICK_VALUES_CH5, "sim/joystick/joy_mapped_axis_value[58]", 100); - registerDref(DREF_JOYSTICK_VALUES_CH6, "sim/joystick/joy_mapped_axis_value[59]", 100); - registerDref(DREF_JOYSTICK_VALUES_CH7, "sim/joystick/joy_mapped_axis_value[60]", 100); - registerDref(DREF_JOYSTICK_VALUES_CH8, "sim/joystick/joy_mapped_axis_value[61]", 100); - delay(250); + pthread_mutex_lock(&initMutex); + while (!atomic_load(&initalized)) { + pthread_cond_wait(&initCond, &initMutex); } + pthread_mutex_unlock(&initMutex); return true; } + +void simXPlaneClose(void) +{ + atomic_store(&shouldStopListenThread, true); + if (listenThread) { + pthread_join(listenThread, NULL); + } + DISABLE_ARMING_FLAG(SIMULATOR_MODE_SITL); + atomic_store(&initalized, false); + pthread_mutex_destroy(&initMutex); + pthread_cond_destroy(&initCond); + close(sockFd); +} diff --git a/src/main/target/SITL/sim/xplane.h b/src/main/target/SITL/sim/xplane.h index 1777a30af2b..619bef615e0 100644 --- a/src/main/target/SITL/sim/xplane.h +++ b/src/main/target/SITL/sim/xplane.h @@ -27,3 +27,4 @@ #define XP_MAX_PWM_OUTS 4 bool simXPlaneInit(char* ip, int port, uint8_t* mapping, uint8_t mapCount, bool imu); +void simXPlaneClose(void); diff --git a/src/main/target/SITL/target.c b/src/main/target/SITL/target.c index e473c636981..67394a815e1 100644 --- a/src/main/target/SITL/target.c +++ b/src/main/target/SITL/target.c @@ -39,6 +39,7 @@ #include #include #include +#include #include #include "target.h" @@ -80,6 +81,25 @@ static void printVersion(void) { fprintf(stderr, "INAV %d.%d.%d SITL (%s)\n", FC_VERSION_MAJOR, FC_VERSION_MINOR, FC_VERSION_PATCH_LEVEL, shortGitRevision); } +static void cleanupAndExit(int code, bool shouldExit) { + if (sitlSim == SITL_SIM_XPLANE) { + simXPlaneClose(); + } else if (sitlSim == SITL_SIM_REALFLIGHT) { + simRealFlightClose(); + } + pthread_mutex_destroy(&mainLoopLock); + + if (shouldExit) { + exit(code); + } +} + +static void on_sigint(int sig) { + UNUSED(sig); + fprintf(stderr, "\n[SYSTEM] Caught SIGINT, exiting...\n"); + cleanupAndExit(0, true); +} + void systemInit(void) { printVersion(); clock_gettime(CLOCK_MONOTONIC, &start_time); @@ -100,6 +120,12 @@ void systemInit(void) { exit(1); } + struct sigaction sa; + sa.sa_handler = on_sigint; + sigemptyset(&sa.sa_mask); + sa.sa_flags = 0; + sigaction(SIGINT, &sa, NULL); + if (sitlSim != SITL_SIM_NONE) { fprintf(stderr, "[SIM] Waiting for connection...\n"); } @@ -377,6 +403,7 @@ void delay(timeMs_t ms) void systemReset(void) { fprintf(stderr, "[SYSTEM] Reset\n"); + cleanupAndExit(0, false); #if defined(__CYGWIN__) || defined(__APPLE__) || GCC_MAJOR < 12 for(int j = 3; j < 1024; j++) { close(j); @@ -391,7 +418,7 @@ void systemReset(void) void systemResetToBootloader(void) { fprintf(stderr, "[SYSTEM] Reset to bootloader\n"); - exit(0); + cleanupAndExit(0, true); } void failureMode(failureMode_e mode) { From bdf10e141ca5976b4d440841bf65e8b5f3e3ff15 Mon Sep 17 00:00:00 2001 From: Scavanger Date: Mon, 16 Mar 2026 11:00:43 -0300 Subject: [PATCH 20/48] Remove unnecessary error messages and update control input channel value in RealFlight simulation --- src/main/config/config_streamer_file.c | 1 - src/main/target/SITL/sim/realFlight.c | 3 +-- 2 files changed, 1 insertion(+), 3 deletions(-) diff --git a/src/main/config/config_streamer_file.c b/src/main/config/config_streamer_file.c index 86d677498c1..bce0f5be522 100644 --- a/src/main/config/config_streamer_file.c +++ b/src/main/config/config_streamer_file.c @@ -47,7 +47,6 @@ bool configFileSetPath(char* path) void config_streamer_impl_unlock(void) { if (eepromFd != NULL) { - fprintf(stderr, "[EEPROM] Unable to load %s\n", eepromPath); return; } diff --git a/src/main/target/SITL/sim/realFlight.c b/src/main/target/SITL/sim/realFlight.c index f5903f75546..6ff0ff413af 100644 --- a/src/main/target/SITL/sim/realFlight.c +++ b/src/main/target/SITL/sim/realFlight.c @@ -239,7 +239,7 @@ static void exchangeData(void) } } - char requestBody[1024] = "12"; + char requestBody[1024] = "4095"; for (int i = 0; i < RF_MAX_CHANNEL_COUNT; i++) { char value[32]; @@ -428,7 +428,6 @@ static void* soapWorker(void* arg) // RealFlight seems to not properly close the connection on its side if the connection is interrupted, but only after a timeout of about 30 seconds. // During this time the interface is not usable, but without this step it would be stuck in an unusable state until the next restart of the SITL. if (!restoreOriginalControllerDevice()) { - fprintf(stderr, "[SIM] Failed to restore original controller device in RealFlight. RealFlight might be in a broken state. Retrying...\n"); delay(1000); continue; } From f6281a600c5635bcd00a5c9f0ef3a4f7f86ef8bc Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Thu, 19 Mar 2026 15:15:06 -0500 Subject: [PATCH 21/48] use explicitly assign motor and servo timers before auto timers --- src/main/drivers/pwm_mapping.c | 134 ++++++++++++++------------------- 1 file changed, 57 insertions(+), 77 deletions(-) diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index 2d01127a504..120b3d4fc91 100644 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -270,43 +270,23 @@ uint8_t pwmClaimTimer(HAL_Timer_t *tim, uint32_t usageFlags) { return changed; } -void pwmEnsureEnoughtMotors(uint8_t motorCount) +static void pwmAssignOutput(timMotorServoHardware_t *timOutputs, timerHardware_t *timHw, int type) { - uint8_t motorOnlyOutputs = 0; - - for (int idx = 0; idx < timerHardwareCount; idx++) { - timerHardware_t *timHw = &timerHardware[idx]; - - timerHardwareOverride(timHw); - - if (checkPwmTimerConflicts(timHw)) { - continue; - } - - if (TIM_IS_MOTOR_ONLY(timHw->usageFlags)) { - motorOnlyOutputs++; - motorOnlyOutputs += pwmClaimTimer(timHw->tim, timHw->usageFlags); - } - } - - for (int idx = 0; idx < timerHardwareCount; idx++) { - timerHardware_t *timHw = &timerHardware[idx]; - - if (checkPwmTimerConflicts(timHw)) { - continue; - } - - if (TIM_IS_MOTOR(timHw->usageFlags) && !TIM_IS_MOTOR_ONLY(timHw->usageFlags)) { - if (motorOnlyOutputs < motorCount) { - timHw->usageFlags &= ~TIM_USE_SERVO; - timHw->usageFlags |= TIM_USE_MOTOR; - motorOnlyOutputs++; - motorOnlyOutputs += pwmClaimTimer(timHw->tim, timHw->usageFlags); - } else { - timHw->usageFlags &= ~TIM_USE_MOTOR; - pwmClaimTimer(timHw->tim, timHw->usageFlags); - } - } + switch (type) { + case MAP_TO_MOTOR_OUTPUT: + timHw->usageFlags &= TIM_USE_MOTOR; + timOutputs->timMotors[timOutputs->maxTimMotorCount++] = timHw; + pwmClaimTimer(timHw->tim, timHw->usageFlags); + break; + case MAP_TO_SERVO_OUTPUT: + timHw->usageFlags &= TIM_USE_SERVO; + timOutputs->timServos[timOutputs->maxTimServoCount++] = timHw; + pwmClaimTimer(timHw->tim, timHw->usageFlags); + break; + case MAP_TO_LED_OUTPUT: + timHw->usageFlags &= TIM_USE_LED; + pwmClaimTimer(timHw->tim, timHw->usageFlags); + break; } } @@ -316,54 +296,54 @@ void pwmBuildTimerOutputList(timMotorServoHardware_t * timOutputs, bool isMixerU timOutputs->maxTimMotorCount = 0; timOutputs->maxTimServoCount = 0; - uint8_t motorCount = getMotorCount(); - uint8_t motorIdx = 0; - - pwmEnsureEnoughtMotors(motorCount); + const uint8_t motorCount = getMotorCount(); + // Apply configurator overrides to all timer outputs for (int idx = 0; idx < timerHardwareCount; idx++) { - timerHardware_t *timHw = &timerHardware[idx]; + timerHardwareOverride(&timerHardware[idx]); + } - int type = MAP_TO_NONE; + // Assign outputs in priority order: dedicated first, then auto. + // Within each pass, array order (S1, S2, ...) is preserved. + // Motors and servos cannot share a timer, enforced by pwmHasMotorOnTimer/pwmHasServoOnTimer. + for (int priority = 0; priority < 2; priority++) { + uint8_t motorIdx = timOutputs->maxTimMotorCount; - // Check for known conflicts (i.e. UART, LEDSTRIP, Rangefinder and ADC) - if (checkPwmTimerConflicts(timHw)) { - LOG_WARNING(PWM, "Timer output %d skipped", idx); - continue; - } + for (int idx = 0; idx < timerHardwareCount; idx++) { + timerHardware_t *timHw = &timerHardware[idx]; + outputMode_e mode = timerOverrides(timer2id(timHw->tim))->outputMode; + bool isDedicated = (priority == 0); - // Make sure first motorCount motor outputs get assigned to motor - if (TIM_IS_MOTOR(timHw->usageFlags) && (motorIdx < motorCount)) { - timHw->usageFlags &= ~TIM_USE_SERVO; - pwmClaimTimer(timHw->tim, timHw->usageFlags); - motorIdx += 1; - } + if (checkPwmTimerConflicts(timHw)) { + if (priority == 0) { + LOG_WARNING(PWM, "Timer output %d skipped", idx); + } + continue; + } - if (TIM_IS_SERVO(timHw->usageFlags) && !pwmHasMotorOnTimer(timOutputs, timHw->tim)) { - type = MAP_TO_SERVO_OUTPUT; - } else if (TIM_IS_MOTOR(timHw->usageFlags) && !pwmHasServoOnTimer(timOutputs, timHw->tim)) { - type = MAP_TO_MOTOR_OUTPUT; - } else if (TIM_IS_LED(timHw->usageFlags) && !pwmHasMotorOnTimer(timOutputs, timHw->tim) && !pwmHasServoOnTimer(timOutputs, timHw->tim)) { - type = MAP_TO_LED_OUTPUT; - } + // Motors: dedicated (OUTPUT_MODE_MOTORS) first, then auto + if (TIM_IS_MOTOR(timHw->usageFlags) && motorIdx < motorCount + && !pwmHasServoOnTimer(timOutputs, timHw->tim) + && (isDedicated ? mode == OUTPUT_MODE_MOTORS : mode != OUTPUT_MODE_MOTORS)) { + pwmAssignOutput(timOutputs, timHw, MAP_TO_MOTOR_OUTPUT); + motorIdx++; + continue; + } + + // Servos: dedicated (OUTPUT_MODE_SERVOS) first, then auto + if (TIM_IS_SERVO(timHw->usageFlags) + && !pwmHasMotorOnTimer(timOutputs, timHw->tim) + && (isDedicated ? mode == OUTPUT_MODE_SERVOS : mode != OUTPUT_MODE_SERVOS)) { + pwmAssignOutput(timOutputs, timHw, MAP_TO_SERVO_OUTPUT); + continue; + } - switch(type) { - case MAP_TO_MOTOR_OUTPUT: - timHw->usageFlags &= TIM_USE_MOTOR; - timOutputs->timMotors[timOutputs->maxTimMotorCount++] = timHw; - pwmClaimTimer(timHw->tim, timHw->usageFlags); - break; - case MAP_TO_SERVO_OUTPUT: - timHw->usageFlags &= TIM_USE_SERVO; - timOutputs->timServos[timOutputs->maxTimServoCount++] = timHw; - pwmClaimTimer(timHw->tim, timHw->usageFlags); - break; - case MAP_TO_LED_OUTPUT: - timHw->usageFlags &= TIM_USE_LED; - pwmClaimTimer(timHw->tim, timHw->usageFlags); - break; - default: - break; + // LEDs: only on the auto pass, and only if timer is uncontested + if (!isDedicated && TIM_IS_LED(timHw->usageFlags) + && !pwmHasMotorOnTimer(timOutputs, timHw->tim) + && !pwmHasServoOnTimer(timOutputs, timHw->tim)) { + pwmAssignOutput(timOutputs, timHw, MAP_TO_LED_OUTPUT); + } } } } From 501fc54a46e31d62a257cd57bfcd6beac2e0d4dd Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Sun, 22 Mar 2026 14:00:11 -0500 Subject: [PATCH 22/48] Fix BLUEBERRYH743 target configuration issues - Change TARGET_BOARD_IDENTIFIER from "H743" to "BB43" (unique 4-char ID) - Remove BEEPER_PWM_FREQUENCY (no matching DEF_TIM entry) - Remove USE_SOFTSERIAL1 (unnecessary with 8 hardware UARTs + VCP) - Update SERIAL_PORT_COUNT from 9 to 8 - Remove beeper PWM and softserial timer entries from target.c - Fix trailing whitespace on DEF_TIM S3 line --- src/main/target/BLUEBERRYH743/config.c | 3 --- src/main/target/BLUEBERRYH743/target.c | 8 +------- src/main/target/BLUEBERRYH743/target.h | 9 +++------ 3 files changed, 4 insertions(+), 16 deletions(-) diff --git a/src/main/target/BLUEBERRYH743/config.c b/src/main/target/BLUEBERRYH743/config.c index bcc1e5c91fd..0673117c182 100644 --- a/src/main/target/BLUEBERRYH743/config.c +++ b/src/main/target/BLUEBERRYH743/config.c @@ -28,7 +28,4 @@ void targetConfiguration(void) { pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1; pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER2; - beeperConfigMutable()->pwmMode = true; - serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART1)].functionMask = FUNCTION_MSP; - serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART1)].msp_baudrateIndex = BAUD_115200; } diff --git a/src/main/target/BLUEBERRYH743/target.c b/src/main/target/BLUEBERRYH743/target.c index bc2fb01558a..5245d05e257 100644 --- a/src/main/target/BLUEBERRYH743/target.c +++ b/src/main/target/BLUEBERRYH743/target.c @@ -35,7 +35,7 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 1), // S2 - DEF_TIM(TIM5, CH1, PA0, TIM_USE_OUTPUT_AUTO, 0, 2), // S3 + DEF_TIM(TIM5, CH1, PA0, TIM_USE_OUTPUT_AUTO, 0, 2), // S3 DEF_TIM(TIM5, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 3), // S4 DEF_TIM(TIM5, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 4), // S5 DEF_TIM(TIM5, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 5), // S6 @@ -49,12 +49,6 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM15, CH2, PE6, TIM_USE_OUTPUT_AUTO, 0, 0), // S12 DMA_NONE DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 9), // LED_2812 - DEF_TIM(TIM2, CH1, PA15, TIM_USE_BEEPER, 0, 0), // BEEPER PWM - - // DEF_TIM(TIM8, CH2, PC7, TIM_USE_PPM, 0, 0), // RX6 PPM - DEF_TIM(TIM8, CH1, PC6, TIM_USE_ANY, 0, 0), // TX6 SoftwareSerial - // DEF_TIM(TIM16, CH1, PB8, TIM_USE_ANY, 0, 0), // RX4 - // DEF_TIM(TIM17, CH1, PB9, TIM_USE_ANY, 0, 0), // TX4 }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/BLUEBERRYH743/target.h b/src/main/target/BLUEBERRYH743/target.h index f11e822d633..b69400d0674 100644 --- a/src/main/target/BLUEBERRYH743/target.h +++ b/src/main/target/BLUEBERRYH743/target.h @@ -18,7 +18,7 @@ #pragma once -#define TARGET_BOARD_IDENTIFIER "H743" +#define TARGET_BOARD_IDENTIFIER "BB43" #if defined(BLUEBERRYH743HD) #define USBD_PRODUCT_STRING "BLUEBERRYH743HD" @@ -33,7 +33,6 @@ #define BEEPER PA15 #define BEEPER_INVERTED -#define BEEPER_PWM_FREQUENCY 2500 // *************** IMU generic *********************** #define USE_DUAL_GYRO @@ -152,11 +151,8 @@ #define UART8_TX_PIN PE1 #define UART8_RX_PIN PE0 -#define USE_SOFTSERIAL1 -#define SOFTSERIAL_1_TX_PIN PC6 //TX6 pad -#define SOFTSERIAL_1_RX_PIN PC6 //TX6 pad -#define SERIAL_PORT_COUNT 9 +#define SERIAL_PORT_COUNT 8 #define DEFAULT_RX_TYPE RX_TYPE_SERIAL #define SERIALRX_PROVIDER SERIALRX_CRSF @@ -198,6 +194,7 @@ #define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX) #define CURRENT_METER_SCALE 250 +#define CURRENT_METER_OFFSET 400 #define USE_SERIAL_4WAY_BLHELI_INTERFACE From ffb14ab990374d1b950962f8d49c035c8cfd303f Mon Sep 17 00:00:00 2001 From: Sensei Date: Sun, 29 Mar 2026 09:33:20 -0500 Subject: [PATCH 23/48] Update branch references from master to maintenance Updated instructions to reflect the use of the maintenance branch instead of master for cloning and merging. --- docs/development/Development.md | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/docs/development/Development.md b/docs/development/Development.md index 59063ad89cc..2629a9f6b0f 100755 --- a/docs/development/Development.md +++ b/docs/development/Development.md @@ -101,7 +101,7 @@ The main flow for a contributing is as follows: 1. Login to github, go to the INAV repository and press `fork`. 2. Then using the command line/terminal on your computer: `git clone ` 3. `cd inav` -4. `git checkout master` +4. `git checkout maintenance-10.x` 5. `git checkout -b my-new-code` 6. Make changes 7. `git add ` @@ -114,13 +114,13 @@ The primary thing to remember is that separate pull requests should be created f **Important:** Most contributions should target a maintenance branch, not `master`. See the branching section below for guidance on choosing the correct target branch. -Later, you can get the changes from the INAV repo into your `master` branch by adding INAV as a git remote and merging from it as follows: +Later, you can get the changes from the INAV repo into your version branch by adding INAV as a git remote and merging from it as follows: 1. `git remote add upstream https://github.com/iNavFlight/inav.git` -2. `git checkout master` +2. `git checkout maintenance-10.x` 3. `git fetch upstream` -4. `git merge upstream/master` -5. `git push origin master` is an optional step that will update your fork on github +4. `git merge upstream/maintenance-10.x` +5. `git push origin` is an optional step that will update your fork on github You can also perform the git commands using the git client inside Eclipse. Refer to the Eclipse git manual. From 216b76c212b5c04929fd306d0853c27b20d99c4e Mon Sep 17 00:00:00 2001 From: Daniel Ribeiro Date: Sun, 29 Mar 2026 13:28:25 -0300 Subject: [PATCH 24/48] Add three new MSP2 commands for GCS-initiated flight control MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Adds three new write-only MSP2 commands that allow a ground control station to interact with an active INAV navigation session without requiring direct RC transmitter input: MSP2_INAV_SET_WP_INDEX (0x2221) Jump to a specific waypoint during an active WP mission. Payload: U8 wp_index (0-based, relative to mission start waypoint). Preconditions: aircraft must be armed and NAV_WP_MODE must be active. Transitions any active waypoint FSM state back to PRE_ACTION so INAV re-initialises navigation for the new target waypoint. MSP2_INAV_SET_ALT_TARGET (0x2222) Set a new target altitude while altitude-controlled navigation is active. Payload: I32 altitude_cm (centimetres, relative to home). Preconditions: aircraft must be armed and NAV_CTL_ALT must be active. Calls updateClimbRateToAltitudeController() which works for both multicopter and fixed-wing platforms. MSP2_INAV_SET_CRUISE_HEADING (0x2223) Set the heading target while Cruise or Course Hold mode is active. Payload: I32 heading_centidegrees (0–35999). Preconditions: aircraft must be armed and NAV_COURSE_HOLD_MODE must be active. Sets both posControl.cruise.course and previousCourse to prevent spurious heading adjustments on the next control cycle. Implementation details: - New FSM event NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_JUMP aliases NAV_FSM_EVENT_STATE_SPECIFIC_4 (safe: WP and RTH states never overlap so the same underlying integer can be reused in both groups). - FSM transitions added for PRE_ACTION, IN_PROGRESS, REACHED, HOLD_TIME and FINISHED states, all routing to PRE_ACTION. - Three new public functions declared in navigation.h and implemented in navigation.c: navSetActiveWaypointIndex(), navSetDesiredAltitude(), navSetCruiseHeading(). - MSP handlers added in fc_msp.c; all return MSP_RESULT_ERROR if the aircraft is not in a compatible state. --- src/main/fc/fc_msp.c | 33 +++++++++ src/main/msp/msp_protocol_v2_inav.h | 4 + src/main/navigation/navigation.c | 93 ++++++++++++++++++++++++ src/main/navigation/navigation.h | 3 + src/main/navigation/navigation_private.h | 1 + 5 files changed, 134 insertions(+) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index cf5308067e9..0e33178664d 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -3684,6 +3684,39 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) } break; + case MSP2_INAV_SET_CRUISE_HEADING: + // Set heading while Cruise / Course Hold is active. + // Payload: I32 heading_centidegrees (0–35999) + if (dataSize == 4) { + int32_t headingCd; + if (sbufReadI32Safe(&headingCd, src) && navSetCruiseHeading(headingCd)) { + break; + } + } + return MSP_RESULT_ERROR; + + case MSP2_INAV_SET_WP_INDEX: + // Jump to waypoint N during an active WP mission. + // Payload: U8 wp_index (0-based, relative to mission start waypoint) + if (dataSize == 1) { + uint8_t wpIndex; + if (sbufReadU8Safe(&wpIndex, src) && navSetActiveWaypointIndex(wpIndex)) { + break; + } + } + return MSP_RESULT_ERROR; + + case MSP2_INAV_SET_ALT_TARGET: + // Set a new target altitude while altitude-controlled navigation is active. + // Payload: I32 altitude_cm (centimetres, relative to home) + if (dataSize == 4) { + int32_t altCm; + if (sbufReadI32Safe(&altCm, src) && navSetDesiredAltitude(altCm)) { + break; + } + } + return MSP_RESULT_ERROR; + default: return MSP_RESULT_ERROR; } diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h index 0b893916895..3f20df8e61e 100755 --- a/src/main/msp/msp_protocol_v2_inav.h +++ b/src/main/msp/msp_protocol_v2_inav.h @@ -126,3 +126,7 @@ #define MSP2_INAV_SET_GVAR 0x2214 #define MSP2_INAV_FULL_LOCAL_POSE 0x2220 + +#define MSP2_INAV_SET_WP_INDEX 0x2221 //in message jump to waypoint N during active WP mission; payload: U8 wp_index (0-based, relative to mission start) +#define MSP2_INAV_SET_ALT_TARGET 0x2222 //in message set target altitude for active altitude-hold / cruise / WP mode; payload: I32 altitude_cm (relative to home) +#define MSP2_INAV_SET_CRUISE_HEADING 0x2223 //in message set heading while in Cruise/Course Hold mode; payload: I32 heading_centidegrees (0-35999) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index e7f0774925e..1fd7edc0d30 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -785,6 +785,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED] = NAV_STATE_WAYPOINT_FINISHED, + [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_JUMP] = NAV_STATE_WAYPOINT_PRE_ACTION, // MSP2_INAV_SET_WP_INDEX } }, @@ -806,6 +807,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_JUMP] = NAV_STATE_WAYPOINT_PRE_ACTION, // MSP2_INAV_SET_WP_INDEX } }, @@ -830,6 +832,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_JUMP] = NAV_STATE_WAYPOINT_PRE_ACTION, // MSP2_INAV_SET_WP_INDEX } }, @@ -851,6 +854,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_JUMP] = NAV_STATE_WAYPOINT_PRE_ACTION, // MSP2_INAV_SET_WP_INDEX } }, @@ -909,6 +913,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_JUMP] = NAV_STATE_WAYPOINT_PRE_ACTION, // MSP2_INAV_SET_WP_INDEX } }, @@ -3876,6 +3881,94 @@ int isGCSValid(void) (posControl.navState == NAV_STATE_POSHOLD_3D_IN_PROGRESS)); } +/* + * navSetActiveWaypointIndex - MSP2_INAV_SET_WP_INDEX handler + * + * Jumps to a specific waypoint during an active WP mission without interrupting + * navigation mode. 'index' is 0-based and relative to the mission start + * (i.e. the first waypoint in the loaded mission is index 0, regardless of + * startWpIndex). + * + * Returns true on success, false when the preconditions are not met (not armed, + * not in WP mode, or index out of range). + */ +bool navSetActiveWaypointIndex(uint8_t index) +{ + // Must be armed and actively executing a WP mission + if (!ARMING_FLAG(ARMED) || !FLIGHT_MODE(NAV_WP_MODE)) { + return false; + } + + // Translate user-visible 0-based index to the internal absolute index + int8_t absoluteIndex = (int8_t)index + posControl.startWpIndex; + if (absoluteIndex < posControl.startWpIndex || + absoluteIndex >= posControl.startWpIndex + posControl.waypointCount) { + return false; + } + + posControl.activeWaypointIndex = absoluteIndex; + posControl.wpMissionRestart = false; + + // Transition immediately to WAYPOINT_PRE_ACTION so the new WP is set up + // on this navigation tick. navProcessFSMEvents is safe to call here as + // everything runs in the same main-loop task context. + navProcessFSMEvents(NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_JUMP); + return true; +} + +/* + * navSetDesiredAltitude - MSP2_INAV_SET_ALT_TARGET handler + * + * Commands the aircraft to climb or descend to 'altCm' centimetres above home + * while altitude-controlled navigation (altitude hold, cruise, or WP mode) is + * active. The altitude controller already implements this path for waypoint + * navigation (ROC_TO_ALT_TARGET); this function exposes it via MSP. + * + * Returns true on success, false when altitude control is not currently active. + */ +bool navSetDesiredAltitude(int32_t altCm) +{ + if (!ARMING_FLAG(ARMED)) { + return false; + } + + // Require active altitude control — covers althold, cruise and WP modes + if (!(navGetCurrentStateFlags() & NAV_CTL_ALT)) { + return false; + } + + updateClimbRateToAltitudeController(0.0f, (float)altCm, ROC_TO_ALT_TARGET); + return true; +} + +/* + * navSetCruiseHeading - MSP2_INAV_SET_CRUISE_HEADING handler + * + * Sets the target heading while Cruise or Course Hold mode is active. + * 'headingCd' is in centidegrees (0-35999), matching the internal + * posControl.cruise.course representation. + * + * Returns true on success, false when the preconditions are not met. + */ +bool navSetCruiseHeading(int32_t headingCd) +{ + if (!ARMING_FLAG(ARMED)) { + return false; + } + + // Only valid while Cruise or Course Hold is the active navigation mode + if (!FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) { + return false; + } + + // Clamp to valid centidegree range + headingCd = ((headingCd % 36000) + 36000) % 36000; + + posControl.cruise.course = headingCd; + posControl.cruise.previousCourse = headingCd; + return true; +} + void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData) { gpsLocation_t wpLLH; diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index ddc7e23e76d..bb540d94f04 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -708,6 +708,9 @@ int isGCSValid(void); void getWaypoint(uint8_t wpNumber, navWaypoint_t * wpData); void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData); void resetWaypointList(void); +bool navSetActiveWaypointIndex(uint8_t index); // MSP2_INAV_SET_WP_INDEX: jump to WP during active mission +bool navSetDesiredAltitude(int32_t altCm); // MSP2_INAV_SET_ALT_TARGET: set target altitude (cm, relative to home) +bool navSetCruiseHeading(int32_t headingCd); // MSP2_INAV_SET_CRUISE_HEADING: set cruise/course-hold heading (centidegrees) bool loadNonVolatileWaypointList(bool clearIfLoaded); bool saveNonVolatileWaypointList(void); #ifdef USE_MULTI_MISSION diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index f6cf9329e98..a1f07e470c0 100644 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -178,6 +178,7 @@ typedef enum { NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME = NAV_FSM_EVENT_STATE_SPECIFIC_1, NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND = NAV_FSM_EVENT_STATE_SPECIFIC_2, NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED = NAV_FSM_EVENT_STATE_SPECIFIC_3, + NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_JUMP = NAV_FSM_EVENT_STATE_SPECIFIC_4, // jump to a different WP index mid-mission NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_INITIALIZE = NAV_FSM_EVENT_STATE_SPECIFIC_1, NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_TRACKBACK = NAV_FSM_EVENT_STATE_SPECIFIC_2, NAV_FSM_EVENT_SWITCH_TO_RTH_HEAD_HOME = NAV_FSM_EVENT_STATE_SPECIFIC_3, From 25dd9ddec6c96655a0eaf1fd7a04c7e8174a7d05 Mon Sep 17 00:00:00 2001 From: Daniel Ribeiro Date: Mon, 30 Mar 2026 14:53:18 -0300 Subject: [PATCH 25/48] =?UTF-8?q?Remove=20MSP2=5FINAV=5FSET=5FALT=5FTARGET?= =?UTF-8?q?=20(0x2222)=20=E2=80=94=20superseded=20by=20existing=20implemen?= =?UTF-8?q?tation?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The set-altitude functionality is already covered by a more mature existing implementation in INAV. Remove the MSP2_INAV_SET_ALT_TARGET command added in the previous commit, keeping only MSP2_INAV_SET_WP_INDEX (0x2221) and MSP2_INAV_SET_CRUISE_HEADING (0x2223). Removes: define in msp_protocol_v2_inav.h, navSetDesiredAltitude() in navigation.c/.h, the MSP handler in fc_msp.c, and the docs section. --- docs/development/msp/README.md | 32 ++++++++++++++++++++++++++++- src/main/fc/fc_msp.c | 11 ---------- src/main/msp/msp_protocol_v2_inav.h | 1 - src/main/navigation/navigation.c | 25 ---------------------- src/main/navigation/navigation.h | 1 - 5 files changed, 31 insertions(+), 39 deletions(-) diff --git a/docs/development/msp/README.md b/docs/development/msp/README.md index d85974a65c0..696e7ab7fc1 100644 --- a/docs/development/msp/README.md +++ b/docs/development/msp/README.md @@ -411,7 +411,9 @@ For list of enums, see [Enum documentation page](https://github.com/iNavFlight/i [8722 - MSP2_INAV_GEOZONE_VERTEX](#msp2_inav_geozone_vertex) [8723 - MSP2_INAV_SET_GEOZONE_VERTEX](#msp2_inav_set_geozone_vertex) [8724 - MSP2_INAV_SET_GVAR](#msp2_inav_set_gvar) -[8736 - MSP2_INAV_FULL_LOCAL_POSE](#msp2_inav_full_local_pose) +[8736 - MSP2_INAV_FULL_LOCAL_POSE](#msp2_inav_full_local_pose) +[8737 - MSP2_INAV_SET_WP_INDEX](#msp2_inav_set_wp_index) +[8739 - MSP2_INAV_SET_CRUISE_HEADING](#msp2_inav_set_cruise_heading) [12288 - MSP2_BETAFLIGHT_BIND](#msp2_betaflight_bind) [12289 - MSP2_RX_BIND](#msp2_rx_bind) @@ -4526,6 +4528,34 @@ For list of enums, see [Enum documentation page](https://github.com/iNavFlight/i **Notes:** All attitude angles are in deci-degrees. +## `MSP2_INAV_SET_WP_INDEX (8737 / 0x2221)` +**Description:** Jumps to a specific waypoint during an active waypoint mission, causing the aircraft to immediately begin navigating toward the new target waypoint. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `wp_index` | `uint8_t` | 1 | - | 0-based waypoint index to jump to, relative to the mission start waypoint (`posControl.startWpIndex`) | + +**Reply Payload:** **None** + +**Notes:** Returns error if the aircraft is not armed, `NAV_WP_MODE` is not active, or the index is outside the valid mission range (`startWpIndex` to `startWpIndex + waypointCount - 1`). On success, sets `posControl.activeWaypointIndex` to the requested index and fires `NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_JUMP`, transitioning the navigation FSM back to `NAV_STATE_WAYPOINT_PRE_ACTION` so the flight controller re-initialises navigation for the new target. + +--- + +## `MSP2_INAV_SET_CRUISE_HEADING (8739 / 0x2223)` +**Description:** Sets the course heading target while Cruise or Course Hold mode is active, causing the aircraft to turn to and maintain the new heading. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `heading_centidegrees` | `int32_t` | 4 | centidegrees | Target heading in centidegrees (0–35999). Values are wrapped modulo 36000 before being applied. | + +**Reply Payload:** **None** + +**Notes:** Returns error if the aircraft is not armed or `NAV_COURSE_HOLD_MODE` is not active. On success, sets both `posControl.cruise.course` and `posControl.cruise.previousCourse` to the normalised value, preventing spurious heading adjustments from `getCruiseHeadingAdjustment()` on the next control cycle. + +--- + ## `MSP2_BETAFLIGHT_BIND (12288 / 0x3000)` **Description:** Initiates the receiver binding procedure for supported serial protocols (CRSF, SRXL2). diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 0e33178664d..b7da917520f 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -3706,17 +3706,6 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) } return MSP_RESULT_ERROR; - case MSP2_INAV_SET_ALT_TARGET: - // Set a new target altitude while altitude-controlled navigation is active. - // Payload: I32 altitude_cm (centimetres, relative to home) - if (dataSize == 4) { - int32_t altCm; - if (sbufReadI32Safe(&altCm, src) && navSetDesiredAltitude(altCm)) { - break; - } - } - return MSP_RESULT_ERROR; - default: return MSP_RESULT_ERROR; } diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h index 3f20df8e61e..21ad7e3d135 100755 --- a/src/main/msp/msp_protocol_v2_inav.h +++ b/src/main/msp/msp_protocol_v2_inav.h @@ -128,5 +128,4 @@ #define MSP2_INAV_FULL_LOCAL_POSE 0x2220 #define MSP2_INAV_SET_WP_INDEX 0x2221 //in message jump to waypoint N during active WP mission; payload: U8 wp_index (0-based, relative to mission start) -#define MSP2_INAV_SET_ALT_TARGET 0x2222 //in message set target altitude for active altitude-hold / cruise / WP mode; payload: I32 altitude_cm (relative to home) #define MSP2_INAV_SET_CRUISE_HEADING 0x2223 //in message set heading while in Cruise/Course Hold mode; payload: I32 heading_centidegrees (0-35999) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 1fd7edc0d30..03d4a3da36b 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -3916,31 +3916,6 @@ bool navSetActiveWaypointIndex(uint8_t index) return true; } -/* - * navSetDesiredAltitude - MSP2_INAV_SET_ALT_TARGET handler - * - * Commands the aircraft to climb or descend to 'altCm' centimetres above home - * while altitude-controlled navigation (altitude hold, cruise, or WP mode) is - * active. The altitude controller already implements this path for waypoint - * navigation (ROC_TO_ALT_TARGET); this function exposes it via MSP. - * - * Returns true on success, false when altitude control is not currently active. - */ -bool navSetDesiredAltitude(int32_t altCm) -{ - if (!ARMING_FLAG(ARMED)) { - return false; - } - - // Require active altitude control — covers althold, cruise and WP modes - if (!(navGetCurrentStateFlags() & NAV_CTL_ALT)) { - return false; - } - - updateClimbRateToAltitudeController(0.0f, (float)altCm, ROC_TO_ALT_TARGET); - return true; -} - /* * navSetCruiseHeading - MSP2_INAV_SET_CRUISE_HEADING handler * diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index bb540d94f04..eb1621e9f8d 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -709,7 +709,6 @@ void getWaypoint(uint8_t wpNumber, navWaypoint_t * wpData); void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData); void resetWaypointList(void); bool navSetActiveWaypointIndex(uint8_t index); // MSP2_INAV_SET_WP_INDEX: jump to WP during active mission -bool navSetDesiredAltitude(int32_t altCm); // MSP2_INAV_SET_ALT_TARGET: set target altitude (cm, relative to home) bool navSetCruiseHeading(int32_t headingCd); // MSP2_INAV_SET_CRUISE_HEADING: set cruise/course-hold heading (centidegrees) bool loadNonVolatileWaypointList(bool clearIfLoaded); bool saveNonVolatileWaypointList(void); From 20745ec7397f93d70d0d5601653f7c1122d7f7fa Mon Sep 17 00:00:00 2001 From: b14ckyy Date: Sun, 5 Apr 2026 21:45:49 +0200 Subject: [PATCH 26/48] Fix MSP-over-telemetry multi-frame request assembly Fix initSharedMsp() to initialize the response buffer as empty instead of setting it to the full buffer size. This was causing the response- pending check in handleMspFrame() to incorrectly detect a pending response after every buffer reinitialization. Root cause: initSharedMsp() set responsePacket->buf.end to responseBuffer + sizeof(mspTxBuffer), putting the buffer in writer mode with sbufBytesRemaining() returning the full buffer size (e.g. 512). The response-pending check in handleMspFrame() interpreted this as unsent response data and reset mspStarted to 0. This triggered another call to initSharedMsp(), which reset the buffer to full size again, creating a permanent cycle that dropped every continuation frame. The response-pending check itself is correct and necessary: SmartPort is half-duplex, so new request assembly must be blocked while a response is still being sent. The bug was that initSharedMsp() corrupted the buffer state that the check relies on. The fix changes initSharedMsp() to set buf.end = buf.ptr (empty reader), so sbufBytesRemaining() returns 0 when no response is actually pending. processMspPacket() already sets up the response buffer in writer mode before processing, so this change is safe. Impact: all MSPv1/MSPv2 requests with payload requiring more than one telemetry frame were silently dropped over SmartPort, CRSF, and FPort MSP passthrough. Only zero-payload single-frame requests succeeded. Tested on MATEKF765 with SmartPort MSP passthrough. --- src/main/telemetry/msp_shared.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/telemetry/msp_shared.c b/src/main/telemetry/msp_shared.c index 1589a513596..f3c32eaf7ef 100644 --- a/src/main/telemetry/msp_shared.c +++ b/src/main/telemetry/msp_shared.c @@ -73,7 +73,7 @@ void initSharedMsp(void) mspPackage.responseBuffer = (uint8_t *)&mspTxBuffer; mspPackage.responsePacket = &mspTxPacket; mspPackage.responsePacket->buf.ptr = mspPackage.responseBuffer; - mspPackage.responsePacket->buf.end = mspPackage.responseBuffer + sizeof(mspTxBuffer); + mspPackage.responsePacket->buf.end = mspPackage.responseBuffer; } static void processMspPacket(void) From e93adf376240a6bcd04f699a4d909a0e88bd596d Mon Sep 17 00:00:00 2001 From: b14ckyy Date: Sun, 5 Apr 2026 22:52:16 +0200 Subject: [PATCH 27/48] Add MSP2_INAV_SET_AUX_RC (0x2230) --- docs/development/msp/msp_messages.json | 26 ++++++ src/main/fc/fc_msp.c | 105 +++++++++++++++++++++++++ src/main/msp/msp_protocol_v2_inav.h | 2 + src/main/rx/rx.c | 17 ++++ src/main/rx/rx.h | 4 + 5 files changed, 154 insertions(+) diff --git a/docs/development/msp/msp_messages.json b/docs/development/msp/msp_messages.json index 12d67369bd7..866eb87b3b8 100644 --- a/docs/development/msp/msp_messages.json +++ b/docs/development/msp/msp_messages.json @@ -10943,6 +10943,32 @@ "notes": "All attitude angles are in deci-degrees.", "description": "Provides estimates of current attitude, local NEU position, and velocity." }, + "MSP2_INAV_SET_AUX_RC": { + "code": 8752, + "mspv": 2, + "request": { + "payload": [ + { + "name": "definitionByte", + "ctype": "uint8_t", + "desc": "Packed start channel and resolution. Bits 7-3: start channel index (valid range 8-31 for CH9-CH32; 0-7 rejected as error). Bits 2-0: resolution mode (0=2-bit, 1=4-bit, 2=8-bit, 3=16-bit; 4-7 reserved/error).", + "units": "" + }, + { + "name": "channelData", + "ctype": "uint8_t", + "desc": "Packed channel values, sequential from start channel. Number of channels is derived from data size and resolution. Value 0 means skip (no update). Sub-byte modes (2-bit, 4-bit) are packed MSB-first. 2-bit values 1-3 map to 1000/1500/2000us. 4-bit values 1-15 map to 1000 + (val-1)*1000/14 us. 8-bit values 1-255 map to 1000 + (val-1)*1000/254 us. 16-bit values are direct PWM.", + "units": "PWM (encoded)", + "array": true, + "array_size": 0 + } + ] + }, + "reply": null, + "variable_len": true, + "notes": "CH1-CH8 (index 0-7) are protected and will return `MSP_RESULT_ERROR`. Constraint: `startChannel + channelCount <= 32`. Values persist until overwritten; no timeout. Applied as a post-RX overlay in `calculateRxChannelsAndUpdateFailsafe()` after MSP RC Override but before failsafe. Does not require `USE_RX_MSP` or MSP-RC-OVERRIDE flight mode. Does not affect failsafe detection. Recommended to send with `MSP_FLAG_DONT_REPLY` (flags=0x01) to save bandwidth on telemetry passthrough links. 16-bit mode requires even number of data bytes.", + "description": "Bandwidth-efficient auxiliary RC channel update. Sets CH9-CH32 with configurable resolution (2/4/8/16-bit) without affecting primary flight controls. Designed for extending channel count beyond native RC link capacity via MSP passthrough." + }, "MSP2_BETAFLIGHT_BIND": { "code": 12288, "mspv": 2, diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index cf5308067e9..d0e673f5fcc 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -2412,6 +2412,110 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) } break; #endif + + case MSP2_INAV_SET_AUX_RC: + { + if (dataSize < 2) { + return MSP_RESULT_ERROR; + } + + const uint8_t defByte = sbufReadU8(src); + const uint8_t startChannel = defByte >> 3; // Bits 7-3: start channel index (0-31) + const uint8_t resolutionMode = defByte & 0x07; // Bits 2-0: resolution + + // Safety: CH1-CH8 (index 0-7) are protected + if (startChannel < 8) { + return MSP_RESULT_ERROR; + } + + const uint8_t dataBytes = dataSize - 1; + uint8_t channelCount; + uint8_t bitsPerChannel; + + switch (resolutionMode) { + case 0: // 2-bit + bitsPerChannel = 2; + channelCount = dataBytes * 4; + break; + case 1: // 4-bit + bitsPerChannel = 4; + channelCount = dataBytes * 2; + break; + case 2: // 8-bit + bitsPerChannel = 8; + channelCount = dataBytes; + break; + case 3: // 16-bit + bitsPerChannel = 16; + if (dataBytes % 2 != 0) { + return MSP_RESULT_ERROR; + } + channelCount = dataBytes / 2; + break; + default: + return MSP_RESULT_ERROR; + } + + if (channelCount == 0 || startChannel + channelCount > 32) { + return MSP_RESULT_ERROR; + } + + // Decode and apply channel values + if (bitsPerChannel >= 8) { + // Byte-aligned modes: 8-bit and 16-bit + for (int i = 0; i < channelCount; i++) { + uint16_t rawValue; + if (bitsPerChannel == 16) { + rawValue = sbufReadU16(src); + } else { + rawValue = sbufReadU8(src); + } + + if (rawValue == 0) { + continue; // skip: no update + } + + uint16_t pwmValue; + if (bitsPerChannel == 16) { + pwmValue = constrain(rawValue, 750, 2250); + } else { + // 8-bit: 1-255 → 1000-2000 + pwmValue = 1000 + ((uint32_t)(rawValue - 1) * 1000) / 254; + } + + rxMspAuxOverlaySet(startChannel + i, pwmValue); + } + } else { + // Sub-byte modes: 2-bit and 4-bit + const uint8_t mask = (1 << bitsPerChannel) - 1; + const uint8_t channelsPerByte = 8 / bitsPerChannel; + int ch = 0; + + for (int byteIdx = 0; byteIdx < (int)dataBytes && ch < channelCount; byteIdx++) { + const uint8_t dataByte = sbufReadU8(src); + for (int sub = channelsPerByte - 1; sub >= 0 && ch < channelCount; sub--, ch++) { + const uint8_t rawValue = (dataByte >> (sub * bitsPerChannel)) & mask; + + if (rawValue == 0) { + continue; // skip: no update + } + + uint16_t pwmValue; + if (bitsPerChannel == 2) { + // 2-bit: 1→1000, 2→1500, 3→2000 + pwmValue = 1000 + (rawValue - 1) * 500; + } else { + // 4-bit: 1-15 → 1000-2000 + pwmValue = 1000 + ((uint32_t)(rawValue - 1) * 1000) / 14; + } + + rxMspAuxOverlaySet(startChannel + ch, pwmValue); + } + } + } + } + break; + case MSP2_COMMON_SET_MOTOR_MIXER: sbufReadU8Safe(&tmp_u8, src); if ((dataSize == 9) && (tmp_u8 < MAX_SUPPORTED_MOTORS)) { @@ -4486,6 +4590,7 @@ mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostPro sbuf_t *dst = &reply->buf; sbuf_t *src = &cmd->buf; const uint16_t cmdMSP = cmd->cmd; + // initialize reply by default reply->cmd = cmd->cmd; diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h index 0b893916895..aacccc7806b 100755 --- a/src/main/msp/msp_protocol_v2_inav.h +++ b/src/main/msp/msp_protocol_v2_inav.h @@ -126,3 +126,5 @@ #define MSP2_INAV_SET_GVAR 0x2214 #define MSP2_INAV_FULL_LOCAL_POSE 0x2220 + +#define MSP2_INAV_SET_AUX_RC 0x2230 diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 0531904d063..7df78cc5e50 100755 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -94,6 +94,9 @@ static bool isRxSuspended = false; static rcChannel_t rcChannels[MAX_SUPPORTED_RC_CHANNEL_COUNT]; +// MSP aux channel overlay: non-zero values override rcChannels[].data for CH9-CH32 +static uint16_t mspAuxOverlay[MAX_SUPPORTED_RC_CHANNEL_COUNT]; + rxLinkStatistics_t rxLinkStatistics; rxRuntimeConfig_t rxRuntimeConfig; static uint8_t rcSampleIndex = 0; @@ -512,6 +515,13 @@ bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs) } #endif + // Apply MSP aux channel overlay (CH9-CH32) + for (int i = 8; i < 32; i++) { + if (mspAuxOverlay[i] > 0) { + rcChannels[i].data = mspAuxOverlay[i]; + } + } + // Update failsafe if (rxFlightChannelsValid && rxSignalReceived) { failsafeOnValidDataReceived(); @@ -663,6 +673,13 @@ int16_t rxGetChannelValue(unsigned channelNumber) } } +void rxMspAuxOverlaySet(uint8_t channelIndex, uint16_t value) +{ + if (channelIndex >= 8 && channelIndex < 32) { + mspAuxOverlay[channelIndex] = value; + } +} + void lqTrackerReset(rxLinkQualityTracker_e * lqTracker) { lqTracker->lastUpdatedMs = millis(); diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index 3ed6add3e48..6eb63c83325 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -232,3 +232,7 @@ void resumeRxSignal(void); // filtering and some extra processing like value holding // during failsafe. int16_t rxGetChannelValue(unsigned channelNumber); + +// MSP aux channel overlay (CH9-CH32). Sets a channel value that persists +// across RX update cycles. value=0 clears the overlay for that channel. +void rxMspAuxOverlaySet(uint8_t channelIndex, uint16_t value); From ed06c84def2279c64cdcdff395526478fa9fd66b Mon Sep 17 00:00:00 2001 From: Sensei Date: Wed, 8 Apr 2026 22:29:53 -0500 Subject: [PATCH 28/48] Fix bugs in ICM-45686 IMU driver (#11455) - Fix settings.yaml: quote "ICM45686" string in acc_hardware table - Fix accel LPF fallback: write BYPASS not ODR_DIV_8 on IREG failure - Fix IREG polling: use delayMicroseconds(10) not delay(1) to match waited_us counter - Fix reset polling: replace while(1) with do/while pattern used by all other drivers - Fix temperature formula: use float literals (12.8f) to avoid double promotion on F4 - Add named constant ICM456XX_INT_CONFIG_DELAY_MS for post-interrupt settle delay - Remove unused endianness defines (ICM456XX_RA_SREG_CTRL, SREG_DATA_ENDIAN_SEL_*) - Add missing trailing newlines to all modified files --- src/main/drivers/accgyro/accgyro_icm45686.c | 32 ++++++++------------- src/main/drivers/accgyro/accgyro_icm45686.h | 2 +- src/main/drivers/accgyro/accgyro_mpu.h | 2 +- src/main/drivers/bus.h | 2 +- src/main/fc/settings.yaml | 2 +- src/main/sensors/acceleration.c | 2 +- src/main/sensors/acceleration.h | 2 +- src/main/sensors/gyro.c | 2 +- src/main/sensors/gyro.h | 2 +- src/main/target/common_hardware.c | 2 +- 10 files changed, 21 insertions(+), 29 deletions(-) diff --git a/src/main/drivers/accgyro/accgyro_icm45686.c b/src/main/drivers/accgyro/accgyro_icm45686.c index a9e0ee0b8a5..9b249fb607f 100644 --- a/src/main/drivers/accgyro/accgyro_icm45686.c +++ b/src/main/drivers/accgyro/accgyro_icm45686.c @@ -104,16 +104,6 @@ Note: Now implemented only UI Interface with Low-Noise Mode #define ICM456XX_ACCEL_CONFIG0 0x1B #define ICM456XX_PWR_MGMT0 0x10 #define ICM456XX_TEMP_DATA1 0x0C -// Register map IPREG_TOP1 (for future use) -// Note: SREG_CTRL register provides endian selection capability. -// Currently not utilized as UI interface reads are in device's native endianness. -// Kept as reference for potential future optimization. -#define ICM456XX_RA_SREG_CTRL 0xA267 // To access: 0xA200 + 0x67 - -// SREG_CTRL - 0x67 (bits 1:0 select data endianness) -#define ICM456XX_SREG_DATA_ENDIAN_SEL_LITTLE (0 << 1) // Little endian (native) -#define ICM456XX_SREG_DATA_ENDIAN_SEL_BIG (1 << 1) // Big endian (requires IREG write) - // MGMT0 - 0x10 - Gyro #define ICM456XX_GYRO_MODE_OFF (0x00 << 2) #define ICM456XX_GYRO_MODE_STANDBY (0x01 << 2) @@ -264,6 +254,7 @@ Note: Now implemented only UI Interface with Low-Noise Mode #define ICM456XX_ACCEL_STARTUP_TIME_MS 10 // Min accel startup from OFF/STANDBY/LP #define ICM456XX_GYRO_STARTUP_TIME_MS 35 // Min gyro startup from OFF/STANDBY/LP #define ICM456XX_SENSOR_ENABLE_DELAY_MS 1 // Allow sensors to power on and stabilize +#define ICM456XX_INT_CONFIG_DELAY_MS 15 // Register settle time after interrupt config (matches ICM-426xx convention) #define ICM456XX_IREG_TIMEOUT_US 5000 // IREG operation timeout (5ms max) #define ICM456XX_DATA_LENGTH 6 // 3 axes * 2 bytes per axis @@ -305,7 +296,7 @@ static bool icm45686WriteIREG(const busDevice_t *dev, uint16_t reg, uint8_t valu if (misc2 & ICM456XX_BIT_IREG_DONE) { return true; } - delay(1); + delayMicroseconds(10); } return false; // timeout @@ -357,7 +348,7 @@ static bool icm45686ReadTemperature(gyroDev_t *gyro, int16_t * temp) return false; } // From datasheet: Temperature in Degrees Centigrade = (TEMP_DATA / 128) + 25 - *temp = ( int16_val_little_endian(data, 0) / 12.8 ) + 250; // Temperature stored as degC*10 + *temp = ( int16_val_little_endian(data, 0) / 12.8f ) + 250.0f; // Temperature stored as degC*10 return true; } @@ -398,7 +389,7 @@ static void icm45686AccAndGyroInit(gyroDev_t *gyro) // Set the Accel UI LPF bandwidth cut-off to ODR/8 (Section 7.3 of datasheet) if (!icm45686WriteIREG(dev, ICM456XX_ACCEL_UI_LPF_CFG_IREG_ADDR, ICM456XX_ACCEL_UI_LPFBW_ODR_DIV_8)) { // If LPF configuration fails, fallback to BYPASS - icm45686WriteIREG(dev, ICM456XX_ACCEL_UI_LPF_CFG_IREG_ADDR, ICM456XX_ACCEL_UI_LPFBW_ODR_DIV_8); + icm45686WriteIREG(dev, ICM456XX_ACCEL_UI_LPF_CFG_IREG_ADDR, ICM456XX_ACCEL_UI_LPFBW_BYPASS); } // Setup scale and odr values for gyro busWrite(dev, ICM456XX_GYRO_CONFIG0, ICM456XX_GYRO_FS_SEL_2000DPS | config->gyroConfigValues[1]); @@ -409,7 +400,7 @@ static void icm45686AccAndGyroInit(gyroDev_t *gyro) ICM456XX_INT1_POLARITY_ACTIVE_HIGH); busWrite(dev, ICM456XX_INT1_CONFIG0, ICM456XX_INT1_STATUS_EN_DRDY); - delay(15); + delay(ICM456XX_INT_CONFIG_DELAY_MS); busSetSpeed(dev, BUS_SPEED_FAST); } @@ -424,17 +415,18 @@ static bool icm45686DeviceDetect(busDevice_t * dev) // Perform soft reset directly // Soft reset busWrite(dev, ICM456XX_REG_MISC2, ICM456XX_SOFT_RESET); - // Wait for reset to complete (bit 1 of REG_MISC2 becomes 0) - while (1) { + // Poll until soft reset completes (SOFT_RESET bit clears) per datasheet Section 9.4 + do { busRead(dev, ICM456XX_REG_MISC2, &tmp); if (!(tmp & ICM456XX_SOFT_RESET)) { break; } delay(1); waitedMs++; - if (waitedMs >= 20) { - return false; - } + } while (waitedMs < 20); + + if (tmp & ICM456XX_SOFT_RESET) { + return false; } // Initialize power management to a known state after reset // This ensures sensors are off and ready for proper initialization @@ -497,4 +489,4 @@ bool icm45686GyroDetect(gyroDev_t *gyro) } -#endif \ No newline at end of file +#endif diff --git a/src/main/drivers/accgyro/accgyro_icm45686.h b/src/main/drivers/accgyro/accgyro_icm45686.h index 83c630e13bc..aac195aa220 100644 --- a/src/main/drivers/accgyro/accgyro_icm45686.h +++ b/src/main/drivers/accgyro/accgyro_icm45686.h @@ -19,4 +19,4 @@ bool icm45686AccDetect(accDev_t *acc); -bool icm45686GyroDetect(gyroDev_t *gyro); \ No newline at end of file +bool icm45686GyroDetect(gyroDev_t *gyro); diff --git a/src/main/drivers/accgyro/accgyro_mpu.h b/src/main/drivers/accgyro/accgyro_mpu.h index 16089f5f542..0f59d10d3e8 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.h +++ b/src/main/drivers/accgyro/accgyro_mpu.h @@ -182,4 +182,4 @@ const gyroFilterAndRateConfig_t * mpuChooseGyroConfig(uint8_t desiredLpf, uint16 bool mpuGyroRead(struct gyroDev_s *gyro); bool mpuGyroReadScratchpad(struct gyroDev_s *gyro); bool mpuAccReadScratchpad(struct accDev_s *acc); -bool mpuTemperatureReadScratchpad(struct gyroDev_s *gyro, int16_t * data); \ No newline at end of file +bool mpuTemperatureReadScratchpad(struct gyroDev_s *gyro, int16_t * data); diff --git a/src/main/drivers/bus.h b/src/main/drivers/bus.h index 3c26067a557..353a06b306e 100644 --- a/src/main/drivers/bus.h +++ b/src/main/drivers/bus.h @@ -331,4 +331,4 @@ bool busWrite(const busDevice_t * busdev, uint8_t reg, uint8_t data); bool busTransfer(const busDevice_t * dev, uint8_t * rxBuf, const uint8_t * txBuf, int length); bool busTransferMultiple(const busDevice_t * dev, busTransferDescriptor_t * buffers, int count); -bool busIsBusy(const busDevice_t * dev); \ No newline at end of file +bool busIsBusy(const busDevice_t * dev); diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 58274b1b660..91ef0ff1b3e 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2,7 +2,7 @@ tables: - name: alignment values: ["DEFAULT", "CW0", "CW90", "CW180", "CW270", "CW0FLIP", "CW90FLIP", "CW180FLIP", "CW270FLIP"] - name: acc_hardware - values: ["NONE", "AUTO", "MPU6000", "MPU6500", "MPU9250", "BMI160", "ICM20689", "BMI088", "ICM42605", "BMI270","LSM6DXX", ICM45686, "FAKE"] + values: ["NONE", "AUTO", "MPU6000", "MPU6500", "MPU9250", "BMI160", "ICM20689", "BMI088", "ICM42605", "BMI270","LSM6DXX", "ICM45686", "FAKE"] enum: accelerationSensor_e - name: rangefinder_hardware values: ["NONE", "SRF10", "VL53L0X", "MSP", "BENEWAKE", "VL53L1X", "US42", "TOF10120_I2C", "FAKE", "TERARANGER_EVO", "USD1_V0", "NRA"] diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index d0137c28a59..cc52be637a9 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -709,4 +709,4 @@ void accInitFilters(void) bool accIsHealthy(void) { return true; -} \ No newline at end of file +} diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index 1037f85ca37..ea76b63ca75 100644 --- a/src/main/sensors/acceleration.h +++ b/src/main/sensors/acceleration.h @@ -98,4 +98,4 @@ void accSetCalibrationValues(void); void accInitFilters(void); bool accIsHealthy(void); bool accGetCalibrationAxisStatus(int axis); -uint8_t accGetCalibrationAxisFlags(void); \ No newline at end of file +uint8_t accGetCalibrationAxisFlags(void); diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 5abc4980f97..8544b45dac1 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -619,4 +619,4 @@ void gyroUpdateDynamicLpf(float cutoffFreq) { float averageAbsGyroRates(void) { return (fabsf(gyro.gyroADCf[ROLL]) + fabsf(gyro.gyroADCf[PITCH]) + fabsf(gyro.gyroADCf[YAW])) / 3.0f; -} \ No newline at end of file +} diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index d04eb2f705d..18fe5d9e517 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -120,4 +120,4 @@ bool gyroReadTemperature(void); int16_t gyroGetTemperature(void); int16_t gyroRateDps(int axis); void gyroUpdateDynamicLpf(float cutoffFreq); -float averageAbsGyroRates(void); \ No newline at end of file +float averageAbsGyroRates(void); diff --git a/src/main/target/common_hardware.c b/src/main/target/common_hardware.c index 020e3f7b35c..ddaae1b7b16 100755 --- a/src/main/target/common_hardware.c +++ b/src/main/target/common_hardware.c @@ -477,4 +477,4 @@ BUSDEV_REGISTER_I2C(busdev_pcf8574, DEVHW_PCF8574, PCF8574_I2C_BUS, 0x20, NONE, DEVFLAGS_NONE, 0); #endif -#endif // USE_TARGET_HARDWARE_DESCRIPTORS \ No newline at end of file +#endif // USE_TARGET_HARDWARE_DESCRIPTORS From 9ce07526928a57ff2037e12825d58d2d42df64d9 Mon Sep 17 00:00:00 2001 From: Marc Hoffmann <33039058+b14ckyy@users.noreply.github.com> Date: Thu, 9 Apr 2026 05:58:35 +0200 Subject: [PATCH 29/48] Fix MSP_RESULT_NO_REPLY ignored in MSP-over-telemetry passthrough (#11481) processMspPacket() in msp_shared.c did not check for MSP_RESULT_NO_REPLY returned by mspFcProcessCommand(). When a command was sent with the MSP_FLAG_DONT_REPLY flag (bit 0 of the MSPv2 flags byte), the FC correctly set the result to MSP_RESULT_NO_REPLY, but processMspPacket() unconditionally called sbufSwitchToReader() on the response buffer, causing a reply to be sent anyway. The serial MSP path (msp_serial.c) already handles this correctly by checking 'if (status != MSP_RESULT_NO_REPLY)' before sending. Fix: store the return value of mspFcProcessCommand() and skip sbufSwitchToReader() when the result is MSP_RESULT_NO_REPLY, leaving the response buffer empty so no reply is transmitted. Affects: SmartPort, CRSF, and FPort MSP passthrough. Co-authored-by: b14ckyy --- src/main/telemetry/msp_shared.c | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/src/main/telemetry/msp_shared.c b/src/main/telemetry/msp_shared.c index f3c32eaf7ef..f6d4c6eafbd 100644 --- a/src/main/telemetry/msp_shared.c +++ b/src/main/telemetry/msp_shared.c @@ -76,7 +76,7 @@ void initSharedMsp(void) mspPackage.responsePacket->buf.end = mspPackage.responseBuffer; } -static void processMspPacket(void) +static bool processMspPacket(void) { mspPackage.responsePacket->cmd = 0; mspPackage.responsePacket->result = 0; @@ -84,14 +84,20 @@ static void processMspPacket(void) mspPackage.responsePacket->buf.end = mspPackage.responseBuffer + sizeof(mspTxBuffer); mspPostProcessFnPtr mspPostProcessFn = NULL; - if (mspFcProcessCommand(mspPackage.requestPacket, mspPackage.responsePacket, &mspPostProcessFn) == MSP_RESULT_ERROR) { + const mspResult_e status = mspFcProcessCommand(mspPackage.requestPacket, mspPackage.responsePacket, &mspPostProcessFn); + if (status == MSP_RESULT_ERROR) { sbufWriteU8(&mspPackage.responsePacket->buf, TELEMETRY_MSP_ERROR); } if (mspPostProcessFn) { mspPostProcessFn(NULL); } + if (status == MSP_RESULT_NO_REPLY) { + return false; + } + sbufSwitchToReader(&mspPackage.responsePacket->buf, mspPackage.responseBuffer); + return true; } void sendMspErrorResponse(uint8_t error, int16_t cmd) @@ -194,8 +200,7 @@ bool handleMspFrame(uint8_t *const frameStart, const int payloadLength) sbufAdvance(&mspPackage.requestFrame, payloadExpecting); sbufWriteData(&requestPacket->buf, payload, payloadExpecting); sbufSwitchToReader(&requestPacket->buf, mspPackage.requestBuffer); - processMspPacket(); - return true; + return processMspPacket(); } bool sendMspReply(uint8_t payloadSize, mspResponseFnPtr responseFn) From 6b0aaa8f63ea2cfc3be2711485fac2584d1440f5 Mon Sep 17 00:00:00 2001 From: b14ckyy Date: Thu, 9 Apr 2026 09:42:59 +0200 Subject: [PATCH 30/48] Guard overlay against active RX and MSP_RC_OVERRIDE channels - Clamp 16-bit PWM values to 750-2250 (prevent int16_t overflow) - Reject oversized payloads (dataSize > 49) - Skip channels actively provided by primary RX (SBUS/CRSF/etc.) - When MSP is the RX provider, use actual MSP_SET_RAW_RC channel count - Skip channels controlled by MSP_RC_OVERRIDE bitmask when active --- src/main/fc/fc_msp.c | 3 ++- src/main/rx/msp.c | 7 +++++++ src/main/rx/msp.h | 1 + src/main/rx/rx.c | 29 +++++++++++++++++++++++++---- src/main/rx/rx.h | 2 +- 5 files changed, 36 insertions(+), 6 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 4fff9e169c9..9606302abd3 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -2415,7 +2415,8 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) case MSP2_INAV_SET_AUX_RC: { - if (dataSize < 2) { + // Max valid payload: 1 def byte + 24 channels × 2 bytes (16-bit) = 49 bytes + if (dataSize < 2 || dataSize > 49) { return MSP_RESULT_ERROR; } diff --git a/src/main/rx/msp.c b/src/main/rx/msp.c index b338fbc2d97..e2d310423a6 100644 --- a/src/main/rx/msp.c +++ b/src/main/rx/msp.c @@ -31,6 +31,7 @@ static uint16_t mspFrame[MAX_SUPPORTED_RC_CHANNEL_COUNT]; static bool rxMspFrameDone = false; +static uint8_t mspLastChannelCount = 0; static uint16_t rxMspReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfigPtr, uint8_t chan) { @@ -49,9 +50,15 @@ void rxMspFrameReceive(uint16_t *frame, int channelCount) mspFrame[i] = 0; } + mspLastChannelCount = channelCount; rxMspFrameDone = true; } +uint8_t rxMspGetLastChannelCount(void) +{ + return mspLastChannelCount; +} + static uint8_t rxMspFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig) { UNUSED(rxRuntimeConfig); diff --git a/src/main/rx/msp.h b/src/main/rx/msp.h index 78a9bfd7132..c99fe44d641 100644 --- a/src/main/rx/msp.h +++ b/src/main/rx/msp.h @@ -21,3 +21,4 @@ void rxMspFrameReceive(uint16_t *frame, int channelCount); void rxMspInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig); +uint8_t rxMspGetLastChannelCount(void); diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 7df78cc5e50..d1b8b7833f0 100755 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -515,10 +515,31 @@ bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs) } #endif - // Apply MSP aux channel overlay (CH9-CH32) - for (int i = 8; i < 32; i++) { - if (mspAuxOverlay[i] > 0) { - rcChannels[i].data = mspAuxOverlay[i]; + // Apply MSP aux channel overlay — only to channels NOT actively + // provided by the primary RX (avoids jitter from competing sources) + { + // Determine the effective RX channel count + uint8_t activeRxChannels = rxChannelCount; +#ifdef USE_RX_MSP + // When MSP is the primary RX, rxChannelCount is always 32. + // Use the actual channel count from the last MSP_SET_RAW_RC message. + if (rxConfig()->receiverType == RX_TYPE_MSP) { + const uint8_t mspChannels = rxMspGetLastChannelCount(); + if (mspChannels > 0) { + activeRxChannels = mspChannels; + } + } +#endif + for (int i = MAX(8, activeRxChannels); i < 32; i++) { + if (mspAuxOverlay[i] > 0) { +#if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE) + // Skip channels controlled by MSP RC Override when active + if (IS_RC_MODE_ACTIVE(BOXMSPRCOVERRIDE) && (rxConfig()->mspOverrideChannels & (1U << i))) { + continue; + } +#endif + rcChannels[i].data = mspAuxOverlay[i]; + } } } diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index 6eb63c83325..1c2989fa3dc 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -234,5 +234,5 @@ void resumeRxSignal(void); int16_t rxGetChannelValue(unsigned channelNumber); // MSP aux channel overlay (CH9-CH32). Sets a channel value that persists -// across RX update cycles. value=0 clears the overlay for that channel. +// across RX update cycles. value=0 ignores that channel and skips it. void rxMspAuxOverlaySet(uint8_t channelIndex, uint16_t value); From 3bf93e8f093e0d1a7a80b0f9d00ab16e2952c4ea Mon Sep 17 00:00:00 2001 From: b14ckyy Date: Thu, 9 Apr 2026 17:45:32 +0200 Subject: [PATCH 31/48] Restrict overlay to CH13-CH32, remove rxChannelCount guard MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Raise minimum channel from CH9 to CH13 (index 12) All common protocols deliver at least 12 channels on the wire. - Remove rxChannelCount-based guard (protocol drivers report max capacity, not actual sender channel count — unusable as guard) - Remove rxMspGetLastChannelCount (no longer needed) - Keep MSP_RC_OVERRIDE bitmask guard --- src/main/fc/fc_msp.c | 4 ++-- src/main/rx/msp.c | 7 ------- src/main/rx/msp.h | 1 - src/main/rx/rx.c | 33 +++++++++------------------------ src/main/rx/rx.h | 2 +- 5 files changed, 12 insertions(+), 35 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 9606302abd3..b4ba53ea724 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -2424,8 +2424,8 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) const uint8_t startChannel = defByte >> 3; // Bits 7-3: start channel index (0-31) const uint8_t resolutionMode = defByte & 0x07; // Bits 2-0: resolution - // Safety: CH1-CH8 (index 0-7) are protected - if (startChannel < 8) { + // Safety: CH1-CH12 (index 0-11) are protected + if (startChannel < 12) { return MSP_RESULT_ERROR; } diff --git a/src/main/rx/msp.c b/src/main/rx/msp.c index e2d310423a6..b338fbc2d97 100644 --- a/src/main/rx/msp.c +++ b/src/main/rx/msp.c @@ -31,7 +31,6 @@ static uint16_t mspFrame[MAX_SUPPORTED_RC_CHANNEL_COUNT]; static bool rxMspFrameDone = false; -static uint8_t mspLastChannelCount = 0; static uint16_t rxMspReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfigPtr, uint8_t chan) { @@ -50,15 +49,9 @@ void rxMspFrameReceive(uint16_t *frame, int channelCount) mspFrame[i] = 0; } - mspLastChannelCount = channelCount; rxMspFrameDone = true; } -uint8_t rxMspGetLastChannelCount(void) -{ - return mspLastChannelCount; -} - static uint8_t rxMspFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig) { UNUSED(rxRuntimeConfig); diff --git a/src/main/rx/msp.h b/src/main/rx/msp.h index c99fe44d641..78a9bfd7132 100644 --- a/src/main/rx/msp.h +++ b/src/main/rx/msp.h @@ -21,4 +21,3 @@ void rxMspFrameReceive(uint16_t *frame, int channelCount); void rxMspInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig); -uint8_t rxMspGetLastChannelCount(void); diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index d1b8b7833f0..b85350d8333 100755 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -515,31 +515,16 @@ bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs) } #endif - // Apply MSP aux channel overlay — only to channels NOT actively - // provided by the primary RX (avoids jitter from competing sources) - { - // Determine the effective RX channel count - uint8_t activeRxChannels = rxChannelCount; -#ifdef USE_RX_MSP - // When MSP is the primary RX, rxChannelCount is always 32. - // Use the actual channel count from the last MSP_SET_RAW_RC message. - if (rxConfig()->receiverType == RX_TYPE_MSP) { - const uint8_t mspChannels = rxMspGetLastChannelCount(); - if (mspChannels > 0) { - activeRxChannels = mspChannels; - } - } -#endif - for (int i = MAX(8, activeRxChannels); i < 32; i++) { - if (mspAuxOverlay[i] > 0) { + // Apply MSP aux channel overlay (CH13-CH32) + for (int i = 12; i < 32; i++) { + if (mspAuxOverlay[i] > 0) { #if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE) - // Skip channels controlled by MSP RC Override when active - if (IS_RC_MODE_ACTIVE(BOXMSPRCOVERRIDE) && (rxConfig()->mspOverrideChannels & (1U << i))) { - continue; - } -#endif - rcChannels[i].data = mspAuxOverlay[i]; + // Skip channels controlled by MSP RC Override when active + if (IS_RC_MODE_ACTIVE(BOXMSPRCOVERRIDE) && (rxConfig()->mspOverrideChannels & (1U << i))) { + continue; } +#endif + rcChannels[i].data = mspAuxOverlay[i]; } } @@ -696,7 +681,7 @@ int16_t rxGetChannelValue(unsigned channelNumber) void rxMspAuxOverlaySet(uint8_t channelIndex, uint16_t value) { - if (channelIndex >= 8 && channelIndex < 32) { + if (channelIndex >= 12 && channelIndex < 32) { mspAuxOverlay[channelIndex] = value; } } diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index 1c2989fa3dc..5b5bcea50c3 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -233,6 +233,6 @@ void resumeRxSignal(void); // during failsafe. int16_t rxGetChannelValue(unsigned channelNumber); -// MSP aux channel overlay (CH9-CH32). Sets a channel value that persists +// MSP aux channel overlay (CH13-CH32). Sets a channel value that persists // across RX update cycles. value=0 ignores that channel and skips it. void rxMspAuxOverlaySet(uint8_t channelIndex, uint16_t value); From 86615b9f40bcbabebce6c5c1dbae7dd89fcfe9f2 Mon Sep 17 00:00:00 2001 From: b14ckyy Date: Thu, 9 Apr 2026 18:12:57 +0200 Subject: [PATCH 32/48] Re-add MSP RX channel count guard for overlay When MSP is the primary RX provider (e.g. mLRS), skip channels already covered by MSP_SET_RAW_RC to avoid wasting bandwidth on overlapping updates. --- src/main/rx/msp.c | 7 +++++++ src/main/rx/msp.h | 1 + src/main/rx/rx.c | 26 +++++++++++++++++++------- 3 files changed, 27 insertions(+), 7 deletions(-) diff --git a/src/main/rx/msp.c b/src/main/rx/msp.c index b338fbc2d97..e2d310423a6 100644 --- a/src/main/rx/msp.c +++ b/src/main/rx/msp.c @@ -31,6 +31,7 @@ static uint16_t mspFrame[MAX_SUPPORTED_RC_CHANNEL_COUNT]; static bool rxMspFrameDone = false; +static uint8_t mspLastChannelCount = 0; static uint16_t rxMspReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfigPtr, uint8_t chan) { @@ -49,9 +50,15 @@ void rxMspFrameReceive(uint16_t *frame, int channelCount) mspFrame[i] = 0; } + mspLastChannelCount = channelCount; rxMspFrameDone = true; } +uint8_t rxMspGetLastChannelCount(void) +{ + return mspLastChannelCount; +} + static uint8_t rxMspFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig) { UNUSED(rxRuntimeConfig); diff --git a/src/main/rx/msp.h b/src/main/rx/msp.h index 78a9bfd7132..c99fe44d641 100644 --- a/src/main/rx/msp.h +++ b/src/main/rx/msp.h @@ -21,3 +21,4 @@ void rxMspFrameReceive(uint16_t *frame, int channelCount); void rxMspInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig); +uint8_t rxMspGetLastChannelCount(void); diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index b85350d8333..b897238ed1d 100755 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -516,15 +516,27 @@ bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs) #endif // Apply MSP aux channel overlay (CH13-CH32) - for (int i = 12; i < 32; i++) { - if (mspAuxOverlay[i] > 0) { -#if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE) - // Skip channels controlled by MSP RC Override when active - if (IS_RC_MODE_ACTIVE(BOXMSPRCOVERRIDE) && (rxConfig()->mspOverrideChannels & (1U << i))) { - continue; + { + int overlayStart = 12; +#ifdef USE_RX_MSP + // When MSP is the primary RX, skip channels covered by MSP_SET_RAW_RC + if (rxConfig()->receiverType == RX_TYPE_MSP) { + const uint8_t mspChannels = rxMspGetLastChannelCount(); + if (mspChannels > overlayStart) { + overlayStart = mspChannels; } + } #endif - rcChannels[i].data = mspAuxOverlay[i]; + for (int i = overlayStart; i < 32; i++) { + if (mspAuxOverlay[i] > 0) { +#if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE) + // Skip channels controlled by MSP RC Override when active + if (IS_RC_MODE_ACTIVE(BOXMSPRCOVERRIDE) && (rxConfig()->mspOverrideChannels & (1U << i))) { + continue; + } +#endif + rcChannels[i].data = mspAuxOverlay[i]; + } } } From f025071bcc84efc9df6e0247304045a52e4e30e5 Mon Sep 17 00:00:00 2001 From: b14ckyy Date: Thu, 9 Apr 2026 18:50:20 +0200 Subject: [PATCH 33/48] Add MSP2_INAV_SET_AUX_RC documentation - Rx.md: Add 'MSP Auxiliary RC Channel Overlay' section with key properties, use case, priority order, and safeguard descriptions - Rx.md: Fix MSP RX channel count (18 -> 34) - msp_messages.json: Update channel range to CH13-CH32, add clamp range, payload size limits, and guard descriptions --- docs/Rx.md | 27 +++++++++++++++++++++++++- docs/development/msp/msp_messages.json | 8 ++++---- 2 files changed, 30 insertions(+), 5 deletions(-) diff --git a/docs/Rx.md b/docs/Rx.md index 0f0ae64a453..3c9f5497448 100644 --- a/docs/Rx.md +++ b/docs/Rx.md @@ -201,7 +201,7 @@ bind_msp_rx ## MultiWii serial protocol (MSP RX) -Allows you to use MSP commands as the RC input. Up to 18 channels are supported. +Allows you to use MSP commands as the RC input. Up to 34 channels are supported. Note: * It is necessary to update `MSP_SET_RAW_RC` at 5Hz or faster. * `MSP_SET_RAW_RC` uses the defined RC channel map @@ -213,6 +213,31 @@ Note: Enables the use of a joystick in the INAV SITL with a flight simulator. See the [SITL documentation](SITL/SITL.md). +## MSP Auxiliary RC Channel Overlay (MSP2_INAV_SET_AUX_RC) + +Allows extending the available RC channel count beyond the native RC link capacity using `MSP2_INAV_SET_AUX_RC` (`0x2230`). This is a lightweight, bandwidth-efficient alternative to `MSP_SET_RAW_RC` for auxiliary channels only. + +**Key properties:** +- Controls **CH13–CH32** only (CH1–CH12 are protected and rejected) +- Configurable resolution: 2-bit (3 positions), 4-bit (~71µs steps), 8-bit (~3.9µs steps), or 16-bit (raw PWM) +- Value `0` = skip (no update) — previous value persists indefinitely +- No flight mode or special configuration required — always active +- Does **not** affect failsafe detection +- Recommended to send with `MSP_FLAG_DONT_REPLY` (`flags=0x01`) on telemetry passthrough links + +**Typical use case:** A Lua script on the radio sends `MSP2_INAV_SET_AUX_RC` via SmartPort/CRSF/ELRS telemetry passthrough to control auxiliary functions (lights, camera triggers, gimbal modes) on channels beyond the RC link's native capacity. + +**Priority order** (last writer wins): +1. Primary RX (SBUS, CRSF, FPort, etc.) +2. MSP RC Override (if active) +3. **MSP AUX Overlay** (CH13–CH32) + +**Important:** For serial RX protocols, the firmware cannot detect which channels the sender actively uses. If AUX_RC targets a channel that the RX link also sends, AUX_RC will override it. Configure the start channel above your RC link's active channel range. + +When MSP is the primary RX provider (`receiver_type = MSP`), channels covered by `MSP_SET_RAW_RC` are automatically protected. Channels in the `msp_override_channels` bitmask are also protected when MSP RC Override mode is active. + +See the [MSP documentation](development/msp/README.md) for the full message format. + ## Configuration The receiver type can be set from the configurator or CLI. diff --git a/docs/development/msp/msp_messages.json b/docs/development/msp/msp_messages.json index 866eb87b3b8..bbb7eac3429 100644 --- a/docs/development/msp/msp_messages.json +++ b/docs/development/msp/msp_messages.json @@ -10951,13 +10951,13 @@ { "name": "definitionByte", "ctype": "uint8_t", - "desc": "Packed start channel and resolution. Bits 7-3: start channel index (valid range 8-31 for CH9-CH32; 0-7 rejected as error). Bits 2-0: resolution mode (0=2-bit, 1=4-bit, 2=8-bit, 3=16-bit; 4-7 reserved/error).", + "desc": "Packed start channel and resolution. Bits 7-3: start channel index (valid range 12-31 for CH13-CH32; 0-11 rejected as error). Bits 2-0: resolution mode (0=2-bit, 1=4-bit, 2=8-bit, 3=16-bit; 4-7 reserved/error).", "units": "" }, { "name": "channelData", "ctype": "uint8_t", - "desc": "Packed channel values, sequential from start channel. Number of channels is derived from data size and resolution. Value 0 means skip (no update). Sub-byte modes (2-bit, 4-bit) are packed MSB-first. 2-bit values 1-3 map to 1000/1500/2000us. 4-bit values 1-15 map to 1000 + (val-1)*1000/14 us. 8-bit values 1-255 map to 1000 + (val-1)*1000/254 us. 16-bit values are direct PWM.", + "desc": "Packed channel values, sequential from start channel. Number of channels is derived from data size and resolution. Value 0 means skip (no update). Sub-byte modes (2-bit, 4-bit) are packed MSB-first. 2-bit values 1-3 map to 1000/1500/2000us. 4-bit values 1-15 map to 1000 + (val-1)*1000/14 us. 8-bit values 1-255 map to 1000 + (val-1)*1000/254 us. 16-bit values are direct PWM, clamped to 750-2250us.", "units": "PWM (encoded)", "array": true, "array_size": 0 @@ -10966,8 +10966,8 @@ }, "reply": null, "variable_len": true, - "notes": "CH1-CH8 (index 0-7) are protected and will return `MSP_RESULT_ERROR`. Constraint: `startChannel + channelCount <= 32`. Values persist until overwritten; no timeout. Applied as a post-RX overlay in `calculateRxChannelsAndUpdateFailsafe()` after MSP RC Override but before failsafe. Does not require `USE_RX_MSP` or MSP-RC-OVERRIDE flight mode. Does not affect failsafe detection. Recommended to send with `MSP_FLAG_DONT_REPLY` (flags=0x01) to save bandwidth on telemetry passthrough links. 16-bit mode requires even number of data bytes.", - "description": "Bandwidth-efficient auxiliary RC channel update. Sets CH9-CH32 with configurable resolution (2/4/8/16-bit) without affecting primary flight controls. Designed for extending channel count beyond native RC link capacity via MSP passthrough." + "notes": "CH1-CH12 (index 0-11) are protected and will return `MSP_RESULT_ERROR`. Payload size must be 2-49 bytes. Constraint: `startChannel + channelCount <= 32`. Values persist until overwritten; no timeout. Applied as a post-RX overlay in `calculateRxChannelsAndUpdateFailsafe()` after MSP RC Override but before failsafe. Does not require `USE_RX_MSP` or MSP-RC-OVERRIDE flight mode. Does not affect failsafe detection. When MSP is the primary RX provider, channels covered by `MSP_SET_RAW_RC` are automatically skipped. Channels in the `mspOverrideChannels` bitmask are skipped when MSP RC Override mode is active. Recommended to send with `MSP_FLAG_DONT_REPLY` (flags=0x01) to save bandwidth on telemetry passthrough links. 16-bit mode requires even number of data bytes and values are clamped to 750-2250us.", + "description": "Bandwidth-efficient auxiliary RC channel update. Sets CH13-CH32 with configurable resolution (2/4/8/16-bit) without affecting primary flight controls. Designed for extending channel count beyond native RC link capacity via MSP passthrough." }, "MSP2_BETAFLIGHT_BIND": { "code": 12288, From f397f7ed043a30c7f5e45604bb9a8ffbb33513a7 Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Thu, 9 Apr 2026 22:40:50 -0500 Subject: [PATCH 34/48] telemetry: fix CRSF airspeed integer truncation and empty RPM/temp frames - Fix airspeed always transmitting zero: integer `36 / 100` evaluates to 0 in C; use `36.0f / 100.0f` for correct cm/s to 0.1 km/h conversion - crsfRpm/crsfTemperature now return bool; crsfFinalize is only called when data was actually written, preventing malformed single-byte frames when ESC sensor or temperature sources are absent --- src/main/telemetry/crsf.c | 20 +++++++++++++------- 1 file changed, 13 insertions(+), 7 deletions(-) diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index 159a8c16ce2..e8214fc8c1f 100755 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -321,7 +321,7 @@ static void crsfFrameAirSpeedSensor(sbuf_t *dst) // use sbufWrite since CRC does not include frame length sbufWriteU8(dst, CRSF_FRAME_AIRSPEED_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC); crsfSerialize8(dst, CRSF_FRAMETYPE_AIRSPEED_SENSOR); - crsfSerialize16(dst, (uint16_t)(getAirspeedEstimate() * 36 / 100)); + crsfSerialize16(dst, (uint16_t)(getAirspeedEstimate() * 36.0f / 100.0f)); } #endif @@ -332,7 +332,7 @@ static void crsfFrameAirSpeedSensor(sbuf_t *dst) uint8_t rpm_source_id; // Identifies the source of the RPM data (e.g., 0 = Motor 1, 1 = Motor 2, etc.) int24_t rpm_value[]; // 1 - 19 RPM values with negative ones representing the motor spinning in reverse */ -static void crsfRpm(sbuf_t *dst) +static bool crsfRpm(sbuf_t *dst) { const uint8_t MAX_CRSF_RPM_VALUES = 19; // CRSF protocol limit: 1-19 RPM values uint8_t motorCount = getMotorCount(); @@ -352,7 +352,9 @@ static void crsfRpm(sbuf_t *dst) const escSensorData_t *escState = getEscTelemetry(i); crsfSerialize24(dst, (escState) ? escState->rpm : 0); } + return true; } + return false; } #endif @@ -362,7 +364,7 @@ static void crsfRpm(sbuf_t *dst) uint8_t temp_source_id; // Identifies the source of the temperature data (e.g., 0 = FC including all ESCs, 1 = Ambient, etc.) int16_t temperature[]; // up to 20 temperature values in deci-degree (tenths of a degree) Celsius (e.g., 250 = 25.0°C, -50 = -5.0°C) */ -static void crsfTemperature(sbuf_t *dst) +static bool crsfTemperature(sbuf_t *dst) { const uint8_t MAX_CRSF_TEMPS = 20; // Maximum temperatures per CRSF frame uint8_t tempCount = 0; @@ -393,7 +395,9 @@ static void crsfTemperature(sbuf_t *dst) crsfSerialize8(dst, 0); for (uint8_t i = 0; i < tempCount; i++) crsfSerialize16(dst, temperatures[i]); + return true; } + return false; } typedef enum { @@ -632,15 +636,17 @@ static void processCrsf(void) #ifdef USE_ESC_SENSOR if (currentSchedule & BV(CRSF_FRAME_RPM_INDEX)) { crsfInitializeFrame(dst); - crsfRpm(dst); - crsfFinalize(dst); + if (crsfRpm(dst)) { + crsfFinalize(dst); + } } #endif #if defined(USE_ESC_SENSOR) || defined(USE_TEMPERATURE_SENSOR) if (currentSchedule & BV(CRSF_FRAME_TEMP_INDEX)) { crsfInitializeFrame(dst); - crsfTemperature(dst); - crsfFinalize(dst); + if (crsfTemperature(dst)) { + crsfFinalize(dst); + } } #endif #ifdef USE_GPS From 0f18cbc6946a55a7391bfd2129da5331bd9d62ac Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Thu, 5 Mar 2026 23:11:59 -0600 Subject: [PATCH 35/48] Use single-precision math where double is unnecessary Replace three uses of double-precision library functions with their float equivalents, eliminating software-emulated double math on the Cortex-M7 FPV5-SP-D16 (single-precision-only) FPU: - gps_ublox.c: atof() -> fastA2F() in gpsDecodeProtocolVersion(). atof() pulled in libc_nano strtod (28 KB of double-precision parsing machinery) to parse simple version strings like "18.00". fastA2F() is already in the codebase and covers this use case. - maths.c: exp/pow (double) -> expf/powf in gaussian(). The explicit (double) casts defeated the -fsingle-precision-constant flag. gaussian() is called from the IMU loop at up to 1000 Hz, so switching to hardware-accelerated single-precision also improves runtime performance. - vtx_smartaudio.c: pow(10.0, ...) -> powf(10.0f, ...) in saDbiToMw(). Combined with the maths.c change, removes all callers of double pow, dropping libm e_pow.o from the link entirely. Flash savings on MATEKF722 (STM32F722, 512 KB): text: -16,560 B data: -376 B total: -16,936 B (~16.5 KB) --- src/main/common/maths.c | 2 +- src/main/io/gps_ublox.c | 2 +- src/main/io/vtx_smartaudio.c | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/common/maths.c b/src/main/common/maths.c index 993634d902d..2881d2b8938 100644 --- a/src/main/common/maths.c +++ b/src/main/common/maths.c @@ -526,7 +526,7 @@ bool sensorCalibrationSolveForScale(sensorCalibrationState_t * state, float resu } float gaussian(const float x, const float mu, const float sigma) { - return exp(-pow((double)(x - mu), 2) / (2 * pow((double)sigma, 2))); + return expf(-powf(x - mu, 2.0f) / (2.0f * powf(sigma, 2.0f))); } float bellCurve(const float x, const float curveWidth) diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index f0899aa69d5..d198ed67dc6 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -549,7 +549,7 @@ static void gpsDecodeProtocolVersion(const char *proto, size_t bufferLength) if (bufferLength > 13 && (!strncmp(proto, "PROTVER=", 8) || !strncmp(proto, "PROTVER ", 8))) { proto+=8; - float ver = atof(proto); + float ver = fastA2F(proto); gpsState.swVersionMajor = (uint8_t)ver; gpsState.swVersionMinor = (uint8_t)((ver - gpsState.swVersionMajor) * 100.0f); diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c index 7d5e213cce0..7e24d552929 100644 --- a/src/main/io/vtx_smartaudio.c +++ b/src/main/io/vtx_smartaudio.c @@ -212,7 +212,7 @@ int saDacToPowerIndex(int dac) int saDbiToMw(uint16_t dbi) { - uint16_t mw = (uint16_t)pow(10.0, dbi / 10.0); + uint16_t mw = (uint16_t)powf(10.0f, (float)dbi / 10.0f); if (dbi > 14) { // For powers greater than 25mW round up to a multiple of 50 to match expectations From 03751bc76a9a6e60d6b28cebb3004af5d5149f98 Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Thu, 5 Mar 2026 23:25:04 -0600 Subject: [PATCH 36/48] Code review fixes for float math flash savings Follow-up to the single-precision math change that saves ~17 KB of flash on STM32F722 targets: - maths.c: Replace powf(x, 2.0f) with explicit multiplies in gaussian(). Clearer intent and avoids any dependency on libm reducing integer-exponent powf to a multiply. - vtx_smartaudio.c: Add roundf() before uint16_t cast in saDbiToMw(). powf(10.0f, 1.0f) may return 9.999...f on some libm implementations, which would truncate to 9 mW instead of the correct 10 mW. roundf() closes the hazard at the cost of one FPU instruction. --- src/main/common/maths.c | 2 +- src/main/io/vtx_smartaudio.c | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/common/maths.c b/src/main/common/maths.c index 2881d2b8938..d591e40de81 100644 --- a/src/main/common/maths.c +++ b/src/main/common/maths.c @@ -526,7 +526,7 @@ bool sensorCalibrationSolveForScale(sensorCalibrationState_t * state, float resu } float gaussian(const float x, const float mu, const float sigma) { - return expf(-powf(x - mu, 2.0f) / (2.0f * powf(sigma, 2.0f))); + return expf(-((x - mu) * (x - mu)) / (2.0f * sigma * sigma)); } float bellCurve(const float x, const float curveWidth) diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c index 7e24d552929..acb2a34e6dc 100644 --- a/src/main/io/vtx_smartaudio.c +++ b/src/main/io/vtx_smartaudio.c @@ -212,7 +212,7 @@ int saDacToPowerIndex(int dac) int saDbiToMw(uint16_t dbi) { - uint16_t mw = (uint16_t)powf(10.0f, (float)dbi / 10.0f); + uint16_t mw = (uint16_t)roundf(powf(10.0f, (float)dbi / 10.0f)); if (dbi > 14) { // For powers greater than 25mW round up to a multiple of 50 to match expectations From a66b346c24b7f6741c418d0104b564233b898ad4 Mon Sep 17 00:00:00 2001 From: Sensei Date: Fri, 10 Apr 2026 12:48:31 -0500 Subject: [PATCH 37/48] blueberry h743: Restore PWM beeper timer --- src/main/target/BLUEBERRYH743/target.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/target/BLUEBERRYH743/target.c b/src/main/target/BLUEBERRYH743/target.c index 5245d05e257..2c924ecac48 100644 --- a/src/main/target/BLUEBERRYH743/target.c +++ b/src/main/target/BLUEBERRYH743/target.c @@ -47,8 +47,9 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM15, CH1, PE5, TIM_USE_OUTPUT_AUTO, 0, 0), // S11 DEF_TIM(TIM15, CH2, PE6, TIM_USE_OUTPUT_AUTO, 0, 0), // S12 DMA_NONE - + DEF_TIM(TIM2, CH1, PA15, TIM_USE_BEEPER, 0, 0), // BEEPER PWM DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 9), // LED_2812 + }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); From 50de29acfb02eb836da6f6dec3007639cb0e85b5 Mon Sep 17 00:00:00 2001 From: b14ckyy Date: Sat, 11 Apr 2026 15:54:44 +0200 Subject: [PATCH 38/48] docs: add Backup and Restore documentation for Configurator auto-backup feature - Add new docs/Backup and Restore.md covering automatic backup/restore during firmware flash, migration preview, file locations and formats - Update Installation.md: reference auto-backup in Backup/Restore section - Update Configuration.md: replace outdated GUI backup warning - Update Cli.md: note that Configurator now handles backup automatically Related: iNavFlight/inav-configurator#2603 --- docs/Backup and Restore.md | 107 +++++++++++++++++++++++++++++++++++++ docs/Cli.md | 2 + docs/Configuration.md | 2 +- docs/Installation.md | 6 ++- 4 files changed, 115 insertions(+), 2 deletions(-) create mode 100644 docs/Backup and Restore.md diff --git a/docs/Backup and Restore.md b/docs/Backup and Restore.md new file mode 100644 index 00000000000..9206abafa03 --- /dev/null +++ b/docs/Backup and Restore.md @@ -0,0 +1,107 @@ +# Backup and Restore + +INAV Configurator can automatically back up your configuration before flashing firmware and offer to restore it afterwards. When upgrading across major versions (e.g. 7.x → 8.x → 9.x), settings are automatically migrated to the new firmware format. + +For manual CLI-based backup and restore, see the [CLI documentation](Cli.md#backup-via-cli). + +## Automatic Backup & Restore During Firmware Flash + +### What happens automatically + +1. **Before flashing** (with Full Chip Erase enabled): Your current CLI configuration (`diff all`) is automatically captured and saved to the backup directory. +2. **After flashing**: Depending on the situation, the Configurator offers to restore your settings: + +| Scenario | Behavior | +|----------|----------| +| **Patch update** (e.g. 8.0.0 → 8.0.1) | Auto-restore offered immediately | +| **Minor update** (e.g. 8.0.0 → 8.1.0) | Auto-restore offered immediately | +| **Major upgrade** (e.g. 7.x → 8.x) with migration profile available | Migration preview shown — confirm to restore with converted settings | +| **Major upgrade** without migration profile | Warning shown — restore still possible but some settings may fail | +| **Major downgrade** (e.g. 9.x → 7.x) | Auto-restore blocked — manual restore only (settings may be incompatible) | +| **Local firmware file** (loaded from disk) | No auto-restore offered — backup is still saved | +| **Flash without Full Chip Erase** | Backup taken, no restore offered | + +### Migration Preview + +When updating across major versions (e.g. 7.x → 9.x), the Configurator shows a **migration preview overlay** before restoring. This lists: + +- **Removed settings** — settings that no longer exist in the new firmware (will be skipped) +- **Renamed settings** — settings whose name changed (automatically converted) +- **Renamed commands** — CLI commands that were renamed (automatically converted) +- **Value replacements** — setting values that changed meaning (automatically converted) +- **Setting remappings** — numeric IDs that were renumbered (automatically converted) +- **Warnings** — settings whose semantics changed and require manual review + +You can review all changes before confirming or cancelling the restore. + +Multi-step migrations are handled automatically. For example, a 7.x → 9.x upgrade applies migration profiles in sequence (7→8, then 8→9). + +## Manual Backup & Restore + +The Firmware Flasher tab provides three buttons: + +- **Backup Config** — saves your current CLI configuration to a file (opens save dialog) +- **Restore Config** — loads a backup file and restores it to your flight controller + - If the backup is from a different major version, the migration preview is shown first + - If no migration profile exists for the version gap, a warning is shown but you can still proceed +- **Open Backups Folder** — opens the backup directory in your file manager + +For CLI-based backup and restore procedures, see [Backup via CLI](Cli.md#backup-via-cli) and [Restore via CLI](Cli.md#restore-via-cli). + +## Backup File Location + +Backups are stored in your OS-specific application data directory: + +| OS | Path | +|----|------| +| **Windows** | `%APPDATA%/inav-configurator/backups/` | +| **macOS** | `~/Library/Application Support/inav-configurator/backups/` | +| **Linux** | `~/.config/inav-configurator/backups/` | + +Use the **Open Backups Folder** button in the Firmware Flasher tab to open this directory. + +## Backup File Naming + +| Type | Format | +|------|--------| +| Auto-backups | `UPDATE_inav_backup_{version}_{board}_{YYYY-MM-DD_HHMMSS}.txt` | +| Manual backups | `inav_backup_{version}_{board}_{YYYY-MM-DD_HHMMSS}.txt` | + +Auto-backups are pruned automatically — only the 10 most recent are kept. Files you rename are never pruned. + +## Backup File Format + +Backup files are plain-text CLI dumps with a metadata header: + +``` +# INAV Backup +# Version: 8.0.0 +# Board: SPEEDYBEEF405V4 +# Date: 2026-04-11T10:30:00.000Z +# Craft: MyQuad +# +# INAV/SPEEDYBEEF405V4 8.0.0 Apr 1 2026 / 12:00:00 (abc1234) +# GCC-13.2.1 +# ... +set gyro_main_lpf_hz = 110 +set acc_hardware = AUTO +... +``` + +You can open and edit these files with any text editor. + +## Restore Error Handling + +If errors occur during restore (e.g. unknown settings, invalid values): + +- An error dialog shows the affected lines +- You can choose: + - **Save anyway** — saves the successfully applied settings and reboots + - **Abort** — discards all changes and exits CLI mode + +## Tips + +- **Always flash with Full Chip Erase** when upgrading to a new version. This ensures clean defaults and triggers automatic backup and restore. +- **Review migration previews carefully** — especially the warnings section, which highlights settings whose meaning may have changed. +- **Keep manual backups** before major upgrades. While auto-backup handles this, having an extra copy in a known location gives peace of mind. +- **Use `diff` over `dump`** for backups. The `diff` format only stores settings that differ from defaults, which makes restoring safer across versions. The auto-backup feature already uses `diff all`. diff --git a/docs/Cli.md b/docs/Cli.md index d949937a61b..d27029ee1b5 100644 --- a/docs/Cli.md +++ b/docs/Cli.md @@ -22,6 +22,8 @@ See the other documentation sections for details of the cli commands and setting ## Backup via CLI +> **Note:** The INAV Configurator now performs automatic backups before flashing and can restore settings afterwards, including migration across major versions. See [Backup and Restore](Backup%20and%20Restore.md) for details. The CLI method below remains available for manual backup. + Disconnect main power, connect to cli via USB/FTDI. dump using cli diff --git a/docs/Configuration.md b/docs/Configuration.md index 1ccea15d110..f48de1a10d6 100644 --- a/docs/Configuration.md +++ b/docs/Configuration.md @@ -10,7 +10,7 @@ See the Serial section for more information and see the Board specific sections The GUI cannot currently configure all aspects of the system, the CLI must be used to enable or configure some features and settings. -__Due to ongoing development, the fact that the GUI cannot yet backup all your settings and automatic chrome updates of the GUI app it is highly advisable to backup your settings (using the CLI) so that when a new version of the configurator or firmware is released you can re-apply your settings.__ +__The INAV Configurator now (versions after 9.0.x) automatically backs up your settings before flashing firmware and can restore them afterwards (when Full Chip Erase is enabled), including automatic migration across major versions. For details, see [Backup and Restore](Backup%20and%20Restore.md). You can also manually backup and restore settings using the [CLI](Cli.md#backup-via-cli).__ ## GUI diff --git a/docs/Installation.md b/docs/Installation.md index 38e9470fd81..140f47bd6c9 100644 --- a/docs/Installation.md +++ b/docs/Installation.md @@ -27,4 +27,8 @@ When upgrading be sure to backup / dump your existing settings. Some firmware r ## Backup/Restore process -See the CLI section of the docs for details on how to backup and restore your configuration via the CLI. +The INAV Configurator (after version 9.0.x) automatically backs up your configuration before flashing and offers to restore it afterwards (when Full Chip Erase is enabled) — including automatic settings migration when upgrading across major versions. + +For details on automatic and manual backup/restore, see [Backup and Restore](Backup%20and%20Restore.md). + +For CLI-based backup and restore, see the [CLI documentation](Cli.md#backup-via-cli). From 959aa7848c99864a5457067820885a00e051ea95 Mon Sep 17 00:00:00 2001 From: b14ckyy Date: Sat, 11 Apr 2026 16:00:46 +0200 Subject: [PATCH 39/48] docs: add Backup/Restore architecture reference for migration profiles Developer-facing documentation covering migration engine internals, migration profile schema, and instructions for creating new profiles when major INAV versions are released. Related: iNavFlight/inav-configurator#2603 --- .../Backup Restore Architecture.md | 251 ++++++++++++++++++ 1 file changed, 251 insertions(+) create mode 100644 docs/development/Backup Restore Architecture.md diff --git a/docs/development/Backup Restore Architecture.md b/docs/development/Backup Restore Architecture.md new file mode 100644 index 00000000000..9dd971230c6 --- /dev/null +++ b/docs/development/Backup Restore Architecture.md @@ -0,0 +1,251 @@ +# Backup, Restore & Settings Migration — Architecture + +> **Note:** This document describes the internals of the INAV Configurator's backup/restore and settings migration system. It is intended for Configurator developers, not end users. For user-facing documentation, see the [INAV docs](https://github.com/iNavFlight/inav/blob/master/docs/Backup%20and%20Restore.md). + +## Architecture Overview + +``` +User → Firmware Flasher Tab → STM32.connect(onCliReady) → CLI mode + ↓ + BackupRestore.captureCliDiffAll() + ↓ + Save to file, prune old backups + ↓ + STM32 flash (DFU or serial) + ↓ + onFlashComplete() callback + ↓ + Version check → Migration check → UI overlay + ↓ + User confirms → Poll for FC reconnect + ↓ + BackupRestore.performRestore() or performRestoreWithMigration() + ↓ + saveAndReboot() or abortRestore() +``` + +## Files + +| File | Purpose | +|------|---------| +| `js/backup_restore.js` | Core backup/restore module — CLI protocol, file I/O, auto-backup | +| `js/migration/migration_handler.js` | Version migration engine — profile chaining, line transformation | +| `js/migration/7_to_8.json` | Migration profile: INAV 7.x → 8.0 | +| `js/migration/8_to_9.json` | Migration profile: INAV 8.0 → 9.0 | +| `tabs/firmware_flasher.js` | Flash integration — auto-backup trigger, restore UI, version gating | +| `tabs/firmware_flasher.html` | Overlays and buttons for backup/restore/migration UI | +| `src/css/tabs/firmware_flasher.css` | Overlay styles | +| `js/protocols/stm32.js` | STM32 flash protocol — `onCliReady` callback, DFU timeout fix | +| `js/main/main.js` | Electron main process — IPC handlers for file operations | +| `js/main/preload.js` | IPC bridge — exposes backup API to renderer | +| `locale/en/messages.json` | All i18n translation keys | + +## Adding a New Migration Profile + +When a new major INAV version is released (e.g. 9.x → 10.x), create a migration profile: + +### Step 1: Create the JSON profile + +Create `js/migration/9_to_10.json`: + +```json +{ + "fromVersion": "9", + "toVersion": "10", + "description": "INAV 9.x → 10.0 migration profile", + + "commandRenames": { + "old_command_name": "new_command_name" + }, + + "settingRenames": { + "old_setting_name": "new_setting_name" + }, + + "valueReplacements": { + "setting_name": { + "OLD_VALUE": "NEW_VALUE" + } + }, + + "removed": [ + "deleted_setting_1", + "deleted_setting_2" + ], + + "settingPatternMappings": [ + { + "pattern": "^regex_matching_setting_names$", + "valueMap": { "old_numeric_id": "new_numeric_id" }, + "description": "Human-readable description of remapping" + } + ], + + "warnings": [ + "Human-readable warning about settings whose semantics changed and need manual review." + ] +} +``` + +### Step 2: Register the profile + +In `js/migration/migration_handler.js`, add the import and append to the array: + +```javascript +import profile_9_to_10 from './9_to_10.json'; + +const MIGRATION_PROFILES = [ + profile_7_to_8, + profile_8_to_9, + profile_9_to_10, // ← add here +]; +``` + +The migration engine automatically chains profiles. A 7.x → 10.x upgrade will apply all three profiles in sequence (7→8, 8→9, 9→10). + +### How to determine what goes into a migration profile + +Compare CLI settings between the old and new firmware version: + +1. **Removed settings**: Run `diff all` on old and new firmware with default settings. Settings present in old but not in new → add to `removed` +2. **Renamed settings**: Check INAV release notes and source code for renamed settings → add to `settingRenames` +3. **Renamed commands**: Check for CLI command name changes (e.g. `profile` → `control_profile`) → add to `commandRenames` +4. **Value replacements**: Check for enum value name changes → add to `valueReplacements` +5. **Pattern mappings**: Check for bulk ID renumbering (OSD elements, etc.) → add to `settingPatternMappings` +6. **Warnings**: Check for settings where the meaning/units changed but name stayed the same → add to `warnings` + +Key INAV source files to check: +- `src/main/fc/settings.yaml` — all CLI settings definitions +- `src/main/fc/cli.c` — CLI command implementations +- Release notes on GitHub + +## Migration Profile Schema Reference + +| Field | Type | Required | Description | +|-------|------|----------|-------------| +| `fromVersion` | `string` | Yes | Source major version number (e.g. `"9"`) | +| `toVersion` | `string` | Yes | Target major version number (e.g. `"10"`) | +| `description` | `string` | Yes | Human-readable description | +| `commandRenames` | `object` | Yes | Maps old CLI command names to new names. Applied to any token in the command line. E.g. `"profile" → "control_profile"` transforms `profile 2` to `control_profile 2` | +| `settingRenames` | `object` | Yes | Maps old `set` setting names to new names. Only applies to `set = ` lines | +| `valueReplacements` | `object` | Yes | Maps setting names to value replacement objects `{ "oldval": "newval" }`. Only modifies the value portion after `=` | +| `removed` | `string[]` | Yes | List of setting names to remove entirely. Lines with `set = ...` matching these are dropped | +| `settingPatternMappings` | `array` | Yes | Array of pattern-based value remappings for settings matching a regex. Each entry has `pattern` (regex), `valueMap` (object), `description` (string) | +| `warnings` | `string[]` | Yes | Warning messages about semantic changes requiring manual review. Displayed in migration preview overlay | + +## Existing Migration Profile Details + +### 7_to_8.json (INAV 7.x → 8.0) + +| Category | Changes | +|----------|---------| +| **Command renames** | `profile` → `control_profile` | +| **Value replacements** | `gps_provider`: `UBLOX7` → `UBLOX` | +| **Removed settings** (18) | `control_deadband`, `cpu_underclock`, `disarm_kill_switch`, `dji_workarounds`, `fw_iterm_limit_stick_position`, `gyro_anti_aliasing_lpf_type`, `gyro_hardware_lpf`, `gyro_main_lpf_type`, `gyro_use_dyn_lpf`, `inav_use_gps_no_baro`, `inav_use_gps_velned`, `ledstrip_visual_beeper`, `max_throttle`, `nav_auto_climb_rate`, `nav_manual_climb_rate`, `osd_stats_min_voltage_unit`, `pidsum_limit`, `pidsum_limit_yaw` | +| **Pattern mappings** | `osd_custom_element_N_type`: IDs remapped 4→9, 5→16, 6→7, 7→10 | +| **Warnings** | `nav_fw_wp_tracking_accuracy` semantics changed: was arbitrary tracking response, now distance in meters | + +### 8_to_9.json (INAV 8.0 → 9.0) + +| Category | Changes | +|----------|---------| +| **Command renames** | `controlrate_profile` → `use_control_profile` | +| **Setting renames** | `mixer_pid_profile_linking` → `mixer_control_profile_linking`, `osd_pan_servo_pwm2centideg` → `osd_pan_servo_range_decadegrees` | +| **Value replacements** | None | +| **Removed settings** | None | +| **Pattern mappings** | None | +| **Warnings** | Position estimator defaults changed (`w_z_baro_v`, `inav_w_z_gps_p`, `inav_w_z_gps_v`). `ahrs_acc_ignore_rate` default changed 20→15 | + +## Migration Engine Internals + +### Profile chaining + +`buildMigrationChain(fromVersion, toVersion)` selects all profiles where `profileFrom >= fromMajor` and `profileTo <= toMajor`, sorted by `fromVersion`. A 7.x → 9.x migration applies both 7→8 and 8→9 profiles in sequence. + +### Line processing + +Each non-comment, non-empty line passes through every profile in the chain. For each profile, transformations are applied in this order: + +1. Command renames (any token in the line) +2. Removed settings (line dropped if `set ` matches) +3. Setting renames (`set ` replacement) +4. Value replacements (value after `=` replaced) +5. Setting pattern mappings (regex-matched settings with value remapping) + +### Missing profile detection + +`hasMissingProfiles()` returns `true` when the number of profiles in the chain is fewer than the number of major version steps. The UI shows a warning but still allows restore — some settings may fail. + +## Edge Cases Handled + +1. **Stale FC version after flash**: Real FC version is queried via `MSP_FC_VERSION` after connect, not the cached value +2. **DFU mode (no MSP)**: `FC.CONFIG` null checks prevent crashes when connected in DFU mode +3. **DFU timeout**: UI unlock and progress label update on timeout (no permanent lock) +4. **Local firmware files**: `localFirmwareLoaded` flag prevents stale dropdown version from triggering wrong migration +5. **Backup pruning with mixed versions**: Sort by timestamp portion, not full filename +6. **Multi-step migration**: 7.x → 9.x automatically chains 7→8 + 8→9 profiles +7. **Missing migration profiles**: Warning shown but restore allowed — graceful degradation +8. **Version detection from backup**: Parsed from backup header (`# Version: X.Y.Z`), not from FC state + +## i18n Keys + +All backup/restore/migration translation keys in `locale/en/messages.json`: + +### Backup status +| Key | Text | +|-----|------| +| `backupRestoreStatusEnteringCli` | Entering CLI | +| `backupRestoreStatusReadingConfig` | Reading configuration via CLI | +| `backupRestoreStatusSavingFile` | Saving backup file... | +| `backupRestoreStatusExitingCli` | Exiting CLI mode | +| `backupRestoreBackupSaved` | Backup saved $1 | +| `backupRestoreAutoBackupSaved` | Auto-backup saved to $1 | +| `backupRestoreBackupComplete` | Backup complete | +| `backupRestoreBackupCancelled` | Backup cancelled | +| `backupRestoreBackupFailed` | Backup failed | + +### Restore status +| Key | Text | +|-----|------| +| `backupRestoreStatusConnecting` | Connecting to flight controller | +| `backupRestoreStatusRestoringConfig` | Restoring configuration | +| `backupRestoreStatusRestoringProgress` | Restoring... $1 / $2 | +| `backupRestoreStatusSaving` | Saving configuration | +| `backupRestoreRestoreComplete` | Configuration restored. Flight controller is rebooting. | +| `backupRestoreRestoreCancelled` | Restore cancelled. | +| `backupRestoreRestoreFailed` | Restore failed. | + +### Auto-restore UI +| Key | Text | +|-----|------| +| `backupRestoreAutoRestoreConfirm` | Restore confirmation prompt | +| `backupRestoreAutoRestoreWaiting` | Waiting for FC to reboot after flash | +| `backupRestoreAutoRestoreYes` | Yes, restore settings | +| `backupRestoreAutoRestoreNo` | No, keep current settings | +| `backupRestoreAutoRestoreWaitingPort` | Waiting for port $1 to reconnect | +| `backupRestoreDowngradeNoAutoRestore` | Major downgrade warning | +| `backupRestoreFlashCompleteBackupSaved` | Backup saved (local firmware, no restore offer) | +| `backupRestoreMigrationApplied` | Migration applied: $1 → $2 ($3 changes) | +| `backupRestoreMigrationWarningsHeader` | Migration Warnings: | + +### Migration preview +| Key | Text | +|-----|------| +| `migrationPreviewTitle` | Settings Migration Required | +| `migrationPreviewSubtitle` | Conversion explanation | +| `migrationPreviewRemovedHeader` | Removed Settings: | +| `migrationPreviewRenamedSettingsHeader` | Renamed Settings: | +| `migrationPreviewRenamedCommandsHeader` | Renamed Commands: | +| `migrationPreviewValueReplacementsHeader` | Value Replacements: | +| `migrationPreviewSettingRemappingsHeader` | Setting Remappings: | +| `migrationPreviewContinue` | Continue with migration | +| `migrationPreviewCancel` | Cancel restore | +| `migrationMissingProfileWarning` | Missing profile warning | + +### Error messages +| Key | Text | +|-----|------| +| `backupRestoreErrorTitle` | Restore Errors Detected | +| `backupRestoreErrorText` | Error explanation | +| `backupRestoreErrorAbort` | Abort | +| `backupRestoreErrorSave` | Save anyway | From ea2c1f12c9e2c5aed4b8461d04650e4e4bb0b925 Mon Sep 17 00:00:00 2001 From: b14ckyy Date: Sat, 11 Apr 2026 16:04:00 +0200 Subject: [PATCH 40/48] docs: add migration profile reminder to release checklist Reference Backup Restore Architecture doc in pre-release checklist and breaking changes branch usage section to ensure migration profiles are created when major versions introduce settings changes. --- docs/development/release-create.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/docs/development/release-create.md b/docs/development/release-create.md index 16dccb6292d..f1ba7cf2369 100644 --- a/docs/development/release-create.md +++ b/docs/development/release-create.md @@ -109,6 +109,7 @@ Version numbers are set in: - [ ] Release notes drafted - [ ] Breaking changes documented - [ ] New features documented +- [ ] **Configurator migration profile created** for major version bumps (see [Backup Restore Architecture](Backup%20Restore%20Architecture.md#adding-a-new-migration-profile)) ## Release Workflow @@ -492,6 +493,7 @@ gh api repos/iNavFlight/inav-configurator/git/refs -f ref="refs/heads/maintenanc - **Changes maintaining backward compatibility** → PR to maintenance-X.x (e.g., maintenance-9.x) - **Breaking changes** (MSP protocol, settings structure) → PR to maintenance-(X+1).x (e.g., maintenance-10.x) + - When breaking changes affect CLI settings (renames, removals, value changes), a **Configurator migration profile** must be created. See [Backup Restore Architecture](Backup%20Restore%20Architecture.md#adding-a-new-migration-profile) - **Master** → NOT a PR target (receives merges only) Lower version branches are periodically merged into higher version branches (e.g., maintenance-9.x → master → maintenance-10.x). From 392d21b2c3efaec22bd846fba1667346ea0d645c Mon Sep 17 00:00:00 2001 From: b14ckyy Date: Sat, 11 Apr 2026 16:14:35 +0200 Subject: [PATCH 41/48] clearer wording for backup and restore doc --- docs/Backup and Restore.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/docs/Backup and Restore.md b/docs/Backup and Restore.md index 9206abafa03..771b129762d 100644 --- a/docs/Backup and Restore.md +++ b/docs/Backup and Restore.md @@ -8,8 +8,8 @@ For manual CLI-based backup and restore, see the [CLI documentation](Cli.md#back ### What happens automatically -1. **Before flashing** (with Full Chip Erase enabled): Your current CLI configuration (`diff all`) is automatically captured and saved to the backup directory. -2. **After flashing**: Depending on the situation, the Configurator offers to restore your settings: +1. **Before flashing** (with or without Full Chip Erase enabled): Your current CLI configuration (`diff all`) is automatically captured and saved to the backup directory. +2. **After flashing**: Depending on the situation, the Configurator offers to restore your settings if Full Chip Erase was enabled: | Scenario | Behavior | |----------|----------| From 69db786b614620e12af337f3c6b440ab6bbf4d65 Mon Sep 17 00:00:00 2001 From: frogmane <7685285+xznhj8129@users.noreply.github.com> Date: Sat, 11 Apr 2026 22:23:42 -0400 Subject: [PATCH 42/48] Add MSP2 link stats message + fix missing msp-set-wp-index-and-altitude spec + msp_gen_docs --- docs/development/msp/README.md | 56 ++++++---- docs/development/msp/gen_docs.sh | 2 +- docs/development/msp/inav_enums.json | 121 ++++++++++---------- docs/development/msp/inav_enums_ref.md | 123 +++++++++++---------- docs/development/msp/msp_messages.checksum | 2 +- docs/development/msp/msp_messages.json | 63 +++++++++++ docs/development/msp/rev | 2 +- src/main/fc/fc_msp.c | 6 + src/main/msp/msp_protocol_v2_inav.h | 2 + 9 files changed, 240 insertions(+), 137 deletions(-) diff --git a/docs/development/msp/README.md b/docs/development/msp/README.md index 696e7ab7fc1..833a9f6873c 100644 --- a/docs/development/msp/README.md +++ b/docs/development/msp/README.md @@ -9,7 +9,8 @@ For list of enums, see [Enum documentation page](https://github.com/iNavFlight/i -**JSON file rev: 4** +**JSON file rev: 5 +** **Warning: Verification needed, exercise caution until completely verified for accuracy and cleared, especially for integer signs. Source-based generation/validation is forthcoming. Refer to source for absolute certainty** @@ -66,6 +67,7 @@ For list of enums, see [Enum documentation page](https://github.com/iNavFlight/i **variable_len**: Optional boolean, if true, message does not have a predefined fixed length and needs appropriate handling\ **variants**: Optional special case, message has different cases of reply/request. Key/description is not a strict expression or code; just a readable condition\ **not_implemented**: Optional special case, message is not implemented (never or deprecated)\ +**replaced_by**: Optional array of MSP message names that replace this command. Present when a command is deprecated and scheduled for removal. Empty array if no replacement is needed\ **notes**: String with details of message ## Data dict fields: @@ -403,6 +405,7 @@ For list of enums, see [Enum documentation page](https://github.com/iNavFlight/i [8448 - MSP2_INAV_CUSTOM_OSD_ELEMENTS](#msp2_inav_custom_osd_elements) [8449 - MSP2_INAV_CUSTOM_OSD_ELEMENT](#msp2_inav_custom_osd_element) [8450 - MSP2_INAV_SET_CUSTOM_OSD_ELEMENTS](#msp2_inav_set_custom_osd_elements) +[8451 - MSP2_INAV_GET_LINK_STATS](#msp2_inav_get_link_stats) [8461 - MSP2_INAV_OUTPUT_MAPPING_EXT2](#msp2_inav_output_mapping_ext2) [8704 - MSP2_INAV_SERVO_CONFIG](#msp2_inav_servo_config) [8705 - MSP2_INAV_SET_SERVO_CONFIG](#msp2_inav_set_servo_config) @@ -411,11 +414,11 @@ For list of enums, see [Enum documentation page](https://github.com/iNavFlight/i [8722 - MSP2_INAV_GEOZONE_VERTEX](#msp2_inav_geozone_vertex) [8723 - MSP2_INAV_SET_GEOZONE_VERTEX](#msp2_inav_set_geozone_vertex) [8724 - MSP2_INAV_SET_GVAR](#msp2_inav_set_gvar) -[8736 - MSP2_INAV_FULL_LOCAL_POSE](#msp2_inav_full_local_pose) -[8737 - MSP2_INAV_SET_WP_INDEX](#msp2_inav_set_wp_index) -[8739 - MSP2_INAV_SET_CRUISE_HEADING](#msp2_inav_set_cruise_heading) -[12288 - MSP2_BETAFLIGHT_BIND](#msp2_betaflight_bind) -[12289 - MSP2_RX_BIND](#msp2_rx_bind) +[8736 - MSP2_INAV_FULL_LOCAL_POSE](#msp2_inav_full_local_pose) +[8737 - MSP2_INAV_SET_WP_INDEX](#msp2_inav_set_wp_index) +[8739 - MSP2_INAV_SET_CRUISE_HEADING](#msp2_inav_set_cruise_heading) +[12288 - MSP2_BETAFLIGHT_BIND](#msp2_betaflight_bind) +[12289 - MSP2_RX_BIND](#msp2_rx_bind) ## `MSP_API_VERSION (1 / 0x1)` **Description:** Provides the MSP protocol version and the INAV API version. @@ -2208,7 +2211,7 @@ For list of enums, see [Enum documentation page](https://github.com/iNavFlight/i | `armingFlags` | `uint16_t` | 2 | Bitmask | Bitmask: Flight controller arming flags (`armingFlags`). Note: Truncated to 16 bits | | `accCalibAxisFlags` | `uint8_t` | 1 | Bitmask | Bitmask: Accelerometer calibrated axes flags (`accGetCalibrationAxisFlags()`) | -**Notes:** Superseded by `MSP2_INAV_STATUS` which provides the full 32-bit `armingFlags` and other enhancements. +**Notes:** Superseded by `MSP2_INAV_STATUS` which provides the full 32-bit `armingFlags` and other enhancements. The `accCalibAxisFlags` field is not present in `MSP2_INAV_STATUS` but is available via `MSP_CALIBRATION_DATA`. ## `MSP_SENSOR_STATUS (151 / 0x97)` **Description:** Provides the hardware status for each individual sensor system. @@ -2275,6 +2278,7 @@ For list of enums, see [Enum documentation page](https://github.com/iNavFlight/i | `hdop` | `uint16_t` | 2 | HDOP * 100 | Horizontal Dilution of Precision (`gpsSol.hdop`) | | `eph` | `uint16_t` | 2 | cm | Estimated Horizontal Position Accuracy (`gpsSol.eph`) | | `epv` | `uint16_t` | 2 | cm | Estimated Vertical Position Accuracy (`gpsSol.epv`) | +| `hwVersion` | `uint32_t` | 4 | Version code | GPS hardware version (`gpsState.hwVersion`). Values: 500=UBLOX5, 600=UBLOX6, 700=UBLOX7, 800=UBLOX8, 900=UBLOX9, 1000=UBLOX10, 0=UNKNOWN | **Notes:** Requires `USE_GPS`. @@ -4357,6 +4361,20 @@ For list of enums, see [Enum documentation page](https://github.com/iNavFlight/i **Notes:** Payload length must be (OSD_CUSTOM_ELEMENT_TEXT_SIZE - 1) + (CUSTOM_ELEMENTS_PARTS * 3) + 4 bytes including elementIndex. elementIndex must be < MAX_CUSTOM_ELEMENTS. Each partType must be < CUSTOM_ELEMENT_TYPE_END. Firmware NUL-terminates elementText internally. +## `MSP2_INAV_GET_LINK_STATS (8451 / 0x2103)` +**Description:** Provides uplink RC link statistics for monitoring on a GCS. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `uplinkRSSI_dBm` | `uint8_t` | 1 | -dBm | Uplink RSSI in dBm, sent as a positive magnitude (`getRSSI()`). For example, 70 means -70dBm. | +| `uplinkLQ` | `uint8_t` | 1 | % | Uplink Link Quality (`rxLinkStatistics.uplinkLQ`) | +| `uplinkSNR` | `int8_t` | 1 | dB | Uplink Signal-to-Noise Ratio (`rxLinkStatistics.uplinkSNR`) | + +**Notes:** Useful for GCS monitoring of the active RC link quality and signal margin. + ## `MSP2_INAV_OUTPUT_MAPPING_EXT2 (8461 / 0x210d)` **Description:** Retrieves the full extended output mapping configuration (timer ID, full 32-bit usage flags, and pin label). Supersedes `MSP2_INAV_OUTPUT_MAPPING_EXT`. @@ -4529,33 +4547,29 @@ For list of enums, see [Enum documentation page](https://github.com/iNavFlight/i **Notes:** All attitude angles are in deci-degrees. ## `MSP2_INAV_SET_WP_INDEX (8737 / 0x2221)` -**Description:** Jumps to a specific waypoint during an active waypoint mission, causing the aircraft to immediately begin navigating toward the new target waypoint. - +**Description:** Jumps to a specific waypoint during an active waypoint mission, causing the aircraft to immediately begin navigating toward the new target waypoint. + **Request Payload:** |Field|C Type|Size (Bytes)|Units|Description| |---|---|---|---|---| | `wp_index` | `uint8_t` | 1 | - | 0-based waypoint index to jump to, relative to the mission start waypoint (`posControl.startWpIndex`) | -**Reply Payload:** **None** +**Reply Payload:** **None** **Notes:** Returns error if the aircraft is not armed, `NAV_WP_MODE` is not active, or the index is outside the valid mission range (`startWpIndex` to `startWpIndex + waypointCount - 1`). On success, sets `posControl.activeWaypointIndex` to the requested index and fires `NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_JUMP`, transitioning the navigation FSM back to `NAV_STATE_WAYPOINT_PRE_ACTION` so the flight controller re-initialises navigation for the new target. ---- - ## `MSP2_INAV_SET_CRUISE_HEADING (8739 / 0x2223)` -**Description:** Sets the course heading target while Cruise or Course Hold mode is active, causing the aircraft to turn to and maintain the new heading. - +**Description:** Sets the course heading target while Cruise or Course Hold mode is active, causing the aircraft to turn to and maintain the new heading. + **Request Payload:** |Field|C Type|Size (Bytes)|Units|Description| |---|---|---|---|---| -| `heading_centidegrees` | `int32_t` | 4 | centidegrees | Target heading in centidegrees (0–35999). Values are wrapped modulo 36000 before being applied. | +| `heading_centidegrees` | `int32_t` | 4 | centidegrees | Target heading in centidegrees (0-35999). Values are wrapped modulo 36000 before being applied. | -**Reply Payload:** **None** +**Reply Payload:** **None** **Notes:** Returns error if the aircraft is not armed or `NAV_COURSE_HOLD_MODE` is not active. On success, sets both `posControl.cruise.course` and `posControl.cruise.previousCourse` to the normalised value, preventing spurious heading adjustments from `getCruiseHeadingAdjustment()` on the next control cycle. ---- - ## `MSP2_BETAFLIGHT_BIND (12288 / 0x3000)` **Description:** Initiates the receiver binding procedure for supported serial protocols (CRSF, SRXL2). @@ -4566,14 +4580,14 @@ For list of enums, see [Enum documentation page](https://github.com/iNavFlight/i **Notes:** Requires `rxConfig()->receiverType == RX_TYPE_SERIAL`. Requires `USE_SERIALRX_CRSF` or `USE_SERIALRX_SRXL2`. Calls `crsfBind()` or `srxl2Bind()` respectively. Returns error if receiver type or provider is not supported for binding. ## `MSP2_RX_BIND (12289 / 0x3001)` -**Description:** Initiates binding for MSP receivers (mLRS). - +**Description:** Initiates binding for MSP receivers (mLRS). + **Request Payload:** |Field|C Type|Size (Bytes)|Description| |---|---|---|---| | `port_id` | `uint8_t` | 1 | Port ID | | `reserved_for_custom_use` | `uint8_t[3]` | 3 | Reserved for custom use | - + **Reply Payload:** |Field|C Type|Size (Bytes)|Description| |---|---|---|---| diff --git a/docs/development/msp/gen_docs.sh b/docs/development/msp/gen_docs.sh index cd368cf7f00..84aa852841f 100755 --- a/docs/development/msp/gen_docs.sh +++ b/docs/development/msp/gen_docs.sh @@ -11,7 +11,7 @@ expected="$(awk '{print $1}' msp_messages.checksum)" echo "Hash:" $actual if [[ "$actual" != "$expected" ]]; then n="$(cat rev)" - printf '%d' "$(n + 1)" > rev + printf '%d' "$((n + 1))" > rev echo "File changed, incrementing revision" echo $actual > msp_messages.checksum fi diff --git a/docs/development/msp/inav_enums.json b/docs/development/msp/inav_enums.json index 04a045ad3b9..63f3eb2941b 100644 --- a/docs/development/msp/inav_enums.json +++ b/docs/development/msp/inav_enums.json @@ -12,7 +12,8 @@ "ACC_ICM42605": "8", "ACC_BMI270": "9", "ACC_LSM6DXX": "10", - "ACC_FAKE": "11", + "ACC_ICM45686": "11", + "ACC_FAKE": "12", "ACC_MAX": "ACC_FAKE" }, "accEvent_t": { @@ -736,6 +737,9 @@ "CRSF_FRAMETYPE_VARIO_SENSOR": "7", "CRSF_FRAMETYPE_BATTERY_SENSOR": "8", "CRSF_FRAMETYPE_BAROMETER_ALTITUDE": "9", + "CRSF_FRAMETYPE_AIRSPEED_SENSOR": "10", + "CRSF_FRAMETYPE_RPM": "12", + "CRSF_FRAMETYPE_TEMP": "13", "CRSF_FRAMETYPE_LINK_STATISTICS": "20", "CRSF_FRAMETYPE_RC_CHANNELS_PACKED": "22", "CRSF_FRAMETYPE_ATTITUDE": "30", @@ -760,6 +764,9 @@ "CRSF_FRAME_GPS_INDEX": "", "CRSF_FRAME_VARIO_SENSOR_INDEX": "", "CRSF_FRAME_BAROMETER_ALTITUDE_INDEX": "", + "CRSF_FRAME_TEMP_INDEX": "", + "CRSF_FRAME_RPM_INDEX": "", + "CRSF_FRAME_AIRSPEED_INDEX": "", "CRSF_SCHEDULE_COUNT_MAX": "" }, "crsrRfMode_e": { @@ -801,53 +808,54 @@ "DEVHW_ICM42605": "7", "DEVHW_BMI270": "8", "DEVHW_LSM6D": "9", - "DEVHW_MPU9250": "10", - "DEVHW_BMP085": "11", - "DEVHW_BMP280": "12", - "DEVHW_MS5611": "13", - "DEVHW_MS5607": "14", - "DEVHW_LPS25H": "15", - "DEVHW_SPL06": "16", - "DEVHW_BMP388": "17", - "DEVHW_DPS310": "18", - "DEVHW_B2SMPB": "19", - "DEVHW_HMC5883": "20", - "DEVHW_AK8963": "21", - "DEVHW_AK8975": "22", - "DEVHW_IST8310_0": "23", - "DEVHW_IST8310_1": "24", - "DEVHW_IST8308": "25", - "DEVHW_QMC5883": "26", - "DEVHW_QMC5883P": "27", - "DEVHW_MAG3110": "28", - "DEVHW_LIS3MDL": "29", - "DEVHW_RM3100": "30", - "DEVHW_VCM5883": "31", - "DEVHW_MLX90393": "32", - "DEVHW_LM75_0": "33", - "DEVHW_LM75_1": "34", - "DEVHW_LM75_2": "35", - "DEVHW_LM75_3": "36", - "DEVHW_LM75_4": "37", - "DEVHW_LM75_5": "38", - "DEVHW_LM75_6": "39", - "DEVHW_LM75_7": "40", - "DEVHW_DS2482": "41", - "DEVHW_MAX7456": "42", - "DEVHW_SRF10": "43", - "DEVHW_VL53L0X": "44", - "DEVHW_VL53L1X": "45", - "DEVHW_US42": "46", - "DEVHW_TOF10120_I2C": "47", - "DEVHW_TERARANGER_EVO_I2C": "48", - "DEVHW_MS4525": "49", - "DEVHW_DLVR": "50", - "DEVHW_M25P16": "51", - "DEVHW_W25N": "52", - "DEVHW_UG2864": "53", - "DEVHW_SDCARD": "54", - "DEVHW_IRLOCK": "55", - "DEVHW_PCF8574": "56" + "DEVHW_ICM45686": "10", + "DEVHW_MPU9250": "11", + "DEVHW_BMP085": "12", + "DEVHW_BMP280": "13", + "DEVHW_MS5611": "14", + "DEVHW_MS5607": "15", + "DEVHW_LPS25H": "16", + "DEVHW_SPL06": "17", + "DEVHW_BMP388": "18", + "DEVHW_DPS310": "19", + "DEVHW_B2SMPB": "20", + "DEVHW_HMC5883": "21", + "DEVHW_AK8963": "22", + "DEVHW_AK8975": "23", + "DEVHW_IST8310_0": "24", + "DEVHW_IST8310_1": "25", + "DEVHW_IST8308": "26", + "DEVHW_QMC5883": "27", + "DEVHW_QMC5883P": "28", + "DEVHW_MAG3110": "29", + "DEVHW_LIS3MDL": "30", + "DEVHW_RM3100": "31", + "DEVHW_VCM5883": "32", + "DEVHW_MLX90393": "33", + "DEVHW_LM75_0": "34", + "DEVHW_LM75_1": "35", + "DEVHW_LM75_2": "36", + "DEVHW_LM75_3": "37", + "DEVHW_LM75_4": "38", + "DEVHW_LM75_5": "39", + "DEVHW_LM75_6": "40", + "DEVHW_LM75_7": "41", + "DEVHW_DS2482": "42", + "DEVHW_MAX7456": "43", + "DEVHW_SRF10": "44", + "DEVHW_VL53L0X": "45", + "DEVHW_VL53L1X": "46", + "DEVHW_US42": "47", + "DEVHW_TOF10120_I2C": "48", + "DEVHW_TERARANGER_EVO_I2C": "49", + "DEVHW_MS4525": "50", + "DEVHW_DLVR": "51", + "DEVHW_M25P16": "52", + "DEVHW_W25N": "53", + "DEVHW_UG2864": "54", + "DEVHW_SDCARD": "55", + "DEVHW_IRLOCK": "56", + "DEVHW_PCF8574": "57" }, "deviceFlags_e": { "_source": "inav/src/main/drivers/bus.h", @@ -1527,7 +1535,8 @@ "GYRO_ICM42605": "8", "GYRO_BMI270": "9", "GYRO_LSM6DXX": "10", - "GYRO_FAKE": "11" + "GYRO_ICM45686": "11", + "GYRO_FAKE": "12" }, "HardwareMotorTypes_e": { "_source": "inav/src/main/drivers/pwm_esc_detect.h", @@ -1861,7 +1870,8 @@ "LED_MODE_ANGLE": "3", "LED_MODE_MAG": "4", "LED_MODE_BARO": "5", - "LED_SPECIAL": "6" + "LED_MODE_LOITER": "6", + "LED_SPECIAL": "7" }, "ledOverlayId_e": { "_source": "inav/src/main/io/ledstrip.h", @@ -2325,8 +2335,7 @@ "MULTI_FUNC_3": "3", "MULTI_FUNC_4": "4", "MULTI_FUNC_5": "5", - "MULTI_FUNC_6": "6", - "MULTI_FUNC_END": "7" + "MULTI_FUNC_END": "6" }, "multiFunctionFlags_e": { "_source": "inav/src/main/fc/multifunction.h", @@ -2407,6 +2416,7 @@ "NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME": "NAV_FSM_EVENT_STATE_SPECIFIC_1", "NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND": "NAV_FSM_EVENT_STATE_SPECIFIC_2", "NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED": "NAV_FSM_EVENT_STATE_SPECIFIC_3", + "NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_JUMP": "NAV_FSM_EVENT_STATE_SPECIFIC_4", "NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_INITIALIZE": "NAV_FSM_EVENT_STATE_SPECIFIC_1", "NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_TRACKBACK": "NAV_FSM_EVENT_STATE_SPECIFIC_2", "NAV_FSM_EVENT_SWITCH_TO_RTH_HEAD_HOME": "NAV_FSM_EVENT_STATE_SPECIFIC_3", @@ -3291,9 +3301,8 @@ "_source": "inav/src/main/io/displayport_msp_osd.c", "SD_3016": "0", "HD_5018": "1", - "HD_3016": "2", - "HD_6022": "3", - "HD_5320": "4" + "HD_6022": "2", + "HD_5320": "3" }, "resourceOwner_e": { "_source": "inav/src/main/drivers/resource.h", @@ -3829,7 +3838,7 @@ "THR_HI": "(2 << (2 * THROTTLE))" }, "systemState_e": { - "_source": "inav/src/main/fc/fc_init.c", + "_source": "inav/src/main/fc/fc_init.h", "SYSTEM_STATE_INITIALISING": "0", "SYSTEM_STATE_CONFIG_LOADED": "(1 << 0)", "SYSTEM_STATE_SENSORS_READY": "(1 << 1)", diff --git a/docs/development/msp/inav_enums_ref.md b/docs/development/msp/inav_enums_ref.md index b3aabb41467..802e813ab43 100644 --- a/docs/development/msp/inav_enums_ref.md +++ b/docs/development/msp/inav_enums_ref.md @@ -370,7 +370,8 @@ | `ACC_ICM42605` | 8 | | | `ACC_BMI270` | 9 | | | `ACC_LSM6DXX` | 10 | | -| `ACC_FAKE` | 11 | | +| `ACC_ICM45686` | 11 | | +| `ACC_FAKE` | 12 | | | `ACC_MAX` | ACC_FAKE | | --- @@ -1317,6 +1318,9 @@ | `CRSF_FRAMETYPE_VARIO_SENSOR` | 7 | | | `CRSF_FRAMETYPE_BATTERY_SENSOR` | 8 | | | `CRSF_FRAMETYPE_BAROMETER_ALTITUDE` | 9 | | +| `CRSF_FRAMETYPE_AIRSPEED_SENSOR` | 10 | | +| `CRSF_FRAMETYPE_RPM` | 12 | | +| `CRSF_FRAMETYPE_TEMP` | 13 | | | `CRSF_FRAMETYPE_LINK_STATISTICS` | 20 | | | `CRSF_FRAMETYPE_RC_CHANNELS_PACKED` | 22 | | | `CRSF_FRAMETYPE_ATTITUDE` | 30 | | @@ -1346,6 +1350,9 @@ | `CRSF_FRAME_GPS_INDEX` | | | | `CRSF_FRAME_VARIO_SENSOR_INDEX` | | | | `CRSF_FRAME_BAROMETER_ALTITUDE_INDEX` | | | +| `CRSF_FRAME_TEMP_INDEX` | | | +| `CRSF_FRAME_RPM_INDEX` | | | +| `CRSF_FRAME_AIRSPEED_INDEX` | | | | `CRSF_SCHEDULE_COUNT_MAX` | | | --- @@ -1407,53 +1414,54 @@ | `DEVHW_ICM42605` | 7 | | | `DEVHW_BMI270` | 8 | | | `DEVHW_LSM6D` | 9 | | -| `DEVHW_MPU9250` | 10 | | -| `DEVHW_BMP085` | 11 | | -| `DEVHW_BMP280` | 12 | | -| `DEVHW_MS5611` | 13 | | -| `DEVHW_MS5607` | 14 | | -| `DEVHW_LPS25H` | 15 | | -| `DEVHW_SPL06` | 16 | | -| `DEVHW_BMP388` | 17 | | -| `DEVHW_DPS310` | 18 | | -| `DEVHW_B2SMPB` | 19 | | -| `DEVHW_HMC5883` | 20 | | -| `DEVHW_AK8963` | 21 | | -| `DEVHW_AK8975` | 22 | | -| `DEVHW_IST8310_0` | 23 | | -| `DEVHW_IST8310_1` | 24 | | -| `DEVHW_IST8308` | 25 | | -| `DEVHW_QMC5883` | 26 | | -| `DEVHW_QMC5883P` | 27 | | -| `DEVHW_MAG3110` | 28 | | -| `DEVHW_LIS3MDL` | 29 | | -| `DEVHW_RM3100` | 30 | | -| `DEVHW_VCM5883` | 31 | | -| `DEVHW_MLX90393` | 32 | | -| `DEVHW_LM75_0` | 33 | | -| `DEVHW_LM75_1` | 34 | | -| `DEVHW_LM75_2` | 35 | | -| `DEVHW_LM75_3` | 36 | | -| `DEVHW_LM75_4` | 37 | | -| `DEVHW_LM75_5` | 38 | | -| `DEVHW_LM75_6` | 39 | | -| `DEVHW_LM75_7` | 40 | | -| `DEVHW_DS2482` | 41 | | -| `DEVHW_MAX7456` | 42 | | -| `DEVHW_SRF10` | 43 | | -| `DEVHW_VL53L0X` | 44 | | -| `DEVHW_VL53L1X` | 45 | | -| `DEVHW_US42` | 46 | | -| `DEVHW_TOF10120_I2C` | 47 | | -| `DEVHW_TERARANGER_EVO_I2C` | 48 | | -| `DEVHW_MS4525` | 49 | | -| `DEVHW_DLVR` | 50 | | -| `DEVHW_M25P16` | 51 | | -| `DEVHW_W25N` | 52 | | -| `DEVHW_UG2864` | 53 | | -| `DEVHW_SDCARD` | 54 | | -| `DEVHW_IRLOCK` | 55 | | -| `DEVHW_PCF8574` | 56 | | +| `DEVHW_ICM45686` | 10 | | +| `DEVHW_MPU9250` | 11 | | +| `DEVHW_BMP085` | 12 | | +| `DEVHW_BMP280` | 13 | | +| `DEVHW_MS5611` | 14 | | +| `DEVHW_MS5607` | 15 | | +| `DEVHW_LPS25H` | 16 | | +| `DEVHW_SPL06` | 17 | | +| `DEVHW_BMP388` | 18 | | +| `DEVHW_DPS310` | 19 | | +| `DEVHW_B2SMPB` | 20 | | +| `DEVHW_HMC5883` | 21 | | +| `DEVHW_AK8963` | 22 | | +| `DEVHW_AK8975` | 23 | | +| `DEVHW_IST8310_0` | 24 | | +| `DEVHW_IST8310_1` | 25 | | +| `DEVHW_IST8308` | 26 | | +| `DEVHW_QMC5883` | 27 | | +| `DEVHW_QMC5883P` | 28 | | +| `DEVHW_MAG3110` | 29 | | +| `DEVHW_LIS3MDL` | 30 | | +| `DEVHW_RM3100` | 31 | | +| `DEVHW_VCM5883` | 32 | | +| `DEVHW_MLX90393` | 33 | | +| `DEVHW_LM75_0` | 34 | | +| `DEVHW_LM75_1` | 35 | | +| `DEVHW_LM75_2` | 36 | | +| `DEVHW_LM75_3` | 37 | | +| `DEVHW_LM75_4` | 38 | | +| `DEVHW_LM75_5` | 39 | | +| `DEVHW_LM75_6` | 40 | | +| `DEVHW_LM75_7` | 41 | | +| `DEVHW_DS2482` | 42 | | +| `DEVHW_MAX7456` | 43 | | +| `DEVHW_SRF10` | 44 | | +| `DEVHW_VL53L0X` | 45 | | +| `DEVHW_VL53L1X` | 46 | | +| `DEVHW_US42` | 47 | | +| `DEVHW_TOF10120_I2C` | 48 | | +| `DEVHW_TERARANGER_EVO_I2C` | 49 | | +| `DEVHW_MS4525` | 50 | | +| `DEVHW_DLVR` | 51 | | +| `DEVHW_M25P16` | 52 | | +| `DEVHW_W25N` | 53 | | +| `DEVHW_UG2864` | 54 | | +| `DEVHW_SDCARD` | 55 | | +| `DEVHW_IRLOCK` | 56 | | +| `DEVHW_PCF8574` | 57 | | --- ## `deviceFlags_e` @@ -2488,7 +2496,8 @@ | `GYRO_ICM42605` | 8 | | | `GYRO_BMI270` | 9 | | | `GYRO_LSM6DXX` | 10 | | -| `GYRO_FAKE` | 11 | | +| `GYRO_ICM45686` | 11 | | +| `GYRO_FAKE` | 12 | | --- ## `HardwareMotorTypes_e` @@ -2924,7 +2933,8 @@ | `LED_MODE_ANGLE` | 3 | | | `LED_MODE_MAG` | 4 | | | `LED_MODE_BARO` | 5 | | -| `LED_SPECIAL` | 6 | | +| `LED_MODE_LOITER` | 6 | | +| `LED_SPECIAL` | 7 | | --- ## `ledOverlayId_e` @@ -3531,8 +3541,7 @@ | `MULTI_FUNC_3` | 3 | | | `MULTI_FUNC_4` | 4 | | | `MULTI_FUNC_5` | 5 | | -| `MULTI_FUNC_6` | 6 | | -| `MULTI_FUNC_END` | 7 | | +| `MULTI_FUNC_END` | 6 | | --- ## `multiFunctionFlags_e` @@ -3658,6 +3667,7 @@ | `NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME` | NAV_FSM_EVENT_STATE_SPECIFIC_1 | | | `NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND` | NAV_FSM_EVENT_STATE_SPECIFIC_2 | | | `NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED` | NAV_FSM_EVENT_STATE_SPECIFIC_3 | | +| `NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_JUMP` | NAV_FSM_EVENT_STATE_SPECIFIC_4 | | | `NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_INITIALIZE` | NAV_FSM_EVENT_STATE_SPECIFIC_1 | | | `NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_TRACKBACK` | NAV_FSM_EVENT_STATE_SPECIFIC_2 | | | `NAV_FSM_EVENT_SWITCH_TO_RTH_HEAD_HOME` | NAV_FSM_EVENT_STATE_SPECIFIC_3 | | @@ -4819,9 +4829,8 @@ |---|---:|---| | `SD_3016` | 0 | | | `HD_5018` | 1 | | -| `HD_3016` | 2 | | -| `HD_6022` | 3 | | -| `HD_5320` | 4 | | +| `HD_6022` | 2 | | +| `HD_5320` | 3 | | --- ## `resourceOwner_e` @@ -5627,7 +5636,7 @@ --- ## `systemState_e` -> Source: ../../../src/main/fc/fc_init.h +> Source: ../../../src/main/fc/fc_init.c | Enumerator | Value | Condition | |---|---:|---| @@ -5641,7 +5650,7 @@ --- ## `systemState_e` -> Source: ../../../src/main/fc/fc_init.c +> Source: ../../../src/main/fc/fc_init.h | Enumerator | Value | Condition | |---|---:|---| diff --git a/docs/development/msp/msp_messages.checksum b/docs/development/msp/msp_messages.checksum index ff3e21a1d69..031f3b14179 100644 --- a/docs/development/msp/msp_messages.checksum +++ b/docs/development/msp/msp_messages.checksum @@ -1 +1 @@ -82a3d2eee9d0d1fe609363a08405ed21 +c9458e9a712b7a4f3bc9333aa7bc3dcb diff --git a/docs/development/msp/msp_messages.json b/docs/development/msp/msp_messages.json index 12d67369bd7..c88be881d50 100644 --- a/docs/development/msp/msp_messages.json +++ b/docs/development/msp/msp_messages.json @@ -6953,6 +6953,35 @@ "notes": "Requires `USE_CURRENT_METER`/`USE_ADC` for current-related fields; values fall back to zero when unavailable. Capacity fields are reported in the units configured by `batteryMetersConfig()->capacity_unit` (mAh or mWh).", "description": "Provides detailed analog sensor readings, superseding `MSP_ANALOG` with higher precision and additional fields." }, + "MSP2_INAV_GET_LINK_STATS": { + "code": 8451, + "mspv": 2, + "request": null, + "reply": { + "payload": [ + { + "name": "uplinkRSSI_dBm", + "ctype": "uint8_t", + "desc": "Uplink RSSI in dBm, sent as a positive magnitude (`getRSSI()`). For example, 70 means -70dBm.", + "units": "-dBm" + }, + { + "name": "uplinkLQ", + "ctype": "uint8_t", + "desc": "Uplink Link Quality (`rxLinkStatistics.uplinkLQ`)", + "units": "%" + }, + { + "name": "uplinkSNR", + "ctype": "int8_t", + "desc": "Uplink Signal-to-Noise Ratio (`rxLinkStatistics.uplinkSNR`)", + "units": "dB" + } + ] + }, + "notes": "Useful for GCS monitoring of the active RC link quality and signal margin.", + "description": "Provides uplink RC link statistics for monitoring on a GCS." + }, "MSP2_INAV_MISC": { "code": 8195, "mspv": 2, @@ -10943,6 +10972,40 @@ "notes": "All attitude angles are in deci-degrees.", "description": "Provides estimates of current attitude, local NEU position, and velocity." }, + "MSP2_INAV_SET_WP_INDEX": { + "code": 8737, + "mspv": 2, + "request": { + "payload": [ + { + "name": "wp_index", + "ctype": "uint8_t", + "desc": "0-based waypoint index to jump to, relative to the mission start waypoint (`posControl.startWpIndex`)", + "units": "-" + } + ] + }, + "reply": null, + "notes": "Returns error if the aircraft is not armed, `NAV_WP_MODE` is not active, or the index is outside the valid mission range (`startWpIndex` to `startWpIndex + waypointCount - 1`). On success, sets `posControl.activeWaypointIndex` to the requested index and fires `NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_JUMP`, transitioning the navigation FSM back to `NAV_STATE_WAYPOINT_PRE_ACTION` so the flight controller re-initialises navigation for the new target.", + "description": "Jumps to a specific waypoint during an active waypoint mission, causing the aircraft to immediately begin navigating toward the new target waypoint." + }, + "MSP2_INAV_SET_CRUISE_HEADING": { + "code": 8739, + "mspv": 2, + "request": { + "payload": [ + { + "name": "heading_centidegrees", + "ctype": "int32_t", + "desc": "Target heading in centidegrees (0-35999). Values are wrapped modulo 36000 before being applied.", + "units": "centidegrees" + } + ] + }, + "reply": null, + "notes": "Returns error if the aircraft is not armed or `NAV_COURSE_HOLD_MODE` is not active. On success, sets both `posControl.cruise.course` and `posControl.cruise.previousCourse` to the normalised value, preventing spurious heading adjustments from `getCruiseHeadingAdjustment()` on the next control cycle.", + "description": "Sets the course heading target while Cruise or Course Hold mode is active, causing the aircraft to turn to and maintain the new heading." + }, "MSP2_BETAFLIGHT_BIND": { "code": 12288, "mspv": 2, diff --git a/docs/development/msp/rev b/docs/development/msp/rev index bf0d87ab1b2..7ed6ff82de6 100644 --- a/docs/development/msp/rev +++ b/docs/development/msp/rev @@ -1 +1 @@ -4 \ No newline at end of file +5 diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index b7da917520f..802442ca5b9 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -742,6 +742,12 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU16(dst, getRSSI()); break; + case MSP2_INAV_GET_LINK_STATS: + sbufWriteU8(dst, (uint8_t)-rxLinkStatistics.uplinkRSSI); + sbufWriteU8(dst, rxLinkStatistics.uplinkLQ); + sbufWriteU8(dst, (uint8_t)rxLinkStatistics.uplinkSNR); + break; + case MSP_LOOP_TIME: sbufWriteU16(dst, gyroConfig()->looptime); break; diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h index 21ad7e3d135..7d092ed41fd 100755 --- a/src/main/msp/msp_protocol_v2_inav.h +++ b/src/main/msp/msp_protocol_v2_inav.h @@ -116,6 +116,8 @@ #define MSP2_INAV_CUSTOM_OSD_ELEMENT 0x2101 #define MSP2_INAV_SET_CUSTOM_OSD_ELEMENTS 0x2102 +#define MSP2_INAV_GET_LINK_STATS 0x2103 + #define MSP2_INAV_SERVO_CONFIG 0x2200 #define MSP2_INAV_SET_SERVO_CONFIG 0x2201 From 16b916ab3709e1383a4e0640ceb08a0a408b7f18 Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Sun, 12 Apr 2026 15:02:04 -0500 Subject: [PATCH 43/48] Add new target: PAURCF405V2 STM32F405RGT6-based flight controller by PauRC. Hardware features: - IMU: ICM42605/ICM42688P or BMI270 (board variant, same CS pin PC14) - OSD: MAX7456 on SPI1 - Blackbox: 16MB SPI flash on SPI2 - Baro: SPL06/DPS310/BMP280/MS5611 (I2C1) - 6x UART + 1x SoftSerial (PA2, shared with UART2 TX) - 11x PWM outputs with DShot/DMAR support - LED strip, PINIO (2 channels), airspeed ADC - 30.5x30.5mm mounting pattern Closes #11089 --- src/main/target/PAURCF405V2/CMakeLists.txt | 1 + src/main/target/PAURCF405V2/config.c | 28 ++++ src/main/target/PAURCF405V2/target.c | 48 ++++++ src/main/target/PAURCF405V2/target.h | 166 +++++++++++++++++++++ 4 files changed, 243 insertions(+) create mode 100644 src/main/target/PAURCF405V2/CMakeLists.txt create mode 100644 src/main/target/PAURCF405V2/config.c create mode 100644 src/main/target/PAURCF405V2/target.c create mode 100644 src/main/target/PAURCF405V2/target.h diff --git a/src/main/target/PAURCF405V2/CMakeLists.txt b/src/main/target/PAURCF405V2/CMakeLists.txt new file mode 100644 index 00000000000..2034128e311 --- /dev/null +++ b/src/main/target/PAURCF405V2/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f405xg(PAURCF405V2) diff --git a/src/main/target/PAURCF405V2/config.c b/src/main/target/PAURCF405V2/config.c new file mode 100644 index 00000000000..79a0eb18472 --- /dev/null +++ b/src/main/target/PAURCF405V2/config.c @@ -0,0 +1,28 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#include +#include "platform.h" +#include "fc/fc_msp_box.h" +#include "fc/config.h" +#include "io/piniobox.h" + +void targetConfiguration(void) +{ + pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1; + pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER2; +} diff --git a/src/main/target/PAURCF405V2/target.c b/src/main/target/PAURCF405V2/target.c new file mode 100644 index 00000000000..efc300259d5 --- /dev/null +++ b/src/main/target/PAURCF405V2/target.c @@ -0,0 +1,48 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#include +#include +#include "drivers/bus.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" +#include "drivers/sensor.h" +#include "drivers/timer_def_stm32f4xx.h" + +timerHardware_t timerHardware[] = { + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 D(2,7,7) UP217 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 D(2,2,0) UP217 + DEF_TIM(TIM1, CH3N, PB15, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 D(2,6,0) UP256 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 2), // S4 D(2,3,6) UP256 + + DEF_TIM(TIM2, CH4, PB11, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 D(1,7,3) UP173 + DEF_TIM(TIM2, CH3, PB10, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 D(1,1,3) UP173 + DEF_TIM(TIM2, CH2, PB3, TIM_USE_OUTPUT_AUTO, 0, 0), // S7 D(1,6,3) UP173 + DEF_TIM(TIM2, CH1, PA15, TIM_USE_OUTPUT_AUTO, 0, 0), // S8 D(1,5,3) UP173 + + DEF_TIM(TIM12, CH1, PB14, TIM_USE_OUTPUT_AUTO, 0, 0), // S9 DMA NONE + DEF_TIM(TIM13, CH1, PA6, TIM_USE_OUTPUT_AUTO, 0, 0), // S10 DMA NONE + DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), // S11 D(1,0,2) + + DEF_TIM(TIM3, CH4, PB1, TIM_USE_LED, 0, 0), // 2812LED D(1,2,5) + DEF_TIM(TIM11, CH1, PB9, TIM_USE_BEEPER, 0, 0), // BEEPER PWM + + DEF_TIM(TIM5, CH3, PA2, TIM_USE_ANY, 0, 0), //TX2 softserial1_Tx ISR-driven, does not use DMA1_Stream0 +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/PAURCF405V2/target.h b/src/main/target/PAURCF405V2/target.h new file mode 100644 index 00000000000..67813a2c511 --- /dev/null +++ b/src/main/target/PAURCF405V2/target.h @@ -0,0 +1,166 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#pragma once +#define USE_TARGET_CONFIG + +#define TARGET_BOARD_IDENTIFIER "PA42" +#define USBD_PRODUCT_STRING "PauRcF405v2" + +#define LED0 PA14 //Blue +#define LED1 PA13 //Green + +#define BEEPER PB9 +#define BEEPER_INVERTED +#define BEEPER_PWM_FREQUENCY 2500 + +// *************** SPI1 IMU & OSD ******************* +#define USE_SPI +#define USE_SPI_DEVICE_1 + +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PB4 +#define SPI1_MOSI_PIN PA7 + +// Board ships with either ICM42605/ICM42688P or BMI270 - same CS pin (PC14) +#define USE_IMU_ICM42605 +#define IMU_ICM42605_ALIGN CW270_DEG_FLIP +#define ICM42605_SPI_BUS BUS_SPI1 +#define ICM42605_CS_PIN PC14 + +#define USE_IMU_BMI270 +#define IMU_BMI270_ALIGN CW270_DEG_FLIP +#define BMI270_SPI_BUS BUS_SPI1 +#define BMI270_CS_PIN PC14 + + + +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI1 +#define MAX7456_CS_PIN PB12 + +// *************** SPI2 Flash **************** +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PC2 +#define SPI2_MOSI_PIN PC3 + +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define M25P16_SPI_BUS BUS_SPI2 +#define M25P16_CS_PIN PC13 +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + +// *************** I2C /Baro/Mag ********************* +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB7 + +#define USE_BARO +#define BARO_I2C_BUS BUS_I2C1 +#define USE_BARO_BMP280 +#define USE_BARO_MS5611 +#define USE_BARO_DPS310 +#define USE_BARO_SPL06 + +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C1 +#define USE_MAG_ALL + +#define USE_RANGEFINDER +#define RANGEFINDER_I2C_BUS BUS_I2C1 +#define PITOT_I2C_BUS BUS_I2C1 +#define TEMPERATURE_I2C_BUS BUS_I2C1 + +// *************** UART ***************************** +#define USE_VCP + +#define USE_UART1 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 + +#define USE_UART2 +#define UART2_TX_PIN PA2 +#define UART2_RX_PIN PA3 + +#define USE_UART3 +#define UART3_TX_PIN PC10 +#define UART3_RX_PIN PC11 + +#define USE_UART4 +#define UART4_TX_PIN PA0 +#define UART4_RX_PIN PA1 + +#define USE_UART5 +#define UART5_TX_PIN PC12 +#define UART5_RX_PIN PD2 + +#define USE_UART6 +#define UART6_TX_PIN PC6 +#define UART6_RX_PIN PC7 + +#define USE_SOFTSERIAL1 +#define SOFTSERIAL_1_TX_PIN PA2 // Shared with UART2_TX - mutually exclusive with UART2 +#define SOFTSERIAL_1_RX_PIN PA2 // Shared with UART2_TX - mutually exclusive with UART2 + +#define SERIAL_PORT_COUNT 8 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_CRSF +#define SERIALRX_UART SERIAL_PORT_USART2 + +// *************** ADC *************************** +#define USE_ADC +#define ADC_INSTANCE ADC1 +#define ADC1_DMA_STREAM DMA2_Stream4 +#define ADC_CHANNEL_1_PIN PC4 +#define ADC_CHANNEL_2_PIN PC5 +#define ADC_CHANNEL_3_PIN PB0 +#define ADC_CHANNEL_4_PIN PC0 +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 +#define RSSI_ADC_CHANNEL ADC_CHN_3 +#define AIRSPEED_ADC_CHANNEL ADC_CHN_4 + +// *************** PINIO *************************** +#define USE_PINIO +#define USE_PINIOBOX +#define PINIO1_PIN PA4 +#define PINIO2_PIN PB5 + +// *************** LEDSTRIP ************************ +#define USE_LED_STRIP +#define WS2811_PIN PB1 + +// *************** others ************************ +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX) +#define VBAT_SCALE_DEFAULT 2100 +#define CURRENT_METER_SCALE 150 + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)) + +#define MAX_PWM_OUTPUT_PORTS 11 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE +#define USE_DSHOT +#define USE_DSHOT_DMAR +#define USE_ESC_SENSOR + From 0c240ec25321644f43629c9d654356c788d19567 Mon Sep 17 00:00:00 2001 From: Leander-vh Date: Mon, 13 Apr 2026 23:12:02 +0200 Subject: [PATCH 44/48] Added ledstrip support for FlyingRCF4Wing target thx to 3sr3v3r --- src/main/target/FLYINGRCF4WINGMINI_NOT_RECOMMENDED/target.c | 2 +- src/main/target/FLYINGRCF4WINGMINI_NOT_RECOMMENDED/target.h | 4 ++++ 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/src/main/target/FLYINGRCF4WINGMINI_NOT_RECOMMENDED/target.c b/src/main/target/FLYINGRCF4WINGMINI_NOT_RECOMMENDED/target.c index d86bbb17379..2962c394a3e 100644 --- a/src/main/target/FLYINGRCF4WINGMINI_NOT_RECOMMENDED/target.c +++ b/src/main/target/FLYINGRCF4WINGMINI_NOT_RECOMMENDED/target.c @@ -34,7 +34,7 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM2, CH4, PB11, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 D(1,7,3) UP173 DEF_TIM(TIM2, CH3, PB10, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 D(1,1,3) UP173 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // 2812LED D(1,2,5) + DEF_TIM(TIM3, CH4, PB1, TIM_USE_LED, 0, 0), // 2812LED D(1,2,5) DEF_TIM(TIM5, CH3, PA2, TIM_USE_ANY, 0, 0), //TX2 softserial1_Tx }; diff --git a/src/main/target/FLYINGRCF4WINGMINI_NOT_RECOMMENDED/target.h b/src/main/target/FLYINGRCF4WINGMINI_NOT_RECOMMENDED/target.h index cf98fcaca14..8eecb2c466c 100644 --- a/src/main/target/FLYINGRCF4WINGMINI_NOT_RECOMMENDED/target.h +++ b/src/main/target/FLYINGRCF4WINGMINI_NOT_RECOMMENDED/target.h @@ -95,6 +95,10 @@ #define ADC_CHANNEL_1_PIN PC4 #define VBAT_ADC_CHANNEL ADC_CHN_1 +// *************** LEDSTRIP ************************ +#define USE_LED_STRIP +#define WS2811_PIN PB1 + // *************** others ************************ #define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX) #define VBAT_SCALE_DEFAULT 2100 From c49b564e448934bcf5606e095e9e151c85293a54 Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Mon, 13 Apr 2026 17:34:02 -0500 Subject: [PATCH 45/48] PAURCF405V2: correct IMU alignment --- src/main/target/PAURCF405V2/target.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/target/PAURCF405V2/target.h b/src/main/target/PAURCF405V2/target.h index 67813a2c511..658ee136d4f 100644 --- a/src/main/target/PAURCF405V2/target.h +++ b/src/main/target/PAURCF405V2/target.h @@ -38,12 +38,12 @@ // Board ships with either ICM42605/ICM42688P or BMI270 - same CS pin (PC14) #define USE_IMU_ICM42605 -#define IMU_ICM42605_ALIGN CW270_DEG_FLIP +#define IMU_ICM42605_ALIGN CW0_DEG #define ICM42605_SPI_BUS BUS_SPI1 #define ICM42605_CS_PIN PC14 #define USE_IMU_BMI270 -#define IMU_BMI270_ALIGN CW270_DEG_FLIP +#define IMU_BMI270_ALIGN CW180_DEG #define BMI270_SPI_BUS BUS_SPI1 #define BMI270_CS_PIN PC14 From 04e188c74c0939832622c5681982535dcc08ff36 Mon Sep 17 00:00:00 2001 From: b14ckyy Date: Tue, 21 Apr 2026 13:23:13 +0200 Subject: [PATCH 46/48] Resolve merge conflict in msp_messages.json Merge MSP2_INAV_SET_WP_INDEX (8737) and MSP2_INAV_SET_CRUISE_HEADING (8739) from upstream with MSP2_INAV_SET_AUX_RC (8752), maintaining numeric order. --- docs/development/msp/msp_messages.json | 34 ++++++++++++++++++++++++++ 1 file changed, 34 insertions(+) diff --git a/docs/development/msp/msp_messages.json b/docs/development/msp/msp_messages.json index bbb7eac3429..b687e222c53 100644 --- a/docs/development/msp/msp_messages.json +++ b/docs/development/msp/msp_messages.json @@ -10943,6 +10943,40 @@ "notes": "All attitude angles are in deci-degrees.", "description": "Provides estimates of current attitude, local NEU position, and velocity." }, + "MSP2_INAV_SET_WP_INDEX": { + "code": 8737, + "mspv": 2, + "request": { + "payload": [ + { + "name": "wp_index", + "ctype": "uint8_t", + "desc": "0-based waypoint index to jump to, relative to the mission start waypoint (`posControl.startWpIndex`)", + "units": "-" + } + ] + }, + "reply": null, + "notes": "Returns error if the aircraft is not armed, `NAV_WP_MODE` is not active, or the index is outside the valid mission range (`startWpIndex` to `startWpIndex + waypointCount - 1`). On success, sets `posControl.activeWaypointIndex` to the requested index and fires `NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_JUMP`, transitioning the navigation FSM back to `NAV_STATE_WAYPOINT_PRE_ACTION` so the flight controller re-initialises navigation for the new target.", + "description": "Jumps to a specific waypoint during an active waypoint mission, causing the aircraft to immediately begin navigating toward the new target waypoint." + }, + "MSP2_INAV_SET_CRUISE_HEADING": { + "code": 8739, + "mspv": 2, + "request": { + "payload": [ + { + "name": "heading_centidegrees", + "ctype": "int32_t", + "desc": "Target heading in centidegrees (0-35999). Values are wrapped modulo 36000 before being applied.", + "units": "centidegrees" + } + ] + }, + "reply": null, + "notes": "Returns error if the aircraft is not armed or `NAV_COURSE_HOLD_MODE` is not active. On success, sets both `posControl.cruise.course` and `posControl.cruise.previousCourse` to the normalised value, preventing spurious heading adjustments from `getCruiseHeadingAdjustment()` on the next control cycle.", + "description": "Sets the course heading target while Cruise or Course Hold mode is active, causing the aircraft to turn to and maintain the new heading." + }, "MSP2_INAV_SET_AUX_RC": { "code": 8752, "mspv": 2, From 21ea4c8bf4603caf46522ed0a7ba732daaa3b1b3 Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Fri, 24 Apr 2026 22:27:43 -0500 Subject: [PATCH 47/48] GPS: fix hwVersion field size from uint32_t to uint8_t MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Change gpsState.hwVersion from uint32_t to uint8_t and introduce a structured bit-field encoding for the hardware version byte: bits [7:6] = series (0b01 = u-blox Neo/M series) bits [5:0] = generation (e.g. 8=M8, 9=M9, 10=M10) Update UBX_HW_VERSION_* constants accordingly (e.g. M8=0x48, M9=0x49, M10=0x4A). Change MSP_GPSSTATISTICS to send hwVersion as a single byte (sbufWriteU8) instead of four bytes (sbufWriteU32). The bit-field layout reserves series values 0b10 and 0b11 for future chip families (e.g. u-blox F9, other manufacturers). Tested with M9 and M10 receivers — auto-detection working correctly. --- src/main/fc/fc_msp.c | 2 +- src/main/io/gps_private.h | 2 +- src/main/io/gps_ublox.c | 2 +- src/main/io/gps_ublox.h | 26 +++++++++++++++++++------- 4 files changed, 22 insertions(+), 10 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index cf5308067e9..ac466ece592 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1009,7 +1009,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU16(dst, gpsSol.hdop); sbufWriteU16(dst, gpsSol.eph); sbufWriteU16(dst, gpsSol.epv); - sbufWriteU32(dst, gpsState.hwVersion); + sbufWriteU8(dst, gpsState.hwVersion); break; #endif case MSP2_ADSB_VEHICLE_LIST: diff --git a/src/main/io/gps_private.h b/src/main/io/gps_private.h index e5234e70248..bad8992ce45 100755 --- a/src/main/io/gps_private.h +++ b/src/main/io/gps_private.h @@ -42,7 +42,7 @@ typedef struct { const serialConfig_t * serialConfig; serialPort_t * gpsPort; // Serial GPS only - uint32_t hwVersion; + uint8_t hwVersion; // See UBX_HW_VERSION_* in gps_ublox.h uint8_t swVersionMajor; uint8_t swVersionMinor; diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index 102284723df..4fd0c332a21 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -556,7 +556,7 @@ static void gpsDecodeProtocolVersion(const char *proto, size_t bufferLength) } } -static uint32_t gpsDecodeHardwareVersion(const char * szBuf, unsigned nBufSize) +static uint8_t gpsDecodeHardwareVersion(const char * szBuf, unsigned nBufSize) { // ublox_5 hwVersion 00040005 if (strncmp(szBuf, "00040005", nBufSize) == 0) { diff --git a/src/main/io/gps_ublox.h b/src/main/io/gps_ublox.h index ab0ab930275..d54f4997be0 100644 --- a/src/main/io/gps_ublox.h +++ b/src/main/io/gps_ublox.h @@ -62,13 +62,25 @@ STATIC_ASSERT(MAX_UBLOX_PAYLOAD_SIZE >= 256, ubx_size_too_small); #define UBX_VALID_GPS_TIME(valid) (valid & 1 << 1) #define UBX_VALID_GPS_DATE_TIME(valid) (UBX_VALID_GPS_DATE(valid) && UBX_VALID_GPS_TIME(valid)) -#define UBX_HW_VERSION_UNKNOWN 0 -#define UBX_HW_VERSION_UBLOX5 500 -#define UBX_HW_VERSION_UBLOX6 600 -#define UBX_HW_VERSION_UBLOX7 700 -#define UBX_HW_VERSION_UBLOX8 800 -#define UBX_HW_VERSION_UBLOX9 900 -#define UBX_HW_VERSION_UBLOX10 1000 +/* + * hwVersion encoding (fits in uint8_t): + * bits [7:6] series: 0b00=unknown, 0b01=u-blox Neo/M series + * bits [5:0] generation within series (e.g. 8=M8, 9=M9, 10=M10) + * + * This leaves 0b10 and 0b11 available for future series (e.g. u-blox F9, + * other manufacturers). + */ +#define UBX_HW_SERIES_MASK 0xC0 +#define UBX_HW_GEN_MASK 0x3F +#define UBX_HW_SERIES_UBLOX_NM 0x40 // 0b01 << 6: u-blox Neo/M series + +#define UBX_HW_VERSION_UNKNOWN 0 +#define UBX_HW_VERSION_UBLOX5 (UBX_HW_SERIES_UBLOX_NM | 5) // 0x45 +#define UBX_HW_VERSION_UBLOX6 (UBX_HW_SERIES_UBLOX_NM | 6) // 0x46 +#define UBX_HW_VERSION_UBLOX7 (UBX_HW_SERIES_UBLOX_NM | 7) // 0x47 +#define UBX_HW_VERSION_UBLOX8 (UBX_HW_SERIES_UBLOX_NM | 8) // 0x48 +#define UBX_HW_VERSION_UBLOX9 (UBX_HW_SERIES_UBLOX_NM | 9) // 0x49 +#define UBX_HW_VERSION_UBLOX10 (UBX_HW_SERIES_UBLOX_NM | 10) // 0x4A #define UBLOX_CFG_MSGOUT_NAV_POSLLH_UART1 0x2091002a // U1 #define UBLOX_CFG_MSGOUT_NAV_SAT_UART1 0x20910016 // U1 From 690c1ec04d036ee16aa58984e6ef72535295b0b6 Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Fri, 24 Apr 2026 23:58:53 -0500 Subject: [PATCH 48/48] GPS: update MSP_GPSSTATISTICS docs for uint8_t hwVersion encoding --- docs/development/msp/msp_messages.json | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/docs/development/msp/msp_messages.json b/docs/development/msp/msp_messages.json index 12d67369bd7..f0b6ab0acce 100644 --- a/docs/development/msp/msp_messages.json +++ b/docs/development/msp/msp_messages.json @@ -4914,9 +4914,9 @@ }, { "name": "hwVersion", - "ctype": "uint32_t", - "desc": "GPS hardware version (`gpsState.hwVersion`). Values: 500=UBLOX5, 600=UBLOX6, 700=UBLOX7, 800=UBLOX8, 900=UBLOX9, 1000=UBLOX10, 0=UNKNOWN", - "units": "Version code" + "ctype": "uint8_t", + "desc": "GPS hardware version bit-field: bits[7:6]=series (0b01=u-blox Neo/M), bits[5:0]=generation. E.g. 0x48=M8, 0x49=M9, 0x4A=M10, 0=unknown.", + "units": "" } ] },