From 216b76c212b5c04929fd306d0853c27b20d99c4e Mon Sep 17 00:00:00 2001 From: Daniel Ribeiro Date: Sun, 29 Mar 2026 13:28:25 -0300 Subject: [PATCH 1/2] 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 2/2] =?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);