diff --git a/msg/PositionSetpoint.msg b/msg/PositionSetpoint.msg index 90cb447274..93bb676197 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 380c931c97..1abfe20e1b 100644 --- a/msg/versioned/VehicleCommand.msg +++ b/msg/versioned/VehicleCommand.msg @@ -127,6 +127,7 @@ uint8 FUSION_SOURCE_RNGBCN = 8 # Ranging Beacon uint32 VEHICLE_CMD_PX4_INTERNAL_START = 65537 # Start of PX4 internal only vehicle commands (> UINT16_MAX). 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| +uint32 VEHICLE_CMD_DO_CHANGE_COURSE = 100002 # Change the course setpoint in Course Hold mode. |[rad] Course (bearing) [-PI..PI, NaN = no change]|Unused|Unused|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. diff --git a/msg/versioned/VehicleStatus.msg b/msg/versioned/VehicleStatus.msg index 28cbe379e9..cb79980c5b 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_AUTO_COURSE_HOLD = 7 # Course hold 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 25af281509..8397ddcfcf 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_hold", + "description": "Course Hold" + }, "255": { "name": "unknown", "description": "[Unknown]" diff --git a/src/lib/modes/ui.hpp b/src/lib/modes/ui.hpp index fe2e5a9689..f0088a39f9 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_AUTO_COURSE_HOLD) | (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", + "Course Hold", "Altitude Cruise", "9: unallocated", "Acro", diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 42c7e79732..4292210757 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_hold")) { + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO, + PX4_CUSTOM_SUB_MODE_AUTO_COURSE_HOLD); + } 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); @@ -890,17 +894,28 @@ Commander::handle_command(const vehicle_command_s &cmd) // 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 already in course hold, stay in course hold (navigator handles the altitude update) + uint8_t target_state = (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_COURSE_HOLD) + ? vehicle_status_s::NAVIGATION_STATE_AUTO_COURSE_HOLD + : vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER; + + if (_user_mode_intention.change(target_state)) { 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; } } break; + case vehicle_command_s::VEHICLE_CMD_DO_CHANGE_COURSE: { + // DO_CHANGE_COURSE is only valid in course hold mode, navigator handles the actual course update + cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED; + } + break; + case vehicle_command_s::VEHICLE_CMD_DO_SET_MODE: { uint8_t base_mode = (uint8_t)cmd.param1; uint8_t custom_main_mode = (uint8_t)cmd.param2; @@ -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_AUTO_COURSE_HOLD: + desired_nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_COURSE_HOLD; + 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; diff --git a/src/modules/commander/ModeUtil/control_mode.cpp b/src/modules/commander/ModeUtil/control_mode.cpp index 45b01c6ec5..06c62726fb 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_AUTO_COURSE_HOLD: 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 e6bb4e2102..f2ae701fe0 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_AUTO_COURSE_HOLD: return navigation_mode_t::auto_course_hold; + 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 850a0cf6e9..6935beaa9d 100644 --- a/src/modules/commander/ModeUtil/mode_requirements.cpp +++ b/src/modules/commander/ModeUtil/mode_requirements.cpp @@ -123,6 +123,22 @@ 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_AUTO_COURSE_HOLD (same requirements as AUTO_LOITER) + setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_COURSE_HOLD, flags.mode_req_angular_velocity); + setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_COURSE_HOLD, flags.mode_req_attitude); + + if (vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_COURSE_HOLD, flags.mode_req_global_position_relaxed); + setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_COURSE_HOLD, flags.mode_req_local_position_relaxed); + + } else { + setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_COURSE_HOLD, flags.mode_req_global_position); + setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_COURSE_HOLD, flags.mode_req_local_position); + } + + setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_COURSE_HOLD, flags.mode_req_local_alt); + setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_COURSE_HOLD, flags.mode_req_wind_and_flight_time_compliance); + // 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/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h index fc901d1492..9d22c7ee06 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_AUTO_COURSE_HOLD, 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_AUTO_COURSE_HOLD: + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_COURSE_HOLD; + 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 5202e5a913..64e51bb264 100644 --- a/src/modules/fw_mode_manager/FixedWingModeManager.cpp +++ b/src/modules/fw_mode_manager/FixedWingModeManager.cpp @@ -768,6 +768,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 + 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 b58852dbaf..a48e135f6c 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_AUTO_COURSE_HOLD || 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 a0ba7e57c6..1a658c37da 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_hold.cpp loiter.cpp rtl.cpp rtl_direct.cpp diff --git a/src/modules/navigator/course_hold.cpp b/src/modules/navigator/course_hold.cpp new file mode 100644 index 0000000000..1123edffc8 --- /dev/null +++ b/src/modules/navigator/course_hold.cpp @@ -0,0 +1,92 @@ +/**************************************************************************** + * + * 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_hold.cpp + * + * Course Hold mode: maintain constant course, altitude, and airspeed. + */ + +#include "course_hold.h" +#include "navigator.h" + +CourseHold::CourseHold(Navigator *navigator) : + MissionBlock(navigator, vehicle_status_s::NAVIGATION_STATE_AUTO_COURSE_HOLD) +{ +} + +void +CourseHold::on_activation() +{ + // Capture current state + _course = _navigator->get_local_position()->heading; + _altitude = _navigator->get_global_position()->alt; + _airspeed = -1.f; // default airspeed + + _navigator->reset_cruising_speed(); + + update_course_setpoint(); +} + +void +CourseHold::on_active() +{ + // Check for incoming vehicle commands + // Commands are dispatched from navigator_main, which sets fields on this object + // before calling on_active(). Nothing to poll here - setpoint updates are + // triggered by set_course(), set_altitude(), set_airspeed() calls from navigator_main. +} + +void +CourseHold::update_course_setpoint() +{ + 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.lat = _navigator->get_global_position()->lat; + pos_sp_triplet->current.lon = _navigator->get_global_position()->lon; + pos_sp_triplet->current.alt = _altitude; + pos_sp_triplet->current.course = _course; + pos_sp_triplet->current.cruising_speed = _airspeed; + pos_sp_triplet->current.cruising_throttle = NAN; + pos_sp_triplet->current.yaw = 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_hold.h b/src/modules/navigator/course_hold.h new file mode 100644 index 0000000000..d04f7ed03c --- /dev/null +++ b/src/modules/navigator/course_hold.h @@ -0,0 +1,68 @@ +/**************************************************************************** + * + * 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_hold.h + * + * Course Hold mode: maintain constant course, altitude, and airspeed. + * Accepts DO_CHANGE_COURSE, DO_CHANGE_ALTITUDE, and DO_CHANGE_SPEED commands. + */ + +#pragma once + +#include "navigator_mode.h" +#include "mission_block.h" + +class CourseHold : public MissionBlock +{ +public: + CourseHold(Navigator *navigator); + ~CourseHold() = default; + + void on_activation() override; + void on_active() override; + + void set_course(float course_rad) { _course = course_rad; update_course_setpoint(); } + void set_altitude(float alt_amsl) { _altitude = alt_amsl; update_course_setpoint(); } + void set_airspeed(float airspeed) { _airspeed = airspeed; update_course_setpoint(); } + +private: + /** + * Update the position setpoint triplet to fly the current course. + * Places a waypoint far ahead along the course bearing from the current position. + */ + void update_course_setpoint(); + + float _course{0.f}; ///< [rad] current course bearing + float _altitude{0.f}; ///< [m] AMSL altitude setpoint + float _airspeed{-1.f}; ///< [m/s] cruising speed setpoint (-1 = default) +}; diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 2a50d7a1c6..904d56e1bf 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -41,6 +41,7 @@ #pragma once +#include "course_hold.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 */ + CourseHold *get_course_hold() { return &_course_hold; } 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 */ + CourseHold _course_hold; /**< class that handles course hold */ #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 82319c638c..3e0abe30b0 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_hold(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_hold; /* iterate through navigation modes and initialize _mission_item for each */ for (unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) { @@ -430,75 +432,93 @@ 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. - - // 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 + if (_navigation_mode == &_course_hold) { + // In course hold mode, update altitude directly + float new_alt = PX4_ISFINITE(cmd.param1) ? cmd.param1 : get_global_position()->alt; + _course_hold.set_altitude(new_alt); + // 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_DO_CHANGE_COURSE + && _vstatus.arming_state == vehicle_status_s::ARMING_STATE_ARMED) { + + if (_navigation_mode == &_course_hold && PX4_ISFINITE(cmd.param1)) { + _course_hold.set_course(cmd.param1); } - // DO_CHANGE_ALTITUDE is acknowledged by commander + publish_vehicle_command_ack(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_ORBIT && get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { @@ -703,9 +723,17 @@ void Navigator::run() // XXX not differentiating ground and airspeed yet set_cruising_speed(cmd.param2); + if (_navigation_mode == &_course_hold) { + _course_hold.set_airspeed(cmd.param2); + } + } else { reset_cruising_speed(); + if (_navigation_mode == &_course_hold) { + _course_hold.set_airspeed(-1.f); + } + /* if no speed target was given try to set throttle */ if (cmd.param3 > FLT_EPSILON) { set_cruising_throttle(cmd.param3 / 100); @@ -807,6 +835,11 @@ void Navigator::run() navigation_mode_new = &_loiter; break; + case vehicle_status_s::NAVIGATION_STATE_AUTO_COURSE_HOLD: + _pos_sp_triplet_published_invalid_once = false; + navigation_mode_new = &_course_hold; + break; + case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: // If we are already in mission landing, do not switch.