From 51ebb0f2949fc8f40fe6306365e825313c73d5e0 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 15 Nov 2021 20:51:21 +0100 Subject: [PATCH] FlightTaskAuto: move in helper methods from FlightTaskAutoLineSmoothVel --- .../tasks/Auto/FlightTaskAuto.cpp | 259 +++++++++++++++++ .../tasks/Auto/FlightTaskAuto.hpp | 26 ++ .../FlightTaskAutoLineSmoothVel.cpp | 262 ------------------ .../FlightTaskAutoLineSmoothVel.hpp | 32 --- 4 files changed, 285 insertions(+), 294 deletions(-) diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp index 1c03fe7413..36ed0f3f67 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp @@ -512,3 +512,262 @@ bool FlightTaskAuto::_compute_heading_from_2D_vector(float &heading, Vector2f v) // heading unknown and therefore do not change heading return false; } + +/** + * EKF reset handling functions + * Those functions are called by the base FlightTask in + * case of an EKF reset event + */ +void FlightTaskAuto::_ekfResetHandlerPositionXY(const matrix::Vector2f &delta_xy) +{ + _position_smoothing.forceSetPosition({_position(0), _position(1), NAN}); +} + +void FlightTaskAuto::_ekfResetHandlerVelocityXY(const matrix::Vector2f &delta_vxy) +{ + _position_smoothing.forceSetVelocity({_velocity(0), _velocity(1), NAN}); +} + +void FlightTaskAuto::_ekfResetHandlerPositionZ(float delta_z) +{ + _position_smoothing.forceSetPosition({NAN, NAN, _position(2)}); +} + +void FlightTaskAuto::_ekfResetHandlerVelocityZ(float delta_vz) +{ + _position_smoothing.forceSetVelocity({NAN, NAN, _velocity(2)}); +} + +void FlightTaskAuto::_ekfResetHandlerHeading(float delta_psi) +{ + _yaw_sp_prev += delta_psi; +} + +void FlightTaskAuto::_generateSetpoints() +{ + _checkEmergencyBraking(); + Vector3f waypoints[] = {_prev_wp, _position_setpoint, _next_wp}; + + if (isTargetModified()) { + // In case object avoidance has injected a new setpoint, we take this as the next waypoints + waypoints[2] = _position_setpoint; + } + + const bool should_wait_for_yaw_align = _param_mpc_yaw_mode.get() == 4 && !_yaw_sp_aligned; + _updateTrajConstraints(); + PositionSmoothing::PositionSmoothingSetpoints smoothed_setpoints; + _position_smoothing.generateSetpoints( + _position, + waypoints, + _velocity_setpoint, + _deltatime, + should_wait_for_yaw_align, + smoothed_setpoints + ); + + _jerk_setpoint = smoothed_setpoints.jerk; + _acceleration_setpoint = smoothed_setpoints.acceleration; + _velocity_setpoint = smoothed_setpoints.velocity; + _position_setpoint = smoothed_setpoints.position; + + _unsmoothed_velocity_setpoint = smoothed_setpoints.unsmoothed_velocity; + _want_takeoff = smoothed_setpoints.unsmoothed_velocity(2) < -0.3f; + + if (!PX4_ISFINITE(_yaw_setpoint) && !PX4_ISFINITE(_yawspeed_setpoint)) { + // no valid heading -> generate heading in this flight task + _generateHeading(); + } +} + +void FlightTaskAuto::_generateHeading() +{ + // Generate heading along trajectory if possible, otherwise hold the previous yaw setpoint + if (!_generateHeadingAlongTraj()) { + _yaw_setpoint = _yaw_sp_prev; + } +} + +void FlightTaskAuto::_checkEmergencyBraking() +{ + if (!_is_emergency_braking_active) { + if (_position_smoothing.getCurrentVelocityZ() > (2.f * _param_mpc_z_vel_max_dn.get())) { + _is_emergency_braking_active = true; + } + + } else { + if (fabsf(_position_smoothing.getCurrentVelocityZ()) < 0.01f) { + _is_emergency_braking_active = false; + } + } +} + +bool FlightTaskAuto::_generateHeadingAlongTraj() +{ + bool res = false; + Vector2f vel_sp_xy(_velocity_setpoint); + Vector2f traj_to_target = Vector2f(_target) - Vector2f(_position); + + if ((vel_sp_xy.length() > .1f) && + (traj_to_target.length() > 2.f)) { + // Generate heading from velocity vector, only if it is long enough + // and if the drone is far enough from the target + _compute_heading_from_2D_vector(_yaw_setpoint, vel_sp_xy); + res = true; + } + + return res; +} + +bool FlightTaskAuto::isTargetModified() const +{ + const bool xy_modified = (_target - _position_setpoint).xy().longerThan(FLT_EPSILON); + const bool z_valid = PX4_ISFINITE(_position_setpoint(2)); + const bool z_modified = z_valid && fabs((_target - _position_setpoint)(2)) > FLT_EPSILON; + + return xy_modified || z_modified; +} + +void FlightTaskAuto::_updateTrajConstraints() +{ + // update params of the position smoothing + _position_smoothing.setMaxAllowedHorizontalError(_param_mpc_xy_err_max.get()); + _position_smoothing.setVerticalAcceptanceRadius(_param_nav_mc_alt_rad.get()); + _position_smoothing.setCruiseSpeed(_mc_cruise_speed); + _position_smoothing.setHorizontalTrajectoryGain(_param_mpc_xy_traj_p.get()); + _position_smoothing.setTargetAcceptanceRadius(_target_acceptance_radius); + + // Update the constraints of the trajectories + _position_smoothing.setMaxAccelerationXY(_param_mpc_acc_hor.get()); // TODO : Should be computed using heading + _position_smoothing.setMaxVelocityXY(_param_mpc_xy_vel_max.get()); + float max_jerk = _param_mpc_jerk_auto.get(); + _position_smoothing.setMaxJerk({max_jerk, max_jerk, max_jerk}); // TODO : Should be computed using heading + + if (_is_emergency_braking_active) { + // When initializing with large downward velocity, allow 1g of vertical + // acceleration for fast braking + _position_smoothing.setMaxAccelerationZ(9.81f); + _position_smoothing.setMaxJerkZ(9.81f); + + // If the current velocity is beyond the usual constraints, tell + // the controller to exceptionally increase its saturations to avoid + // cutting out the feedforward + _constraints.speed_down = math::max(fabsf(_position_smoothing.getCurrentVelocityZ()), _param_mpc_z_vel_max_dn.get()); + + } else if (_unsmoothed_velocity_setpoint(2) < 0.f) { // up + float z_accel_constraint = _param_mpc_acc_up_max.get(); + float z_vel_constraint = _param_mpc_z_vel_max_up.get(); + + // The constraints are broken because they are used as hard limits by the position controller, so put this here + // until the constraints don't do things like cause controller integrators to saturate. Once the controller + // doesn't use z speed constraints, this can go in _prepareTakeoffSetpoints(). Accel limit is to + // emulate the motor ramp (also done in the controller) so that the controller can actually track the setpoint. + if (_type == WaypointType::takeoff && _dist_to_ground < _param_mpc_land_alt1.get()) { + z_vel_constraint = _param_mpc_tko_speed.get(); + z_accel_constraint = math::min(z_accel_constraint, _param_mpc_tko_speed.get() / _param_mpc_tko_ramp_t.get()); + + // Keep the altitude setpoint at the current altitude + // to avoid having it going down into the ground during + // the initial ramp as the velocity does not start at 0 + _position_smoothing.forceSetPosition({NAN, NAN, _position(2)}); + } + + _position_smoothing.setMaxVelocityZ(z_vel_constraint); + _position_smoothing.setMaxAccelerationZ(z_accel_constraint); + + } else { // down + _position_smoothing.setMaxAccelerationZ(_param_mpc_acc_down_max.get()); + _position_smoothing.setMaxVelocityZ(_param_mpc_z_vel_max_dn.get()); + } +} + +void FlightTaskAuto::_prepareIdleSetpoints() +{ + // Send zero thrust setpoint + _position_setpoint.setNaN(); // Don't require any position/velocity setpoints + _velocity_setpoint.setNaN(); + _acceleration_setpoint = Vector3f(0.f, 0.f, 100.f); // High downwards acceleration to make sure there's no thrust +} + +void FlightTaskAuto::_prepareLandSetpoints() +{ + _velocity_setpoint.setNaN(); // Don't take over any smoothed velocity setpoint + + // Slow down automatic descend close to ground + float land_speed = math::gradual(_dist_to_ground, + _param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(), + _param_mpc_land_speed.get(), _constraints.speed_down); + + if (_type_previous != WaypointType::land) { + // initialize xy-position and yaw to waypoint such that home is reached exactly without user input + _land_position = Vector3f(_target(0), _target(1), NAN); + _land_heading = _yaw_setpoint; + _stick_acceleration_xy.resetPosition(Vector2f(_target(0), _target(1))); + } + + // User input assisted landing + if (_param_mpc_land_rc_help.get() && _sticks.checkAndSetStickInputs()) { + // Stick full up -1 -> stop, stick full down 1 -> double the speed + land_speed *= (1 + _sticks.getPositionExpo()(2)); + + _stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _land_heading, + _sticks.getPositionExpo()(3) * math::radians(_param_mpc_man_y_max.get()), _yaw, _deltatime); + _stick_acceleration_xy.generateSetpoints(_sticks.getPositionExpo().slice<2, 1>(0, 0), _yaw, _land_heading, _position, + _velocity_setpoint_feedback.xy(), _deltatime); + _stick_acceleration_xy.getSetpoints(_land_position, _velocity_setpoint, _acceleration_setpoint); + + // Hack to make sure the MPC_YAW_MODE 4 alignment doesn't stop the vehicle from descending when there's yaw input + if (fabsf(_yawspeed_setpoint) > FLT_EPSILON) { + _yaw_sp_aligned = true; + } + + } else { + // Make sure we have a valid land position even in the case we loose RC while amending it + if (!PX4_ISFINITE(_land_position(0))) { + _land_position.xy() = Vector2f(_position); + } + } + + _position_setpoint = _land_position; // The last element of the land position has to stay NAN + _yaw_setpoint = _land_heading; + _velocity_setpoint(2) = land_speed; + _gear.landing_gear = landing_gear_s::GEAR_DOWN; +} + +void FlightTaskAuto::_prepareTakeoffSetpoints() +{ + // Takeoff is completely defined by target position + _position_setpoint = _target; + _velocity_setpoint = Vector3f(NAN, NAN, NAN); + + _gear.landing_gear = landing_gear_s::GEAR_DOWN; +} + +void FlightTaskAuto::_prepareVelocitySetpoints() +{ + // XY Velocity waypoint + // TODO : Rewiew that. What is the expected behavior? + _position_setpoint = Vector3f(NAN, NAN, _position(2)); + Vector2f vel_sp_xy = Vector2f(_velocity).unit_or_zero() * _mc_cruise_speed; + _velocity_setpoint = Vector3f(vel_sp_xy(0), vel_sp_xy(1), NAN); +} + +void FlightTaskAuto::_preparePositionSetpoints() +{ + // Simple waypoint navigation: go to xyz target, with standard limitations + _position_setpoint = _target; + _velocity_setpoint.setNaN(); // No special velocity limitations +} + +bool FlightTaskAuto::_highEnoughForLandingGear() +{ + // return true if altitude is above two meters + return _dist_to_ground > 2.0f; +} + +void FlightTaskAuto::updateParams() +{ + FlightTask::updateParams(); + + // make sure that alt1 is above alt2 + _param_mpc_land_alt1.set(math::max(_param_mpc_land_alt1.get(), _param_mpc_land_alt2.get())); +} diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp index 1485e65ec1..e46d60e791 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp @@ -101,6 +101,32 @@ protected: void _updateInternalWaypoints(); /**< Depending on state of vehicle, the internal waypoints might differ from target (for instance if offtrack). */ bool _compute_heading_from_2D_vector(float &heading, matrix::Vector2f v); /**< Computes and sets heading a 2D vector */ + /** Reset position or velocity setpoints in case of EKF reset event */ + void _ekfResetHandlerPositionXY(const matrix::Vector2f &delta_xy) override; + void _ekfResetHandlerVelocityXY(const matrix::Vector2f &delta_vxy) override; + void _ekfResetHandlerPositionZ(float delta_z) override; + void _ekfResetHandlerVelocityZ(float delta_vz) override; + void _ekfResetHandlerHeading(float delta_psi) override; + + void _generateSetpoints(); /**< Generate setpoints along line. */ + void _generateHeading(); + void _checkEmergencyBraking(); + bool _generateHeadingAlongTraj(); /**< Generates heading along trajectory. */ + bool isTargetModified() const; + void _updateTrajConstraints(); + + /** determines when to trigger a takeoff (ignored in flight) */ + bool _checkTakeoff() override { return _want_takeoff; }; + + void _prepareIdleSetpoints(); + void _prepareLandSetpoints(); + void _prepareVelocitySetpoints(); + void _prepareTakeoffSetpoints(); + void _preparePositionSetpoints(); + bool _highEnoughForLandingGear(); /**< Checks if gears can be lowered. */ + + void updateParams() override; /**< See ModuleParam class */ + matrix::Vector3f _prev_prev_wp{}; /**< Pre-previous waypoint (local frame). This will be used for smoothing trajectories -> not used yet. */ matrix::Vector3f _prev_wp{}; /**< Previous waypoint (local frame). If no previous triplet is available, the prev_wp is set to current position. */ bool _prev_was_valid{false}; diff --git a/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp b/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp index da1919a1c9..6c044a7a4c 100644 --- a/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp +++ b/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp @@ -80,36 +80,6 @@ void FlightTaskAutoLineSmoothVel::reActivate() _position_smoothing.reset({0.f, 0.f, 0.f}, {0.f, 0.f, 0.7f}, _position); } -/** - * EKF reset handling functions - * Those functions are called by the base FlightTask in - * case of an EKF reset event - */ -void FlightTaskAutoLineSmoothVel::_ekfResetHandlerPositionXY(const matrix::Vector2f &delta_xy) -{ - _position_smoothing.forceSetPosition({_position(0), _position(1), NAN}); -} - -void FlightTaskAutoLineSmoothVel::_ekfResetHandlerVelocityXY(const matrix::Vector2f &delta_vxy) -{ - _position_smoothing.forceSetVelocity({_velocity(0), _velocity(1), NAN}); -} - -void FlightTaskAutoLineSmoothVel::_ekfResetHandlerPositionZ(float delta_z) -{ - _position_smoothing.forceSetPosition({NAN, NAN, _position(2)}); -} - -void FlightTaskAutoLineSmoothVel::_ekfResetHandlerVelocityZ(float delta_vz) -{ - _position_smoothing.forceSetVelocity({NAN, NAN, _velocity(2)}); -} - -void FlightTaskAutoLineSmoothVel::_ekfResetHandlerHeading(float delta_psi) -{ - _yaw_sp_prev += delta_psi; -} - bool FlightTaskAutoLineSmoothVel::update() { bool ret = FlightTaskAuto::update(); @@ -173,235 +143,3 @@ bool FlightTaskAutoLineSmoothVel::update() return ret; } - -void FlightTaskAutoLineSmoothVel::_generateSetpoints() -{ - _checkEmergencyBraking(); - Vector3f waypoints[] = {_prev_wp, _position_setpoint, _next_wp}; - - if (isTargetModified()) { - // In case object avoidance has injected a new setpoint, we take this as the next waypoints - waypoints[2] = _position_setpoint; - } - - const bool should_wait_for_yaw_align = _param_mpc_yaw_mode.get() == 4 && !_yaw_sp_aligned; - _updateTrajConstraints(); - PositionSmoothing::PositionSmoothingSetpoints smoothed_setpoints; - _position_smoothing.generateSetpoints( - _position, - waypoints, - _velocity_setpoint, - _deltatime, - should_wait_for_yaw_align, - smoothed_setpoints - ); - - _jerk_setpoint = smoothed_setpoints.jerk; - _acceleration_setpoint = smoothed_setpoints.acceleration; - _velocity_setpoint = smoothed_setpoints.velocity; - _position_setpoint = smoothed_setpoints.position; - - _unsmoothed_velocity_setpoint = smoothed_setpoints.unsmoothed_velocity; - _want_takeoff = smoothed_setpoints.unsmoothed_velocity(2) < -0.3f; - - if (!PX4_ISFINITE(_yaw_setpoint) && !PX4_ISFINITE(_yawspeed_setpoint)) { - // no valid heading -> generate heading in this flight task - _generateHeading(); - } -} - -void FlightTaskAutoLineSmoothVel::_checkEmergencyBraking() -{ - if (!_is_emergency_braking_active) { - if (_position_smoothing.getCurrentVelocityZ() > (2.f * _param_mpc_z_vel_max_dn.get())) { - _is_emergency_braking_active = true; - } - - } else { - if (fabsf(_position_smoothing.getCurrentVelocityZ()) < 0.01f) { - _is_emergency_braking_active = false; - } - } -} - - -void FlightTaskAutoLineSmoothVel::_generateHeading() -{ - // Generate heading along trajectory if possible, otherwise hold the previous yaw setpoint - if (!_generateHeadingAlongTraj()) { - _yaw_setpoint = _yaw_sp_prev; - } -} - -bool FlightTaskAutoLineSmoothVel::_generateHeadingAlongTraj() -{ - bool res = false; - Vector2f vel_sp_xy(_velocity_setpoint); - Vector2f traj_to_target = Vector2f(_target) - Vector2f(_position); - - if ((vel_sp_xy.length() > .1f) && - (traj_to_target.length() > 2.f)) { - // Generate heading from velocity vector, only if it is long enough - // and if the drone is far enough from the target - _compute_heading_from_2D_vector(_yaw_setpoint, vel_sp_xy); - res = true; - } - - return res; -} - - -bool FlightTaskAutoLineSmoothVel::isTargetModified() const -{ - const bool xy_modified = (_target - _position_setpoint).xy().longerThan(FLT_EPSILON); - const bool z_valid = PX4_ISFINITE(_position_setpoint(2)); - const bool z_modified = z_valid && fabs((_target - _position_setpoint)(2)) > FLT_EPSILON; - - return xy_modified || z_modified; -} - - -void FlightTaskAutoLineSmoothVel::_updateTrajConstraints() -{ - // update params of the position smoothing - _position_smoothing.setMaxAllowedHorizontalError(_param_mpc_xy_err_max.get()); - _position_smoothing.setVerticalAcceptanceRadius(_param_nav_mc_alt_rad.get()); - _position_smoothing.setCruiseSpeed(_mc_cruise_speed); - _position_smoothing.setHorizontalTrajectoryGain(_param_mpc_xy_traj_p.get()); - _position_smoothing.setTargetAcceptanceRadius(_target_acceptance_radius); - - // Update the constraints of the trajectories - _position_smoothing.setMaxAccelerationXY(_param_mpc_acc_hor.get()); // TODO : Should be computed using heading - _position_smoothing.setMaxVelocityXY(_param_mpc_xy_vel_max.get()); - float max_jerk = _param_mpc_jerk_auto.get(); - _position_smoothing.setMaxJerk({max_jerk, max_jerk, max_jerk}); // TODO : Should be computed using heading - - if (_is_emergency_braking_active) { - // When initializing with large downward velocity, allow 1g of vertical - // acceleration for fast braking - _position_smoothing.setMaxAccelerationZ(9.81f); - _position_smoothing.setMaxJerkZ(9.81f); - - // If the current velocity is beyond the usual constraints, tell - // the controller to exceptionally increase its saturations to avoid - // cutting out the feedforward - _constraints.speed_down = math::max(fabsf(_position_smoothing.getCurrentVelocityZ()), _param_mpc_z_vel_max_dn.get()); - - } else if (_unsmoothed_velocity_setpoint(2) < 0.f) { // up - float z_accel_constraint = _param_mpc_acc_up_max.get(); - float z_vel_constraint = _param_mpc_z_vel_max_up.get(); - - // The constraints are broken because they are used as hard limits by the position controller, so put this here - // until the constraints don't do things like cause controller integrators to saturate. Once the controller - // doesn't use z speed constraints, this can go in _prepareTakeoffSetpoints(). Accel limit is to - // emulate the motor ramp (also done in the controller) so that the controller can actually track the setpoint. - if (_type == WaypointType::takeoff && _dist_to_ground < _param_mpc_land_alt1.get()) { - z_vel_constraint = _param_mpc_tko_speed.get(); - z_accel_constraint = math::min(z_accel_constraint, _param_mpc_tko_speed.get() / _param_mpc_tko_ramp_t.get()); - - // Keep the altitude setpoint at the current altitude - // to avoid having it going down into the ground during - // the initial ramp as the velocity does not start at 0 - _position_smoothing.forceSetPosition({NAN, NAN, _position(2)}); - } - - _position_smoothing.setMaxVelocityZ(z_vel_constraint); - _position_smoothing.setMaxAccelerationZ(z_accel_constraint); - - } else { // down - _position_smoothing.setMaxAccelerationZ(_param_mpc_acc_down_max.get()); - _position_smoothing.setMaxVelocityZ(_param_mpc_z_vel_max_dn.get()); - } -} - -void FlightTaskAutoLineSmoothVel::_prepareIdleSetpoints() -{ - // Send zero thrust setpoint - _position_setpoint.setNaN(); // Don't require any position/velocity setpoints - _velocity_setpoint.setNaN(); - _acceleration_setpoint = Vector3f(0.f, 0.f, 100.f); // High downwards acceleration to make sure there's no thrust -} - -void FlightTaskAutoLineSmoothVel::_prepareLandSetpoints() -{ - _velocity_setpoint.setNaN(); // Don't take over any smoothed velocity setpoint - - // Slow down automatic descend close to ground - float land_speed = math::gradual(_dist_to_ground, - _param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(), - _param_mpc_land_speed.get(), _constraints.speed_down); - - if (_type_previous != WaypointType::land) { - // initialize xy-position and yaw to waypoint such that home is reached exactly without user input - _land_position = Vector3f(_target(0), _target(1), NAN); - _land_heading = _yaw_setpoint; - _stick_acceleration_xy.resetPosition(Vector2f(_target(0), _target(1))); - } - - // User input assisted landing - if (_param_mpc_land_rc_help.get() && _sticks.checkAndSetStickInputs()) { - // Stick full up -1 -> stop, stick full down 1 -> double the speed - land_speed *= (1 + _sticks.getPositionExpo()(2)); - - _stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _land_heading, - _sticks.getPositionExpo()(3) * math::radians(_param_mpc_man_y_max.get()), _yaw, _deltatime); - _stick_acceleration_xy.generateSetpoints(_sticks.getPositionExpo().slice<2, 1>(0, 0), _yaw, _land_heading, _position, - _velocity_setpoint_feedback.xy(), _deltatime); - _stick_acceleration_xy.getSetpoints(_land_position, _velocity_setpoint, _acceleration_setpoint); - - // Hack to make sure the MPC_YAW_MODE 4 alignment doesn't stop the vehicle from descending when there's yaw input - if (fabsf(_yawspeed_setpoint) > FLT_EPSILON) { - _yaw_sp_aligned = true; - } - - } else { - // Make sure we have a valid land position even in the case we loose RC while amending it - if (!PX4_ISFINITE(_land_position(0))) { - _land_position.xy() = Vector2f(_position); - } - } - - _position_setpoint = _land_position; // The last element of the land position has to stay NAN - _yaw_setpoint = _land_heading; - _velocity_setpoint(2) = land_speed; - _gear.landing_gear = landing_gear_s::GEAR_DOWN; -} - -void FlightTaskAutoLineSmoothVel::_prepareTakeoffSetpoints() -{ - // Takeoff is completely defined by target position - _position_setpoint = _target; - _velocity_setpoint = Vector3f(NAN, NAN, NAN); - - _gear.landing_gear = landing_gear_s::GEAR_DOWN; -} - -void FlightTaskAutoLineSmoothVel::_prepareVelocitySetpoints() -{ - // XY Velocity waypoint - // TODO : Rewiew that. What is the expected behavior? - _position_setpoint = Vector3f(NAN, NAN, _position(2)); - Vector2f vel_sp_xy = Vector2f(_velocity).unit_or_zero() * _mc_cruise_speed; - _velocity_setpoint = Vector3f(vel_sp_xy(0), vel_sp_xy(1), NAN); -} - -void FlightTaskAutoLineSmoothVel::_preparePositionSetpoints() -{ - // Simple waypoint navigation: go to xyz target, with standard limitations - _position_setpoint = _target; - _velocity_setpoint.setNaN(); // No special velocity limitations -} - -void FlightTaskAutoLineSmoothVel::updateParams() -{ - FlightTaskAuto::updateParams(); - - // make sure that alt1 is above alt2 - _param_mpc_land_alt1.set(math::max(_param_mpc_land_alt1.get(), _param_mpc_land_alt2.get())); -} - -bool FlightTaskAutoLineSmoothVel::_highEnoughForLandingGear() -{ - // return true if altitude is above two meters - return _dist_to_ground > 2.0f; -} diff --git a/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp b/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp index 459d9ad9ec..c314c02541 100644 --- a/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp +++ b/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp @@ -51,36 +51,4 @@ public: bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override; void reActivate() override; bool update() override; - -protected: - - /** Reset position or velocity setpoints in case of EKF reset event */ - void _ekfResetHandlerPositionXY(const matrix::Vector2f &delta_xy) override; - void _ekfResetHandlerVelocityXY(const matrix::Vector2f &delta_vxy) override; - void _ekfResetHandlerPositionZ(float delta_z) override; - void _ekfResetHandlerVelocityZ(float delta_vz) override; - void _ekfResetHandlerHeading(float delta_psi) override; - - void _generateSetpoints(); /**< Generate setpoints along line. */ - void _generateHeading(); - void _checkEmergencyBraking(); - bool _generateHeadingAlongTraj(); /**< Generates heading along trajectory. */ - - - bool isTargetModified() const; - - void _prepareSetpoints(); /**< Generate velocity target points for the trajectory generator. */ - void _updateTrajConstraints(); - - /** determines when to trigger a takeoff (ignored in flight) */ - bool _checkTakeoff() override { return _want_takeoff; }; - - void _prepareIdleSetpoints(); - void _prepareLandSetpoints(); - void _prepareVelocitySetpoints(); - void _prepareTakeoffSetpoints(); - void _preparePositionSetpoints(); - bool _highEnoughForLandingGear(); /**< Checks if gears can be lowered. */ - - void updateParams() override; /**< See ModuleParam class */ };