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 ec80ca1479..ef1da37902 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -54,6 +54,7 @@ #include #include #include +#include #include #include @@ -108,8 +109,30 @@ public: const math::Vector<3> &line_a, const math::Vector<3> &line_b, math::Vector<3> &res); private: - bool _task_should_exit; /**< if true, task should exit */ - bool _gear_state_initialized; ///< true if the gear state has been initialized + + /** 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; + + bool _task_should_exit = false; /** pos_p; math::Vector<3> vel_p; @@ -221,24 +267,6 @@ private: hrt_abstime _ref_timestamp; hrt_abstime _last_warn; - bool _reset_pos_sp; - bool _reset_alt_sp; - bool _do_reset_alt_pos_flag; - bool _mode_auto; - bool _pos_hold_engaged; - bool _alt_hold_engaged; - bool _run_pos_control; - bool _run_alt_control; - bool _triplet_lat_lon_finite; - - bool _reset_int_z = true; - bool _reset_int_xy = true; - bool _reset_int_z_manual = false; - bool _reset_yaw_sp = true; - - bool _hold_offboard_xy = false; - bool _hold_offboard_z = false; - math::Vector<3> _thrust_int; math::Vector<3> _pos; math::Vector<3> _pos_sp; @@ -249,13 +277,16 @@ private: math::Vector<3> _vel_err_d; /**< derivative of current velocity */ math::Vector<3> _curr_pos_sp; /**< current setpoint of the triplets */ math::Vector<3> _prev_pos_sp; /**< previous setpoint of the triples */ + matrix::Vector2f _stick_input_xy_prev; /*for manual controlled mode to detect direction change */ math::Matrix<3, 3> _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 _vel_max_xy; /**< equal to vel_max except in auto mode when close to target */ - - bool _in_takeoff = false; /**< flag for smooth velocity setpoint takeoff ramp */ + 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 _takeoff_vel_limit; /**< velocity limit value which gets ramped up */ // counters for reset events on position and velocity states @@ -339,6 +370,10 @@ private: float get_vel_close(const matrix::Vector2f &unit_prev_to_current, const matrix::Vector2f &unit_current_to_next); + void set_manual_acceleration_xy(matrix::Vector2f &stick_input_xy_NED, const float dt); + + void set_manual_acceleration_z(float &max_acc_z, const float stick_input_z_NED, const float dt); + /** * limit altitude based on several conditions */ @@ -362,10 +397,9 @@ namespace pos_control MulticopterPositionControl *g_control; } + MulticopterPositionControl::MulticopterPositionControl() : SuperBlock(nullptr, "MPC"), - _task_should_exit(false), - _gear_state_initialized(false), _control_task(-1), _mavlink_log_pub(nullptr), @@ -398,31 +432,34 @@ MulticopterPositionControl::MulticopterPositionControl() : _z_vel_man_expo(this, "Z_MAN_EXPO"), _hold_dz(this, "HOLD_DZ"), _acceleration_hor_max(this, "ACC_HOR_MAX", true), - _acceleration_hor_manual(this, "ACC_HOR_MAN", true), + _acceleration_hor(this, "ACC_HOR", true), + _deceleration_hor_slow(this, "DEC_HOR_SLOW", true), _acceleration_z_max_up(this, "ACC_UP_MAX", true), _acceleration_z_max_down(this, "ACC_DOWN_MAX", true), _cruise_speed_90(this, "CRUISE_90", true), _velocity_hor_manual(this, "VEL_MANUAL", true), - _takeoff_ramp_time(this, "TKO_RAMP_T", true), _nav_rad(this, "NAV_ACC_RAD", false), + _takeoff_ramp_time(this, "TKO_RAMP_T", true), + _jerk_hor_max(this, "JERK_MAX", true), + _jerk_hor_min(this, "JERK_MIN", true), _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_timestamp(0), _last_warn(0), - _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), - _triplet_lat_lon_finite(true), _yaw(0.0f), _yaw_takeoff(0.0f), _vel_max_xy(0.0f), + _acceleration_state_dependent_xy(0.0f), + _acceleration_state_dependent_z(0.0f), + _manual_jerk_limit_xy(1.0f), + _manual_jerk_limit_z(1.0f), _takeoff_vel_limit(0.0f), _z_reset_counter(0), _xy_reset_counter(0), @@ -435,6 +472,9 @@ MulticopterPositionControl::MulticopterPositionControl() : _ref_pos = {}; + /* set trigger time for manual direction change detection */ + _manual_direction_change_hysteresis.set_hysteresis_time_from(false, DIRECTION_CHANGE_TRIGGER_TIME_US); + _params.pos_p.zero(); _params.vel_p.zero(); _params.vel_i.zero(); @@ -449,7 +489,7 @@ MulticopterPositionControl::MulticopterPositionControl() : _vel_err_d.zero(); _curr_pos_sp.zero(); _prev_pos_sp.zero(); - + _stick_input_xy_prev = {}; _R.identity(); _R_setpoint.identity(); @@ -485,6 +525,8 @@ MulticopterPositionControl::MulticopterPositionControl() : _params_handles.hold_max_z = param_find("MPC_HOLD_MAX_Z"); _params_handles.alt_mode = param_find("MPC_ALT_MODE"); _params_handles.opt_recover = param_find("VT_OPT_RECOV_EN"); + _params_handles.rc_flt_cutoff = param_find("RC_FLT_CUTOFF"); + _params_handles.rc_flt_smp_rate = param_find("RC_FLT_SMP_RATE"); /* fetch initial parameter values */ parameters_update(true); @@ -587,6 +629,16 @@ MulticopterPositionControl::parameters_update(bool force) _params.hold_max_xy = math::max(0.f, v); param_get(_params_handles.hold_max_z, &v); _params.hold_max_z = math::max(0.f, v); + param_get(_params_handles.rc_flt_smp_rate, &(_params.rc_flt_smp_rate)); + _params.rc_flt_smp_rate = math::max(1.0f, _params.rc_flt_smp_rate); + /* since we use filter to detect manual direction change, take half the cutoff of the stick filtering */ + param_get(_params_handles.rc_flt_cutoff, &(_params.rc_flt_cutoff)); + /* make sure the filter is in its stable region -> fc < fs/2 */ + _params.rc_flt_cutoff = math::constrain(_params.rc_flt_cutoff, 0.1f, (_params.rc_flt_smp_rate / 2.0f) - 1.f); + + /* update filters */ + _filter_manual_pitch.set_cutoff_frequency(_params.rc_flt_smp_rate, _params.rc_flt_cutoff); + _filter_manual_roll.set_cutoff_frequency(_params.rc_flt_smp_rate, _params.rc_flt_cutoff); /* make sure that vel_cruise_xy is always smaller than vel_max */ _params.vel_cruise_xy = math::min(_params.vel_cruise_xy, _params.vel_max_xy); @@ -618,6 +670,28 @@ MulticopterPositionControl::parameters_update(bool force) /* takeoff and land velocities should not exceed maximum */ _params.tko_speed = fminf(_params.tko_speed, _params.vel_max_up); _params.land_speed = fminf(_params.land_speed, _params.vel_max_down); + + /* 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 > decerleration 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; @@ -854,7 +928,7 @@ MulticopterPositionControl::limit_altitude() float delta_t = -_vel(2) / _acceleration_z_max_down.get(); /* predicted position */ - float pos_z_next = _pos(2) + _vel(2) * delta_t + 0.5f * _acceleration_z_max_down.get() * delta_t *delta_t; + float pos_z_next = _pos(2) + _vel(2) * delta_t + 0.5f * _acceleration_z_max_down.get() * delta_t *delta_t; if (pos_z_next <= -_vehicle_land_detected.alt_max) { _pos_sp(2) = -_vehicle_land_detected.alt_max; @@ -959,6 +1033,277 @@ MulticopterPositionControl::get_cruising_speed_xy() _pos_sp_triplet.current.cruising_speed : _params.vel_cruise_xy); } +void +MulticopterPositionControl::set_manual_acceleration_z(float &max_acceleration, const float stick_z, const float dt) +{ + + + /* 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, const float dt) +{ + + /* + * 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(float dt) { @@ -1008,12 +1353,21 @@ MulticopterPositionControl::control_manual(float dt) /* 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, dt); + float stick_z = man_vel_sp(2); + float max_acc_z; + set_manual_acceleration_z(max_acc_z, stick_z, dt); + /* 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) ? _params.vel_max_down : _params.vel_max_up); - - /* setpoint in NED frame and scaled to cruise velocity */ - man_vel_sp = matrix::Dcmf(matrix::Eulerf(0.0f, 0.0f, yaw_input_frame)) * man_vel_sp.emult(vel_cruise_scale); + /* 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 @@ -1021,11 +1375,10 @@ MulticopterPositionControl::control_manual(float dt) */ /* 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 && fabsf(man_vel_sp(2)) < FLT_EPSILON; + 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 && - fabsf(man_vel_sp(0)) < FLT_EPSILON && fabsf(man_vel_sp(1)) < FLT_EPSILON; + const bool pos_hold_desired = _control_mode.flag_control_position_enabled && (_user_intention_xy == brake); /* check vertical hold engaged flag */ if (_alt_hold_engaged) { @@ -1034,15 +1387,12 @@ MulticopterPositionControl::control_manual(float dt) } else { /* check if we switch to alt_hold_engaged */ - bool smooth_alt_transition = alt_hold_desired && + bool smooth_alt_transition = alt_hold_desired && ((max_acc_z - _acceleration_state_dependent_z) < FLT_EPSILON) && (_params.hold_max_z < FLT_EPSILON || fabsf(_vel(2)) < _params.hold_max_z); /* during transition predict setpoint forward */ if (smooth_alt_transition) { - /* get max acceleration */ - float max_acc_z = (_vel(2) < 0.0f ? _acceleration_z_max_down.get() : -_acceleration_z_max_up.get()); - /* time to travel from current velocity to zero velocity */ float delta_t = fabsf(_vel(2) / max_acc_z); @@ -1055,17 +1405,26 @@ MulticopterPositionControl::control_manual(float dt) /* 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 && + bool smooth_pos_transition = pos_hold_desired + && (fabsf(_acceleration_hor_max.get() - _acceleration_state_dependent_xy) < FLT_EPSILON) && (_params.hold_max_xy < FLT_EPSILON || vel_xy_mag < _params.hold_max_xy); /* 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(); @@ -1096,21 +1455,10 @@ MulticopterPositionControl::control_manual(float dt) } if (_vehicle_land_detected.landed) { - /* don't run controller when landed */ - _reset_pos_sp = true; - _reset_alt_sp = true; - _mode_auto = false; - _reset_int_z = true; - _reset_int_xy = true; - - _R_setpoint.identity(); - - _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(); + /* don't run controller when landed + * NOTE: + * This only works in manual since once we give throttle input, the + * landdetector will exit the landing state */ } else { control_position(dt); @@ -1310,21 +1658,30 @@ MulticopterPositionControl::control_offboard(float dt) void MulticopterPositionControl::vel_sp_slewrate(float dt) { - math::Vector<3> acc = (_vel_sp - _vel_sp_prev) / dt; - float acc_xy_mag = sqrtf(acc(0) * acc(0) + acc(1) * acc(1)); - - float acc_limit = _acceleration_hor_max.get(); + 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 vel_xy(_vel(0), _vel(1)); + matrix::Vector2f acc_xy = (vel_sp_xy - vel_sp_prev_xy) / dt; /* limit total horizontal acceleration */ - if (acc_xy_mag > acc_limit) { - _vel_sp(0) = acc_limit * acc(0) / acc_xy_mag * dt + _vel_sp_prev(0); - _vel_sp(1) = acc_limit * acc(1) / acc_xy_mag * dt + _vel_sp_prev(1); + 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 max_acc_z = acc(2) < 0.0f ? -_acceleration_z_max_up.get() : _acceleration_z_max_down.get(); + float acc_z = (_vel_sp(2) - _vel_sp_prev(2)) / dt; + float max_acc_z; - if (fabsf(acc(2)) > fabsf(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); } } @@ -1467,7 +1824,6 @@ void MulticopterPositionControl::control_auto(float dt) 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; } @@ -1476,6 +1832,7 @@ void MulticopterPositionControl::control_auto(float dt) /* 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*/ } if (_pos_sp_triplet.next.valid) { @@ -1723,8 +2080,8 @@ void MulticopterPositionControl::control_auto(float dt) * be used by auto and manual */ float acc_track = (final_cruise_speed - vel_sp_along_track_prev) / dt; - if (acc_track > _acceleration_hor_manual.get()) { - vel_sp_along_track = _acceleration_hor_manual.get() * dt + vel_sp_along_track_prev; + if (acc_track > _acceleration_hor.get()) { + vel_sp_along_track = _acceleration_hor.get() * dt + vel_sp_along_track_prev; } /* enforce minimum cruise speed */ @@ -1887,7 +2244,6 @@ void MulticopterPositionControl::control_auto(float dt) && _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 { @@ -1994,6 +2350,9 @@ MulticopterPositionControl::do_control(float dt) _hold_offboard_z = false; } 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(dt); } } @@ -2062,7 +2421,6 @@ MulticopterPositionControl::calculate_velocity_setpoint(float dt) _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; @@ -2339,7 +2697,6 @@ MulticopterPositionControl::calculate_thrust_setpoint(float dt) math::Vector<3> body_y; math::Vector<3> body_z; - if (thrust_sp.length() > SIGMA_NORM) { body_z = -thrust_sp.normalized(); @@ -2440,7 +2797,6 @@ MulticopterPositionControl::generate_attitude_setpoint(float dt) if (fabsf(yaw_offs) < yaw_offset_max || (_att_sp.yaw_sp_move_rate > 0 && yaw_offs < 0) || (_att_sp.yaw_sp_move_rate < 0 && yaw_offs > 0)) { - _att_sp.yaw_body = yaw_target; } } @@ -2542,7 +2898,6 @@ MulticopterPositionControl::task_main() /* 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_armed = false; bool was_landed = true; hrt_abstime t_prev = 0; @@ -2585,19 +2940,36 @@ MulticopterPositionControl::task_main() /* set default max velocity in xy to vel_max */ _vel_max_xy = _params.vel_max_xy; - if (_control_mode.flag_armed && !was_armed) { - /* reset setpoints and integrals on arming */ + + /* reset flags when landed */ + if (_vehicle_land_detected.landed) { _reset_pos_sp = true; _reset_alt_sp = true; _do_reset_alt_pos_flag = true; - _vel_sp_prev.zero(); + _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; - _yaw_takeoff = _yaw; - } + _hold_offboard_xy = false; + _hold_offboard_z = false; + _in_takeoff = false; + _in_landing = false; + _lnd_reached_ground = false; - was_armed = _control_mode.flag_armed; + /* 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 setpoints and integrators VTOL in FW mode */ if (_vehicle_status.is_vtol && !_vehicle_status.is_rotary_wing) { @@ -2689,15 +3061,17 @@ MulticopterPositionControl::task_main() _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. Also do not publish - * if the vehicle is a VTOL and it's just doing a transition (the VTOL attitude control module will generate + * 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_control_offboard_enabled && - !(_control_mode.flag_control_position_enabled || - _control_mode.flag_control_velocity_enabled || - _control_mode.flag_control_acceleration_enabled))) { + 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)))) { if (_att_sp_pub != nullptr) { orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp); @@ -2706,10 +3080,6 @@ MulticopterPositionControl::task_main() _att_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp); } } - - /* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */ - _reset_int_z_manual = _control_mode.flag_armed && _control_mode.flag_control_manual_enabled - && !_control_mode.flag_control_climb_rate_enabled; } mavlink_log_info(&_mavlink_log_pub, "[mpc] stopped"); 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 ba75cb9645..65d8393090 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -414,7 +414,7 @@ PARAM_DEFINE_FLOAT(MPC_VELD_LP, 5.0f); PARAM_DEFINE_FLOAT(MPC_ACC_HOR_MAX, 10.0f); /** - * Maximum horizontal manual acceleration + * Acceleration for auto and for manual * * @unit m/s/s * @min 2.0 @@ -423,7 +423,20 @@ PARAM_DEFINE_FLOAT(MPC_ACC_HOR_MAX, 10.0f); * @decimal 2 * @group Multicopter Position Control */ -PARAM_DEFINE_FLOAT(MPC_ACC_HOR_MAN, 5.0f); + +PARAM_DEFINE_FLOAT(MPC_ACC_HOR, 5.0f); + +/** + * Slow horizontal manual deceleration for manual mode + * + * @unit m/s/s + * @min 0.5 + * @max 10.0 + * @increment 1 + * @decimal 2 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_DEC_HOR_SLOW, 5.0f); /** * Maximum vertical acceleration in velocity controlled modes upward @@ -447,7 +460,36 @@ PARAM_DEFINE_FLOAT(MPC_ACC_UP_MAX, 10.0f); * @decimal 2 * @group Multicopter Position Control */ -PARAM_DEFINE_FLOAT(MPC_ACC_DOWN_MAX, 5.0f); +PARAM_DEFINE_FLOAT(MPC_ACC_DOWN_MAX, 10.0f); + +/** + * Maximum jerk in manual controlled mode for BRAKING to zero. + * If this value is below MPC_JERK_MIN, the acceleration limit in xy and z + * is MPC_ACC_HOR_MAX and MPC_ACC_UP_MAX respectively instantaneously when the + * user demands brake (=zero stick input). + * Otherwise the acceleration limit increases from current acceleration limit + * towards MPC_ACC_HOR_MAX/MPC_ACC_UP_MAX with jerk limit + * + * @unit m/s/s/s + * @min 0.0 + * @max 15.0 + * @increment 1 + * @decimal 2 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_JERK_MAX, 0.0f); + +/** + * Minimum jerk in manual controlled mode for BRAKING to zero + * + * @unit m/s/s/s + * @min 0.5 + * @max 10.0 + * @increment 1 + * @decimal 2 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_JERK_MIN, 1.0f); /** * Altitude control mode, note mode 1 only tested with LPE