diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index a839a95dd6..01fe073faa 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -35,16 +35,10 @@ * @file mc_pos_control_main.cpp * Multicopter position controller. * - * Original publication for the desired attitude generation: - * Daniel Mellinger and Vijay Kumar. Minimum Snap Trajectory Generation and Control for Quadrotors. - * Int. Conf. on Robotics and Automation, Shanghai, China, May 2011 - * * The controller has two loops: P loop for position error and PID loop for velocity error. - * Output of velocity controller is thrust vector that splitted to thrust direction - * (i.e. rotation matrix for multicopter orientation) and thrust module (i.e. multicopter thrust itself). + * Output of velocity controller is thrust vector that is split to thrust direction + * (i.e. rotation matrix for multicopter orientation) and thrust scalar (i.e. multicopter thrust itself). * Controller doesn't use Euler angles for work, they generated only for more human-friendly control and logging. - * - * @author Anton Babushkin */ #include @@ -56,11 +50,7 @@ #include #include -#include #include -#include -#include -#include #include #include #include @@ -69,7 +59,6 @@ #include #include -#include #include #include @@ -79,10 +68,6 @@ #include "PositionControl.hpp" #include "Utility/ControlMath.hpp" -#define SIGMA_SINGLE_OP 0.000001f -#define SIGMA_NORM 0.001f - - /** * Multicopter position control app start / stop handling function * @@ -110,218 +95,68 @@ public: */ int start(); - bool cross_sphere_line(const matrix::Vector3f &sphere_c, const float sphere_r, - const matrix::Vector3f &line_a, const matrix::Vector3f &line_b, matrix::Vector3f &res); - private: - /** Time in us that direction change condition has to be true for direction change state */ - static constexpr uint64_t DIRECTION_CHANGE_TRIGGER_TIME_US = 100000; - /** Timeout in us for trajectory data to get considered invalid */ - static constexpr uint64_t TRAJECTORY_STREAM_TIMEOUT_US = 500000; - bool _task_should_exit = false; /**) _test_flight_tasks, /**< temporary flag for the transition to flight tasks */ - (ParamInt) _test_obstacle_avoidance, /**< temporary flag to enable obstacle avoidance */ - (ParamFloat) _manual_thr_min, /**< minimal throttle output when flying in manual mode */ - (ParamFloat) _manual_thr_max, /**< maximal throttle output when flying in manual mode */ - (ParamFloat) - _xy_vel_man_expo, /**< ratio of exponential curve for stick input in xy direction pos mode */ - (ParamFloat) - _z_vel_man_expo, /**< ratio of exponential curve for stick input in xy direction pos mode */ - (ParamFloat) - _hold_dz, /**< deadzone around the center for the sticks when flying in position mode */ - (ParamFloat) - _acceleration_hor_max, /**) - _acceleration_hor, /**) - _deceleration_hor_slow, /**< slow velocity setpoint slewrate for manual deceleration*/ - (ParamFloat) _acceleration_z_max_up, /** max acceleration up */ - (ParamFloat) _acceleration_z_max_down, /** max acceleration down */ - (ParamFloat) - _cruise_speed_90, /**) - _velocity_hor_manual, /**< target velocity in manual controlled mode at full speed*/ - (ParamFloat) - _nav_rad, /**< radius that is used by navigator that defines when to update triplets */ - (ParamFloat) _takeoff_ramp_time, /**< time contant for smooth takeoff ramp */ - (ParamFloat) - _jerk_hor_max, /**< maximum jerk in manual controlled mode when braking to zero */ - (ParamFloat) - _jerk_hor_min, /**< minimum jerk in manual controlled mode when braking to zero */ - (ParamFloat) - _mis_yaw_error, /**< yaw error threshold that is used in mission as update criteria */ - - (ParamFloat) _thr_min, - (ParamFloat) _thr_max, - (ParamFloat) _thr_hover, - (ParamFloat) _z_p, - (ParamFloat) _z_vel_p, - (ParamFloat) _z_vel_i, - (ParamFloat) _z_vel_d, + (ParamFloat) _takeoff_ramp_time, /**< time constant for smooth takeoff ramp */ (ParamFloat) _vel_max_up, (ParamFloat) _vel_max_down, - (ParamFloat) _slow_land_alt1, - (ParamFloat) _slow_land_alt2, - (ParamFloat) _xy_p, - (ParamFloat) _xy_vel_p, - (ParamFloat) _xy_vel_i, - (ParamFloat) _xy_vel_d, - (ParamFloat) _vel_max_xy_param, - (ParamFloat) _vel_cruise_xy, - (ParamFloat) _tilt_max_air_deg, (ParamFloat) _land_speed, (ParamFloat) _tko_speed, - (ParamFloat) _tilt_max_land_deg, - (ParamFloat) _man_tilt_max_deg, - (ParamFloat) _man_yaw_max_deg, - (ParamFloat) _global_yaw_max_deg, - (ParamFloat) _mc_att_yaw_p, - (ParamFloat) _hold_max_xy, - (ParamFloat) _hold_max_z, - (ParamInt) _alt_mode, - (ParamFloat) _rc_flt_cutoff, - (ParamFloat) _rc_flt_smp_rate, - (ParamFloat) _acc_max_estimator_xy - + (ParamFloat) MPC_LAND_ALT2, // altitude at which speed limit downwards reached minimum speed + (ParamInt) MPC_POS_MODE ); + control::BlockDerivative _vel_x_deriv; /**< velocity derivative in x */ + control::BlockDerivative _vel_y_deriv; /**< velocity derivative in y */ + control::BlockDerivative _vel_z_deriv; /**< velocity derivative in z */ - control::BlockDerivative _vel_x_deriv; - control::BlockDerivative _vel_y_deriv; - control::BlockDerivative _vel_z_deriv; + FlightTasks _flight_tasks; /**< class that generates position controller tracking setpoints*/ + PositionControl _control; /**< class that handles the core PID position controller */ + PositionControlStates _states; /**< structure that contains required state information for position control */ + hrt_abstime _last_warn = 0; /**< timer when the last warn message was sent out */ + static constexpr uint64_t IDLE_BOFORE_TAKEOFF_TIME_US = + 2500000; /**< time required to stay idle before enabling smooth takeoff */ - FlightTasks _flight_tasks; /**< class handling all ways to generate position controller setpoints */ - PositionControl _control{}; /**< class handling the core PID position controller */ - - systemlib::Hysteresis _manual_direction_change_hysteresis; - - math::LowPassFilter2p _filter_manual_pitch; - math::LowPassFilter2p _filter_manual_roll; - - enum manual_stick_input { - brake, - direction_change, - acceleration, - deceleration - }; - - manual_stick_input _user_intention_xy; /**< defines what the user intends to do derived from the stick input */ - manual_stick_input - _user_intention_z; /**< defines what the user intends to do derived from the stick input in z direciton */ - - matrix::Vector3f _pos_p; - matrix::Vector3f _vel_p; - matrix::Vector3f _vel_i; - matrix::Vector3f _vel_d; - float _tilt_max_air; /**< maximum tilt angle [rad] */ - float _tilt_max_land; /**< maximum tilt angle during landing [rad] */ - float _man_tilt_max; - float _man_yaw_max; - float _global_yaw_max; - - struct map_projection_reference_s _ref_pos; - float _ref_alt; - bool _ref_alt_is_global; /** true when the reference altitude is defined in a global reference frame */ - hrt_abstime _ref_timestamp; - hrt_abstime _last_warn; - - matrix::Vector3f _thrust_int; - matrix::Vector3f _pos; - matrix::Vector3f _pos_sp; - matrix::Vector3f _vel; - matrix::Vector3f _vel_sp; - matrix::Vector3f _vel_prev; /**< velocity on previous step */ - matrix::Vector3f _vel_sp_prev; - matrix::Vector3f _vel_err_d; /**< derivative of current velocity */ - matrix::Vector3f _vel_sp_desired; /**< the desired velocity can be overwritten by obstacle avoidance */ - matrix::Vector3f _curr_pos_sp; /**< current setpoint of the triplets */ - matrix::Vector3f _next_pos_sp; /**< next setpoint of the triplets */ - matrix::Vector3f _prev_pos_sp; /**< previous setpoint of the triples */ - matrix::Vector2f _stick_input_xy_prev; /**< for manual controlled mode to detect direction change */ - - matrix::Dcmf _R; /**< rotation matrix from attitude quaternions */ - float _yaw; /**< yaw angle (euler) */ - float _yaw_takeoff; /**< home yaw angle present when vehicle was taking off (euler) */ - float _man_yaw_offset; /**< current yaw offset in manual mode */ - - float _vel_max_xy; /**< equal to vel_max except in auto mode when close to target */ - bool _vel_sp_significant; /** true when the velocity setpoint is over 50% of the _vel_max_xy limit */ - float _acceleration_state_dependent_xy; /**< acceleration limit applied in manual mode */ - float _acceleration_state_dependent_z; /**< acceleration limit applied in manual mode in z */ - float _manual_jerk_limit_xy; /**< jerk limit in manual mode dependent on stick input */ - float _manual_jerk_limit_z; /**< jerk limit in manual mode in z */ - float _z_derivative; /**< velocity in z that agrees with position rate */ - - float _takeoff_vel_limit; /**< velocity limit value which gets ramped up */ - - // counters for reset events on position and velocity states - // they are used to identify a reset event - uint8_t _z_reset_counter; - uint8_t _xy_reset_counter; - uint8_t _heading_reset_counter; - - matrix::Dcmf _R_setpoint; + /** + * Hysteresis that turns true once vehicle is armed for IDLE_BOFORE_TAKEOFF_TIME_US microseconds. + * A real vehicle requires some time to accelerates the propellers to IDLE speed. To ensure + * that the propellers reach idle speed before initiating a takeoff, a delay of IDLE_BOFORE_TAKEOFF_TIME_US + * is added. + */ + systemlib::Hysteresis _arm_hysteresis{false}; /**< becomes true once vehicle is armed for IDLE_BOFORE_TAKEOFF_TIME_US */ /** * Update our local parameter cache. + * Parameter update can be forced when argument is true. + * @param force forces parameter update. */ int parameters_update(bool force); @@ -330,120 +165,63 @@ private: */ void poll_subscriptions(); - float throttle_curve(float ctl, float ctr); + /** + * Check for validity of positon/velocity states. + * @param vel_sp_z velocity setpoint in z-direction + */ + void check_vehicle_states(const float &vel_sp_z); /** - * Update reference for local position projection + * Limit altitude based on land-detector. + * @param setpoint needed to detect vehicle intention. */ - void update_ref(); + void limit_altitude(vehicle_local_position_setpoint_s &setpoint); /** - * Reset position setpoint to current position. - * - * This reset will only occur if the _reset_pos_sp flag has been set. - * The general logic is to first "activate" the flag in the flight - * regime where a switch to a position control mode should hold the - * very last position. Once switching to a position control mode - * the last position is stored once. + * Prints a warning message at a lowered rate. + * @param str the message that has to be printed. */ - void reset_pos_sp(); - - /** - * Reset altitude setpoint to current altitude. - * - * This reset will only occur if the _reset_alt_sp flag has been set. - * The general logic follows the reset_pos_sp() architecture. - */ - void reset_alt_sp(); - - /** - * Set position setpoint using manual control - */ - void control_manual(); - - void control_non_manual(); - - /** - * Set position setpoint using offboard control - */ - void control_offboard(); - - /** - * Set position setpoint for AUTO - */ - void control_auto(); - - void control_position(); - void calculate_velocity_setpoint(); - void calculate_thrust_setpoint(); - - void vel_sp_slewrate(); - - void wrap_yaw_speed(float yaw_speed); - - void update_velocity_derivative(); - - void do_control(); - - void generate_attitude_setpoint(); - - float get_cruising_speed_xy(); - - bool in_auto_takeoff(); - - float get_vel_close(const matrix::Vector2f &unit_prev_to_current, const matrix::Vector2f &unit_current_to_next); - - void constrain_velocity_setpoint(); - - void set_manual_acceleration_xy(matrix::Vector2f &stick_input_xy_NED); - - void set_manual_acceleration_z(float &max_acc_z, const float stick_input_z_NED); - - - /** - * limit altitude based on several conditions - */ - void limit_altitude(); - void warn_rate_limited(const char *str); - bool manual_wants_takeoff(); - - bool manual_wants_landing(); - - void set_takeoff_velocity(float &vel_sp_z); - - void landdetection_thrust_limit(matrix::Vector3f &thrust_sp); - - void set_idle_state(); - /** - * trajectory generation + * Publish attitude. */ - void execute_avoidance_position_waypoint(); - - void execute_avoidance_velocity_waypoint(); - - bool use_obstacle_avoidance(); - - bool use_avoidance_position_waypoint(); - - bool use_avoidance_velocity_waypoint(); - - void update_avoidance_waypoint_desired(const int point_number, const matrix::Vector3f &position_wp, - const matrix::Vector3f &velocity_wp, const matrix::Vector3f &acceleration_wp, const float yaw, const float yaw_speed); - - void reset_avoidance_waypoint_desired(); - - /** - * Temporary method for flight control compuation - */ - void updateConstraints(Controller::Constraints &constrains); - void publish_attitude(); + /** + * Publish local position setpoint. + * This is only required for logging. + */ void publish_local_pos_sp(); + /** + * Checks if smooth takeoff is initiated. + * @param position_setpoint_z the position setpoint in the z-Direction + * @param velocity setpoint_z the velocity setpoint in the z-Direction + */ + void check_for_smooth_takeoff(const float &position_setpoint_z, const float &velocity_setpoint_z, + const vehicle_constraints_s &constraints); + + /** + * Check if smooth takeoff has ended and updates accordingly. + * @param position_setpoint_z the position setpoint in the z-Direction + * @param velocity setpoint_z the velocity setpoint in the z-Direction + */ + void update_smooth_takeoff(const float &position_setpoint_z, const float &velocity_setpoint_z); + + /** + * Adjust the thrust setpoint during landing. + * Thrust is adjusted to support the land-detector during detection. + * @param thrust_setpoint gets adjusted based on land-detector state + */ + void limit_thrust_during_landing(matrix::Vector3f &thrust_sepoint); + + /** + * Start flightasks based on navigation state. + * This methods activates a taks basedn on the navigation state. + */ + void start_flight_task(); + /** * Shim for calling task_main from task_create. */ @@ -460,111 +238,35 @@ namespace pos_control MulticopterPositionControl *g_control; } - MulticopterPositionControl::MulticopterPositionControl() : SuperBlock(nullptr, "MPC"), ModuleParams(nullptr), - _control_task(-1), - _mavlink_log_pub(nullptr), - - /* subscriptions */ - _vehicle_attitude_sub(-1), - _control_mode_sub(-1), - _params_sub(-1), - _manual_sub(-1), - _local_pos_sub(-1), - _pos_sp_triplet_sub(-1), - _home_pos_sub(-1), - _traj_wp_avoidance_sub(-1), - - /* publications */ - _att_sp_pub(nullptr), - _local_pos_sp_pub(nullptr), - _traj_wp_avoidance_desired_pub(nullptr), - _attitude_setpoint_id(nullptr), - _vehicle_status{}, - _vehicle_land_detected{}, - _att{}, - _att_sp{}, - _manual{}, - _control_mode{}, - _local_pos{}, - _pos_sp_triplet{}, - _local_pos_sp{}, - _home_pos{}, - _traj_wp_avoidance{}, - _traj_wp_avoidance_desired{}, _vel_x_deriv(this, "VELD"), _vel_y_deriv(this, "VELD"), _vel_z_deriv(this, "VELD"), - _manual_direction_change_hysteresis(false), - _filter_manual_pitch(50.0f, 10.0f), - _filter_manual_roll(50.0f, 10.0f), - _user_intention_xy(brake), - _user_intention_z(brake), - _ref_alt(0.0f), - _ref_alt_is_global(false), - _ref_timestamp(0), - _last_warn(0), - _yaw(0.0f), - _yaw_takeoff(0.0f), - _man_yaw_offset(0.0f), - _vel_max_xy(0.0f), - _vel_sp_significant(false), - _acceleration_state_dependent_xy(0.0f), - _acceleration_state_dependent_z(0.0f), - _manual_jerk_limit_xy(1.0f), - _manual_jerk_limit_z(1.0f), - _z_derivative(0.0f), - _takeoff_vel_limit(0.0f), - _z_reset_counter(0), - _xy_reset_counter(0), - _heading_reset_counter(0) + _control(this) { - /* Make the attitude quaternion valid */ - _att.q[0] = 1.0f; - - _ref_pos = {}; - - /* set trigger time for manual direction change detection */ - _manual_direction_change_hysteresis.set_hysteresis_time_from(false, DIRECTION_CHANGE_TRIGGER_TIME_US); - - _pos.zero(); - _pos_sp.zero(); - _vel.zero(); - _vel_sp.zero(); - _vel_prev.zero(); - _vel_sp_prev.zero(); - _vel_err_d.zero(); - _vel_sp_desired.zero(); - _curr_pos_sp.zero(); - _next_pos_sp.zero(); - _prev_pos_sp.zero(); - _stick_input_xy_prev.zero(); - - _R.identity(); - _R_setpoint.identity(); - - _thrust_int.zero(); - - /* fetch initial parameter values */ + // fetch initial parameter values parameters_update(true); + + // set trigger time for arm hysteresis + _arm_hysteresis.set_hysteresis_time_from(false, IDLE_BOFORE_TAKEOFF_TIME_US); } MulticopterPositionControl::~MulticopterPositionControl() { if (_control_task != -1) { - /* task wakes up every 100ms or so at the longest */ + // task wakes up every 100ms or so at the longest _task_should_exit = true; - /* wait for a second for the task to quit at our request */ + // wait for a second for the task to quit at our request unsigned i = 0; do { - /* wait 20ms */ + // wait 20ms usleep(20000); - /* if we have given up, kill it */ + // if we have given up, kill it if (++i > 50) { px4_task_delete(_control_task); break; @@ -605,73 +307,8 @@ MulticopterPositionControl::parameters_update(bool force) _flight_tasks.handleParameterUpdate(); /* initialize vectors from params and enforce constraints */ - - _pos_p(0) = _xy_p.get(); - _pos_p(1) = _xy_p.get(); - _pos_p(2) = _z_p.get(); - - _vel_p(0) = _xy_vel_p.get(); - _vel_p(1) = _xy_vel_p.get(); - _vel_p(2) = _z_vel_p.get(); - - _vel_i(0) = _xy_vel_i.get(); - _vel_i(1) = _xy_vel_i.get(); - _vel_i(2) = _z_vel_i.get(); - - _vel_d(0) = _xy_vel_d.get(); - _vel_d(1) = _xy_vel_d.get(); - _vel_d(2) = _z_vel_d.get(); - - _thr_hover.set(math::constrain(_thr_hover.get(), _thr_min.get(), _thr_max.get())); - - _tilt_max_air = math::radians(_tilt_max_air_deg.get()); - _tilt_max_land = math::radians(_tilt_max_land_deg.get()); - - _hold_max_xy.set(math::max(0.f, _hold_max_xy.get())); - _hold_max_z.set(math::max(0.f, _hold_max_z.get())); - _rc_flt_smp_rate.set(math::max(1.0f, _rc_flt_smp_rate.get())); - /* make sure the filter is in its stable region -> fc < fs/2 */ - _rc_flt_cutoff.set(math::min(_rc_flt_cutoff.get(), (_rc_flt_smp_rate.get() / 2.0f) - 1.f)); - - /* update filters */ - _filter_manual_pitch.set_cutoff_frequency(_rc_flt_smp_rate.get(), _rc_flt_cutoff.get()); - _filter_manual_roll.set_cutoff_frequency(_rc_flt_smp_rate.get(), _rc_flt_cutoff.get()); - - /* make sure that vel_cruise_xy is always smaller than vel_max */ - _vel_cruise_xy.set(math::min(_vel_cruise_xy.get(), _vel_max_xy_param.get())); - - /* mc attitude control parameters*/ - _slow_land_alt1.set(math::max(_slow_land_alt1.get(), _slow_land_alt2.get())); - - /* manual control scale */ - _man_tilt_max = math::radians(_man_tilt_max_deg.get()); - _man_yaw_max = math::radians(_man_yaw_max_deg.get()); - _global_yaw_max = math::radians(_global_yaw_max_deg.get()); - - /* takeoff and land velocities should not exceed maximum */ _tko_speed.set(math::min(_tko_speed.get(), _vel_max_up.get())); _land_speed.set(math::min(_land_speed.get(), _vel_max_down.get())); - - /* default limit for acceleration and manual jerk*/ - _acceleration_state_dependent_xy = _acceleration_hor_max.get(); - _manual_jerk_limit_xy = _jerk_hor_max.get(); - - /* acceleration up must be larger than acceleration down */ - if (_acceleration_z_max_up.get() < _acceleration_z_max_down.get()) { - _acceleration_z_max_up.set(_acceleration_z_max_down.get()); - } - - /* acceleration horizontal max > deceleration hor */ - if (_acceleration_hor_max.get() < _deceleration_hor_slow.get()) { - _acceleration_hor_max.set(_deceleration_hor_slow.get()); - } - - /* for z direction we use fixed jerk for now - * TODO: check if other jerk value is required */ - _acceleration_state_dependent_z = _acceleration_z_max_up.get(); - /* we only use jerk for braking if jerk_hor_max > jerk_hor_min; otherwise just set jerk very large */ - _manual_jerk_limit_z = (_jerk_hor_max.get() > _jerk_hor_min.get()) ? _jerk_hor_max.get() : 1000000.f; - } return OK; @@ -687,7 +324,7 @@ MulticopterPositionControl::poll_subscriptions() if (updated) { orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); - /* set correct uORB ID, depending on if vehicle is VTOL or not */ + // set correct uORB ID, depending on if vehicle is VTOL or not if (!_attitude_setpoint_id) { if (_vehicle_status.is_vtol) { _attitude_setpoint_id = ORB_ID(mc_virtual_attitude_setpoint); @@ -704,81 +341,16 @@ MulticopterPositionControl::poll_subscriptions() orb_copy(ORB_ID(vehicle_land_detected), _vehicle_land_detected_sub, &_vehicle_land_detected); } - orb_check(_vehicle_attitude_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &_att); - - /* get current rotation matrix and euler angles from control state quaternions */ - _R = matrix::Quatf(_att.q); - _yaw = matrix::Eulerf(_R).psi(); - - if (_control_mode.flag_control_manual_enabled) { - if (_heading_reset_counter != _att.quat_reset_counter) { - - _heading_reset_counter = _att.quat_reset_counter; - - // we only extract the heading change from the delta quaternion - _att_sp.yaw_body += matrix::Eulerf(matrix::Quatf(_att.delta_q_reset)).psi(); - } - } - } - orb_check(_control_mode_sub, &updated); if (updated) { orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); } - orb_check(_manual_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &_manual); - } - orb_check(_local_pos_sub, &updated); if (updated) { orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos); - - // check if a reset event has happened - // if the vehicle is in manual mode we will shift the setpoints of the - // states which were reset. In auto mode we do not shift the setpoints - // since we want the vehicle to track the original state. - if (_control_mode.flag_control_manual_enabled) { - if (_z_reset_counter != _local_pos.z_reset_counter) { - _pos_sp(2) = _local_pos.z; - } - - if (_xy_reset_counter != _local_pos.xy_reset_counter) { - _pos_sp(0) = _local_pos.x; - _pos_sp(1) = _local_pos.y; - } - } - - // update the reset counters in any case - _z_reset_counter = _local_pos.z_reset_counter; - _xy_reset_counter = _local_pos.xy_reset_counter; - } - - orb_check(_pos_sp_triplet_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet); - - /* to be a valid current triplet, altitude has to be finite */ - - if (!PX4_ISFINITE(_pos_sp_triplet.current.alt)) { - _pos_sp_triplet.current.valid = false; - } - - /* to be a valid previous triplet, lat/lon/alt has to be finite */ - - if (!PX4_ISFINITE(_pos_sp_triplet.previous.lat) || - !PX4_ISFINITE(_pos_sp_triplet.previous.lon) || - !PX4_ISFINITE(_pos_sp_triplet.previous.alt)) { - _pos_sp_triplet.previous.valid = false; - } } orb_check(_home_pos_sub, &updated); @@ -786,25 +358,6 @@ MulticopterPositionControl::poll_subscriptions() if (updated) { orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos); } - - orb_check(_traj_wp_avoidance_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(vehicle_trajectory_waypoint), _traj_wp_avoidance_sub, &_traj_wp_avoidance); - } -} - -float -MulticopterPositionControl::throttle_curve(float ctl, float ctr) -{ - /* piecewise linear mapping: 0:ctr -> 0:0.5 - * and ctr:1 -> 0.5:1 */ - if (ctl < 0.5f) { - return 2 * ctl * ctr; - - } else { - return ctr + 2 * (ctl - 0.5f) * (1.0f - ctr); - } } int @@ -815,2266 +368,134 @@ MulticopterPositionControl::task_main_trampoline(int argc, char *argv[]) } void -MulticopterPositionControl::update_ref() +MulticopterPositionControl::limit_altitude(vehicle_local_position_setpoint_s &setpoint) { - // The reference point is only allowed to change when the vehicle is in standby state which is the - // normal state when the estimator origin is set. Changing reference point in flight causes large controller - // setpoint changes. Changing reference point in other arming states is untested and shoud not be performed. - if ((_local_pos.ref_timestamp != _ref_timestamp) - && ((_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) - || (!_ref_alt_is_global && _local_pos.z_global))) { - double lat_sp, lon_sp; - float alt_sp = 0.0f; + if (_vehicle_land_detected.alt_max < 0.0f || !_home_pos.valid_alt || !_local_pos.v_z_valid) { + // there is no altitude limitation present or the required information not available + return; + } - if (_ref_timestamp != 0) { - // calculate current position setpoint in global frame - map_projection_reproject(&_ref_pos, _pos_sp(0), _pos_sp(1), &lat_sp, &lon_sp); + float altitude_above_home = -(_states.position(2) - _home_pos.z); - // the altitude setpoint is the reference altitude (Z up) plus the (Z down) - // NED setpoint, multiplied out to minus - alt_sp = _ref_alt - _pos_sp(2); + if (altitude_above_home > _vehicle_land_detected.alt_max) { + // we are above maximum altitude + setpoint.z = -_vehicle_land_detected.alt_max + _home_pos.z; + setpoint.vz = 0.0f; + + } else if (setpoint.vz <= 0.0f) { + // we want to fly upwards: check if vehicle does not exceed altitude + + float delta_p = _vehicle_land_detected.alt_max - altitude_above_home; + + if (fabsf(setpoint.vz) * _dt > delta_p) { + setpoint.z = -_vehicle_land_detected.alt_max + _home_pos.z; + setpoint.vz = 0.0f; } - - // update local projection reference including altitude - map_projection_init(&_ref_pos, _local_pos.ref_lat, _local_pos.ref_lon); - _ref_alt = _local_pos.ref_alt; - - if (_local_pos.z_global) { - _ref_alt_is_global = true; - } - - if (_ref_timestamp != 0) { - // reproject position setpoint to new reference - // this effectively adjusts the position setpoint to keep the vehicle - // in its current local position. It would only change its - // global position on the next setpoint update. - map_projection_project(&_ref_pos, lat_sp, lon_sp, &_pos_sp(0), &_pos_sp(1)); - _pos_sp(2) = -(alt_sp - _ref_alt); - } - - _ref_timestamp = _local_pos.ref_timestamp; - } } void -MulticopterPositionControl::reset_pos_sp() +MulticopterPositionControl::check_vehicle_states(const float &vel_sp_z) { - if (_reset_pos_sp) { - _reset_pos_sp = false; - - // we have logic in the main function which chooses the velocity setpoint such that the attitude setpoint is - // continuous when switching into velocity controlled mode, therefore, we don't need to bother about resetting - // position in a special way. In position control mode the position will be reset anyway until the vehicle has reduced speed. - _pos_sp(0) = _pos(0); - _pos_sp(1) = _pos(1); - } -} - -void -MulticopterPositionControl::reset_alt_sp() -{ - if (_reset_alt_sp) { - _reset_alt_sp = false; - - // we have logic in the main function which choosed the velocity setpoint such that the attitude setpoint is - // continuous when switching into velocity controlled mode, therefore, we don't need to bother about resetting - // altitude in a special way - _pos_sp(2) = _pos(2); - } -} - -void -MulticopterPositionControl::limit_altitude() -{ - // TODO : This limit calculation should be moved to updateConstraints in FlightTask - - // Calculate vertical position limit - float poz_z_min_limit; - - if (_terrain_follow) { - poz_z_min_limit = fmaxf(-_local_pos.hagl_max, -_vehicle_land_detected.alt_max); - - //printf("terrain follow alt limit : %f\n", altitude_limit); - - } else { - poz_z_min_limit = fmaxf(-_local_pos.hagl_max + _pos(2) + _local_pos.dist_bottom, - -_vehicle_land_detected.alt_max + _home_pos.z); - - //printf("altitude follow alt limit : %f\n", altitude_limit); - } - - // Don't allow the z position setpoint to exceed the limit - if (_pos_sp(2) < poz_z_min_limit) { - _pos_sp(2) = poz_z_min_limit; - } - - // limit the velocity setpoint to not exceed a value that will allow controlled - // deceleration to a stop at the height limit - float vel_z_sp_altctl = (poz_z_min_limit - _pos(2)) * _pos_p(2); - vel_z_sp_altctl = fminf(vel_z_sp_altctl, _vel_max_down.get()); - _vel_sp(2) = fmaxf(_vel_sp(2), vel_z_sp_altctl); - -} - -bool -MulticopterPositionControl::in_auto_takeoff() -{ - /* - * in auto mode, check if we do a takeoff - */ - return (_pos_sp_triplet.current.valid && - _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) || - _control_mode.flag_control_offboard_enabled; -} - -float -MulticopterPositionControl::get_vel_close(const matrix::Vector2f &unit_prev_to_current, - const matrix::Vector2f &unit_current_to_next) -{ - - /* minimum cruise speed when passing waypoint */ - float min_cruise_speed = 1.0f; - - /* make sure that cruise speed is larger than minimum*/ - if ((get_cruising_speed_xy() - min_cruise_speed) < SIGMA_NORM) { - return get_cruising_speed_xy(); - } - - /* middle cruise speed is a number between maximum cruising speed and minimum cruising speed and corresponds to speed at angle of 90degrees - * it needs to be always larger than minimum cruise speed */ - float middle_cruise_speed = _cruise_speed_90.get(); - - if ((middle_cruise_speed - min_cruise_speed) < SIGMA_NORM) { - middle_cruise_speed = min_cruise_speed + SIGMA_NORM; - } - - if ((get_cruising_speed_xy() - middle_cruise_speed) < SIGMA_NORM) { - middle_cruise_speed = (get_cruising_speed_xy() + min_cruise_speed) * 0.5f; - } - - /* if middle cruise speed is exactly in the middle, then compute - * vel_close linearly - */ - bool use_linear_approach = false; - - if (((get_cruising_speed_xy() + min_cruise_speed) * 0.5f) - middle_cruise_speed < SIGMA_NORM) { - use_linear_approach = true; - } - - /* angle = cos(x) + 1.0 - * angle goes from 0 to 2 with 0 = large angle, 2 = small angle: 0 = PI ; 2 = PI*0 */ - float angle = 2.0f; - - if (unit_current_to_next.length() > SIGMA_NORM) { - angle = unit_current_to_next * (unit_prev_to_current * -1.0f) + 1.0f; - } - - /* compute velocity target close to waypoint */ - float vel_close; - - if (use_linear_approach) { - - /* velocity close to target adjusted to angle - * vel_close = m*x+q - */ - float slope = -(get_cruising_speed_xy() - min_cruise_speed) / 2.0f; - vel_close = slope * angle + get_cruising_speed_xy(); - - } else { - - /* velocity close to target adjusted to angle - * vel_close = a *b ^x + c; where at angle = 0 -> vel_close = vel_cruise; angle = 1 -> vel_close = middle_cruise_speed (this means that at 90degrees - * the velocity at target is middle_cruise_speed); - * angle = 2 -> vel_close = min_cruising_speed */ - - /* from maximum cruise speed, minimum cruise speed and middle cruise speed compute constants a, b and c */ - float a = -((middle_cruise_speed - get_cruising_speed_xy()) * (middle_cruise_speed - get_cruising_speed_xy())) / - (2.0f * middle_cruise_speed - get_cruising_speed_xy() - min_cruise_speed); - float c = get_cruising_speed_xy() - a; - float b = (middle_cruise_speed - c) / a; - vel_close = a * powf(b, angle) + c; - } - - /* vel_close needs to be in between max and min */ - return math::constrain(vel_close, min_cruise_speed, get_cruising_speed_xy()); - -} - -void -MulticopterPositionControl::constrain_velocity_setpoint() -{ - - /* make sure velocity setpoint is constrained in all directions (xyz) */ - float vel_norm_xy = sqrtf(_vel_sp(0) * _vel_sp(0) + _vel_sp(1) * _vel_sp(1)); - - if (vel_norm_xy > _vel_max_xy) { - _vel_sp(0) = _vel_sp(0) * _vel_max_xy / vel_norm_xy; - _vel_sp(1) = _vel_sp(1) * _vel_max_xy / vel_norm_xy; - } - - _vel_sp(2) = math::constrain(_vel_sp(2), -_vel_max_up.get(), _vel_max_down.get()); - -} - -float -MulticopterPositionControl::get_cruising_speed_xy() -{ - /* - * in mission the user can choose cruising speed different to default - */ - return ((PX4_ISFINITE(_pos_sp_triplet.current.cruising_speed) && !(_pos_sp_triplet.current.cruising_speed < 0.0f)) ? - _pos_sp_triplet.current.cruising_speed : _vel_cruise_xy.get()); -} - -void -MulticopterPositionControl::set_manual_acceleration_z(float &max_acceleration, const float stick_z) -{ - - - /* in manual altitude control apply acceleration limit based on stick input - * we consider two states - * 1.) brake - * 2.) accelerate */ - - /* check if zero input stick */ - const bool is_current_zero = (fabsf(stick_z) <= FLT_EPSILON); - - /* default is acceleration */ - manual_stick_input intention = acceleration; - - /* check zero input stick */ - if (is_current_zero) { - intention = brake; - } - - /* get max and min acceleration where min acceleration is just 1/5 of max acceleration */ - max_acceleration = (stick_z <= 0.0f) ? _acceleration_z_max_up.get() : _acceleration_z_max_down.get(); - - /* - * update user input - */ - if ((_user_intention_z != brake) && (intention == brake)) { - - /* we start with lowest acceleration */ - _acceleration_state_dependent_z = _acceleration_z_max_down.get(); - - /* reset slew rate */ - _vel_sp_prev(2) = _vel(2); - _user_intention_z = brake; - } - - _user_intention_z = intention; - - /* - * apply acceleration depending on state - */ - if (_user_intention_z == brake) { - - /* limit jerk when braking to zero */ - float jerk = (_acceleration_z_max_up.get() - _acceleration_state_dependent_z) / _dt; - - if (jerk > _manual_jerk_limit_z) { - _acceleration_state_dependent_z = _manual_jerk_limit_z * _dt + _acceleration_state_dependent_z; - - } else { - _acceleration_state_dependent_z = _acceleration_z_max_up.get(); - } - } - - if (_user_intention_z == acceleration) { - _acceleration_state_dependent_z = (max_acceleration - _acceleration_z_max_down.get()) * fabsf( - stick_z) + _acceleration_z_max_down.get(); - } -} - -void -MulticopterPositionControl::set_manual_acceleration_xy(matrix::Vector2f &stick_xy) -{ - - /* - * In manual mode we consider four states with different acceleration handling: - * 1. user wants to stop - * 2. user wants to quickly change direction - * 3. user wants to accelerate - * 4. user wants to decelerate - */ - - /* get normalized stick input vector */ - matrix::Vector2f stick_xy_norm = (stick_xy.length() > 0.0f) ? stick_xy.normalized() : stick_xy; - matrix::Vector2f stick_xy_prev_norm = (_stick_input_xy_prev.length() > 0.0f) ? _stick_input_xy_prev.normalized() : - _stick_input_xy_prev; - - /* check if stick direction and current velocity are within 60angle */ - const bool is_aligned = (stick_xy_norm * stick_xy_prev_norm) > 0.5f; - - /* check if zero input stick */ - const bool is_prev_zero = (fabsf(_stick_input_xy_prev.length()) <= FLT_EPSILON); - const bool is_current_zero = (fabsf(stick_xy.length()) <= FLT_EPSILON); - - /* check acceleration */ - const bool do_acceleration = is_prev_zero || (is_aligned && - ((stick_xy.length() > _stick_input_xy_prev.length()) || (fabsf(stick_xy.length() - 1.0f) < FLT_EPSILON))); - - const bool do_deceleration = (is_aligned && (stick_xy.length() <= _stick_input_xy_prev.length())); - - const bool do_direction_change = !is_aligned; - - manual_stick_input intention; - - if (is_current_zero) { - /* we want to stop */ - intention = brake; - - } else if (do_acceleration) { - /* we do manual acceleration */ - intention = acceleration; - - } else if (do_deceleration) { - /* we do manual deceleration */ - intention = deceleration; - - } else if (do_direction_change) { - /* we have a direction change */ - intention = direction_change; - - } else { - /* catchall: acceleration */ - intention = acceleration; - } - - - /* - * update user intention - */ - - /* we always want to break starting with slow deceleration */ - if ((_user_intention_xy != brake) && (intention == brake)) { - - if (_jerk_hor_max.get() > _jerk_hor_min.get()) { - _manual_jerk_limit_xy = (_jerk_hor_max.get() - _jerk_hor_min.get()) / _velocity_hor_manual.get() * - sqrtf(_vel(0) * _vel(0) + _vel(1) * _vel(1)) + _jerk_hor_min.get(); - - /* we start braking with lowest accleration */ - _acceleration_state_dependent_xy = _deceleration_hor_slow.get(); - - } else { - - /* set the jerk limit large since we don't know it better*/ - _manual_jerk_limit_xy = 1000000.f; - - /* at brake we use max acceleration */ - _acceleration_state_dependent_xy = _acceleration_hor_max.get(); - - } - - /* reset slew rate */ - _vel_sp_prev(0) = _vel(0); - _vel_sp_prev(1) = _vel(1); - - } - - switch (_user_intention_xy) { - case brake: { - if (intention != brake) { - _user_intention_xy = acceleration; - /* we initialize with lowest acceleration */ - _acceleration_state_dependent_xy = _deceleration_hor_slow.get(); - } - - break; - } - - case direction_change: { - /* only exit direction change if brake or aligned */ - matrix::Vector2f vel_xy(_vel(0), _vel(1)); - matrix::Vector2f vel_xy_norm = (vel_xy.length() > 0.0f) ? vel_xy.normalized() : vel_xy; - bool stick_vel_aligned = (vel_xy_norm * stick_xy_norm > 0.0f); - - /* update manual direction change hysteresis */ - _manual_direction_change_hysteresis.set_state_and_update(!stick_vel_aligned); - - - /* exit direction change if one of the condition is met */ - if (intention == brake) { - _user_intention_xy = intention; - - } else if (stick_vel_aligned) { - _user_intention_xy = acceleration; - - } else if (_manual_direction_change_hysteresis.get_state()) { - - /* TODO: find conditions which are always continuous - * only if stick input is large*/ - if (stick_xy.length() > 0.6f) { - _acceleration_state_dependent_xy = _acceleration_hor_max.get(); - } - } - - break; - } - - case acceleration: { - _user_intention_xy = intention; - - if (_user_intention_xy == direction_change) { - _vel_sp_prev(0) = _vel(0); - _vel_sp_prev(1) = _vel(1); - } - - break; - } - - case deceleration: { - _user_intention_xy = intention; - - if (_user_intention_xy == direction_change) { - _vel_sp_prev(0) = _vel(0); - _vel_sp_prev(1) = _vel(1); - } - - break; - } - } - - /* - * apply acceleration based on state - */ - switch (_user_intention_xy) { - case brake: { - - /* limit jerk when braking to zero */ - float jerk = (_acceleration_hor_max.get() - _acceleration_state_dependent_xy) / _dt; - - if (jerk > _manual_jerk_limit_xy) { - _acceleration_state_dependent_xy = _manual_jerk_limit_xy * _dt + _acceleration_state_dependent_xy; - - } else { - _acceleration_state_dependent_xy = _acceleration_hor_max.get(); - } - - break; - } - - case direction_change: { - - /* limit acceleration linearly on stick input*/ - _acceleration_state_dependent_xy = (_acceleration_hor.get() - _deceleration_hor_slow.get()) * stick_xy.length() + - _deceleration_hor_slow.get(); - break; - } - - case acceleration: { - /* limit acceleration linearly on stick input*/ - float acc_limit = (_acceleration_hor.get() - _deceleration_hor_slow.get()) * stick_xy.length() - + _deceleration_hor_slow.get(); - - if (_acceleration_state_dependent_xy > acc_limit) { - acc_limit = _acceleration_state_dependent_xy; - } - - _acceleration_state_dependent_xy = acc_limit; - break; - } - - case deceleration: { - _acceleration_state_dependent_xy = _deceleration_hor_slow.get(); - break; - } - - default : - warn_rate_limited("User intention not recognized"); - _acceleration_state_dependent_xy = _acceleration_hor_max.get(); - - } - - /* update previous stick input */ - _stick_input_xy_prev = matrix::Vector2f(_filter_manual_pitch.apply(stick_xy(0)), - _filter_manual_roll.apply(stick_xy(1))); - - - if (_stick_input_xy_prev.length() > 1.0f) { - _stick_input_xy_prev = _stick_input_xy_prev.normalized(); - } -} - -void -MulticopterPositionControl::control_manual() -{ - /* Entering manual control from non-manual control mode, reset alt/pos setpoints */ - if (_mode_auto) { - _mode_auto = false; - - /* Reset alt pos flags if resetting is enabled */ - if (_do_reset_alt_pos_flag) { - _reset_pos_sp = true; - _reset_alt_sp = true; - } - } - - /* - * Map from stick input to velocity setpoint - */ - - /* velocity setpoint commanded by user stick input */ - matrix::Vector3f man_vel_sp; - - if (_control_mode.flag_control_altitude_enabled) { - /* set vertical velocity setpoint with throttle stick, remapping of manual.z [0,1] to up and down command [-1,1] */ - man_vel_sp(2) = -math::expo_deadzone((_manual.z - 0.5f) * 2.f, _z_vel_man_expo.get(), _hold_dz.get()); - - /* reset alt setpoint to current altitude if needed */ - reset_alt_sp(); - } - - if (_control_mode.flag_control_position_enabled) { - /* set horizontal velocity setpoint with roll/pitch stick */ - man_vel_sp(0) = math::expo_deadzone(_manual.x, _xy_vel_man_expo.get(), _hold_dz.get()); - man_vel_sp(1) = math::expo_deadzone(_manual.y, _xy_vel_man_expo.get(), _hold_dz.get()); - - const float man_vel_hor_length = ((matrix::Vector2f)man_vel_sp.slice<2, 1>(0, 0)).length(); - - /* saturate such that magnitude is never larger than 1 */ - if (man_vel_hor_length > 1.0f) { - man_vel_sp(0) /= man_vel_hor_length; - man_vel_sp(1) /= man_vel_hor_length; - } - - /* reset position setpoint to current position if needed */ - reset_pos_sp(); - - } - - /* prepare yaw to rotate into NED frame */ - float yaw_input_frame = _control_mode.flag_control_fixed_hdg_enabled ? _yaw_takeoff : _att_sp.yaw_body; - - /* setpoint in NED frame */ - man_vel_sp = matrix::Dcmf(matrix::Eulerf(0.0f, 0.0f, yaw_input_frame)) * man_vel_sp; - - /* adjust acceleration based on stick input */ - matrix::Vector2f stick_xy(man_vel_sp(0), man_vel_sp(1)); - set_manual_acceleration_xy(stick_xy); - float stick_z = man_vel_sp(2); - float max_acc_z; - set_manual_acceleration_z(max_acc_z, stick_z); - - /* prepare cruise speed (m/s) vector to scale the velocity setpoint */ - float vel_mag = (_velocity_hor_manual.get() < _vel_max_xy) ? _velocity_hor_manual.get() : _vel_max_xy; - matrix::Vector3f vel_cruise_scale(vel_mag, vel_mag, (man_vel_sp(2) > 0.0f) ? _vel_max_down.get() : _vel_max_up.get()); - /* Setpoint scaled to cruise speed */ - man_vel_sp = man_vel_sp.emult(vel_cruise_scale); - - /* - * assisted velocity mode: user controls velocity, but if velocity is small enough, position - * hold is activated for the corresponding axis - */ - - /* want to get/stay in altitude hold if user has z stick in the middle (accounted for deadzone already) */ - const bool alt_hold_desired = _control_mode.flag_control_altitude_enabled && (_user_intention_z == brake); - - /* want to get/stay in position hold if user has xy stick in the middle (accounted for deadzone already) */ - const bool pos_hold_desired = _control_mode.flag_control_position_enabled && (_user_intention_xy == brake); - - /* check vertical hold engaged flag */ - if (_alt_hold_engaged) { - _alt_hold_engaged = alt_hold_desired; - - } else { - - /* check if we switch to alt_hold_engaged */ - bool smooth_alt_transition = alt_hold_desired && ((max_acc_z - _acceleration_state_dependent_z) < FLT_EPSILON) && - (_hold_max_z.get() < FLT_EPSILON || fabsf(_vel(2)) < _hold_max_z.get()); - - /* during transition predict setpoint forward */ - if (smooth_alt_transition) { - - /* time to travel from current velocity to zero velocity */ - float delta_t = fabsf(_vel(2) / max_acc_z); - - /* set desired position setpoint assuming max acceleration */ - _pos_sp(2) = _pos(2) + _vel(2) * delta_t + 0.5f * max_acc_z * delta_t *delta_t; - - _alt_hold_engaged = true; - } - } - - /* check horizontal hold engaged flag */ - if (_pos_hold_engaged) { - - /* check if contition still true */ - _pos_hold_engaged = pos_hold_desired; - - /* use max acceleration */ - if (_pos_hold_engaged) { - _acceleration_state_dependent_xy = _acceleration_hor_max.get(); - } - - } else { - - /* check if we switch to pos_hold_engaged */ - float vel_xy_mag = sqrtf(_vel(0) * _vel(0) + _vel(1) * _vel(1)); - bool smooth_pos_transition = pos_hold_desired - && (fabsf(_acceleration_hor_max.get() - _acceleration_state_dependent_xy) < FLT_EPSILON) && - (_hold_max_xy.get() < FLT_EPSILON || vel_xy_mag < _hold_max_xy.get()); - - /* during transition predict setpoint forward */ - if (smooth_pos_transition) { - - /* time to travel from current velocity to zero velocity */ - float delta_t = sqrtf(_vel(0) * _vel(0) + _vel(1) * _vel(1)) / _acceleration_hor_max.get(); - - /* p pos_sp in xy from max acceleration and current velocity */ - matrix::Vector2f pos(_pos(0), _pos(1)); - matrix::Vector2f vel(_vel(0), _vel(1)); - matrix::Vector2f pos_sp = pos + vel * delta_t - vel.normalized() * 0.5f * _acceleration_hor_max.get() * delta_t - * delta_t; - _pos_sp(0) = pos_sp(0); - _pos_sp(1) = pos_sp(1); - - _pos_hold_engaged = true; - } - } - - /* set requested velocity setpoints */ - if (!_alt_hold_engaged) { - _pos_sp(2) = _pos(2); - _run_alt_control = false; /* request velocity setpoint to be used, instead of altitude setpoint */ - _vel_sp(2) = man_vel_sp(2); - } - - if (!_pos_hold_engaged) { - _pos_sp(0) = _pos(0); - _pos_sp(1) = _pos(1); - _run_pos_control = false; /* request velocity setpoint to be used, instead of position setpoint */ - _vel_sp(0) = man_vel_sp(0); - _vel_sp(1) = man_vel_sp(1); - } - - control_position(); -} - -void -MulticopterPositionControl::control_non_manual() -{ - /* select control source */ - if (_control_mode.flag_control_offboard_enabled) { - /* offboard control */ - control_offboard(); - _mode_auto = false; - - } else { - _hold_offboard_xy = false; - _hold_offboard_z = false; - - /* AUTO */ - control_auto(); - } - - // guard against any bad velocity values - bool velocity_valid = PX4_ISFINITE(_pos_sp_triplet.current.vx) && - PX4_ISFINITE(_pos_sp_triplet.current.vy) && - _pos_sp_triplet.current.velocity_valid; - - // do not go slower than the follow target velocity when position tracking is active (set to valid) - if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET && - velocity_valid && - _pos_sp_triplet.current.position_valid) { - - matrix::Vector3f ft_vel(_pos_sp_triplet.current.vx, _pos_sp_triplet.current.vy, 0.0f); - - float cos_ratio = (ft_vel * _vel_sp) / (ft_vel.length() * _vel_sp.length()); - - // only override velocity set points when uav is traveling in same direction as target and vector component - // is greater than calculated position set point velocity component - - if (cos_ratio > 0) { - ft_vel *= (cos_ratio); - // min speed a little faster than target vel - ft_vel += ft_vel.normalized() * 1.5f; - - } else { - ft_vel.zero(); - } - - _vel_sp(0) = fabsf(ft_vel(0)) > fabsf(_vel_sp(0)) ? ft_vel(0) : _vel_sp(0); - _vel_sp(1) = fabsf(ft_vel(1)) > fabsf(_vel_sp(1)) ? ft_vel(1) : _vel_sp(1); - - // track target using velocity only - - } else if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET && - velocity_valid) { - - _vel_sp(0) = _pos_sp_triplet.current.vx; - _vel_sp(1) = _pos_sp_triplet.current.vy; - } - - /* use constant descend rate when landing, ignore altitude setpoint */ - if (_pos_sp_triplet.current.valid - && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { - _vel_sp(2) = _land_speed.get(); - _run_alt_control = false; - } - - if (_pos_sp_triplet.current.valid - && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { - /* idle state, don't run controller and set zero thrust */ - _R_setpoint.identity(); - - matrix::Quatf qd = _R_setpoint; - qd.copyTo(_att_sp.q_d); - _att_sp.q_d_valid = true; - - _att_sp.roll_body = 0.0f; - _att_sp.pitch_body = 0.0f; - _att_sp.yaw_body = _yaw; - _att_sp.thrust = 0.0f; - - _att_sp.timestamp = hrt_absolute_time(); - - } else { - control_position(); - } -} - -void -MulticopterPositionControl::control_offboard() -{ - if (_pos_sp_triplet.current.valid) { - - if (_control_mode.flag_control_position_enabled && _pos_sp_triplet.current.position_valid) { - /* control position */ - _pos_sp(0) = _pos_sp_triplet.current.x; - _pos_sp(1) = _pos_sp_triplet.current.y; - _run_pos_control = true; - - _hold_offboard_xy = false; - - } else if (_control_mode.flag_control_velocity_enabled && _pos_sp_triplet.current.velocity_valid) { - /* control velocity */ - - /* reset position setpoint to current position if needed */ - reset_pos_sp(); - - if (fabsf(_pos_sp_triplet.current.vx) <= FLT_EPSILON && - fabsf(_pos_sp_triplet.current.vy) <= FLT_EPSILON && - _local_pos.xy_valid) { - - if (!_hold_offboard_xy) { - _pos_sp(0) = _pos(0); - _pos_sp(1) = _pos(1); - _hold_offboard_xy = true; - } - - _run_pos_control = true; - - } else { - - if (_pos_sp_triplet.current.velocity_frame == position_setpoint_s::VELOCITY_FRAME_LOCAL_NED) { - /* set position setpoint move rate */ - _vel_sp(0) = _pos_sp_triplet.current.vx; - _vel_sp(1) = _pos_sp_triplet.current.vy; - - } else if (_pos_sp_triplet.current.velocity_frame == position_setpoint_s::VELOCITY_FRAME_BODY_NED) { - // Transform velocity command from body frame to NED frame - _vel_sp(0) = cosf(_yaw) * _pos_sp_triplet.current.vx - sinf(_yaw) * _pos_sp_triplet.current.vy; - _vel_sp(1) = sinf(_yaw) * _pos_sp_triplet.current.vx + cosf(_yaw) * _pos_sp_triplet.current.vy; - - } else { - warn_rate_limited("Unknown velocity offboard coordinate frame"); - } - - _run_pos_control = false; - - _hold_offboard_xy = false; - } - } - - if (_control_mode.flag_control_altitude_enabled && _pos_sp_triplet.current.alt_valid) { - /* control altitude as it is enabled */ - _pos_sp(2) = _pos_sp_triplet.current.z; - _run_alt_control = true; - - _hold_offboard_z = false; - - } else if (_control_mode.flag_control_climb_rate_enabled && _pos_sp_triplet.current.velocity_valid) { - - /* reset alt setpoint to current altitude if needed */ - reset_alt_sp(); - - if (fabsf(_pos_sp_triplet.current.vz) <= FLT_EPSILON && - _local_pos.z_valid) { - - if (!_hold_offboard_z) { - _pos_sp(2) = _pos(2); - _hold_offboard_z = true; - } - - _run_alt_control = true; - - } else { - /* set position setpoint move rate */ - _vel_sp(2) = _pos_sp_triplet.current.vz; - _run_alt_control = false; - - _hold_offboard_z = false; - } - } - - if (_pos_sp_triplet.current.yaw_valid) { - _att_sp.yaw_body = _pos_sp_triplet.current.yaw; - - } else if (_pos_sp_triplet.current.yawspeed_valid) { - - wrap_yaw_speed(_pos_sp_triplet.current.yawspeed); - } - - } else { - _hold_offboard_xy = false; - _hold_offboard_z = false; - reset_pos_sp(); - reset_alt_sp(); - } -} - -void -MulticopterPositionControl::vel_sp_slewrate() -{ - matrix::Vector2f vel_sp_xy(_vel_sp(0), _vel_sp(1)); - matrix::Vector2f vel_sp_prev_xy(_vel_sp_prev(0), _vel_sp_prev(1)); - matrix::Vector2f acc_xy = (vel_sp_xy - vel_sp_prev_xy) / _dt; - - /* limit total horizontal acceleration */ - if (acc_xy.length() > _acceleration_state_dependent_xy) { - vel_sp_xy = _acceleration_state_dependent_xy * acc_xy.normalized() * _dt + vel_sp_prev_xy; - _vel_sp(0) = vel_sp_xy(0); - _vel_sp(1) = vel_sp_xy(1); - } - - /* limit vertical acceleration */ - float acc_z = (_vel_sp(2) - _vel_sp_prev(2)) / _dt; - float max_acc_z; - - if (_control_mode.flag_control_manual_enabled) { - max_acc_z = (acc_z < 0.0f) ? -_acceleration_state_dependent_z : _acceleration_state_dependent_z; - - } else { - max_acc_z = (acc_z < 0.0f) ? -_acceleration_z_max_up.get() : _acceleration_z_max_down.get(); - } - - if (fabsf(acc_z) > fabsf(max_acc_z)) { - _vel_sp(2) = max_acc_z * _dt + _vel_sp_prev(2); - } -} - -bool -MulticopterPositionControl::cross_sphere_line(const matrix::Vector3f &sphere_c, const float sphere_r, - const matrix::Vector3f &line_a, const matrix::Vector3f &line_b, matrix::Vector3f &res) -{ - /* project center of sphere on line */ - /* normalized AB */ - matrix::Vector3f ab_norm = line_b - line_a; - - if (ab_norm.length() < 0.01f) { - return true; - } - - ab_norm.normalize(); - matrix::Vector3f d = line_a + ab_norm * ((sphere_c - line_a) * ab_norm); - float cd_len = (sphere_c - d).length(); - - if (sphere_r > cd_len) { - /* we have triangle CDX with known CD and CX = R, find DX */ - float dx_len = sqrtf(sphere_r * sphere_r - cd_len * cd_len); - - if ((sphere_c - line_b) * ab_norm > 0.0f) { - /* target waypoint is already behind us */ - res = line_b; - - } else { - /* target is in front of us */ - res = d + ab_norm * dx_len; // vector A->B on line - } - - return true; - - } else { - - /* have no roots, return D */ - res = d; /* go directly to line */ - - /* previous waypoint is still in front of us */ - if ((sphere_c - line_a) * ab_norm < 0.0f) { - res = line_a; - } - - /* target waypoint is already behind us */ - if ((sphere_c - line_b) * ab_norm > 0.0f) { - res = line_b; - } - - return false; - } -} - -void MulticopterPositionControl::control_auto() -{ - /* reset position setpoint on AUTO mode activation or if we are not in MC mode */ - if (!_mode_auto || !_vehicle_status.is_rotary_wing) { - if (!_mode_auto) { - _mode_auto = true; - //set _triplet_lat_lon_finite true once switch to AUTO(e.g. LAND) - _triplet_lat_lon_finite = true; - } - - _reset_pos_sp = true; - _reset_alt_sp = true; - } - - // Always check reset state of altitude and position control flags in auto - reset_pos_sp(); - reset_alt_sp(); - - bool current_setpoint_valid = false; - bool previous_setpoint_valid = false; - bool next_setpoint_valid = false; - bool triplet_updated = false; - - matrix::Vector3f prev_sp; - matrix::Vector3f next_sp; - - if (_pos_sp_triplet.current.valid) { - - matrix::Vector3f curr_pos_sp = _curr_pos_sp; - - //only project setpoints if they are finite, else use current position - if (PX4_ISFINITE(_pos_sp_triplet.current.lat) && - PX4_ISFINITE(_pos_sp_triplet.current.lon)) { - /* project setpoint to local frame */ - map_projection_project(&_ref_pos, - _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon, - &curr_pos_sp(0), &curr_pos_sp(1)); - - _triplet_lat_lon_finite = true; - - } else { // use current position if NAN -> e.g. land - if (_triplet_lat_lon_finite) { - curr_pos_sp(0) = _pos(0); - curr_pos_sp(1) = _pos(1); - _triplet_lat_lon_finite = false; - } - } - - // only project setpoints if they are finite, else use current position - if (PX4_ISFINITE(_pos_sp_triplet.current.alt)) { - curr_pos_sp(2) = -(_pos_sp_triplet.current.alt - _ref_alt); - - } - - - /* sanity check */ - if (PX4_ISFINITE(_curr_pos_sp(0)) && - PX4_ISFINITE(_curr_pos_sp(1)) && - PX4_ISFINITE(_curr_pos_sp(2))) { - current_setpoint_valid = true; - } - - /* check if triplets have been updated - * note: we only can look at xy since navigator applies slewrate to z */ - float diff; - - if (_triplet_lat_lon_finite) { - diff = matrix::Vector2f((_curr_pos_sp(0) - curr_pos_sp(0)), (_curr_pos_sp(1) - curr_pos_sp(1))).length(); - - } else { - diff = fabsf(_curr_pos_sp(2) - curr_pos_sp(2)); - } - - if (diff > FLT_EPSILON || !PX4_ISFINITE(diff)) { - triplet_updated = true; - } - - /* we need to update _curr_pos_sp always since navigator applies slew rate on z */ - _curr_pos_sp = curr_pos_sp; - } - - if (_pos_sp_triplet.previous.valid) { - map_projection_project(&_ref_pos, - _pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, - &prev_sp(0), &prev_sp(1)); - prev_sp(2) = -(_pos_sp_triplet.previous.alt - _ref_alt); - - if (PX4_ISFINITE(prev_sp(0)) && - PX4_ISFINITE(prev_sp(1)) && - PX4_ISFINITE(prev_sp(2))) { - _prev_pos_sp = prev_sp; - previous_setpoint_valid = true; - } - } - - /* set previous setpoint to current position if no previous setpoint available */ - if (!previous_setpoint_valid && triplet_updated) { - _prev_pos_sp = _pos; - previous_setpoint_valid = true; /* currrently not necessary to set to true since not used*/ - } - - // reset next_pos_sp - _next_pos_sp = matrix::Vector3f(NAN, NAN, NAN); - - if (_pos_sp_triplet.next.valid) { - map_projection_project(&_ref_pos, - _pos_sp_triplet.next.lat, _pos_sp_triplet.next.lon, - &next_sp(0), &next_sp(1)); - - next_sp(2) = -(_pos_sp_triplet.next.alt - _ref_alt); - - if (PX4_ISFINITE(next_sp(0)) && - PX4_ISFINITE(next_sp(1)) && - PX4_ISFINITE(next_sp(2))) { - _next_pos_sp = next_sp; - next_setpoint_valid = true; - } - } - - /* Auto logic: - * The vehicle should follow the line previous-current. - * - if there is no next setpoint or the current is a loiter point, then slowly approach the current along the line - * - if there is a next setpoint, then the velocity is adjusted depending on the angle of the corner prev-current-next. - * When following the line, the pos_sp is computed from the orthogonal distance to the closest point on line and the desired cruise speed along the track. - */ - - /* create new _pos_sp from triplets */ - if (current_setpoint_valid && - (_pos_sp_triplet.current.type != position_setpoint_s::SETPOINT_TYPE_IDLE)) { - - const bool follow_me_target_on = _pos_sp_triplet.current.yawspeed_valid - && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET; - - /* update yaw setpoint if needed */ - if ((use_obstacle_avoidance() - && PX4_ISFINITE(_traj_wp_avoidance.waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw_speed)) - || follow_me_target_on) { - - /* default is triplet yaw-speed */ - float yaw_speed = _pos_sp_triplet.current.yawspeed; - - if (use_obstacle_avoidance() - && PX4_ISFINITE(_traj_wp_avoidance.waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw_speed)) { - yaw_speed = _traj_wp_avoidance.waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw_speed; - } - - wrap_yaw_speed(yaw_speed); - - - } else { - - if (use_obstacle_avoidance() - && PX4_ISFINITE(_traj_wp_avoidance.waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw)) { - _att_sp.yaw_body = _traj_wp_avoidance.waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw; - - } else if (PX4_ISFINITE(_pos_sp_triplet.current.yaw)) { - _att_sp.yaw_body = _pos_sp_triplet.current.yaw; - } - } - - float yaw_diff = wrap_pi(_att_sp.yaw_body - _yaw); - - /* only follow previous-current-line for specific triplet type */ - if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION || - _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER || - _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET) { - - /* by default use current setpoint as is */ - matrix::Vector3f pos_sp = _curr_pos_sp; - - /* - * Z-DIRECTION - */ - - /* get various distances */ - float total_dist_z = fabsf(_curr_pos_sp(2) - _prev_pos_sp(2)); - float dist_to_prev_z = fabsf(_pos(2) - _prev_pos_sp(2)); - float dist_to_current_z = fabsf(_curr_pos_sp(2) - _pos(2)); - - /* if pos_sp has not reached target setpoint (=curr_pos_sp(2)), - * then compute setpoint depending on vel_max */ - if ((total_dist_z > SIGMA_NORM) && (fabsf(_pos_sp(2) - _curr_pos_sp(2)) > SIGMA_NORM)) { - - /* check sign */ - bool flying_upward = _curr_pos_sp(2) < _pos(2); - - /* final_vel_z is the max velocity which depends on the distance of total_dist_z - * with default params.vel_max_up/down - */ - float final_vel_z = flying_upward ? _vel_max_up.get() : _vel_max_down.get(); - - /* target threshold defines the distance to _curr_pos_sp(2) at which - * the vehicle starts to slow down to approach the target smoothly - */ - float target_threshold_z = final_vel_z * 1.5f; - - /* if the total distance in z is NOT 2x distance of target_threshold, we - * will need to adjust the final_vel_z - */ - bool is_2_target_threshold_z = total_dist_z >= 2.0f * target_threshold_z; - float slope = (final_vel_z) / (target_threshold_z); /* defines the the acceleration when slowing down */ - float min_vel_z = 0.2f; // minimum velocity: this is needed since estimation is not perfect - - if (!is_2_target_threshold_z) { - /* adjust final_vel_z since we are already very close - * to current and therefore it is not necessary to accelerate - * up to full speed (=final_vel_z) - */ - target_threshold_z = total_dist_z * 0.5f; - /* get the velocity at target_threshold_z */ - float final_vel_z_tmp = slope * (target_threshold_z) + min_vel_z; - - /* make sure that final_vel_z is never smaller than 0.5 of the default final_vel_z - * this is mainly done because the estimation in z is not perfect and therefore - * it is necessary to have a minimum speed - */ - final_vel_z = math::constrain(final_vel_z_tmp, final_vel_z * 0.5f, final_vel_z); - } - - float vel_sp_z = final_vel_z; - - /* we want to slow down */ - if (dist_to_current_z < target_threshold_z) { - - vel_sp_z = slope * dist_to_current_z + min_vel_z; - - } else if (dist_to_prev_z < target_threshold_z) { - /* we want to accelerate */ - - float acc_z = (vel_sp_z - fabsf(_vel_sp(2))) / _dt; - float acc_max = (flying_upward) ? (_acceleration_z_max_up.get() * 0.5f) : (_acceleration_z_max_down.get() * 0.5f); - - if (acc_z > acc_max) { - vel_sp_z = _acceleration_z_max_up.get() * _dt + fabsf(_vel_sp(2)); - } - - } - - /* if we already close to current, then just take over the velocity that - * we would have computed if going directly to the current setpoint - */ - if (vel_sp_z >= (dist_to_current_z * _pos_p(2))) { - vel_sp_z = dist_to_current_z * _pos_p(2); - } - - /* make sure vel_sp_z is always positive */ - vel_sp_z = math::constrain(vel_sp_z, 0.0f, final_vel_z); - /* get the sign of vel_sp_z */ - vel_sp_z = (flying_upward) ? -vel_sp_z : vel_sp_z; - /* compute pos_sp(2) */ - pos_sp(2) = _pos(2) + vel_sp_z / _pos_p(2); - } - - /* - * XY-DIRECTION - */ - - /* line from previous to current and from pos to current */ - matrix::Vector2f vec_prev_to_current((_curr_pos_sp(0) - _prev_pos_sp(0)), (_curr_pos_sp(1) - _prev_pos_sp(1))); - matrix::Vector2f vec_pos_to_current((_curr_pos_sp(0) - _pos(0)), (_curr_pos_sp(1) - _pos(1))); - - - /* check if we just want to stay at current position */ - matrix::Vector2f pos_sp_diff((_curr_pos_sp(0) - _pos_sp(0)), (_curr_pos_sp(1) - _pos_sp(1))); - bool stay_at_current_pos = (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER - || !next_setpoint_valid) - && ((pos_sp_diff.length()) < SIGMA_NORM); - - /* only follow line if previous to current has a minimum distance */ - if ((vec_prev_to_current.length() > _nav_rad.get()) && !stay_at_current_pos) { - - /* normalize prev-current line (always > nav_rad) */ - matrix::Vector2f unit_prev_to_current = vec_prev_to_current.normalized(); - - /* unit vector from current to next */ - matrix::Vector2f unit_current_to_next(0.0f, 0.0f); - - if (next_setpoint_valid) { - unit_current_to_next = matrix::Vector2f((next_sp(0) - pos_sp(0)), (next_sp(1) - pos_sp(1))); - unit_current_to_next = (unit_current_to_next.length() > SIGMA_NORM) ? unit_current_to_next.normalized() : - unit_current_to_next; - } - - /* point on line closest to pos */ - matrix::Vector2f closest_point = matrix::Vector2f(_prev_pos_sp(0), _prev_pos_sp(1)) + unit_prev_to_current * - (matrix::Vector2f((_pos(0) - _prev_pos_sp(0)), (_pos(1) - _prev_pos_sp(1))) * unit_prev_to_current); - - matrix::Vector2f vec_closest_to_current((_curr_pos_sp(0) - closest_point(0)), (_curr_pos_sp(1) - closest_point(1))); - - /* compute vector from position-current and previous-position */ - matrix::Vector2f vec_prev_to_pos((_pos(0) - _prev_pos_sp(0)), (_pos(1) - _prev_pos_sp(1))); - - /* current velocity along track */ - float vel_sp_along_track_prev = matrix::Vector2f(_vel_sp(0), _vel_sp(1)) * unit_prev_to_current; - - if (use_obstacle_avoidance()) { - vel_sp_along_track_prev = matrix::Vector2f(&_vel_sp_desired(0)) * unit_prev_to_current; - } - - /* distance to target when brake should occur */ - float target_threshold_xy = 1.5f * get_cruising_speed_xy(); - - bool close_to_current = vec_pos_to_current.length() < target_threshold_xy; - bool close_to_prev = (vec_prev_to_pos.length() < target_threshold_xy) && - (vec_prev_to_pos.length() < vec_pos_to_current.length()); - - /* indicates if we are at least half the distance from previous to current close to previous */ - bool is_2_target_threshold = vec_prev_to_current.length() >= 2.0f * target_threshold_xy; - - /* check if the current setpoint is behind */ - bool current_behind = ((vec_pos_to_current * -1.0f) * unit_prev_to_current) > 0.0f; - - /* check if the previous is in front */ - bool previous_in_front = (vec_prev_to_pos * unit_prev_to_current) < 0.0f; - - /* default velocity along line prev-current */ - float vel_sp_along_track = get_cruising_speed_xy(); - - /* - * compute velocity setpoint along track - */ - - /* only go directly to previous setpoint if more than 5m away and previous in front*/ - if (previous_in_front && (vec_prev_to_pos.length() > 5.0f)) { - - /* just use the default velocity along track */ - vel_sp_along_track = vec_prev_to_pos.length() * _pos_p(0); - - if (vel_sp_along_track > get_cruising_speed_xy()) { - vel_sp_along_track = get_cruising_speed_xy(); - } - - } else if (current_behind) { - /* go directly to current setpoint */ - vel_sp_along_track = vec_pos_to_current.length() * _pos_p(0); - vel_sp_along_track = (vel_sp_along_track < get_cruising_speed_xy()) ? vel_sp_along_track : get_cruising_speed_xy(); - - } else if (close_to_prev) { - /* accelerate from previous setpoint towards current setpoint */ - - /* we are close to previous and current setpoint - * we first compute the start velocity when close to current septoint and use - * this velocity as final velocity when transition occurs from acceleration to deceleration. - * This ensures smooth transition */ - float final_cruise_speed = get_cruising_speed_xy(); - - if (!is_2_target_threshold) { - - /* set target threshold to half dist pre-current */ - float target_threshold_tmp = target_threshold_xy; - target_threshold_xy = vec_prev_to_current.length() * 0.5f; - - if ((target_threshold_xy - _nav_rad.get()) < SIGMA_NORM) { - target_threshold_xy = _nav_rad.get(); - } - - /* velocity close to current setpoint with default zero if no next setpoint is available */ - float vel_close = 0.0f; - float acceptance_radius = 0.0f; - - /* we want to pass and need to compute the desired velocity close to current setpoint */ - if (next_setpoint_valid && !(_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER)) { - /* get velocity close to current that depends on angle between prev-current and current-next line */ - vel_close = get_vel_close(unit_prev_to_current, unit_current_to_next); - acceptance_radius = _nav_rad.get(); - } - - /* compute velocity at transition where vehicle switches from acceleration to deceleration */ - if ((target_threshold_tmp - acceptance_radius) < SIGMA_NORM) { - final_cruise_speed = vel_close; - - } else { - float slope = (get_cruising_speed_xy() - vel_close) / (target_threshold_tmp - acceptance_radius); - final_cruise_speed = slope * (target_threshold_xy - acceptance_radius) + vel_close; - final_cruise_speed = (final_cruise_speed > vel_close) ? final_cruise_speed : vel_close; - } - } - - /* make sure final cruise speed is larger than 0*/ - final_cruise_speed = (final_cruise_speed > SIGMA_NORM) ? final_cruise_speed : SIGMA_NORM; - vel_sp_along_track = final_cruise_speed; - - /* we want to accelerate not too fast - * TODO: change the name acceleration_hor_man to something that can - * be used by auto and manual */ - float acc_track = (final_cruise_speed - vel_sp_along_track_prev) / _dt; - - /* if yaw offset is large, only accelerate with 0.5m/s^2 */ - float acc = (fabsf(yaw_diff) > math::radians(_mis_yaw_error.get())) ? 0.5f : _acceleration_hor.get(); - - if (acc_track > acc) { - vel_sp_along_track = acc * _dt + vel_sp_along_track_prev; - } - - /* enforce minimum cruise speed */ - vel_sp_along_track = math::constrain(vel_sp_along_track, SIGMA_NORM, final_cruise_speed); - - } else if (close_to_current) { - /* slow down when close to current setpoint */ - - /* check if altidue is within acceptance radius */ - bool reached_altitude = (dist_to_current_z < _nav_rad.get()) ? true : false; - - if (reached_altitude && next_setpoint_valid - && !(_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER)) { - /* since we have a next setpoint use the angle prev-current-next to compute velocity setpoint limit */ - - /* get velocity close to current that depends on angle between prev-current and current-next line */ - float vel_close = get_vel_close(unit_prev_to_current, unit_current_to_next); - - /* compute velocity along line which depends on distance to current setpoint */ - if (vec_closest_to_current.length() < _nav_rad.get()) { - vel_sp_along_track = vel_close; - - } else { - - if (target_threshold_xy - _nav_rad.get() < SIGMA_NORM) { - vel_sp_along_track = vel_close; - - } else { - float slope = (get_cruising_speed_xy() - vel_close) / (target_threshold_xy - _nav_rad.get()) ; - vel_sp_along_track = slope * (vec_closest_to_current.length() - _nav_rad.get()) + vel_close; - } - } - - /* since we want to slow down take over previous velocity setpoint along track if it was lower */ - if ((vel_sp_along_track_prev < vel_sp_along_track) && (vel_sp_along_track * vel_sp_along_track_prev > 0.0f)) { - vel_sp_along_track = vel_sp_along_track_prev; - } - - /* if we are close to target and the previous velocity setpoints was smaller than - * vel_sp_along_track, then take over the previous one - * this ensures smoothness since we anyway want to slow down - */ - if ((vel_sp_along_track_prev < vel_sp_along_track) && (vel_sp_along_track * vel_sp_along_track_prev > 0.0f) - && (vel_sp_along_track_prev > vel_close)) { - vel_sp_along_track = vel_sp_along_track_prev; - } - - /* make sure that vel_sp_along track is at least min */ - vel_sp_along_track = (vel_sp_along_track < vel_close) ? vel_close : vel_sp_along_track; - - - } else { - - /* we want to stop at current setpoint */ - float slope = (get_cruising_speed_xy()) / target_threshold_xy; - vel_sp_along_track = slope * (vec_closest_to_current.length()); - - /* since we want to slow down take over previous velocity setpoint along track if it was lower but ensure its not zero */ - if ((vel_sp_along_track_prev < vel_sp_along_track) && (vel_sp_along_track * vel_sp_along_track_prev > 0.0f) - && (vel_sp_along_track_prev > 0.5f)) { - vel_sp_along_track = vel_sp_along_track_prev; - } - } - } - - /* compute velocity orthogonal to prev-current-line to position*/ - matrix::Vector2f vec_pos_to_closest = closest_point - matrix::Vector2f(_pos(0), _pos(1)); - float vel_sp_orthogonal = vec_pos_to_closest.length() * _pos_p(0); - - /* compute the cruise speed from velocity along line and orthogonal velocity setpoint */ - float cruise_sp_mag = sqrtf(vel_sp_orthogonal * vel_sp_orthogonal + vel_sp_along_track * vel_sp_along_track); - - /* sanity check */ - cruise_sp_mag = (PX4_ISFINITE(cruise_sp_mag)) ? cruise_sp_mag : vel_sp_orthogonal; - - /* orthogonal velocity setpoint is smaller than cruise speed */ - if (vel_sp_orthogonal < get_cruising_speed_xy() && !current_behind) { - - /* we need to limit vel_sp_along_track such that cruise speed is never exceeded but still can keep velocity orthogonal to track */ - if (cruise_sp_mag > get_cruising_speed_xy()) { - vel_sp_along_track = sqrtf(get_cruising_speed_xy() * get_cruising_speed_xy() - vel_sp_orthogonal * vel_sp_orthogonal); - } - - pos_sp(0) = closest_point(0) + unit_prev_to_current(0) * vel_sp_along_track / _pos_p(0); - pos_sp(1) = closest_point(1) + unit_prev_to_current(1) * vel_sp_along_track / _pos_p(1); - - } else if (current_behind) { - /* current is behind */ - - if (vec_pos_to_current.length() > 0.01f) { - pos_sp(0) = _pos(0) + vec_pos_to_current(0) / vec_pos_to_current.length() * vel_sp_along_track / _pos_p(0); - pos_sp(1) = _pos(1) + vec_pos_to_current(1) / vec_pos_to_current.length() * vel_sp_along_track / _pos_p(1); - - } else { - pos_sp(0) = _curr_pos_sp(0); - pos_sp(1) = _curr_pos_sp(1); - } - - } else { - /* we are more than cruise_speed away from track */ - - /* if previous is in front just go directly to previous point */ - if (previous_in_front) { - vec_pos_to_closest(0) = _prev_pos_sp(0) - _pos(0); - vec_pos_to_closest(1) = _prev_pos_sp(1) - _pos(1); - } - - /* make sure that we never exceed maximum cruise speed */ - float cruise_sp = vec_pos_to_closest.length() * _pos_p(0); - - if (cruise_sp > get_cruising_speed_xy()) { - cruise_sp = get_cruising_speed_xy(); - } - - /* sanity check: don't divide by zero */ - if (vec_pos_to_closest.length() > SIGMA_NORM) { - pos_sp(0) = _pos(0) + vec_pos_to_closest(0) / vec_pos_to_closest.length() * cruise_sp / _pos_p(0); - pos_sp(1) = _pos(1) + vec_pos_to_closest(1) / vec_pos_to_closest.length() * cruise_sp / _pos_p(1); - - } else { - pos_sp(0) = closest_point(0); - pos_sp(1) = closest_point(1); - } - } - } - - _pos_sp = pos_sp; - - } else if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_VELOCITY) { - - float vel_xy_mag = sqrtf(_vel(0) * _vel(0) + _vel(1) * _vel(1)); - - if (vel_xy_mag > SIGMA_NORM) { - _vel_sp(0) = _vel(0) / vel_xy_mag * get_cruising_speed_xy(); - _vel_sp(1) = _vel(1) / vel_xy_mag * get_cruising_speed_xy(); - - } else { - /* TODO: we should go in the direction we are heading - * if current velocity is zero - */ - _vel_sp(0) = 0.0f; - _vel_sp(1) = 0.0f; - } - - _run_pos_control = false; - - } else { - /* just go to the target point */; - _pos_sp = _curr_pos_sp; - - /* set max velocity to cruise */ - _vel_max_xy = get_cruising_speed_xy(); - } - - /* sanity check */ - if (!(PX4_ISFINITE(_pos_sp(0)) && PX4_ISFINITE(_pos_sp(1)) && - PX4_ISFINITE(_pos_sp(2)))) { - - warn_rate_limited("Auto: Position setpoint not finite"); - _pos_sp = _curr_pos_sp; - } - - - /* - * if we're already near the current takeoff setpoint don't reset in case we switch back to posctl. - * this makes the takeoff finish smoothly. - */ - if ((_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF - || _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) - && _pos_sp_triplet.current.acceptance_radius > 0.0f - /* need to detect we're close a bit before the navigator switches from takeoff to next waypoint */ - && (_pos - _pos_sp).length() < _pos_sp_triplet.current.acceptance_radius * 1.2f) { - _do_reset_alt_pos_flag = false; - - } else { - /* otherwise: in case of interrupted mission don't go to waypoint but stay at current position */ - _do_reset_alt_pos_flag = true; - } - - // Handle the landing gear based on the manual landing alt - const bool high_enough_for_landing_gear = (-_pos(2) + _home_pos.z > 2.0f); - - // During a mission or in loiter it's safe to retract the landing gear. - if ((_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION || - _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) && - !_vehicle_land_detected.landed && - high_enough_for_landing_gear) { - - _att_sp.landing_gear = vehicle_attitude_setpoint_s::LANDING_GEAR_UP; - - } else if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF || - _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND || - !high_enough_for_landing_gear) { - - // During takeoff and landing, we better put it down again. - _att_sp.landing_gear = vehicle_attitude_setpoint_s::LANDING_GEAR_DOWN; - - // For the rest of the setpoint types, just leave it as is. - } - - } else { - /* idle or triplet not valid, set velocity setpoint to zero */ - _vel_sp.zero(); - _run_pos_control = false; - _run_alt_control = false; - } -} - -void -MulticopterPositionControl::wrap_yaw_speed(float yaw_speed) -{ - /* we want to know the real constraint, and global overrides manual */ - const float yaw_rate_max = (_man_yaw_max < _global_yaw_max) ? _man_yaw_max : _global_yaw_max; - const float yaw_offset_max = yaw_rate_max / _mc_att_yaw_p.get(); - - float yaw_target = wrap_pi(_att_sp.yaw_body + yaw_speed * _dt); - float yaw_offs = wrap_pi(yaw_target - _yaw); - - // If the yaw offset became too big for the system to track stop - // shifting it, only allow if it would make the offset smaller again. - if (fabsf(yaw_offs) < yaw_offset_max || - (yaw_speed > 0 && yaw_offs < 0) || - (yaw_speed < 0 && yaw_offs > 0)) { - _att_sp.yaw_body = yaw_target; - } -} - -void -MulticopterPositionControl::update_velocity_derivative() -{ - /* Update velocity derivative, - * independent of the current flight mode - */ if (_local_pos.timestamp == 0) { return; } - // TODO: this logic should be in the estimator, not the controller! - if (PX4_ISFINITE(_local_pos.x) && - PX4_ISFINITE(_local_pos.y) && - PX4_ISFINITE(_local_pos.z)) { - - _pos(0) = _local_pos.x; - _pos(1) = _local_pos.y; - - if ((_alt_mode.get() == 1) && _local_pos.dist_bottom_valid) { - if (!_terrain_follow) { - _terrain_follow = true; - _reset_alt_sp = true; - reset_alt_sp(); - } - - _pos(2) = -_local_pos.dist_bottom; - - } else { - if (_terrain_follow) { - _terrain_follow = false; - _reset_alt_sp = true; - reset_alt_sp(); - } - - _pos(2) = _local_pos.z; - } - } - - if (PX4_ISFINITE(_local_pos.vx) && - PX4_ISFINITE(_local_pos.vy) && - PX4_ISFINITE(_local_pos.vz)) { - - _vel(0) = _local_pos.vx; - _vel(1) = _local_pos.vy; - - if (_terrain_follow) { - _vel(2) = -_local_pos.dist_bottom_rate; - - } else { - _vel(2) = _local_pos.vz; - } - - if (!_run_alt_control) { - /* set velocity to the derivative of position - * because it has less bias but blend it in across the landing speed range*/ - float weighting = fminf(fabsf(_vel_sp(2)) / _land_speed.get(), 1.0f); - _vel(2) = _z_derivative * weighting + _vel(2) * (1.0f - weighting); - - } - - } - - if (PX4_ISFINITE(_local_pos.z_deriv)) { - _z_derivative = _local_pos.z_deriv; - }; - - _vel_err_d(0) = _vel_x_deriv.update(-_vel(0)); - - _vel_err_d(1) = _vel_y_deriv.update(-_vel(1)); - - _vel_err_d(2) = _vel_z_deriv.update(-_vel(2)); -} - -void -MulticopterPositionControl::do_control() -{ - /* by default, run position/altitude controller. the control_* functions - * can disable this and run velocity controllers directly in this cycle */ - _run_pos_control = true; - _run_alt_control = true; - - if (_control_mode.flag_control_manual_enabled) { - /* manual control */ - control_manual(); - _mode_auto = false; - - /* we set triplets to false - * this ensures that when switching to auto, the position - * controller will not use the old triplets but waits until triplets - * have been updated */ - _pos_sp_triplet.current.valid = false; - _pos_sp_triplet.previous.valid = false; - _curr_pos_sp = matrix::Vector3f(NAN, NAN, NAN); - - _hold_offboard_xy = false; - _hold_offboard_z = false; + // only set position states if valid and finite + if (PX4_ISFINITE(_local_pos.x) && PX4_ISFINITE(_local_pos.y) && _local_pos.xy_valid) { + _states.position(0) = _local_pos.x; + _states.position(1) = _local_pos.y; } else { - /* reset acceleration to default */ - _acceleration_state_dependent_xy = _acceleration_hor_max.get(); - _acceleration_state_dependent_z = _acceleration_z_max_up.get(); - control_non_manual(); + _states.position(0) = _states.position(1) = NAN; } -} -void -MulticopterPositionControl::control_position() -{ - calculate_velocity_setpoint(); - - if (_control_mode.flag_control_climb_rate_enabled || _control_mode.flag_control_velocity_enabled || - _control_mode.flag_control_acceleration_enabled) { - calculate_thrust_setpoint(); + if (PX4_ISFINITE(_local_pos.z) && _local_pos.z_valid) { + _states.position(2) = _local_pos.z; } else { - _reset_int_z = true; - } -} - -void -MulticopterPositionControl::calculate_velocity_setpoint() -{ - if (use_avoidance_position_waypoint()) { - execute_avoidance_position_waypoint(); + _states.position(2) = NAN; } - /* run position & altitude controllers, if enabled (otherwise use already computed velocity setpoints) */ - if (_run_pos_control) { - - // If for any reason, we get a NaN position setpoint, we better just stay where we are. - if (PX4_ISFINITE(_pos_sp(0)) && PX4_ISFINITE(_pos_sp(1))) { - _vel_sp(0) = (_pos_sp(0) - _pos(0)) * _pos_p(0); - _vel_sp(1) = (_pos_sp(1) - _pos(1)) * _pos_p(1); - - } else { - _vel_sp(0) = 0.0f; - _vel_sp(1) = 0.0f; - warn_rate_limited("Caught invalid pos_sp in x and y"); - - } - } - - /* in auto the setpoint is already limited by the navigator */ - if (!_control_mode.flag_control_auto_enabled) { - limit_altitude(); - } - - if (_run_alt_control) { - if (PX4_ISFINITE(_pos_sp(2))) { - _vel_sp(2) = (_pos_sp(2) - _pos(2)) * _pos_p(2); - - } else { - _vel_sp(2) = 0.0f; - warn_rate_limited("Caught invalid pos_sp in z"); - } - } - - if (!_control_mode.flag_control_position_enabled) { - _reset_pos_sp = true; - } - - if (!_control_mode.flag_control_altitude_enabled) { - _reset_alt_sp = true; - } - - if (!_control_mode.flag_control_velocity_enabled) { - _vel_sp_prev(0) = _vel(0); - _vel_sp_prev(1) = _vel(1); - _vel_sp(0) = 0.0f; - _vel_sp(1) = 0.0f; - } - - if (!_control_mode.flag_control_climb_rate_enabled) { - _vel_sp(2) = 0.0f; - } - - /* limit vertical upwards speed in auto takeoff and close to ground */ - float altitude_above_home = -_pos(2) + _home_pos.z; - - if (_pos_sp_triplet.current.valid - && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF - && !_control_mode.flag_control_manual_enabled) { - float vel_limit = math::gradual(altitude_above_home, - _slow_land_alt2.get(), _slow_land_alt1.get(), - _tko_speed.get(), _vel_max_up.get()); - _vel_sp(2) = math::max(_vel_sp(2), -vel_limit); - } - - /* encourage pilot to respect estimator height limitations when in manually controlled modes and not landing */ - if (PX4_ISFINITE(_local_pos.hagl_min) // We need height limiting - && _control_mode.flag_control_manual_enabled // Vehicle is under manual control - && _control_mode.flag_control_altitude_enabled // Altitude controller is running - && !manual_wants_landing()) { // Operator is not trying to land - if (_local_pos.dist_bottom < _local_pos.hagl_min) { - // If distance to ground is less than limit, increment setpoint upwards at up to the landing descent rate - float climb_rate_bias = fminf(1.5f * _pos_p(2) * (_local_pos.hagl_min - _local_pos.dist_bottom), _land_speed.get()); - _vel_sp(2) -= climb_rate_bias; - _pos_sp(2) -= climb_rate_bias * _dt; - - } - } - - /* limit vertical downwards speed (positive z) close to ground - * for now we use the altitude above home and assume that we want to land at same height as we took off */ - float vel_limit = math::gradual(altitude_above_home, - _slow_land_alt2.get(), _slow_land_alt1.get(), - _land_speed.get(), _vel_max_down.get()); - - _vel_sp(2) = math::min(_vel_sp(2), vel_limit); - - /* apply slewrate (aka acceleration limit) for smooth flying */ - if (!_control_mode.flag_control_auto_enabled && !_in_smooth_takeoff) { - vel_sp_slewrate(); - } - - /* special velocity setpoint limitation for smooth takeoff (after slewrate!) */ - if (_in_smooth_takeoff) { - set_takeoff_velocity(_vel_sp(2)); - } - - /* constrain velocity: this is desired velocity before any obstacle avoidance */ - constrain_velocity_setpoint(); - _vel_sp_desired = matrix::Vector3f(_vel_sp(0), _vel_sp(1), _vel_sp(2)); - - /* check obstacle avoidance */ - if (use_avoidance_velocity_waypoint() && !_in_smooth_takeoff) { - - execute_avoidance_velocity_waypoint(); - - /* If obstacle avoidanc is active, the previous velocity - * setpoint will be set to desired setpoint to ensure that setpoint - * increases linearly with acceleration. - */ - _vel_sp_prev = _vel_sp_desired; + if (PX4_ISFINITE(_local_pos.vx) && PX4_ISFINITE(_local_pos.vy) && _local_pos.v_xy_valid) { + _states.velocity(0) = _local_pos.vx; + _states.velocity(1) = _local_pos.vy; + _states.acceleration(0) = _vel_x_deriv.update(-_states.velocity(0)); + _states.acceleration(1) = _vel_y_deriv.update(-_states.velocity(1)); } else { - _vel_sp_prev = _vel_sp; + _states.velocity(0) = _states.velocity(1) = NAN; + _states.acceleration(0) = _states.acceleration(1) = NAN; + + // since no valid velocity, update derivate with 0 + _vel_x_deriv.update(0.0f); + _vel_y_deriv.update(0.0f); } - /* make sure velocity setpoint is constrained in all directions (xyz) */ - float vel_norm_xy = sqrtf(_vel_sp(0) * _vel_sp(0) + _vel_sp(1) * _vel_sp(1)); + if (PX4_ISFINITE(_local_pos.vz)) { - /* check if the velocity demand is significant */ - _vel_sp_significant = vel_norm_xy > 0.5f * _vel_max_xy; - -} - -void -MulticopterPositionControl::calculate_thrust_setpoint() -{ - /* reset integrals if needed */ - if (_control_mode.flag_control_climb_rate_enabled) { - if (_reset_int_z) { - _reset_int_z = false; - _thrust_int(2) = 0.0f; + if (PX4_ISFINITE(vel_sp_z) && fabsf(vel_sp_z) > FLT_EPSILON && PX4_ISFINITE(_local_pos.z_deriv)) { + // A change in velocity is demanded. Set velocity to the derivative of position + // because it has less bias but blend it in across the landing speed range + float weighting = fminf(fabsf(vel_sp_z) / _land_speed.get(), 1.0f); + _states.velocity(2) = _local_pos.z_deriv * weighting + _local_pos.vz * (1.0f - weighting); } + _states.velocity(2) = _local_pos.vz; + _states.acceleration(2) = _vel_z_deriv.update(-_states.velocity(2)); + } else { - _reset_int_z = true; + _states.velocity(2) = _states.acceleration(2) = NAN; + // since no valid velocity, update derivate with 0 + _vel_z_deriv.update(0.0f); + } - if (_control_mode.flag_control_velocity_enabled) { - if (_reset_int_xy) { - _reset_int_xy = false; - _thrust_int(0) = 0.0f; - _thrust_int(1) = 0.0f; - } - - } else { - _reset_int_xy = true; + if (PX4_ISFINITE(_local_pos.yaw)) { + _states.yaw = _local_pos.yaw; } - - /* if any of the velocity setpoint is bogus, it's probably safest to command no velocity at all. */ - for (int i = 0; i < 3; ++i) { - if (!PX4_ISFINITE(_vel_sp(i))) { - _vel_sp(i) = 0.0f; - } - } - - /* velocity error */ - matrix::Vector3f vel_err = _vel_sp - _vel; - - /* thrust vector in NED frame */ - matrix::Vector3f thrust_sp; - - if (_control_mode.flag_control_acceleration_enabled && _pos_sp_triplet.current.acceleration_valid) { - thrust_sp = matrix::Vector3f(_pos_sp_triplet.current.a_x, _pos_sp_triplet.current.a_y, _pos_sp_triplet.current.a_z); - - } else { - thrust_sp = vel_err.emult(_vel_p) + _vel_err_d.emult(_vel_d) - + _thrust_int - matrix::Vector3f(0.0f, 0.0f, _thr_hover.get()); - } - - if (!_control_mode.flag_control_velocity_enabled && !_control_mode.flag_control_acceleration_enabled) { - thrust_sp(0) = 0.0f; - thrust_sp(1) = 0.0f; - } - - // reduce thrust in early landing detection states to check if the vehicle still moves - landdetection_thrust_limit(thrust_sp); - - if (!_control_mode.flag_control_climb_rate_enabled && !_control_mode.flag_control_acceleration_enabled) { - thrust_sp(2) = 0.0f; - } - - /* limit thrust vector and check for saturation */ - bool saturation_xy = false; - bool saturation_z = false; - - /* limit min lift */ - float thr_min = _thr_min.get(); - - if (!_control_mode.flag_control_velocity_enabled && thr_min < 0.0f) { - /* don't allow downside thrust direction in manual attitude mode */ - thr_min = 0.0f; - } - - float tilt_max = _tilt_max_air; - float thr_max = _thr_max.get(); - - // We can only run the control if we're already in-air, have a takeoff setpoint, - // or if we're in offboard control. - // Otherwise, we should just bail out - if (_vehicle_land_detected.landed && !in_auto_takeoff() && !manual_wants_takeoff()) { - // Keep throttle low while still on ground. - thr_max = 0.0f; - - } else if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && - _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { - - /* adjust limits for landing mode */ - /* limit max tilt and min lift when landing */ - tilt_max = _tilt_max_land; - } - - /* limit min lift */ - if (-thrust_sp(2) < thr_min) { - thrust_sp(2) = -thr_min; - /* Don't freeze altitude integral if it wants to throttle up */ - saturation_z = vel_err(2) > 0.0f ? true : saturation_z; - } - - if (_control_mode.flag_control_velocity_enabled || _control_mode.flag_control_acceleration_enabled) { - - /* limit max tilt */ - if (thr_min >= 0.0f && tilt_max < M_PI_F / 2.0f - 0.05f) { - /* absolute horizontal thrust */ - float thrust_sp_xy_len = matrix::Vector2f(thrust_sp(0), thrust_sp(1)).length(); - - if (thrust_sp_xy_len > 0.01f) { - /* max horizontal thrust for given vertical thrust*/ - float thrust_xy_max = -thrust_sp(2) * tanf(tilt_max); - - if (thrust_sp_xy_len > thrust_xy_max) { - float k = thrust_xy_max / thrust_sp_xy_len; - thrust_sp(0) *= k; - thrust_sp(1) *= k; - /* Don't freeze x,y integrals if they both want to throttle down */ - saturation_xy = ((vel_err(0) * _vel_sp(0) < 0.0f) && (vel_err(1) * _vel_sp(1) < 0.0f)) ? saturation_xy : true; - } - } - } - } - - if (_control_mode.flag_control_climb_rate_enabled && !_control_mode.flag_control_velocity_enabled) { - /* thrust compensation when vertical velocity but not horizontal velocity is controlled */ - float att_comp; - - const float tilt_cos_max = 0.7f; - - if (_R(2, 2) > tilt_cos_max) { - att_comp = 1.0f / _R(2, 2); - - } else if (_R(2, 2) > 0.0f) { - att_comp = ((1.0f / tilt_cos_max - 1.0f) / tilt_cos_max) * _R(2, 2) + 1.0f; - saturation_z = true; - - } else { - att_comp = 1.0f; - saturation_z = true; - } - - thrust_sp(2) *= att_comp; - } - - /* Calculate desired total thrust amount in body z direction. */ - /* To compensate for excess thrust during attitude tracking errors we - * project the desired thrust force vector F onto the real vehicle's thrust axis in NED: - * body thrust axis [0,0,-1]' rotated by R is: R*[0,0,-1]' = -R_z */ - matrix::Vector3f R_z(_R(0, 2), _R(1, 2), _R(2, 2)); - - /* recalculate because it might have changed */ - float thrust_body_z = thrust_sp.dot(-R_z); - - /* limit max thrust */ - if (fabsf(thrust_body_z) > thr_max) { - if (thrust_sp(2) < 0.0f) { - if (-thrust_sp(2) > thr_max) { - /* thrust Z component is too large, limit it */ - thrust_sp(0) = 0.0f; - thrust_sp(1) = 0.0f; - thrust_sp(2) = -thr_max; - saturation_xy = true; - /* Don't freeze altitude integral if it wants to throttle down */ - saturation_z = vel_err(2) < 0.0f ? true : saturation_z; - - } else { - /* preserve thrust Z component and lower XY, keeping altitude is more important than position */ - float thrust_xy_max = sqrtf(thr_max * thr_max - thrust_sp(2) * thrust_sp(2)); - float thrust_xy_abs = matrix::Vector2f(thrust_sp(0), thrust_sp(1)).length(); - float k = thrust_xy_max / thrust_xy_abs; - thrust_sp(0) *= k; - thrust_sp(1) *= k; - /* Don't freeze x,y integrals if they both want to throttle down */ - saturation_xy = ((vel_err(0) * _vel_sp(0) < 0.0f) && (vel_err(1) * _vel_sp(1) < 0.0f)) ? saturation_xy : true; - } - - } else { - /* Z component is positive, going down (Z is positive down in NED), simply limit thrust vector */ - float k = thr_max / fabsf(thrust_body_z); - thrust_sp *= k; - saturation_xy = true; - saturation_z = true; - } - - thrust_body_z = thr_max; - } - - /* if any of the thrust setpoint is bogus, send out a warning */ - if (!PX4_ISFINITE(thrust_sp(0)) || !PX4_ISFINITE(thrust_sp(1)) || !PX4_ISFINITE(thrust_sp(2))) { - warn_rate_limited("Thrust setpoint not finite"); - } - - _att_sp.thrust = math::max(thrust_body_z, thr_min); - - /* update integrals */ - if (_control_mode.flag_control_velocity_enabled && !saturation_xy) { - _thrust_int(0) += vel_err(0) * _vel_i(0) * _dt; - _thrust_int(1) += vel_err(1) * _vel_i(1) * _dt; - } - - if (_control_mode.flag_control_climb_rate_enabled && !saturation_z) { - _thrust_int(2) += vel_err(2) * _vel_i(2) * _dt; - } - - /* calculate attitude setpoint from thrust vector */ - if (_control_mode.flag_control_velocity_enabled || _control_mode.flag_control_acceleration_enabled) { - /* desired body_z axis = -normalize(thrust_vector) */ - matrix::Vector3f body_x; - matrix::Vector3f body_y; - matrix::Vector3f body_z; - - if (thrust_sp.length() > SIGMA_NORM) { - body_z = -thrust_sp.normalized(); - - } else { - /* no thrust, set Z axis to safe value */ - body_z = {0.0f, 0.0f, 1.0f}; - } - - /* vector of desired yaw direction in XY plane, rotated by PI/2 */ - matrix::Vector3f y_C(-sinf(_att_sp.yaw_body), cosf(_att_sp.yaw_body), 0.0f); - - if (fabsf(body_z(2)) > SIGMA_SINGLE_OP) { - /* desired body_x axis, orthogonal to body_z */ - body_x = y_C % body_z; - - /* keep nose to front while inverted upside down */ - if (body_z(2) < 0.0f) { - body_x = -body_x; - } - - body_x.normalize(); - - } else { - /* desired thrust is in XY plane, set X downside to construct correct matrix, - * but yaw component will not be used actually */ - body_x.zero(); - body_x(2) = 1.0f; - } - - /* desired body_y axis */ - body_y = body_z % body_x; - - /* fill rotation matrix */ - for (int i = 0; i < 3; i++) { - _R_setpoint(i, 0) = body_x(i); - _R_setpoint(i, 1) = body_y(i); - _R_setpoint(i, 2) = body_z(i); - } - - /* copy quaternion setpoint to attitude setpoint topic */ - matrix::Quatf q_sp = _R_setpoint; - q_sp.copyTo(_att_sp.q_d); - _att_sp.q_d_valid = true; - - /* calculate euler angles, for logging only, must not be used for control */ - matrix::Eulerf euler = _R_setpoint; - _att_sp.roll_body = euler(0); - _att_sp.pitch_body = euler(1); - /* yaw already used to construct rot matrix, but actual rotation matrix can have different yaw near singularity */ - - } else if (!_control_mode.flag_control_manual_enabled) { - /* autonomous altitude control without position control (failsafe landing), - * force level attitude, don't change yaw */ - _R_setpoint = matrix::Eulerf(0.0f, 0.0f, _att_sp.yaw_body); - - /* copy quaternion setpoint to attitude setpoint topic */ - matrix::Quatf q_sp = _R_setpoint; - q_sp.copyTo(_att_sp.q_d); - _att_sp.q_d_valid = true; - - _att_sp.roll_body = 0.0f; - _att_sp.pitch_body = 0.0f; - } - - /* save thrust setpoint for logging */ - _local_pos_sp.acc_x = thrust_sp(0) * CONSTANTS_ONE_G; - _local_pos_sp.acc_y = thrust_sp(1) * CONSTANTS_ONE_G; - _local_pos_sp.acc_z = thrust_sp(2) * CONSTANTS_ONE_G; - - _att_sp.timestamp = hrt_absolute_time(); -} - -void -MulticopterPositionControl::generate_attitude_setpoint() -{ - // yaw setpoint is integrated over time, but we don't want to integrate the offset's - _att_sp.yaw_body -= _man_yaw_offset; - _man_yaw_offset = 0.f; - - /* reset yaw setpoint to current position if needed */ - if (_reset_yaw_sp) { - _reset_yaw_sp = false; - _att_sp.yaw_body = _yaw; - - } else if (!_vehicle_land_detected.landed && - !(!_control_mode.flag_control_altitude_enabled && _manual.z < 0.1f)) { - - /* do not move yaw while sitting on the ground */ - - /* we want to know the real constraint, and global overrides manual */ - const float yaw_rate_max = (_man_yaw_max < _global_yaw_max) ? _man_yaw_max : _global_yaw_max; - - _att_sp.yaw_sp_move_rate = _manual.r * yaw_rate_max; - - /* check if obstacle avoidance is on */ - if (use_obstacle_avoidance() - && PX4_ISFINITE(_traj_wp_avoidance.waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw_speed)) { - _att_sp.yaw_sp_move_rate = _traj_wp_avoidance.waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw_speed; - } - - wrap_yaw_speed(_att_sp.yaw_sp_move_rate); - } - - /* control throttle directly if no climb rate controller is active */ - if (!_control_mode.flag_control_climb_rate_enabled) { - float thr_val = throttle_curve(_manual.z, _thr_hover.get()); - _att_sp.thrust = math::min(thr_val, _manual_thr_max.get()); - - /* enforce minimum throttle if not landed */ - if (!_vehicle_land_detected.landed) { - _att_sp.thrust = math::max(_att_sp.thrust, _manual_thr_min.get()); - } - } - - /* control roll and pitch directly if no aiding velocity controller is active */ - if (!_control_mode.flag_control_velocity_enabled) { - - /* - * Input mapping for roll & pitch setpoints - * ---------------------------------------- - * This simplest thing to do is map the y & x inputs directly to roll and pitch, and scale according to the max - * tilt angle. - * But this has several issues: - * - The maximum tilt angle cannot easily be restricted. By limiting the roll and pitch separately, - * it would be possible to get to a higher tilt angle by combining roll and pitch (the difference is - * around 15 degrees maximum, so quite noticeable). Limiting this angle is not simple in roll-pitch-space, - * it requires to limit the tilt angle = acos(cos(roll) * cos(pitch)) in a meaningful way (eg. scaling both - * roll and pitch). - * - Moving the stick diagonally, such that |x| = |y|, does not move the vehicle towards a 45 degrees angle. - * The direction angle towards the max tilt in the XY-plane is atan(1/cos(x)). Which means it even depends - * on the tilt angle (for a tilt angle of 35 degrees, it's off by about 5 degrees). - * - * So instead we control the following 2 angles: - * - tilt angle, given by sqrt(x*x + y*y) - * - the direction of the maximum tilt in the XY-plane, which also defines the direction of the motion - * - * This allows a simple limitation of the tilt angle, the vehicle flies towards the direction that the stick - * points to, and changes of the stick input are linear. - */ - const float x = _manual.x * _man_tilt_max; - const float y = _manual.y * _man_tilt_max; - - // we want to fly towards the direction of (x, y), so we use a perpendicular axis angle vector in the XY-plane - matrix::Vector2f v = matrix::Vector2f(y, -x); - float v_norm = v.norm(); // the norm of v defines the tilt angle - - if (v_norm > _man_tilt_max) { // limit to the configured maximum tilt angle - v *= _man_tilt_max / v_norm; - } - - matrix::Quatf q_sp_rpy = matrix::AxisAnglef(v(0), v(1), 0.f); - // The axis angle can change the yaw as well (but only at higher tilt angles. Note: we're talking - // about the world frame here, in terms of body frame the yaw rate will be unaffected). - // This the the formula by how much the yaw changes: - // let a := tilt angle, b := atan(y/x) (direction of maximum tilt) - // yaw = atan(-2 * sin(b) * cos(b) * sin^2(a/2) / (1 - 2 * cos^2(b) * sin^2(a/2))). - matrix::Eulerf euler_sp = q_sp_rpy; - // Since the yaw setpoint is integrated, we extract the offset here, - // so that we can remove it before the next iteration - _man_yaw_offset = euler_sp(2); - - // update the setpoints - _att_sp.roll_body = euler_sp(0); - _att_sp.pitch_body = euler_sp(1); - _att_sp.yaw_body += euler_sp(2); - - /* only if we're a VTOL modify roll/pitch */ - if (_vehicle_status.is_vtol) { - // construct attitude setpoint rotation matrix. modify the setpoints for roll - // and pitch such that they reflect the user's intention even if a yaw error - // (yaw_sp - yaw) is present. In the presence of a yaw error constructing a rotation matrix - // from the pure euler angle setpoints will lead to unexpected attitude behaviour from - // the user's view as the euler angle sequence uses the yaw setpoint and not the current - // heading of the vehicle. - // The effect of that can be seen with: - // - roll/pitch into one direction, keep it fixed (at high angle) - // - apply a fast yaw rotation - // - look at the roll and pitch angles: they should stay pretty much the same as when not yawing - - // calculate our current yaw error - float yaw_error = wrap_pi(_att_sp.yaw_body - _yaw); - - // compute the vector obtained by rotating a z unit vector by the rotation - // given by the roll and pitch commands of the user - matrix::Vector3f zB = {0.0f, 0.0f, 1.0f}; - matrix::Dcmf R_sp_roll_pitch = matrix::Eulerf(_att_sp.roll_body, _att_sp.pitch_body, 0.0f); - matrix::Vector3f z_roll_pitch_sp = R_sp_roll_pitch * zB; - - // transform the vector into a new frame which is rotated around the z axis - // by the current yaw error. this vector defines the desired tilt when we look - // into the direction of the desired heading - matrix::Dcmf R_yaw_correction = matrix::Eulerf(0.0f, 0.0f, -yaw_error); - z_roll_pitch_sp = R_yaw_correction * z_roll_pitch_sp; - - // use the formula z_roll_pitch_sp = R_tilt * [0;0;1] - // R_tilt is computed from_euler; only true if cos(roll) not equal zero - // -> valid if roll is not +-pi/2; - _att_sp.roll_body = -asinf(z_roll_pitch_sp(1)); - _att_sp.pitch_body = atan2f(z_roll_pitch_sp(0), z_roll_pitch_sp(2)); - } - - /* copy quaternion setpoint to attitude setpoint topic */ - matrix::Quatf q_sp = matrix::Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body); - q_sp.copyTo(_att_sp.q_d); - _att_sp.q_d_valid = true; - } - - // Only switch the landing gear up if we are not landed and if - // the user switched from gear down to gear up. - // If the user had the switch in the gear up position and took off ignore it - // until he toggles the switch to avoid retracting the gear immediately on takeoff. - if (_manual.gear_switch == manual_control_setpoint_s::SWITCH_POS_ON && _gear_state_initialized && - !_vehicle_land_detected.landed) { - _att_sp.landing_gear = vehicle_attitude_setpoint_s::LANDING_GEAR_UP; - - } else if (_manual.gear_switch == manual_control_setpoint_s::SWITCH_POS_OFF) { - _att_sp.landing_gear = vehicle_attitude_setpoint_s::LANDING_GEAR_DOWN; - // Switching the gear off does put it into a safe defined state - _gear_state_initialized = true; - } - - _att_sp.timestamp = hrt_absolute_time(); -} - -bool MulticopterPositionControl::manual_wants_takeoff() -{ - const bool has_manual_control_present = _control_mode.flag_control_manual_enabled && _manual.timestamp > 0; - - // Manual takeoff is triggered if the throttle stick is above 65%. - return (has_manual_control_present && (_manual.z > 0.65f || !_control_mode.flag_control_climb_rate_enabled)); -} - -bool MulticopterPositionControl::manual_wants_landing() -{ - const bool has_manual_control_present = _control_mode.flag_control_manual_enabled && _manual.timestamp > 0; - - // Operator is trying to land if the throttle stick is under 15%. - return (has_manual_control_present && (_manual.z < 0.15f || !_control_mode.flag_control_climb_rate_enabled)); } void MulticopterPositionControl::task_main() { - /* - * do subscriptions - */ + // do subscriptions _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); _vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected)); - _vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); - _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); - _pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); _home_pos_sub = orb_subscribe(ORB_ID(home_position)); - _traj_wp_avoidance_sub = orb_subscribe(ORB_ID(vehicle_trajectory_waypoint)); parameters_update(true); - /* get an initial update for all sensor and status data */ + // get an initial update for all sensor and status data poll_subscriptions(); - /* We really need to know from the beginning if we're landed or in-air. */ + // We really need to know from the beginning if we're landed or in-air. orb_copy(ORB_ID(vehicle_land_detected), _vehicle_land_detected_sub, &_vehicle_land_detected); - bool was_landed = true; - hrt_abstime t_prev = 0; // Let's be safe and have the landing gear down by default _att_sp.landing_gear = vehicle_attitude_setpoint_s::LANDING_GEAR_DOWN; - /* wakeup source */ + // wakeup source px4_pollfd_struct_t fds[1]; fds[0].fd = _local_pos_sub; fds[0].events = POLLIN; while (!_task_should_exit) { - /* wait for up to 20ms for data */ + // wait for up to 20ms for data int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 20); - /* timed out - periodic check for _task_should_exit */ + // timed out - periodic check for _task_should_exit if (pret == 0) { // Go through the loop anyway to copy manual input at 50 Hz. } - /* this is undesirable but not much we can do */ + // this is undesirable but not much we can do if (pret < 0) { warn("poll error %d, %d", pret, errno); continue; @@ -3088,304 +509,155 @@ MulticopterPositionControl::task_main() const float dt = t_prev != 0 ? (t - t_prev) / 1e6f : 0.004f; t_prev = t; - /* set dt for control blocks */ + // set dt for control blocks setDt(dt); - /* Update velocity limits. Use the minimum of the estimator demanded and vehicle - * limits, and allow a minimum of 0.3 m/s for repositioning */ - if (PX4_ISFINITE(_local_pos.vxy_max)) { - _vel_max_xy = fmaxf(fminf(_vel_max_xy_param.get(), _local_pos.vxy_max), 0.3f); + if (_control_mode.flag_armed) { + // as soon vehicle is armed, start flighttask + start_flight_task(); + // arm hysteresis prevents vehicle to takeoff + // before propeller reached idle speed. + _arm_hysteresis.set_state_and_update(true); } else { - /* If the estimator stopped demanding a limit, release the limit gradually */ - if (_vel_sp_significant) { - if (_vel_max_xy < _vel_max_xy_param.get()) { - _vel_max_xy += dt * _acc_max_estimator_xy.get(); + // disable flighttask + _flight_tasks.switchTask(FlightTaskIndex::None); + // reset arm hysteresis + _arm_hysteresis.set_state_and_update(false); + } + + // check if any task is active + if (_flight_tasks.isAnyTaskActive()) { + + // setpoints from flighttask + vehicle_local_position_setpoint_s setpoint; + + // update task + if (!_flight_tasks.update()) { + // FAILSAFE + // Task was not able to update correctly. Do Failsafe. + setpoint.x = setpoint.y = setpoint.z = NAN; + setpoint.vx = setpoint.vy = setpoint.vz = NAN; + setpoint.thrust[0] = setpoint.thrust[1] = setpoint.thrust[2] = NAN; + + if (PX4_ISFINITE(_states.velocity(2))) { + // We have a valid velocity in D-direction. + // descend downwards with landspeed. + setpoint.vz = _land_speed.get(); + setpoint.thrust[0] = setpoint.thrust[1] = 0.0f; + warn_rate_limited("Failsafe: Descend with land-speed."); } else { - _vel_max_xy = _vel_max_xy_param.get(); + // Use the failsafe from the PositionController. + warn_rate_limited("Failsafe: Descend with just attitude control."); + } + + } else { + setpoint = _flight_tasks.getPositionSetpoint(); + } + + vehicle_constraints_s constraints = _flight_tasks.getConstraints(); + + // check if all local states are valid and map accordingly + check_vehicle_states(setpoint.vz); + + // we can only do a smooth takeoff if a valid velocity or position is available and are + // armed long enough + if (_arm_hysteresis.get_state() && PX4_ISFINITE(_states.position(2)) && PX4_ISFINITE(_states.velocity(2))) { + check_for_smooth_takeoff(setpoint.z, setpoint.vz, constraints); + update_smooth_takeoff(setpoint.z, setpoint.vz); + } + + if (_in_smooth_takeoff) { + // during smooth takeoff, constrain speed to takeoff speed + constraints.speed_up = _takeoff_speed; + // disable yaw command + setpoint.yaw = setpoint.yawspeed = NAN; + // don't control position in xy + setpoint.x = setpoint.y = NAN; + setpoint.vx = setpoint.vy = 0.0f; + } + + if (_vehicle_land_detected.landed && !_in_smooth_takeoff && !PX4_ISFINITE(setpoint.thrust[2])) { + // Keep throttle low when landed and NOT in smooth takeoff + setpoint.thrust[0] = setpoint.thrust[1] = setpoint.thrust[2] = 0.0f; + setpoint.x = setpoint.y = setpoint.z = NAN; + setpoint.vx = setpoint.vy = setpoint.vz = NAN; + setpoint.yawspeed = NAN; + setpoint.yaw = _states.yaw; + constraints.landing_gear = vehicle_constraints_s::GEAR_KEEP; + } + + // limit altitude only if local position is valid + if (PX4_ISFINITE(_states.position(2))) {limit_altitude(setpoint);} + + // Update states, setpoints and constraints. + _control.updateConstraints(constraints); + _control.updateState(_states); + _control.updateSetpoint(setpoint); + + // Generate desired thrust and yaw. + _control.generateThrustYawSetpoint(_dt); + + matrix::Vector3f thr_sp = _control.getThrustSetpoint(); + + // Adjust thrust setpoint based on landdetector only if the + // vehicle is NOT in pure Manual mode and NOT in smooth takeoff + if (!_in_smooth_takeoff && !PX4_ISFINITE(setpoint.thrust[2])) {limit_thrust_during_landing(thr_sp);} + + // Fill local position, velocity and thrust setpoint. + _local_pos_sp.timestamp = hrt_absolute_time(); + _local_pos_sp.x = _control.getPosSp()(0); + _local_pos_sp.y = _control.getPosSp()(1); + _local_pos_sp.z = _control.getPosSp()(2); + _local_pos_sp.yaw = _control.getYawSetpoint(); + _local_pos_sp.yawspeed = _control.getYawspeedSetpoint(); + + _local_pos_sp.vx = _control.getVelSp()(0); + _local_pos_sp.vy = _control.getVelSp()(1); + _local_pos_sp.vz = _control.getVelSp()(2); + thr_sp.copyTo(_local_pos_sp.thrust); + + // Fill attitude setpoint. Attitude is computed from yaw and thrust setpoint. + _att_sp = ControlMath::thrustToAttitude(thr_sp, _control.getYawSetpoint()); + _att_sp.yaw_sp_move_rate = _control.getYawspeedSetpoint(); + _att_sp.fw_control_yaw = false; + _att_sp.disable_mc_yaw_control = false; + _att_sp.apply_flaps = false; + + if (!constraints.landing_gear) { + if (constraints.landing_gear == vehicle_constraints_s::GEAR_UP) { + _att_sp.landing_gear = vehicle_attitude_setpoint_s::LANDING_GEAR_UP; + } + + if (constraints.landing_gear == vehicle_constraints_s::GEAR_DOWN) { + _att_sp.landing_gear = vehicle_attitude_setpoint_s::LANDING_GEAR_DOWN; } } - } - - /* reset flags when landed */ - if (_vehicle_land_detected.landed) { - _reset_pos_sp = true; - _reset_alt_sp = true; - _do_reset_alt_pos_flag = true; - _mode_auto = false; - _pos_hold_engaged = false; - _alt_hold_engaged = false; - _run_pos_control = true; - _run_alt_control = true; - _reset_int_z = true; - _reset_int_xy = true; - _reset_yaw_sp = true; - _hold_offboard_xy = false; - _hold_offboard_z = false; - _in_landing = false; - _lnd_reached_ground = false; - - /* also reset previous setpoints */ - _yaw_takeoff = _yaw; - _vel_sp_prev.zero(); - _vel_prev.zero(); - - /* make sure attitude setpoint output "disables" attitude control - * TODO: we need a defined setpoint to do this properly especially when adjusting the mixer */ - _att_sp.thrust = 0.0f; - _att_sp.timestamp = hrt_absolute_time(); - - /* reset velocity limit */ - _vel_max_xy = _vel_max_xy_param.get(); - } - - /* reset setpoints and integrators VTOL in FW mode */ - if (_vehicle_status.is_vtol && !_vehicle_status.is_rotary_wing) { - _reset_alt_sp = true; - _reset_int_xy = true; - _reset_int_z = true; - _reset_pos_sp = true; - _reset_yaw_sp = true; - _vel_sp_prev = _vel; - } - - if (!_in_smooth_takeoff && _vehicle_land_detected.landed && _control_mode.flag_armed && - (in_auto_takeoff() || manual_wants_takeoff())) { - _in_smooth_takeoff = true; - // This ramp starts negative and goes to positive later because we want to - // be as smooth as possible. If we start at 0, we alrady jump to hover throttle. - _takeoff_vel_limit = -0.5f; - } - - else if (!_control_mode.flag_armed) { - // If we're disarmed and for some reason were in a smooth takeoff, we reset that. - _in_smooth_takeoff = false; - } - - /* set triplets to invalid if we just landed */ - if (_vehicle_land_detected.landed && !was_landed) { - _pos_sp_triplet.current.valid = false; - } - - was_landed = _vehicle_land_detected.landed; - - update_ref(); - - update_velocity_derivative(); - - // reset the horizontal and vertical position hold flags for non-manual modes - // or if position / altitude is not controlled - if (!_control_mode.flag_control_position_enabled || !_control_mode.flag_control_manual_enabled) { - _pos_hold_engaged = false; - } - - if (!_control_mode.flag_control_altitude_enabled || !_control_mode.flag_control_manual_enabled) { - _alt_hold_engaged = false; - } - - if (_test_flight_tasks.get()) { - switch (_vehicle_status.nav_state) { - case vehicle_status_s::NAVIGATION_STATE_ALTCTL: - _flight_tasks.switchTask(FlightTaskIndex::Altitude); - break; - - case vehicle_status_s::NAVIGATION_STATE_POSCTL: - _flight_tasks.switchTask(FlightTaskIndex::Position); - break; - - case vehicle_status_s::NAVIGATION_STATE_MANUAL: - _flight_tasks.switchTask(FlightTaskIndex::Stabilized); - break; - - default: - /* not supported yet */ - _flight_tasks.switchTask(FlightTaskIndex::None); - } - - } else { - /* make sure to disable any task when we are not testing them */ - _flight_tasks.switchTask(FlightTaskIndex::None); - } - - if (_test_flight_tasks.get() && _flight_tasks.isAnyTaskActive()) { - - _flight_tasks.update(); - - /* Get Flighttask setpoints */ - vehicle_local_position_setpoint_s setpoint = _flight_tasks.getPositionSetpoint(); - - /* Get _contstraints depending on flight mode - * This logic will be set by FlightTasks */ - Controller::Constraints constraints; - updateConstraints(constraints); - - /* For takeoff we adjust the velocity setpoint in the z-direction */ - if (_in_smooth_takeoff) { - /* Adjust velocity setpoint in z if we are in smooth takeoff */ - set_takeoff_velocity(setpoint.vz); - } - - /* this logic is only temporary. - * Mode switch related things will be handled within - * Flighttask activate method - */ - if (_vehicle_status.nav_state - == _vehicle_status.NAVIGATION_STATE_MANUAL) { - /* we set triplets to false - * this ensures that when switching to auto, the position - * controller will not use the old triplets but waits until triplets - * have been updated */ - _mode_auto = false; - _pos_sp_triplet.current.valid = false; - _pos_sp_triplet.previous.valid = false; - _hold_offboard_xy = false; - _hold_offboard_z = false; - - } - - // We can only run the control if we're already in-air, have a takeoff setpoint, - // or if we're in offboard control. - // Otherwise, we should just bail out - if (_vehicle_land_detected.landed && !in_auto_takeoff() && !manual_wants_takeoff()) { - // Keep throttle low while still on ground. - set_idle_state(); - - } else if (_vehicle_status.nav_state == _vehicle_status.NAVIGATION_STATE_MANUAL || - _vehicle_status.nav_state == _vehicle_status.NAVIGATION_STATE_POSCTL || - _vehicle_status.nav_state == _vehicle_status.NAVIGATION_STATE_ALTCTL) { - - - _control.updateState(_local_pos, matrix::Vector3f(&(_vel_err_d(0)))); - _control.updateSetpoint(setpoint); - _control.updateConstraints(constraints); - _control.generateThrustYawSetpoint(_dt); - - /* fill local position, velocity and thrust setpoint */ - _local_pos_sp.timestamp = hrt_absolute_time(); - _local_pos_sp.x = _control.getPosSp()(0); - _local_pos_sp.y = _control.getPosSp()(1); - _local_pos_sp.z = _control.getPosSp()(2); - _local_pos_sp.yaw = _control.getYawSetpoint(); - _local_pos_sp.yawspeed = _control.getYawspeedSetpoint(); - _local_pos_sp.vx = _control.getVelSp()(0); - _local_pos_sp.vy = _control.getVelSp()(1); - _local_pos_sp.vz = _control.getVelSp()(2); - _control.getThrustSetpoint().copyTo(_local_pos_sp.thrust); - - /* We adjust thrust setpoint based on landdetector */ - matrix::Vector3f thr_sp = _control.getThrustSetpoint(); - landdetection_thrust_limit(thr_sp); //TODO: only do that if not in pure manual - - _att_sp = ControlMath::thrustToAttitude(thr_sp, _control.getYawSetpoint()); - _att_sp.yaw_sp_move_rate = _control.getYawspeedSetpoint(); - - } + // Publish local position setpoint (for logging only) and attitude setpoint (for attitude controller). publish_local_pos_sp(); + + // publish attitude setpoint + // Note: this requires review. The reason for not sending + // an attitude setpoint is because for none-flighttask modes + // the attitude septoint should come from another source, otherwise + // they might conflict with each other such as in offboard attitude control. publish_attitude(); } else { - if (_control_mode.flag_control_altitude_enabled || - _control_mode.flag_control_position_enabled || - _control_mode.flag_control_climb_rate_enabled || - _control_mode.flag_control_velocity_enabled || - _control_mode.flag_control_acceleration_enabled) { - do_control(); - - /* fill local position, velocity and thrust setpoint */ - _local_pos_sp.timestamp = hrt_absolute_time(); - _local_pos_sp.x = _pos_sp(0); - _local_pos_sp.y = _pos_sp(1); - _local_pos_sp.z = _pos_sp(2); - _local_pos_sp.yaw = _att_sp.yaw_body; - _local_pos_sp.vx = _vel_sp(0); - _local_pos_sp.vy = _vel_sp(1); - _local_pos_sp.vz = _vel_sp(2); - - /* desired waypoints for obstacle avoidance: - * point_0 contains the current position with the desired velocity - * point_1 contains _pos_sp_triplet.current if valid - * point_2 contains _pos_sp_triplet.next if valid */ - - update_avoidance_waypoint_desired(vehicle_trajectory_waypoint_s::POINT_0, _pos, _vel_sp_desired, matrix::Vector3f(NAN, - NAN, NAN), _yaw, NAN); - - if (_pos_sp_triplet.current.valid) { - update_avoidance_waypoint_desired(vehicle_trajectory_waypoint_s::POINT_1, _curr_pos_sp, matrix::Vector3f(NAN, NAN, NAN), - matrix::Vector3f(NAN, NAN, NAN), _pos_sp_triplet.current.yaw, NAN); - } - - if (_pos_sp_triplet.next.valid) { - update_avoidance_waypoint_desired(vehicle_trajectory_waypoint_s::POINT_2, _next_pos_sp, matrix::Vector3f(NAN, NAN, NAN), - matrix::Vector3f(NAN, NAN, NAN), _pos_sp_triplet.next.yaw, NAN); - } - - /* publish local position setpoint */ - if (_local_pos_sp_pub != nullptr) { - orb_publish(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_pub, &_local_pos_sp); - - } else { - _local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &_local_pos_sp); - } - - /* publish desired waypoint*/ - if (_traj_wp_avoidance_desired_pub != nullptr) { - orb_publish(ORB_ID(vehicle_trajectory_waypoint_desired), _traj_wp_avoidance_desired_pub, &_traj_wp_avoidance_desired); - - } else { - _traj_wp_avoidance_desired_pub = orb_advertise(ORB_ID(vehicle_trajectory_waypoint_desired), - &_traj_wp_avoidance_desired); - } - - reset_avoidance_waypoint_desired(); - - } else { - /* position controller disabled, reset setpoints */ - _reset_pos_sp = true; - _reset_alt_sp = true; - _do_reset_alt_pos_flag = true; - _mode_auto = false; - _reset_int_z = true; - _reset_int_xy = true; - - /* store last velocity in case a mode switch to position control occurs */ - _vel_sp_prev = _vel; - } - - /* generate attitude setpoint from manual controls */ - if (_control_mode.flag_control_manual_enabled && _control_mode.flag_control_attitude_enabled) { - - generate_attitude_setpoint(); - - } else { - _reset_yaw_sp = true; - _att_sp.yaw_sp_move_rate = 0.0f; - } - - /* update previous velocity for velocity controller D part */ - _vel_prev = _vel; - - /* publish attitude setpoint - * Do not publish if - * - offboard is enabled but position/velocity/accel control is disabled, - * in this case the attitude setpoint is published by the mavlink app. - * - if the vehicle is a VTOL and it's just doing a transition (the VTOL attitude control module will generate - * attitude setpoints for the transition). - */ - if (!(_control_mode.flag_control_offboard_enabled && - !(_control_mode.flag_control_position_enabled || - _control_mode.flag_control_velocity_enabled || - _control_mode.flag_control_acceleration_enabled))) { - - if (_att_sp_pub != nullptr) { - orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp); - - } else if (_attitude_setpoint_id) { - _att_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp); - } - } + // no flighttask is active: set attitude setpoint to idle + _att_sp.roll_body = _att_sp.pitch_body = 0.0f; + _att_sp.yaw_body = _local_pos.yaw; + _att_sp.yaw_sp_move_rate = 0.0f; + _att_sp.fw_control_yaw = false; + _att_sp.disable_mc_yaw_control = false; + _att_sp.apply_flaps = false; + matrix::Quatf q_sp = matrix::Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body); + q_sp.copyTo(_att_sp.q_d); + _att_sp.q_d_valid = true; + _att_sp.thrust = 0.0f; } } @@ -3395,40 +667,200 @@ MulticopterPositionControl::task_main() } void -MulticopterPositionControl::set_takeoff_velocity(float &vel_sp_z) +MulticopterPositionControl::start_flight_task() { - _in_smooth_takeoff = _takeoff_vel_limit < -vel_sp_z; - /* ramp vertical velocity limit up to takeoff speed */ - _takeoff_vel_limit += -vel_sp_z * _dt / _takeoff_ramp_time.get(); - /* limit vertical velocity to the current ramp value */ - vel_sp_z = math::max(vel_sp_z, -_takeoff_vel_limit); + bool task_failure = false; + + // offboard + if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD) { + int error = _flight_tasks.switchTask(FlightTaskIndex::Offboard); + + if (error != 0) { + PX4_WARN("Offboard activation failded with error: %s", _flight_tasks.errorToString(error)); + task_failure = true; + } + } + + // Auto-follow me + if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET) { + int error = _flight_tasks.switchTask(FlightTaskIndex::AutoFollowMe); + + if (error != 0) { + PX4_WARN("Follow-Me activation failed with error: %s", _flight_tasks.errorToString(error)); + task_failure = true; + } + + } else if (_control_mode.flag_control_auto_enabled) { + // Auto relate maneuvers + int error = _flight_tasks.switchTask(FlightTaskIndex::AutoLine); + + if (error != 0) { + PX4_WARN("Auto activation failed with error: %s", _flight_tasks.errorToString(error)); + task_failure = true; + } + } + + // manual position control + if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_POSCTL || task_failure) { + + int error = 0; + + switch (MPC_POS_MODE.get()) { + case 0: + error = _flight_tasks.switchTask(FlightTaskIndex::Position); + break; + + case 1: + error = _flight_tasks.switchTask(FlightTaskIndex::PositionSmooth); + break; + + case 2: + error = _flight_tasks.switchTask(FlightTaskIndex::Sport); + break; + + default: + error = _flight_tasks.switchTask(FlightTaskIndex::Position); + break; + } + + if (error != 0) { + PX4_WARN("Position-Ctrl activation failed with error: %s", _flight_tasks.errorToString(error)); + task_failure = true; + + } else { + task_failure = false; + } + } + + // manual altitude control + if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_ALTCTL || task_failure) { + int error = _flight_tasks.switchTask(FlightTaskIndex::Altitude); + + if (error != 0) { + PX4_WARN("Altitude-Ctrl activation failed with error: %s", _flight_tasks.errorToString(error)); + task_failure = true; + + } else { + task_failure = false; + } + } + + + // manual stabilized control + if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_MANUAL + || _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_STAB || task_failure) { + int error = _flight_tasks.switchTask(FlightTaskIndex::Stabilized); + + if (error != 0) { + PX4_WARN("Stabilized-Ctrl failed with error: %s", _flight_tasks.errorToString(error)); + task_failure = true; + + } else { + task_failure = false; + } + } + + // check task failure + if (task_failure) { + // No task was activated. + _flight_tasks.switchTask(FlightTaskIndex::None); + warn_rate_limited("No Flighttask is running"); + } + +} + +void +MulticopterPositionControl::check_for_smooth_takeoff(const float &z_sp, const float &vz_sp, + const vehicle_constraints_s &constraints) +{ + // Check for smooth takeoff + if (_vehicle_land_detected.landed && !_in_smooth_takeoff) { + // Vehicle is still landed and no takeoff was initiated yet. + // Adjust for different takeoff cases. + // The minimum takeoff altitude needs to be at least 20cm above minimum distance or, if valid, above minimum distance + // above ground. + float min_altitude = PX4_ISFINITE(constraints.min_distance_to_ground) ? (constraints.min_distance_to_ground + 0.05f) : + 0.2f; + + if ((PX4_ISFINITE(z_sp) && z_sp < _states.position(2) - min_altitude) || + (PX4_ISFINITE(vz_sp) && vz_sp < math::min(-_tko_speed.get(), -0.6f))) { + // There is a position setpoint above current position or velocity setpoint larger than + // takeoff speed. Enable smooth takeoff. + _in_smooth_takeoff = true; + _takeoff_speed = -0.5f; + + } else { + // Default + _in_smooth_takeoff = false; + } + } +} + +void +MulticopterPositionControl::update_smooth_takeoff(const float &z_sp, const float &vz_sp) +{ + // If in smooth takeoff, adjust setpoints based on what is valid: + // 1. position setpoint is valid -> go with takeoffspeed to specific altitude + // 2. position setpoint not valid but velocity setpoint valid: ramp up velocity + if (_in_smooth_takeoff) { + float desired_tko_speed = -vz_sp; + + // If there is a valid position setpoint, then set the desired speed to the takeoff speed. + if (PX4_ISFINITE(z_sp)) { + desired_tko_speed = _tko_speed.get(); + } + + // Ramp up takeoff speed. + _takeoff_speed += desired_tko_speed * _dt / _takeoff_ramp_time.get(); + _takeoff_speed = math::min(_takeoff_speed, desired_tko_speed); + + // Smooth takeoff is achieved once desired altitude/velocity setpoint is reached. + if (PX4_ISFINITE(z_sp)) { + _in_smooth_takeoff = _states.position(2) - 0.2f > math::max(z_sp, -MPC_LAND_ALT2.get()); + + } else { + _in_smooth_takeoff = _takeoff_speed < -vz_sp; + } + + } else { + _in_smooth_takeoff = false; + } +} + +void +MulticopterPositionControl::limit_thrust_during_landing(matrix::Vector3f &thr_sp) +{ + if (_vehicle_land_detected.ground_contact) { + // Set thrust in xy to zero + thr_sp(0) = 0.0f; + thr_sp(1) = 0.0f; + // Reset integral in xy is required because PID-controller does + // know about the overwrite and would therefore increase the intragral term + _control.resetIntegralXY(); + } + + if (_vehicle_land_detected.maybe_landed) { + // we set thrust to zero + // this will help to decide if we are actually landed or not + thr_sp.zero(); + // We need to reset all integral terms otherwise the PID-controller + // will continue to integrate + _control.resetIntegralXY(); + _control.resetIntegralZ(); + } } void MulticopterPositionControl::publish_attitude() { - /* publish attitude setpoint - * Do not publish if - * - offboard is enabled but position/velocity/accel control is disabled, - * in this case the attitude setpoint is published by the mavlink app. - * - if the vehicle is a VTOL and it's just doing a transition (the VTOL attitude control module will generate - * attitude setpoints for the transition). - * - if not armed - */ - if (_control_mode.flag_armed && - (!(_control_mode.flag_control_offboard_enabled && - !(_control_mode.flag_control_position_enabled || - _control_mode.flag_control_velocity_enabled || - _control_mode.flag_control_acceleration_enabled)))) { - _att_sp.timestamp = hrt_absolute_time(); + _att_sp.timestamp = hrt_absolute_time(); - if (_att_sp_pub != nullptr) { - orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp); + if (_att_sp_pub != nullptr) { + orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp); - } else if (_attitude_setpoint_id) { - _att_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp); - } + } else if (_attitude_setpoint_id) { + _att_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp); } } @@ -3438,7 +870,7 @@ MulticopterPositionControl::publish_local_pos_sp() _local_pos_sp.timestamp = hrt_absolute_time(); - /* publish local position setpoint */ + // publish local position setpoint if (_local_pos_sp_pub != nullptr) { orb_publish(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_pub, &_local_pos_sp); @@ -3450,203 +882,10 @@ MulticopterPositionControl::publish_local_pos_sp() } } -void -MulticopterPositionControl::set_idle_state() -{ - _local_pos_sp.x = _pos(0); - _local_pos_sp.y = _pos(1); - _local_pos_sp.z = _pos(2) + 1.0f; //1m into ground when idle - _local_pos_sp.vx = 0.0f; - _local_pos_sp.vy = 0.0f; - _local_pos_sp.vz = 1.0f; //1m/s into ground - _local_pos_sp.yaw = _yaw; - _local_pos_sp.yawspeed = 0.0f; - - _att_sp.roll_body = 0.0f; - _att_sp.pitch_body = 0.0f; - _att_sp.yaw_body = _yaw; - _att_sp.yaw_sp_move_rate = 0.0f; - matrix::Quatf q_sp = matrix::Eulerf(0.0f, 0.0f, _yaw); - q_sp.copyTo(_att_sp.q_d); - _att_sp.q_d_valid = true; //TODO: check if this flag is used anywhere - _att_sp.thrust = 0.0f; -} - -void -MulticopterPositionControl::execute_avoidance_position_waypoint() -{ - - _pos_sp = _traj_wp_avoidance.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position; - -} - -void -MulticopterPositionControl::execute_avoidance_velocity_waypoint() -{ - _vel_sp = _traj_wp_avoidance.waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity; - - /* we always constrain velocity since we do not know what the avoidance module sends out */ - constrain_velocity_setpoint(); -} - -bool -MulticopterPositionControl::use_obstacle_avoidance() -{ - - /* check that external obstacle avoidance is sending data and that the first point is valid */ - return (_test_obstacle_avoidance.get() - && (hrt_elapsed_time((hrt_abstime *)&_traj_wp_avoidance.timestamp) < TRAJECTORY_STREAM_TIMEOUT_US) - && (_traj_wp_avoidance.waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid == true) - && ((_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) || - (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL))); -} - -bool -MulticopterPositionControl::use_avoidance_position_waypoint() -{ - return use_obstacle_avoidance() - && PX4_ISFINITE(_traj_wp_avoidance.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[0]) && - PX4_ISFINITE(_traj_wp_avoidance.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[1]) - && PX4_ISFINITE(_traj_wp_avoidance.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[2]); -} - -bool -MulticopterPositionControl::use_avoidance_velocity_waypoint() -{ - return use_obstacle_avoidance() - && PX4_ISFINITE(_traj_wp_avoidance.waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity[0]) && - PX4_ISFINITE(_traj_wp_avoidance.waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity[1]) - && PX4_ISFINITE(_traj_wp_avoidance.waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity[2]); -} - -void MulticopterPositionControl::update_avoidance_waypoint_desired(const int point_number, - const matrix::Vector3f &position_wp, const matrix::Vector3f &velocity_wp, const matrix::Vector3f &acceleration_wp, - const float yaw, const float yaw_speed) -{ - _traj_wp_avoidance_desired.timestamp = hrt_absolute_time(); - _traj_wp_avoidance_desired.type = vehicle_trajectory_waypoint_s::MAV_TRAJECTORY_REPRESENTATION_WAYPOINTS; - - - struct trajectory_waypoint_s *array = nullptr; - - switch (point_number) { - case vehicle_trajectory_waypoint_s::POINT_0: { - array = &_traj_wp_avoidance_desired.waypoints[vehicle_trajectory_waypoint_s::POINT_0]; - - break; - } - - case vehicle_trajectory_waypoint_s::POINT_1: { - array = &_traj_wp_avoidance_desired.waypoints[vehicle_trajectory_waypoint_s::POINT_1]; - - break; - } - - case vehicle_trajectory_waypoint_s::POINT_2: { - array = &_traj_wp_avoidance_desired.waypoints[vehicle_trajectory_waypoint_s::POINT_2]; - - break; - } - - case vehicle_trajectory_waypoint_s::POINT_3: { - array = &_traj_wp_avoidance_desired.waypoints[vehicle_trajectory_waypoint_s::POINT_3]; - - break; - } - - case vehicle_trajectory_waypoint_s::POINT_4: { - array = &_traj_wp_avoidance_desired.waypoints[vehicle_trajectory_waypoint_s::POINT_4]; - - break; - } - - default : - array = &_traj_wp_avoidance_desired.waypoints[vehicle_trajectory_waypoint_s::POINT_0]; - array->point_valid = false; - return; - - } - - position_wp.copyTo(array->position); - velocity_wp.copyTo(array->velocity); - acceleration_wp.copyTo(array->acceleration); - array->yaw = yaw; - array->yaw_speed = yaw_speed; - array->point_valid = true; -} - -void -MulticopterPositionControl::reset_avoidance_waypoint_desired() -{ - - const matrix::Vector3f empty_wp(NAN, NAN, NAN); - - for (int i = 0; i < vehicle_trajectory_waypoint_s::NUMBER_POINTS; ++i) { - empty_wp.copyTo(_traj_wp_avoidance_desired.waypoints[i].position); - empty_wp.copyTo(_traj_wp_avoidance_desired.waypoints[i].velocity); - empty_wp.copyTo(_traj_wp_avoidance_desired.waypoints[i].acceleration); - _traj_wp_avoidance_desired.waypoints[i].yaw = NAN; - _traj_wp_avoidance_desired.waypoints[i].yaw_speed = NAN; - _traj_wp_avoidance_desired.waypoints[i].point_valid = false; - } -} - -void -MulticopterPositionControl::updateConstraints(Controller::Constraints &constraints) -{ - /* _constraints */ - constraints.tilt_max = NAN; // Default no maximum tilt - - /* Set maximum tilt */ - if (!_control_mode.flag_control_manual_enabled - && _pos_sp_triplet.current.valid - && _pos_sp_triplet.current.type - == position_setpoint_s::SETPOINT_TYPE_LAND) { - - /* Auto landing tilt */ - constraints.tilt_max = _tilt_max_land; - - } else { - /* Velocity/acceleration control tilt */ - constraints.tilt_max = _tilt_max_air; - } -} - -void -MulticopterPositionControl::landdetection_thrust_limit(matrix::Vector3f &thrust_sp) -{ - if (!in_auto_takeoff() && !manual_wants_takeoff()) { - if (_vehicle_land_detected.ground_contact) { - /* if still or already on ground command zero xy thrust_sp in body - * frame to consider uneven ground */ - - /* thrust setpoint in body frame*/ - matrix::Vector3f thrust_sp_body = _R.transpose() * thrust_sp; - - /* we dont want to make any correction in body x and y*/ - thrust_sp_body(0) = 0.0f; - thrust_sp_body(1) = 0.0f; - - /* make sure z component of thrust_sp_body is larger than 0 (positive thrust is downward) */ - thrust_sp_body(2) = thrust_sp(2) > 0.0f ? thrust_sp(2) : 0.0f; - - /* convert back to local frame (NED) */ - thrust_sp = _R * thrust_sp_body; - } - - if (_vehicle_land_detected.maybe_landed) { - /* we set thrust to zero - * this will help to decide if we are actually landed or not - */ - thrust_sp.zero(); - } - } -} - int MulticopterPositionControl::start() { - /* start the task */ + // start the task _control_task = px4_task_spawn_cmd("mc_pos_control", SCHED_DEFAULT, SCHED_PRIORITY_POSITION_CONTROL, diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index 3bf3c8265d..1654311f63 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -609,16 +609,24 @@ PARAM_DEFINE_FLOAT(MPC_LAND_ALT2, 5.0f); PARAM_DEFINE_FLOAT(MPC_TKO_RAMP_T, 0.4f); /** - * Flag to test flight tasks instead of legacy functionality - * Temporary Parameter during the transition to flight tasks + * Manual-Position control sub-mode. + * + * The supported sub-modes are: + * 0 Default position control where sticks map to position/velocity directly. Maximum speeds + * is MPC_VEL_MANUAL. + * 1 Smooth position control where setpoints are adjusted based on acceleration limits + * and jerk limits. + * 2 Sport mode that is the same Default position control but with velocity limits set to + * the maximum allowed speeds (MPC_XY_VEL_MAX) * * @min 0 - * @max 1 - * @value 0 Legacy Functionality - * @value 1 Test flight tasks + * @max 2 + * @value 0 Default position control + * @value 1 Smooth position control + * @value 2 Sport position control * @group Multicopter Position Control */ -PARAM_DEFINE_INT32(MPC_FLT_TSK, 0); +PARAM_DEFINE_INT32(MPC_POS_MODE, 1); /** * Flag to enable obstacle avoidance