diff --git a/msg/versioned/VehicleCommand.msg b/msg/versioned/VehicleCommand.msg index 37e74ab83f..3f202fa8f2 100644 --- a/msg/versioned/VehicleCommand.msg +++ b/msg/versioned/VehicleCommand.msg @@ -126,7 +126,8 @@ 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 mode. |[rad] Course (bearing) [-PI..PI, NaN = no change]|Unused|Unused|Unused|Unused|Unused|Unused| + +uint16 VEHICLE_CMD_DO_CHANGE_COURSE = 43007 # Change the course setpoint in Course mode. |[deg] Course (bearing) [0..360, 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/src/modules/commander/ModeUtil/mode_requirements.cpp b/src/modules/commander/ModeUtil/mode_requirements.cpp index 245147172a..bd47dca5cf 100644 --- a/src/modules/commander/ModeUtil/mode_requirements.cpp +++ b/src/modules/commander/ModeUtil/mode_requirements.cpp @@ -123,19 +123,11 @@ 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 (same requirements as AUTO_LOITER) + // NAVIGATION_STATE_AUTO_COURSE + // Heading mode is GPS-independent, course mode internally checks for GPS availability. + // Only require attitude and altitude, not global position. setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_COURSE, flags.mode_req_angular_velocity); setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_COURSE, flags.mode_req_attitude); - - if (vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { - setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_COURSE, flags.mode_req_global_position_relaxed); - setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_COURSE, flags.mode_req_local_position_relaxed); - - } else { - setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_COURSE, flags.mode_req_global_position); - setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_COURSE, flags.mode_req_local_position); - } - setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_COURSE, flags.mode_req_local_alt); setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_COURSE, flags.mode_req_wind_and_flight_time_compliance); diff --git a/src/modules/fw_mode_manager/FixedWingModeManager.cpp b/src/modules/fw_mode_manager/FixedWingModeManager.cpp index 64e51bb264..30aa44065d 100644 --- a/src/modules/fw_mode_manager/FixedWingModeManager.cpp +++ b/src/modules/fw_mode_manager/FixedWingModeManager.cpp @@ -768,7 +768,7 @@ 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 + // 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; @@ -794,6 +794,28 @@ FixedWingModeManager::control_auto_position(const float control_interval, const return; } + // Heading Hold: if yaw is explicitly set (no course), control airspeed direction directly (GPS-independent) + if (PX4_ISFINITE(pos_sp_curr.yaw)) { + const float target_airspeed = pos_sp_curr.cruising_speed > FLT_EPSILON ? pos_sp_curr.cruising_speed : NAN; + + fixed_wing_lateral_setpoint_s lateral_ctrl_sp{empty_lateral_control_setpoint}; + lateral_ctrl_sp.timestamp = hrt_absolute_time(); + lateral_ctrl_sp.airspeed_direction = pos_sp_curr.yaw; + _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/navigator/course.cpp b/src/modules/navigator/course.cpp index 5fa438f174..9f79cc1e9a 100644 --- a/src/modules/navigator/course.cpp +++ b/src/modules/navigator/course.cpp @@ -33,7 +33,7 @@ /** * @file course.cpp * - * Course mode: maintain constant course, altitude, and airspeed. + * Course/Heading mode: maintain constant course or heading, altitude, and airspeed. */ #include "course.h" @@ -48,10 +48,21 @@ void Course::on_activation() { // Capture current state - _course = _navigator->get_local_position()->heading; + _heading = _navigator->get_local_position()->heading; _altitude = _navigator->get_global_position()->alt; _airspeed = -1.f; // default airspeed + // Select best mode based on GPS availability + if (_navigator->gps_position_valid()) { + // GPS available: use course mode (ground track control) + _course = _navigator->get_local_position()->heading; + _use_heading = false; + + } else { + // No GPS: use heading mode (nose direction only) + _use_heading = true; + } + _navigator->reset_cruising_speed(); update_course_setpoint(); @@ -63,7 +74,21 @@ Course::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. + // triggered by set_course(), set_heading(), set_altitude(), set_airspeed() calls from navigator_main. +} + +bool +Course::set_course(float course_rad) +{ + if (!_navigator->gps_position_valid()) { + // No valid GPS - cannot use course mode + return false; + } + + _course = course_rad; + _use_heading = false; + update_course_setpoint(); + return true; } void @@ -75,13 +100,35 @@ Course::update_course_setpoint() 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; + + if (_use_heading) { + // Heading mode: control nose direction (yaw) + // Use dummy lat/lon if GPS not available - they're not used for heading control + // but need to be finite for the setpoint to be considered valid + 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 { + // No GPS - use dummy position (not used in heading mode anyway) + pos_sp_triplet->current.lat = 0.0; + pos_sp_triplet->current.lon = 0.0; + } + + pos_sp_triplet->current.yaw = _heading; + pos_sp_triplet->current.course = NAN; + + } else { + // Course mode: control ground track (requires GPS, checked by set_course()) + pos_sp_triplet->current.lat = _navigator->get_global_position()->lat; + pos_sp_triplet->current.lon = _navigator->get_global_position()->lon; + pos_sp_triplet->current.yaw = NAN; + 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(); @@ -90,3 +137,5 @@ Course::update_course_setpoint() _navigator->set_position_setpoint_triplet_updated(); } + + diff --git a/src/modules/navigator/course.h b/src/modules/navigator/course.h index 321143d6e7..6b71d90f3f 100644 --- a/src/modules/navigator/course.h +++ b/src/modules/navigator/course.h @@ -33,9 +33,12 @@ /** * @file course.h * - * Course mode: maintain constant course, altitude, and airspeed. - * Accepts DO_CHANGE_COURSE, CONDITION_YAW, DO_CHANGE_ALTITUDE, and DO_CHANGE_SPEED commands. - * For Condition YAW, only the Angle and relative/absolute fields are supported. + * Course/Heading mode: maintain constant course or heading, altitude, and airspeed. + * On activation, tracks course if GPS available, otherwise heading. + * + * Accepts MAV_CMD_DO_CHANGE_ALTITUDE, DO_CHANGE_COURSE, CONDITION_YAW and DO_CHANGE_SPEED commands. + * - DO_CHANGE_COURSE: sets course (ground track direction, requires GPS). + * - CONDITION_YAW: sets heading (airspeed direction). */ #pragma once @@ -52,18 +55,31 @@ public: void on_activation() override; void on_active() override; - void set_course(float course_rad) { _course = course_rad; update_course_setpoint(); } + /** + * Set course (ground track direction). Requires GPS. + * @param course_rad Course angle in radians, 0 = north + * @return true if GPS available and course set, false otherwise + */ + bool set_course(float course_rad); + + /** + * Set heading (nose/airspeed direction). GPS-independent. + * @param heading_rad Heading angle in radians, 0 = north + */ + void set_heading(float heading_rad) { _heading = heading_rad; _use_heading = true; 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. + * Update the position setpoint triplet to fly the current course or heading. */ void update_course_setpoint(); - float _course{0.f}; ///< [rad] current course bearing + float _course{0.f}; ///< [rad] current course bearing (ground track) + float _heading{0.f}; ///< [rad] current heading setpoint (nose direction) + bool _use_heading{false}; ///< true: control heading (yaw), false: control course 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 d19a551bb8..d444f3d252 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -189,6 +189,11 @@ public: bool home_global_position_valid() { return (_home_pos.valid_alt && _home_pos.valid_hpos); } + /** + * Check if GPS position is valid (has recent 3D fix) + */ + bool gps_position_valid() { return (_gps_pos.fix_type >= 3) && (hrt_elapsed_time(&_gps_pos.timestamp) < 2_s); } + Geofence &get_geofence() { return _geofence; } float get_default_loiter_rad() { return fabsf(_param_nav_loiter_rad.get()); } diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 0a183aaf53..51dcc60695 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -514,29 +514,41 @@ void Navigator::run() } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_CHANGE_COURSE && _vstatus.arming_state == vehicle_status_s::ARMING_STATE_ARMED) { + uint8_t result{vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED}; + if (_navigation_mode == &_course && PX4_ISFINITE(cmd.param1)) { - _course.set_course(cmd.param1); + // param1: course angle [deg], 0 = north, converted to radians + float course_rad = cmd.param1 * M_DEG_TO_RAD_F; + + if (_course.set_course(course_rad)) { + result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED; + } + + // DENIED if no GPS available for course mode } - publish_vehicle_command_ack(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); + publish_vehicle_command_ack(cmd, result); } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_CONDITION_YAW && _vstatus.arming_state == vehicle_status_s::ARMING_STATE_ARMED) { + uint8_t result{vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED}; + if (_navigation_mode == &_course && PX4_ISFINITE(cmd.param1)) { + // CONDITION_YAW sets heading (airspeed direction), not course // param1: target angle [deg], 0 = north // param4: 0 = absolute, 1 = relative to current heading - float course_rad = cmd.param1 * M_DEG_TO_RAD_F; + float heading_rad = cmd.param1 * M_DEG_TO_RAD_F; - if (cmd.param4 > 0.5f) { - // Relative: add to current course - course_rad = matrix::wrap_2pi(get_local_position()->heading + course_rad); + if (static_cast(cmd.param4) > 0.5f) { + // Relative: add to current heading + heading_rad = matrix::wrap_2pi(get_local_position()->heading + heading_rad); } - _course.set_course(course_rad); + _course.set_heading(heading_rad); } - publish_vehicle_command_ack(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); + 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) {