From 8b3ef1cf9e33704c9afdeb3355a50c29fa138762 Mon Sep 17 00:00:00 2001 From: mahima-yoga Date: Fri, 22 May 2026 15:17:25 +0200 Subject: [PATCH] feat(navigator): add Guided Course mode for fixed-wing Implements a new GUIDED_COURSE navigator mode that maintains a constant ground-track bearing, altitude, and airspeed without manual stick input. The mode is activated via MAVLink and accepts real-time in-flight updates: - MAV_CMD_GUIDED_CHANGE_HEADING (HEADING_TYPE_COURSE_OVER_GROUND): set course - MAV_CMD_DO_CHANGE_ALTITUDE: adjust target altitude - MAV_CMD_DO_CHANGE_SPEED: adjust target airspeed On activation the vehicle captures its current velocity vector as the initial course bearing. A valid horizontal velocity estimate (GPS or dead-reckoning) is required; course commands are rejected if unavailable. --- msg/PositionSetpoint.msg | 1 + msg/versioned/VehicleCommand.msg | 2 + msg/versioned/VehicleStatus.msg | 2 +- src/lib/events/enums.json | 4 + src/lib/modes/ui.hpp | 3 +- src/modules/commander/Commander.cpp | 41 +++-- .../commander/ModeUtil/control_mode.cpp | 1 + .../commander/ModeUtil/conversions.hpp | 2 + .../commander/ModeUtil/mode_requirements.cpp | 7 + src/modules/commander/failsafe/failsafe.cpp | 1 + src/modules/commander/px4_custom_mode.h | 6 + .../fw_mode_manager/FixedWingModeManager.cpp | 32 ++++ .../mavlink/streams/UTM_GLOBAL_POSITION.hpp | 1 + src/modules/navigator/CMakeLists.txt | 1 + src/modules/navigator/course.cpp | 119 ++++++++++++ src/modules/navigator/course.h | 73 ++++++++ src/modules/navigator/navigator.h | 5 +- src/modules/navigator/navigator_main.cpp | 173 ++++++++++++------ 18 files changed, 400 insertions(+), 74 deletions(-) create mode 100644 src/modules/navigator/course.cpp create mode 100644 src/modules/navigator/course.h diff --git a/msg/PositionSetpoint.msg b/msg/PositionSetpoint.msg index 90cb4472748..93bb6761979 100644 --- a/msg/PositionSetpoint.msg +++ b/msg/PositionSetpoint.msg @@ -33,6 +33,7 @@ uint8 loiter_pattern # loitern pattern to follow float32 acceptance_radius # horizontal acceptance_radius (meters) float32 alt_acceptance_radius # vertical acceptance radius, only used for fixed wing guidance, NAN = let guidance choose (meters) +float32 course # [rad] desired course (bearing) over ground, NaN = unused float32 cruising_speed # the generally desired cruising speed (not a hard constraint) bool gliding_enabled # commands the vehicle to glide if the capability is available (fixed wing only) float32 cruising_throttle # the generally desired cruising throttle (not a hard constraint), only has an effect for rover diff --git a/msg/versioned/VehicleCommand.msg b/msg/versioned/VehicleCommand.msg index 380c931c97b..fb3121eba39 100644 --- a/msg/versioned/VehicleCommand.msg +++ b/msg/versioned/VehicleCommand.msg @@ -128,6 +128,8 @@ uint32 VEHICLE_CMD_PX4_INTERNAL_START = 65537 # Start of PX4 internal only vehic uint32 VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN = 100000 # Sets the GPS coordinates of the vehicle local origin (0,0,0) position. |Unused|Unused|Unused|Unused|Latitude (WGS-84)|Longitude (WGS-84)|[m] Altitude (AMSL from GNSS, positive above ground)| uint32 VEHICLE_CMD_SET_NAV_STATE = 100001 # Change mode by specifying nav_state directly. |nav_state|Unused|Unused|Unused|Unused|Unused|Unused| +uint16 VEHICLE_CMD_GUIDED_CHANGE_HEADING = 43002 # Change heading/course. param1: heading type (0=course-over-ground, 1=heading). param2: target [deg]. param3: max rate [deg/s]. |Heading type (HEADING_TYPE enum)|[deg] Target bearing [0..360]|[deg/s] Max rate of change|Unused|Unused|Unused|Unused| + uint8 VEHICLE_MOUNT_MODE_RETRACT = 0 # Load and keep safe position (Roll,Pitch,Yaw) from permanent memory and stop stabilization. uint8 VEHICLE_MOUNT_MODE_NEUTRAL = 1 # Load and keep neutral position (Roll,Pitch,Yaw) from permanent memory. uint8 VEHICLE_MOUNT_MODE_MAVLINK_TARGETING = 2 # Load neutral position and start MAVLink Roll,Pitch,Yaw control with stabilization. diff --git a/msg/versioned/VehicleStatus.msg b/msg/versioned/VehicleStatus.msg index 28cbe379e90..4992fd38d2c 100644 --- a/msg/versioned/VehicleStatus.msg +++ b/msg/versioned/VehicleStatus.msg @@ -36,7 +36,7 @@ uint8 NAVIGATION_STATE_AUTO_MISSION = 3 # Auto mission mode uint8 NAVIGATION_STATE_AUTO_LOITER = 4 # Auto loiter mode uint8 NAVIGATION_STATE_AUTO_RTL = 5 # Auto return to launch mode uint8 NAVIGATION_STATE_POSITION_SLOW = 6 -uint8 NAVIGATION_STATE_FREE5 = 7 +uint8 NAVIGATION_STATE_GUIDED_COURSE = 7 # Guided Course mode (FW: maintain course/alt/speed) uint8 NAVIGATION_STATE_ALTITUDE_CRUISE = 8 # Altitude with Cruise mode uint8 NAVIGATION_STATE_FREE3 = 9 uint8 NAVIGATION_STATE_ACRO = 10 # Acro mode diff --git a/src/lib/events/enums.json b/src/lib/events/enums.json index 25af281509d..aad75ea6a5e 100644 --- a/src/lib/events/enums.json +++ b/src/lib/events/enums.json @@ -592,6 +592,10 @@ "name": "altitude_cruise", "description": "Altitude Cruise" }, + "25": { + "name": "auto_course", + "description": "Course" + }, "255": { "name": "unknown", "description": "[Unknown]" diff --git a/src/lib/modes/ui.hpp b/src/lib/modes/ui.hpp index fe2e5a96896..8918039dc4c 100644 --- a/src/lib/modes/ui.hpp +++ b/src/lib/modes/ui.hpp @@ -51,6 +51,7 @@ static inline uint32_t getValidNavStates() (1u << vehicle_status_s::NAVIGATION_STATE_POSCTL) | (1u << vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) | (1u << vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER) | + (1u << vehicle_status_s::NAVIGATION_STATE_GUIDED_COURSE) | (1u << vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) | (1u << vehicle_status_s::NAVIGATION_STATE_POSITION_SLOW) | (1u << vehicle_status_s::NAVIGATION_STATE_ACRO) | @@ -75,7 +76,7 @@ const char *const nav_state_names[vehicle_status_s::NAVIGATION_STATE_MAX] = { "Hold", "Return", "Position Slow", - "7: unallocated", + "Guided Course", "Altitude Cruise", "9: unallocated", "Acro", diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 42c7e797323..36569946b2d 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -489,6 +489,10 @@ int Commander::custom_command(int argc, char *argv[]) send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_LOITER); + } else if (!strcmp(argv[1], "auto:course")) { + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO, + PX4_CUSTOM_SUB_MODE_GUIDED_COURSE); + } else if (!strcmp(argv[1], "auto:rtl")) { send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_RTL); @@ -872,11 +876,19 @@ Commander::handle_command(const vehicle_command_s &cmd) answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED); } else { - if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, getSourceFromCommand(cmd))) { + // If already in course mode, stay in course mode (navigator handles any altitude update) + // Only switch to loiter if a specific lat/lon target is given, or we are not in course mode. + const bool has_position_target = PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6); + const bool in_course_mode = _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_GUIDED_COURSE; + const uint8_t target_state = (in_course_mode && !has_position_target) + ? vehicle_status_s::NAVIGATION_STATE_GUIDED_COURSE + : vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER; + + if (_user_mode_intention.change(target_state, getSourceFromCommand(cmd))) { cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED; } else { - printRejectMode(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER); + printRejectMode(target_state); cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } } @@ -884,20 +896,23 @@ Commander::handle_command(const vehicle_command_s &cmd) break; case vehicle_command_s::VEHICLE_CMD_DO_CHANGE_ALTITUDE: { + // Accept only in modes where the navigator handles altitude changes in-place. + // No mode switching: if the current mode doesn't support it, deny. + const uint8_t nav_state = _vehicle_status.nav_state; - // Just switch the flight mode here, the navigator takes care of - // doing something sensible with the coordinates. Its designed - // to not require navigator and command to receive / process - // the data at the exact same time. - - if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER)) { + if (nav_state == vehicle_status_s::NAVIGATION_STATE_GUIDED_COURSE + || nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER) { cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED; } else { - printRejectMode(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER); - cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED; } + } + break; + case vehicle_command_s::VEHICLE_CMD_GUIDED_CHANGE_HEADING: { + // Navigator handles this command: it acks ACCEPTED when + // the vehicle is in course mode with a valid position, DENIED otherwise. } break; @@ -964,6 +979,10 @@ Commander::handle_command(const vehicle_command_s &cmd) desired_nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND; break; + case PX4_CUSTOM_SUB_MODE_GUIDED_COURSE: + desired_nav_state = vehicle_status_s::NAVIGATION_STATE_GUIDED_COURSE; + break; + case PX4_CUSTOM_SUB_MODE_EXTERNAL1...PX4_CUSTOM_SUB_MODE_EXTERNAL8: desired_nav_state = vehicle_status_s::NAVIGATION_STATE_EXTERNAL1 + (custom_sub_mode - PX4_CUSTOM_SUB_MODE_EXTERNAL1); break; @@ -3217,7 +3236,7 @@ The commander module contains the state machine for mode switching and failsafe PRINT_MODULE_USAGE_COMMAND("land"); PRINT_MODULE_USAGE_COMMAND_DESCR("transition", "VTOL transition"); PRINT_MODULE_USAGE_COMMAND_DESCR("mode", "Change flight mode"); - PRINT_MODULE_USAGE_ARG("manual|acro|offboard|stabilized|altctl|posctl|altitude_cruise|position:slow|auto:mission|auto:loiter|auto:rtl|auto:takeoff|auto:land|auto:precland|ext1", + PRINT_MODULE_USAGE_ARG("manual|acro|offboard|stabilized|altctl|posctl|altitude_cruise|position:slow|auto:mission|auto:loiter|auto:course|auto:rtl|auto:takeoff|auto:land|auto:precland|ext1", "Flight mode", false); PRINT_MODULE_USAGE_COMMAND("pair"); PRINT_MODULE_USAGE_COMMAND("termination"); diff --git a/src/modules/commander/ModeUtil/control_mode.cpp b/src/modules/commander/ModeUtil/control_mode.cpp index 45b01c6ec5a..4ce20ab018c 100644 --- a/src/modules/commander/ModeUtil/control_mode.cpp +++ b/src/modules/commander/ModeUtil/control_mode.cpp @@ -89,6 +89,7 @@ void getVehicleControlMode(uint8_t nav_state, uint8_t vehicle_type, case vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND: case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER: + case vehicle_status_s::NAVIGATION_STATE_GUIDED_COURSE: case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF: case vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF: vehicle_control_mode.flag_control_auto_enabled = true; diff --git a/src/modules/commander/ModeUtil/conversions.hpp b/src/modules/commander/ModeUtil/conversions.hpp index e6bb4e21029..77b8e26d1fe 100644 --- a/src/modules/commander/ModeUtil/conversions.hpp +++ b/src/modules/commander/ModeUtil/conversions.hpp @@ -58,6 +58,8 @@ static inline navigation_mode_t navigation_mode(uint8_t nav_state) case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER: return navigation_mode_t::auto_loiter; + case vehicle_status_s::NAVIGATION_STATE_GUIDED_COURSE: return navigation_mode_t::auto_course; + case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: return navigation_mode_t::auto_rtl; case vehicle_status_s::NAVIGATION_STATE_POSITION_SLOW: return navigation_mode_t::position_slow; diff --git a/src/modules/commander/ModeUtil/mode_requirements.cpp b/src/modules/commander/ModeUtil/mode_requirements.cpp index 850a0cf6e9d..659b521d8d3 100644 --- a/src/modules/commander/ModeUtil/mode_requirements.cpp +++ b/src/modules/commander/ModeUtil/mode_requirements.cpp @@ -123,6 +123,13 @@ void getModeRequirements(uint8_t vehicle_type, failsafe_flags_s &flags) setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, flags.mode_req_local_alt); setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, flags.mode_req_wind_and_flight_time_compliance); + // NAVIGATION_STATE_GUIDED_COURSE + setRequirement(vehicle_status_s::NAVIGATION_STATE_GUIDED_COURSE, flags.mode_req_angular_velocity); + setRequirement(vehicle_status_s::NAVIGATION_STATE_GUIDED_COURSE, flags.mode_req_attitude); + setRequirement(vehicle_status_s::NAVIGATION_STATE_GUIDED_COURSE, flags.mode_req_local_alt); + setRequirement(vehicle_status_s::NAVIGATION_STATE_GUIDED_COURSE, flags.mode_req_wind_and_flight_time_compliance); + setRequirement(vehicle_status_s::NAVIGATION_STATE_GUIDED_COURSE, flags.mode_req_local_position_relaxed); + // NAVIGATION_STATE_AUTO_RTL setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL, flags.mode_req_angular_velocity); setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL, flags.mode_req_attitude); diff --git a/src/modules/commander/failsafe/failsafe.cpp b/src/modules/commander/failsafe/failsafe.cpp index 037572cb976..9d49f73e97c 100644 --- a/src/modules/commander/failsafe/failsafe.cpp +++ b/src/modules/commander/failsafe/failsafe.cpp @@ -509,6 +509,7 @@ bool Failsafe::isFailsafeIgnored(uint8_t user_intended_mode, int32_t exception_m case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET: case vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND: case vehicle_status_s::NAVIGATION_STATE_ORBIT: + case vehicle_status_s::NAVIGATION_STATE_GUIDED_COURSE: return exception_mask_parameter & (int)LinkLossExceptionBits::AutoModes; case vehicle_status_s::NAVIGATION_STATE_OFFBOARD: diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h index fc901d14922..fc6809dd072 100644 --- a/src/modules/commander/px4_custom_mode.h +++ b/src/modules/commander/px4_custom_mode.h @@ -66,6 +66,7 @@ enum PX4_CUSTOM_SUB_MODE_AUTO { PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET, PX4_CUSTOM_SUB_MODE_AUTO_PRECLAND, PX4_CUSTOM_SUB_MODE_AUTO_VTOL_TAKEOFF, + PX4_CUSTOM_SUB_MODE_GUIDED_COURSE, PX4_CUSTOM_SUB_MODE_EXTERNAL1, PX4_CUSTOM_SUB_MODE_EXTERNAL2, PX4_CUSTOM_SUB_MODE_EXTERNAL3, @@ -192,6 +193,11 @@ static inline union px4_custom_mode get_px4_custom_mode(uint8_t nav_state) custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_VTOL_TAKEOFF; break; + case vehicle_status_s::NAVIGATION_STATE_GUIDED_COURSE: + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_GUIDED_COURSE; + break; + case vehicle_status_s::NAVIGATION_STATE_EXTERNAL1: custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_EXTERNAL1; diff --git a/src/modules/fw_mode_manager/FixedWingModeManager.cpp b/src/modules/fw_mode_manager/FixedWingModeManager.cpp index 5202e5a913e..ce138f1d999 100644 --- a/src/modules/fw_mode_manager/FixedWingModeManager.cpp +++ b/src/modules/fw_mode_manager/FixedWingModeManager.cpp @@ -570,6 +570,12 @@ FixedWingModeManager::control_auto(const float control_interval, const Vector2d position_setpoint_s current_sp = pos_sp_curr; move_position_setpoint_for_vtol_transition(current_sp); + // Course setpoints are handled directly to avoid entering hold mode + if (PX4_ISFINITE(current_sp.course)) { + control_auto_position(control_interval, curr_pos, ground_speed, pos_sp_prev, current_sp); + return; + } + const uint8_t position_sp_type = handle_setpoint_type(current_sp, pos_sp_next); _position_sp_type = position_sp_type; @@ -768,6 +774,32 @@ void FixedWingModeManager::control_auto_position(const float control_interval, const Vector2d &curr_pos, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr) { + // Course Hold: if a course is explicitly set, navigate along that bearing (ground track) + if (PX4_ISFINITE(pos_sp_curr.course)) { + const float target_airspeed = pos_sp_curr.cruising_speed > FLT_EPSILON ? pos_sp_curr.cruising_speed : NAN; + + const Vector2f curr_pos_local{_local_pos.x, _local_pos.y}; + const DirectionalGuidanceOutput sp = navigateBearing(curr_pos_local, pos_sp_curr.course, ground_speed, _wind_vel); + + fixed_wing_lateral_setpoint_s lateral_ctrl_sp{empty_lateral_control_setpoint}; + lateral_ctrl_sp.timestamp = hrt_absolute_time(); + lateral_ctrl_sp.course = sp.course_setpoint; + lateral_ctrl_sp.lateral_acceleration = sp.lateral_acceleration_feedforward; + _lateral_ctrl_sp_pub.publish(lateral_ctrl_sp); + + const fixed_wing_longitudinal_setpoint_s fw_longitudinal_control_sp = { + .timestamp = hrt_absolute_time(), + .altitude = pos_sp_curr.alt, + .height_rate = NAN, + .equivalent_airspeed = target_airspeed, + .pitch_direct = NAN, + .throttle_direct = NAN + }; + + _longitudinal_ctrl_sp_pub.publish(fw_longitudinal_control_sp); + return; + } + const float acc_rad = _directional_guidance.switchDistance(500.0f); const float target_airspeed = pos_sp_curr.cruising_speed > FLT_EPSILON ? pos_sp_curr.cruising_speed : NAN; diff --git a/src/modules/mavlink/streams/UTM_GLOBAL_POSITION.hpp b/src/modules/mavlink/streams/UTM_GLOBAL_POSITION.hpp index b58852dbafa..01188b38588 100644 --- a/src/modules/mavlink/streams/UTM_GLOBAL_POSITION.hpp +++ b/src/modules/mavlink/streams/UTM_GLOBAL_POSITION.hpp @@ -144,6 +144,7 @@ private: || vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND || vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION || vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER + || vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_GUIDED_COURSE || vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF || vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL); diff --git a/src/modules/navigator/CMakeLists.txt b/src/modules/navigator/CMakeLists.txt index a0ba7e57c6b..6c160f99e65 100644 --- a/src/modules/navigator/CMakeLists.txt +++ b/src/modules/navigator/CMakeLists.txt @@ -44,6 +44,7 @@ set(NAVIGATOR_SOURCES mission_base.cpp mission_block.cpp mission.cpp + course.cpp loiter.cpp rtl.cpp rtl_direct.cpp diff --git a/src/modules/navigator/course.cpp b/src/modules/navigator/course.cpp new file mode 100644 index 00000000000..18c4c8e1800 --- /dev/null +++ b/src/modules/navigator/course.cpp @@ -0,0 +1,119 @@ +/**************************************************************************** + * + * Copyright (c) 2024 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file course.cpp + * + * Course mode: maintain constant course, altitude, and airspeed. + */ + +#include "course.h" +#include "navigator.h" + +#include + +Course::Course(Navigator *navigator) : + MissionBlock(navigator, vehicle_status_s::NAVIGATION_STATE_GUIDED_COURSE) +{ +} + +void +Course::on_activation() +{ + const vehicle_local_position_s *lpos = _navigator->get_local_position(); + + _altitude = _navigator->get_global_position()->alt; + + if (lpos->v_xy_valid) { + _course = matrix::wrap_2pi(atan2f(lpos->vy, lpos->vx)); + } + + _navigator->reset_cruising_speed(); + + update_setpoint_triplet(); +} + +void +Course::on_active() +{ +} + +bool +Course::set_course(float course_rad) +{ + if (!_navigator->get_local_position()->v_xy_valid) { + // No valid velocity estimate - cannot compute or maintain a ground track + return false; + } + + _course = course_rad; + update_setpoint_triplet(); + return true; +} + +void +Course::update_setpoint_triplet() +{ + position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + pos_sp_triplet->previous.valid = false; + + pos_sp_triplet->current.valid = true; + pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; + pos_sp_triplet->current.alt = _altitude; + + // Course mode: control ground track. + // lat/lon are not used for course guidance but FixedWingModeManager::set_control_mode_current() requires + // PX4_ISFINITE(lat) && PX4_ISFINITE(lon) to classify the setpoint as valid and enter FW_POSCTRL_MODE_AUTO. + // Use current position if available (including during dead-reckoning), otherwise dummy values. + if (_navigator->get_local_position()->xy_global) { + pos_sp_triplet->current.lat = _navigator->get_global_position()->lat; + pos_sp_triplet->current.lon = _navigator->get_global_position()->lon; + + } else { + pos_sp_triplet->current.lat = 0.0; + pos_sp_triplet->current.lon = 0.0; + } + + pos_sp_triplet->current.yaw = NAN; + pos_sp_triplet->current.course = _course; + + pos_sp_triplet->current.cruising_speed = _navigator->get_cruising_speed(); + pos_sp_triplet->current.cruising_throttle = NAN; + pos_sp_triplet->current.loiter_radius = NAN; + pos_sp_triplet->current.acceptance_radius = _navigator->get_acceptance_radius(); + pos_sp_triplet->current.timestamp = hrt_absolute_time(); + + pos_sp_triplet->next.valid = false; + + _navigator->set_position_setpoint_triplet_updated(); +} diff --git a/src/modules/navigator/course.h b/src/modules/navigator/course.h new file mode 100644 index 00000000000..81c5da83dc9 --- /dev/null +++ b/src/modules/navigator/course.h @@ -0,0 +1,73 @@ +/**************************************************************************** + * + * Copyright (c) 2024 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file course.h + * + * Course mode: maintain constant course, altitude, and airspeed. + * + * Accepts MAV_CMD_DO_CHANGE_ALTITUDE, MAV_CMD_GUIDED_CHANGE_HEADING and MAV_CMD_DO_CHANGE_SPEED commands. + * - MAV_CMD_GUIDED_CHANGE_HEADING (param1=HEADING_TYPE_COURSE_OVER_GROUND): sets course (ground track direction, requires valid position). + */ + +#pragma once + +#include "navigator_mode.h" +#include "mission_block.h" + +class Course : public MissionBlock +{ +public: + Course(Navigator *navigator); + ~Course() = default; + + void on_activation() override; + void on_active() override; + + /** + * Set course (ground track direction). Requires valid horizontal velocity. + * @param course_rad Course angle in radians, 0 = north + * @return true if horizontal velocity available and course set, false otherwise + */ + bool set_course(float course_rad); + + void set_altitude(float alt_amsl) { _altitude = alt_amsl; update_setpoint_triplet(); } + +private: + /** + * Update the position setpoint triplet to fly the current course. + */ + void update_setpoint_triplet(); + + float _course{0.f}; ///< [rad] current course bearing (ground track) + float _altitude{0.f}; ///< [m] AMSL altitude setpoint +}; diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 2a50d7a1c63..d19a551bb87 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -41,6 +41,7 @@ #pragma once +#include "course.h" #include "geofence.h" #include "land.h" #include "precland.h" @@ -98,7 +99,7 @@ using namespace time_literals; /** * Number of navigation modes that need on_active/on_inactive calls */ -#define NAVIGATOR_MODE_ARRAY_SIZE 8 +#define NAVIGATOR_MODE_ARRAY_SIZE 9 class Navigator : public ModuleBase, public ModuleParams { @@ -176,6 +177,7 @@ public: vehicle_status_s *get_vstatus() { return &_vstatus; } PrecLand *get_precland() { return &_precland; } /**< allow others, e.g. Mission, to use the precision land block */ + Course *get_course() { return &_course; } const PositionYawSetpoint &get_last_pos_with_gcs_heartbeat() const { return _last_pos_with_gcs_heartbeat; } @@ -386,6 +388,7 @@ private: Land _land; /**< class for handling land commands */ PrecLand _precland; /**< class for handling precision land commands */ RTL _rtl; /**< class that handles RTL */ + Course _course; /**< class that handles course */ #if CONFIG_NAVIGATOR_ADSB AdsbConflict _adsb_conflict; /**< class that handles ADSB conflict avoidance */ traffic_buffer_s _traffic_buffer{}; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 82319c638c4..5410cd9553b 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -80,7 +80,8 @@ Navigator::Navigator() : #endif //CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF _land(this), _precland(this), - _rtl(this) + _rtl(this), + _course(this) { /* Create a list of our possible navigation types */ _navigation_mode_array[0] = &_mission; @@ -92,6 +93,7 @@ Navigator::Navigator() : #if CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF _navigation_mode_array[6] = &_vtol_takeoff; #endif //CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF + _navigation_mode_array[7] = &_course; /* iterate through navigation modes and initialize _mission_item for each */ for (unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) { @@ -430,75 +432,120 @@ void Navigator::run() // only update the setpoint if armed, as it otherwise won't get executed until the vehicle switches to loiter, // which can lead to dangerous and unexpected behaviors (see loiter.cpp, there is an if(armed) in there too) - // A VEHICLE_CMD_DO_CHANGE_ALTITUDE has the exact same effect as a VEHICLE_CMD_DO_REPOSITION with only the altitude - // field populated, this logic is copied from above. + if (_navigation_mode == &_course) { + // In course mode, update altitude directly (after geofence check) + float new_alt = PX4_ISFINITE(cmd.param1) ? cmd.param1 : get_global_position()->alt; - // only supports MAV_FRAME_GLOBAL and MAV_FRAMEs with absolute altitude amsl + vehicle_global_position_s position_setpoint{}; + position_setpoint.lat = get_global_position()->lat; + position_setpoint.lon = get_global_position()->lon; + position_setpoint.alt = new_alt; - vehicle_global_position_s position_setpoint{}; - position_setpoint.lat = get_global_position()->lat; - position_setpoint.lon = get_global_position()->lon; - position_setpoint.alt = PX4_ISFINITE(cmd.param1) ? cmd.param1 : get_global_position()->alt; - - // Wait for vehicle_status before handling the next command, otherwise the setpoint could be overwritten - _wait_for_vehicle_status_timestamp = hrt_absolute_time(); - - if (geofence_allows_position(position_setpoint)) { - position_setpoint_triplet_s *rep = get_reposition_triplet(); - position_setpoint_triplet_s *curr = get_position_setpoint_triplet(); - - // store current position as previous position and goal as next - rep->previous.yaw = get_local_position()->heading; - rep->previous.lat = get_global_position()->lat; - rep->previous.lon = get_global_position()->lon; - rep->previous.alt = get_global_position()->alt; - - rep->current.type = position_setpoint_s::SETPOINT_TYPE_LOITER; - - // on entering Loiter mode, reset speed setpoint to default - if (_navigation_mode != &_loiter) { - rep->current.cruising_speed = -1.f; + if (geofence_allows_position(position_setpoint)) { + _course.set_altitude(new_alt); } else { - rep->current.cruising_speed = get_cruising_speed(); + mavlink_log_critical(&_mavlink_log_pub, "Altitude change is outside geofence\t"); + events::send(events::ID("navigator_course_change_altitude_outside_geofence"), {events::Log::Error, events::LogInternal::Info}, + "Altitude change is outside geofence"); } - rep->current.cruising_throttle = get_cruising_throttle(); - rep->current.acceptance_radius = get_acceptance_radius(); - rep->current.yaw = NAN; - - // Position is not changing, thus we keep the setpoint - rep->current.lat = PX4_ISFINITE(curr->current.lat) ? curr->current.lat : get_global_position()->lat; - rep->current.lon = PX4_ISFINITE(curr->current.lon) ? curr->current.lon : get_global_position()->lon; - - // set the altitude corresponding to command - rep->current.alt = PX4_ISFINITE(cmd.param1) ? cmd.param1 : get_global_position()->alt; - - if (PX4_ISFINITE(curr->current.loiter_radius) && curr->current.loiter_radius > FLT_EPSILON) { - rep->current.loiter_radius = curr->current.loiter_radius; - - } else { - rep->current.loiter_radius = get_default_loiter_rad(); - } - - rep->current.loiter_direction_counter_clockwise = curr->current.loiter_direction_counter_clockwise; - - rep->previous.timestamp = hrt_absolute_time(); - - rep->current.valid = true; - rep->current.timestamp = hrt_absolute_time(); - - rep->next.valid = false; - - _time_loitering_after_gf_breach = 0; // have to manually reset this in all LOITER cases + // DO_CHANGE_ALTITUDE is acknowledged by commander } else { - mavlink_log_critical(&_mavlink_log_pub, "Altitude change is outside geofence\t"); - events::send(events::ID("navigator_change_altitude_outside_geofence"), {events::Log::Error, events::LogInternal::Info}, - "Altitude change is outside geofence"); + + // A VEHICLE_CMD_DO_CHANGE_ALTITUDE has the exact same effect as a VEHICLE_CMD_DO_REPOSITION with only the altitude + // field populated, this logic is copied from above. + + // only supports MAV_FRAME_GLOBAL and MAV_FRAMEs with absolute altitude amsl + + vehicle_global_position_s position_setpoint{}; + position_setpoint.lat = get_global_position()->lat; + position_setpoint.lon = get_global_position()->lon; + position_setpoint.alt = PX4_ISFINITE(cmd.param1) ? cmd.param1 : get_global_position()->alt; + + // Wait for vehicle_status before handling the next command, otherwise the setpoint could be overwritten + _wait_for_vehicle_status_timestamp = hrt_absolute_time(); + + if (geofence_allows_position(position_setpoint)) { + position_setpoint_triplet_s *rep = get_reposition_triplet(); + position_setpoint_triplet_s *curr = get_position_setpoint_triplet(); + + // store current position as previous position and goal as next + rep->previous.yaw = get_local_position()->heading; + rep->previous.lat = get_global_position()->lat; + rep->previous.lon = get_global_position()->lon; + rep->previous.alt = get_global_position()->alt; + + rep->current.type = position_setpoint_s::SETPOINT_TYPE_LOITER; + + // on entering Loiter mode, reset speed setpoint to default + if (_navigation_mode != &_loiter) { + rep->current.cruising_speed = -1.f; + + } else { + rep->current.cruising_speed = get_cruising_speed(); + } + + rep->current.cruising_throttle = get_cruising_throttle(); + rep->current.acceptance_radius = get_acceptance_radius(); + rep->current.yaw = NAN; + + // Position is not changing, thus we keep the setpoint + rep->current.lat = PX4_ISFINITE(curr->current.lat) ? curr->current.lat : get_global_position()->lat; + rep->current.lon = PX4_ISFINITE(curr->current.lon) ? curr->current.lon : get_global_position()->lon; + + // set the altitude corresponding to command + rep->current.alt = PX4_ISFINITE(cmd.param1) ? cmd.param1 : get_global_position()->alt; + + if (PX4_ISFINITE(curr->current.loiter_radius) && curr->current.loiter_radius > FLT_EPSILON) { + rep->current.loiter_radius = curr->current.loiter_radius; + + } else { + rep->current.loiter_radius = get_default_loiter_rad(); + } + + rep->current.loiter_direction_counter_clockwise = curr->current.loiter_direction_counter_clockwise; + + rep->previous.timestamp = hrt_absolute_time(); + + rep->current.valid = true; + rep->current.timestamp = hrt_absolute_time(); + + rep->next.valid = false; + + _time_loitering_after_gf_breach = 0; // have to manually reset this in all LOITER cases + + } else { + mavlink_log_critical(&_mavlink_log_pub, "Altitude change is outside geofence\t"); + events::send(events::ID("navigator_change_altitude_outside_geofence"), {events::Log::Error, events::LogInternal::Info}, + "Altitude change is outside geofence"); + } + + // DO_CHANGE_ALTITUDE is acknowledged by commander + } // else (not course hold) + + } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_GUIDED_CHANGE_HEADING + && _vstatus.arming_state == vehicle_status_s::ARMING_STATE_ARMED) { + + uint8_t result{vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED}; + + // param1: heading type (0 = HEADING_TYPE_COURSE_OVER_GROUND) + // param2: target bearing [deg, 0=north] + const bool control_course = (lroundf(cmd.param1) == 0); + + if (_vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING + && _navigation_mode == &_course && control_course && PX4_ISFINITE(cmd.param2)) { + float course_rad = cmd.param2 * M_DEG_TO_RAD_F; + + if (_course.set_course(course_rad)) { + result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED; + } + + // DENIED if not FW, not in course mode, not correct heading type, or no positioning available } - // DO_CHANGE_ALTITUDE is acknowledged by commander + publish_vehicle_command_ack(cmd, result); } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_ORBIT && get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { @@ -807,6 +854,11 @@ void Navigator::run() navigation_mode_new = &_loiter; break; + case vehicle_status_s::NAVIGATION_STATE_GUIDED_COURSE: + _pos_sp_triplet_published_invalid_once = false; + navigation_mode_new = &_course; + break; + case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: // If we are already in mission landing, do not switch. @@ -1209,6 +1261,7 @@ void Navigator::reset_position_setpoint(position_setpoint_s &sp) sp.lat = static_cast(NAN); sp.lon = static_cast(NAN); sp.yaw = NAN; + sp.course = NAN; sp.loiter_radius = get_default_loiter_rad(); sp.acceptance_radius = get_default_acceptance_radius(); sp.cruising_speed = get_cruising_speed();