diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp index 4c5fbc8be7..606776aea8 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp @@ -170,7 +170,8 @@ bool FlightTaskAuto::update() waypoints[2] = _position_setpoint; } - const bool should_wait_for_yaw_align = _param_mpc_yaw_mode.get() == 4 && !_yaw_sp_aligned; + const bool should_wait_for_yaw_align = _param_mpc_yaw_mode.get() == int32_t(yaw_mode::towards_waypoint_yaw_first) + && !_yaw_sp_aligned; const bool force_zero_velocity_setpoint = should_wait_for_yaw_align || _is_emergency_braking_active; _updateTrajConstraints(); PositionSmoothing::PositionSmoothingSetpoints smoothed_setpoints; @@ -532,32 +533,37 @@ void FlightTaskAuto::_set_heading_from_mode() Vector2f v; // Vector that points towards desired location - switch (_param_mpc_yaw_mode.get()) { + switch (yaw_mode(_param_mpc_yaw_mode.get())) { - case 0: // Heading points towards the current waypoint. - case 4: // Same as 0 but yaw first and then go + case yaw_mode::towards_waypoint: // Heading points towards the current waypoint. + case yaw_mode::towards_waypoint_yaw_first: // Same as 0 but yaw first and then go v = Vector2f(_target) - Vector2f(_position); break; - case 1: // Heading points towards home. + case yaw_mode::towards_home: // Heading points towards home. if (_sub_home_position.get().valid_lpos) { v = Vector2f(&_sub_home_position.get().x) - Vector2f(_position); } break; - case 2: // Heading point away from home. + case yaw_mode::away_from_home: // Heading point away from home. if (_sub_home_position.get().valid_lpos) { v = Vector2f(_position) - Vector2f(&_sub_home_position.get().x); } break; - case 3: // Along trajectory. + case yaw_mode::along_trajectory: // Along trajectory. // The heading depends on the kind of setpoint generation. This needs to be implemented // in the subclasses where the velocity setpoints are generated. v.setAll(NAN); break; + + case yaw_mode::yaw_fixed: // Yaw fixed. + // Yaw is operated via manual control or MAVLINK messages. + break; + } if (v.isAllFinite()) { diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp index fc671cd5e5..90a98cac23 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp @@ -81,6 +81,15 @@ enum class State { none /**< Vehicle is in normal tracking mode from triplet previous to triplet target */ }; +enum class yaw_mode : int32_t { + towards_waypoint = 0, + towards_home = 1, + away_from_home = 2, + along_trajectory = 3, + towards_waypoint_yaw_first = 4, + yaw_fixed = 5, +}; + class FlightTaskAuto : public FlightTask { public: diff --git a/src/modules/mc_pos_control/multicopter_autonomous_params.c b/src/modules/mc_pos_control/multicopter_autonomous_params.c index 1bfc336fce..8c97e779be 100644 --- a/src/modules/mc_pos_control/multicopter_autonomous_params.c +++ b/src/modules/mc_pos_control/multicopter_autonomous_params.c @@ -172,6 +172,7 @@ PARAM_DEFINE_FLOAT(MPC_YAWRAUTO_ACC, 60.f); * @value 2 away from home * @value 3 along trajectory * @value 4 towards waypoint (yaw first) + * @value 5 yaw fixed * @group Mission */ PARAM_DEFINE_INT32(MPC_YAW_MODE, 0);