From d642c5a9989febe269cb301b15d6e22813323891 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Sat, 11 May 2019 10:54:16 +0200 Subject: [PATCH 01/18] mc_pos_control: refactor smooth takeoff call There were two rather confusing function calls one to check if smooth takeoff needs to be ran and one that updates it. I combined them and documented the interface correctly making the parameters non-reference const floats. --- .../mc_pos_control/mc_pos_control_main.cpp | 32 ++++++------------- 1 file changed, 10 insertions(+), 22 deletions(-) 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 53098e683094..740684dc0b3e 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -233,19 +233,13 @@ class MulticopterPositionControl : public ModuleBase void publish_trajectory_sp(const vehicle_local_position_setpoint_s &traj); /** - * 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 + * Update smooth takeoff setpoint ramp to bring the vehicle off the ground without step. + * @param z_sp position setpoint in the z-Direction + * @param vz_sp velocity setpoint in the z-Direction + * @param jz_sp jerk setpoint in the z-Direction + * @param min_ground_clearance minimal distance to the ground in which e.g. optical flow works correctly */ - void check_for_smooth_takeoff(const float &position_setpoint_z, const float &velocity_setpoint_z, - const float &jerk_sp, 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); + void update_smooth_takeoff(const float z_sp, const float vz_sp, const float jz_sp, const float min_ground_clearance); /** * Adjust the setpoint during landing. @@ -655,8 +649,7 @@ MulticopterPositionControl::run() // do smooth takeoff after delay if there's a valid vertical velocity or position if (_spoolup_time_hysteresis.get_state() && PX4_ISFINITE(_states.position(2)) && PX4_ISFINITE(_states.velocity(2))) { - check_for_smooth_takeoff(setpoint.z, setpoint.vz, setpoint.jerk_z, constraints); - update_smooth_takeoff(setpoint.z, setpoint.vz); + update_smooth_takeoff(setpoint.z, setpoint.vz, setpoint.jerk_z, constraints.min_distance_to_ground); } // disable horizontal / yaw control during smooth takeoff and limit maximum speed upwards @@ -987,8 +980,7 @@ MulticopterPositionControl::start_flight_task() } void -MulticopterPositionControl::check_for_smooth_takeoff(const float &z_sp, const float &vz_sp, - const float &jerk_sp, const vehicle_constraints_s &constraints) +MulticopterPositionControl::update_smooth_takeoff(const float z_sp, const float vz_sp, const float jz_sp, const float min_ground_clearance) { // Check for smooth takeoff if (_vehicle_land_detected.landed && !_in_smooth_takeoff) { @@ -996,12 +988,12 @@ MulticopterPositionControl::check_for_smooth_takeoff(const float &z_sp, const fl // 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) : + float min_altitude = PX4_ISFINITE(min_ground_clearance) ? (min_ground_clearance + 0.05f) : 0.2f; // takeoff was initiated through velocity setpoint _smooth_velocity_takeoff = PX4_ISFINITE(vz_sp) && vz_sp < -0.1f; - bool jerk_triggered_takeoff = PX4_ISFINITE(jerk_sp) && jerk_sp < -FLT_EPSILON; + bool jerk_triggered_takeoff = PX4_ISFINITE(jz_sp) && jz_sp < -FLT_EPSILON; _smooth_velocity_takeoff |= jerk_triggered_takeoff; if ((PX4_ISFINITE(z_sp) && z_sp < _states.position(2) - min_altitude) || _smooth_velocity_takeoff) { @@ -1013,11 +1005,7 @@ MulticopterPositionControl::check_for_smooth_takeoff(const float &z_sp, const fl } } -} -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 From d83da144264749cea563c4465964c0deec3e8ac6 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Sat, 11 May 2019 13:34:59 +0200 Subject: [PATCH 02/18] mc_pos_control: refactor smooth takeoff names The comments and variable names were partly misleading. I grouped all members, hopefully gave them more understandable names and adjusted the comments. --- .../mc_pos_control/mc_pos_control_main.cpp | 105 +++++++++--------- 1 file changed, 51 insertions(+), 54 deletions(-) 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 740684dc0b3e..aa9536649459 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -102,8 +102,11 @@ class MulticopterPositionControl : public ModuleBase int print_status() override; private: - - bool _in_smooth_takeoff = false; /**< true if takeoff ramp is applied */ + // smooth takeoff vertical velocity ramp state + bool _in_takeoff_ramp = false; /**< true while takeoff ramp is applied */ + bool _velocity_triggered_takeoff = false; /**< true if takeoff was triggered by a velocity setpoint */ + float _takeoff_ramp_velocity = -1.f; /**< current value of the smooth takeoff ramp */ + float _takeoff_reference_z; /**< z-position when takeoff ramp was initiated */ orb_advert_t _att_sp_pub{nullptr}; /**< attitude setpoint publication */ orb_advert_t _traj_sp_pub{nullptr}; /**< trajectory setpoints publication */ @@ -122,11 +125,6 @@ class MulticopterPositionControl : public ModuleBase int _task_failure_count{0}; /**< counter for task failures */ - float _takeoff_speed = -1.f; /**< For flighttask interface used only. It can be thrust or velocity setpoints */ - float _takeoff_reference_z; /**< Z-position when takeoff was initiated */ - bool _smooth_velocity_takeoff = - false; /**< Smooth velocity takeoff can be initiated either through position or velocity setpoint */ - vehicle_status_s _vehicle_status{}; /**< vehicle status */ /**< vehicle-land-detection: initialze to landed */ vehicle_land_detected_s _vehicle_land_detected = { @@ -233,13 +231,13 @@ class MulticopterPositionControl : public ModuleBase void publish_trajectory_sp(const vehicle_local_position_setpoint_s &traj); /** - * Update smooth takeoff setpoint ramp to bring the vehicle off the ground without step. + * Update smooth takeoff vertical velocity ramp to bring the vehicle off the ground without step * @param z_sp position setpoint in the z-Direction * @param vz_sp velocity setpoint in the z-Direction * @param jz_sp jerk setpoint in the z-Direction * @param min_ground_clearance minimal distance to the ground in which e.g. optical flow works correctly */ - void update_smooth_takeoff(const float z_sp, const float vz_sp, const float jz_sp, const float min_ground_clearance); + void update_takeoff_ramp(const float z_sp, const float vz_sp, const float jz_sp, const float min_distance_to_ground); /** * Adjust the setpoint during landing. @@ -649,14 +647,14 @@ MulticopterPositionControl::run() // do smooth takeoff after delay if there's a valid vertical velocity or position if (_spoolup_time_hysteresis.get_state() && PX4_ISFINITE(_states.position(2)) && PX4_ISFINITE(_states.velocity(2))) { - update_smooth_takeoff(setpoint.z, setpoint.vz, setpoint.jerk_z, constraints.min_distance_to_ground); + update_takeoff_ramp(setpoint.z, setpoint.vz, setpoint.jerk_z, constraints.min_distance_to_ground); } // disable horizontal / yaw control during smooth takeoff and limit maximum speed upwards - if (_in_smooth_takeoff) { + if (_in_takeoff_ramp) { - // during smooth takeoff, constrain speed to takeoff speed - constraints.speed_up = _takeoff_speed; + // during smooth takeoff, constrain speed to ramp state + constraints.speed_up = _takeoff_ramp_velocity; // altitude above reference takeoff const float alt_above_tko = -(_states.position(2) - _takeoff_reference_z); @@ -675,7 +673,7 @@ MulticopterPositionControl::run() } } - if (_vehicle_land_detected.landed && !_in_smooth_takeoff && !PX4_ISFINITE(setpoint.thrust[2])) { + if (_vehicle_land_detected.landed && !_in_takeoff_ramp && !PX4_ISFINITE(setpoint.thrust[2])) { // Keep throttle low when landed and NOT in smooth takeoff reset_setpoint_to_nan(setpoint); setpoint.thrust[0] = setpoint.thrust[1] = setpoint.thrust[2] = 0.0f; @@ -734,7 +732,7 @@ MulticopterPositionControl::run() // Part of landing logic: if ground-contact/maybe landed was detected, turn off // controller. This message does not have to be logged as part of the vehicle_local_position_setpoint topic. // Note: only adust thrust output if there was not thrust-setpoint demand in D-direction. - if (!_in_smooth_takeoff && !PX4_ISFINITE(setpoint.thrust[2])) { + if (!_in_takeoff_ramp && !PX4_ISFINITE(setpoint.thrust[2])) { limit_thrust_during_landing(local_pos_sp); } @@ -980,61 +978,60 @@ MulticopterPositionControl::start_flight_task() } void -MulticopterPositionControl::update_smooth_takeoff(const float z_sp, const float vz_sp, const float jz_sp, const float min_ground_clearance) +MulticopterPositionControl::update_takeoff_ramp(const float z_sp, const float vz_sp, const float jz_sp, const float min_distance_to_ground) { - // 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(min_ground_clearance) ? (min_ground_clearance + 0.05f) : - 0.2f; - - // takeoff was initiated through velocity setpoint - _smooth_velocity_takeoff = PX4_ISFINITE(vz_sp) && vz_sp < -0.1f; - bool jerk_triggered_takeoff = PX4_ISFINITE(jz_sp) && jz_sp < -FLT_EPSILON; - _smooth_velocity_takeoff |= jerk_triggered_takeoff; - - if ((PX4_ISFINITE(z_sp) && z_sp < _states.position(2) - min_altitude) || _smooth_velocity_takeoff) { - // 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.f; - _takeoff_reference_z = _states.position(2); + // check if takeoff is triggered + if (_vehicle_land_detected.landed && !_in_takeoff_ramp) { + // vehicle is still landed and no takeoff was initiated yet, check if takeoff is triggered + // minimum takeoff altitude either 20cm or if valid above minimum altitude specified by flight task/estimator + float min_altitude = 0.2f; + if (PX4_ISFINITE(min_distance_to_ground)) { + min_altitude = min_distance_to_ground + 0.05f; + } + // upwards velocity setpoint larger than a minimal speed + _velocity_triggered_takeoff = PX4_ISFINITE(vz_sp) && vz_sp < -0.1f; + // upwards jerk setpoint + const bool jerk_triggered_takeoff = PX4_ISFINITE(jz_sp) && jz_sp < -FLT_EPSILON; + // position setpoint above the minimum altitude + const bool position_triggered_takeoff = PX4_ISFINITE(z_sp) && (z_sp < (_states.position(2) - min_altitude)); + + _velocity_triggered_takeoff |= jerk_triggered_takeoff; + if (_velocity_triggered_takeoff || position_triggered_takeoff) { + // takeoff was triggered, start velocity ramp + _takeoff_ramp_velocity = 0.f; + _in_takeoff_ramp = true; + _takeoff_reference_z = _states.position(2); } } - // 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 in smooth takeoff limit upwards velocity setpoint to a smooth ramp + if (_in_takeoff_ramp) { + float takeoff_desired_velocity = -vz_sp; - // If there is a valid position setpoint, then set the desired speed to the takeoff speed. - if (!_smooth_velocity_takeoff) { - desired_tko_speed = _param_mpc_tko_speed.get(); + // if takeoff was triggered by a position setpoint, ramp to the takeoff speed parameter + if (!_velocity_triggered_takeoff) { + takeoff_desired_velocity = _param_mpc_tko_speed.get(); } - // Ramp up takeoff speed. + // ramp up vrtical velocity in TKO_RAMP_T seconds if (_param_mpc_tko_ramp_t.get() > _dt) { - _takeoff_speed += desired_tko_speed * _dt / _param_mpc_tko_ramp_t.get(); + _takeoff_ramp_velocity += takeoff_desired_velocity * _dt / _param_mpc_tko_ramp_t.get(); } else { - // No ramp, directly go to desired takeoff speed - _takeoff_speed = desired_tko_speed; + // no ramp, directly go to desired takeoff speed + _takeoff_ramp_velocity = takeoff_desired_velocity; } - _takeoff_speed = math::min(_takeoff_speed, _param_mpc_tko_speed.get()); + _takeoff_ramp_velocity = math::min(_takeoff_ramp_velocity, _param_mpc_tko_speed.get()); - // Smooth takeoff is achieved once desired altitude/velocity setpoint is reached. - if (!_smooth_velocity_takeoff) { - _in_smooth_takeoff = _states.position(2) - 0.2f > math::max(z_sp, _takeoff_reference_z - _param_mpc_land_alt2.get()); + // smooth takeoff is achieved once desired altitude/velocity setpoint is reached + if (!_velocity_triggered_takeoff) { + _in_takeoff_ramp = _states.position(2) - 0.2f > math::max(z_sp, _takeoff_reference_z - _param_mpc_land_alt2.get()); } else { - // Make sure to stay in smooth takeoff if takeoff has not been detected yet by the land detector - _in_smooth_takeoff = (_takeoff_speed < -vz_sp) || _vehicle_land_detected.landed; + // make sure to stay in smooth takeoff if takeoff has not been detected yet by the land detector + _in_takeoff_ramp = (_takeoff_ramp_velocity < -vz_sp) || _vehicle_land_detected.landed; } } } From f1609025102e3c2bbb7d9b9ca48ec1d34d5187a5 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Sat, 11 May 2019 19:28:21 +0100 Subject: [PATCH 03/18] mc_pos_control: fix smooth takeoff ramp - start from a velocity setpoint pushing into the ground to ramp from idle thrust up. - start with a bit higher velocity setpoint threshold to make sure the vehicle has a chance to really get off the ground. - calculate ramp slope from initialization setpoint to the desired one instead from zero to the desired. this ramps up quicker when you demand a very small upwards velocity like the AutoLineSmoothVel and ManualPositionSmoothVel tasks do at the beginning. - don't stay in the takeoff ramp depending on the land detector, this is unnecessary. --- .../mc_pos_control/mc_pos_control_main.cpp | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) 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 aa9536649459..2ccf1cfbff5a 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -593,7 +593,7 @@ MulticopterPositionControl::run() _wv_controller->update(matrix::Quatf(_att_sp.q_d), _states.yaw); } - // start takeoff after delay to allow motors to reach idle speed + // takeoff delay for motors to reach idle speed _spoolup_time_hysteresis.set_state_and_update(_control_mode.flag_armed); if (_spoolup_time_hysteresis.get_state()) { @@ -980,6 +980,8 @@ MulticopterPositionControl::start_flight_task() void MulticopterPositionControl::update_takeoff_ramp(const float z_sp, const float vz_sp, const float jz_sp, const float min_distance_to_ground) { + const float takeoff_ramp_initialization = -0.7f; + // check if takeoff is triggered if (_vehicle_land_detected.landed && !_in_takeoff_ramp) { // vehicle is still landed and no takeoff was initiated yet, check if takeoff is triggered @@ -990,7 +992,7 @@ MulticopterPositionControl::update_takeoff_ramp(const float z_sp, const float vz } // upwards velocity setpoint larger than a minimal speed - _velocity_triggered_takeoff = PX4_ISFINITE(vz_sp) && vz_sp < -0.1f; + _velocity_triggered_takeoff = PX4_ISFINITE(vz_sp) && vz_sp < -0.3f; // upwards jerk setpoint const bool jerk_triggered_takeoff = PX4_ISFINITE(jz_sp) && jz_sp < -FLT_EPSILON; // position setpoint above the minimum altitude @@ -999,7 +1001,7 @@ MulticopterPositionControl::update_takeoff_ramp(const float z_sp, const float vz _velocity_triggered_takeoff |= jerk_triggered_takeoff; if (_velocity_triggered_takeoff || position_triggered_takeoff) { // takeoff was triggered, start velocity ramp - _takeoff_ramp_velocity = 0.f; + _takeoff_ramp_velocity = takeoff_ramp_initialization; _in_takeoff_ramp = true; _takeoff_reference_z = _states.position(2); } @@ -1016,22 +1018,22 @@ MulticopterPositionControl::update_takeoff_ramp(const float z_sp, const float vz // ramp up vrtical velocity in TKO_RAMP_T seconds if (_param_mpc_tko_ramp_t.get() > _dt) { - _takeoff_ramp_velocity += takeoff_desired_velocity * _dt / _param_mpc_tko_ramp_t.get(); + _takeoff_ramp_velocity += (takeoff_desired_velocity - takeoff_ramp_initialization) * _dt / _param_mpc_tko_ramp_t.get(); } else { // no ramp, directly go to desired takeoff speed _takeoff_ramp_velocity = takeoff_desired_velocity; } - _takeoff_ramp_velocity = math::min(_takeoff_ramp_velocity, _param_mpc_tko_speed.get()); + // make sure we cannot overshoot the desired setpoint with the ramp + _takeoff_ramp_velocity = math::min(_takeoff_ramp_velocity, takeoff_desired_velocity); // smooth takeoff is achieved once desired altitude/velocity setpoint is reached if (!_velocity_triggered_takeoff) { _in_takeoff_ramp = _states.position(2) - 0.2f > math::max(z_sp, _takeoff_reference_z - _param_mpc_land_alt2.get()); } else { - // make sure to stay in smooth takeoff if takeoff has not been detected yet by the land detector - _in_takeoff_ramp = (_takeoff_ramp_velocity < -vz_sp) || _vehicle_land_detected.landed; + _in_takeoff_ramp = (_takeoff_ramp_velocity < -vz_sp); } } } From 336f978b650ddcd46e172f95dbb3386b4578fd13 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Sat, 11 May 2019 19:41:45 +0100 Subject: [PATCH 04/18] mc_pos_control: refactor takeoff trigger conditions to be positive --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) 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 2ccf1cfbff5a..357305ad2ccf 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -104,7 +104,7 @@ class MulticopterPositionControl : public ModuleBase private: // smooth takeoff vertical velocity ramp state bool _in_takeoff_ramp = false; /**< true while takeoff ramp is applied */ - bool _velocity_triggered_takeoff = false; /**< true if takeoff was triggered by a velocity setpoint */ + bool _position_triggered_takeoff = false; /**< true if takeoff was triggered by a position setpoint */ float _takeoff_ramp_velocity = -1.f; /**< current value of the smooth takeoff ramp */ float _takeoff_reference_z; /**< z-position when takeoff ramp was initiated */ @@ -992,14 +992,13 @@ MulticopterPositionControl::update_takeoff_ramp(const float z_sp, const float vz } // upwards velocity setpoint larger than a minimal speed - _velocity_triggered_takeoff = PX4_ISFINITE(vz_sp) && vz_sp < -0.3f; + const bool velocity_triggered_takeoff = PX4_ISFINITE(vz_sp) && vz_sp < -0.3f; // upwards jerk setpoint const bool jerk_triggered_takeoff = PX4_ISFINITE(jz_sp) && jz_sp < -FLT_EPSILON; // position setpoint above the minimum altitude - const bool position_triggered_takeoff = PX4_ISFINITE(z_sp) && (z_sp < (_states.position(2) - min_altitude)); + _position_triggered_takeoff = PX4_ISFINITE(z_sp) && (z_sp < (_states.position(2) - min_altitude)); - _velocity_triggered_takeoff |= jerk_triggered_takeoff; - if (_velocity_triggered_takeoff || position_triggered_takeoff) { + if (velocity_triggered_takeoff || jerk_triggered_takeoff || _position_triggered_takeoff) { // takeoff was triggered, start velocity ramp _takeoff_ramp_velocity = takeoff_ramp_initialization; _in_takeoff_ramp = true; @@ -1012,7 +1011,7 @@ MulticopterPositionControl::update_takeoff_ramp(const float z_sp, const float vz float takeoff_desired_velocity = -vz_sp; // if takeoff was triggered by a position setpoint, ramp to the takeoff speed parameter - if (!_velocity_triggered_takeoff) { + if (_position_triggered_takeoff) { takeoff_desired_velocity = _param_mpc_tko_speed.get(); } @@ -1029,7 +1028,7 @@ MulticopterPositionControl::update_takeoff_ramp(const float z_sp, const float vz _takeoff_ramp_velocity = math::min(_takeoff_ramp_velocity, takeoff_desired_velocity); // smooth takeoff is achieved once desired altitude/velocity setpoint is reached - if (!_velocity_triggered_takeoff) { + if (_position_triggered_takeoff) { _in_takeoff_ramp = _states.position(2) - 0.2f > math::max(z_sp, _takeoff_reference_z - _param_mpc_land_alt2.get()); } else { From c38eda42e514f27ec4b5564901155b69212fc26f Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 15 May 2019 13:35:05 +0100 Subject: [PATCH 05/18] mc_pos_control: initialize takeoff altitude reference according to @dagar's review comment. --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 357305ad2ccf..4b33d4a4a603 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -106,7 +106,7 @@ class MulticopterPositionControl : public ModuleBase bool _in_takeoff_ramp = false; /**< true while takeoff ramp is applied */ bool _position_triggered_takeoff = false; /**< true if takeoff was triggered by a position setpoint */ float _takeoff_ramp_velocity = -1.f; /**< current value of the smooth takeoff ramp */ - float _takeoff_reference_z; /**< z-position when takeoff ramp was initiated */ + float _takeoff_reference_z = 0.f; /**< z-position when takeoff ramp was initiated */ orb_advert_t _att_sp_pub{nullptr}; /**< attitude setpoint publication */ orb_advert_t _traj_sp_pub{nullptr}; /**< trajectory setpoints publication */ From 0c81a19decde6ddfe4ce87c34c762ea15fd3ab09 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Sat, 11 May 2019 21:40:07 +0200 Subject: [PATCH 06/18] mc_att_control: fix having high thrust when disarmed After boot the user is in manual mode and if he has an RC but doesn't switch out the thrust gets set to the throttle stick position. When he then starts a takeoff from tablet the thrust is still high while arming and the land detector immediately sees a takeoff skiping smooth takeoff from the position controller. --- src/modules/mc_att_control/mc_att_control_main.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index b12bdaed6e83..2e5e84fc4bc9 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -500,7 +500,10 @@ MulticopterAttitudeControl::generate_attitude_setpoint(float dt, bool reset_yaw_ q_sp.copyTo(attitude_setpoint.q_d); attitude_setpoint.q_d_valid = true; - attitude_setpoint.thrust_body[2] = -throttle_curve(_manual_control_sp.z); + // hotfix: make sure to leave a zero thrust setpoint for position controller to take over after initialization in manual mode + // without this the actuator_controls thrust stays like it was set here when switching to a position controlled mode and the + // land detector will immediately detect takeoff when arming + attitude_setpoint.thrust_body[2] = _v_control_mode.flag_armed ? -throttle_curve(_manual_control_sp.z) : 0.0f; attitude_setpoint.timestamp = hrt_absolute_time(); if (_attitude_sp_id != nullptr) { orb_publish_auto(_attitude_sp_id, &_vehicle_attitude_setpoint_pub, &attitude_setpoint, nullptr, ORB_PRIO_DEFAULT); From c3d7878b8002c9fa46121071fe506f2adeeee573 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 13 May 2019 21:07:05 +0200 Subject: [PATCH 07/18] mc_pos_control/FlightTasks: trigger takeoff based on task The initial idea of the flight task architecture was that a task can freely set it's setpoints and doesn't have to worry about takeoff and landing. It would just takeoff when it's landed and there's a setpoint to go up and land when it puts a setpoint that pushes into the ground. With the takeoff logic there are some significant interface problems depending on the way a task is implemented: From the setpoint is not high enough to trigger to an unexpected takeoff because of some estimator fluctuation affecting the setpoint. It's easiest to solve this by allowing the task to determine when a takeoff is triggered. If no condition is implemented by default a task is not allowing a takeoff and cannot be used to get the vehicle off the ground. --- msg/vehicle_constraints.msg | 13 +++-- .../FlightTasks/tasks/Auto/FlightTaskAuto.cpp | 12 ++++ .../FlightTasks/tasks/Auto/FlightTaskAuto.hpp | 1 + .../tasks/FlightTask/FlightTask.cpp | 1 + .../tasks/FlightTask/FlightTask.hpp | 2 +- .../FlightTaskManualAltitude.cpp | 7 +++ .../FlightTaskManualAltitude.hpp | 1 + .../mc_pos_control/mc_pos_control_main.cpp | 57 ++++++------------- 8 files changed, 47 insertions(+), 47 deletions(-) diff --git a/msg/vehicle_constraints.msg b/msg/vehicle_constraints.msg index 50a145cf9a75..da8c39aa2471 100644 --- a/msg/vehicle_constraints.msg +++ b/msg/vehicle_constraints.msg @@ -1,12 +1,13 @@ # Local setpoint constraints in NED frame # setting something to NaN means that no limit is provided -uint64 timestamp # time since system start (microseconds) +uint64 timestamp # time since system start (microseconds) -float32 yawspeed # in radians/sec -float32 speed_xy # in meters/sec -float32 speed_up # in meters/sec -float32 speed_down # in meters/sec -float32 tilt # in radians [0, PI] +float32 yawspeed # in radians/sec +float32 speed_xy # in meters/sec +float32 speed_up # in meters/sec +float32 speed_down # in meters/sec +float32 tilt # in radians [0, PI] float32 min_distance_to_ground # in meters float32 max_distance_to_ground # in meters +bool want_takeoff # tell the controller to initiate takeoff when idling (ignored during flight) diff --git a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp index 8e08a5199018..58e5ab456275 100644 --- a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp +++ b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp @@ -106,6 +106,7 @@ bool FlightTaskAuto::updateFinalize() // If the FlightTask generates a yaw or a yawrate setpoint that exceeds this value // it will see its setpoint constrained here _limitYawRate(); + _constraints.want_takeoff = _checkTakeoff(); return true; } @@ -128,6 +129,17 @@ void FlightTaskAuto::_limitYawRate() } } +bool FlightTaskAuto::_checkTakeoff() { + // position setpoint above the minimum altitude + float min_altitude = 0.2f; + const float min_distance_to_ground = _sub_vehicle_local_position->get().hagl_min; + if (PX4_ISFINITE(min_distance_to_ground)) { + min_altitude = min_distance_to_ground + 0.05f; + } + + return PX4_ISFINITE(_position_setpoint(2)) && _position_setpoint(2) < (_position(2) - min_altitude); +} + bool FlightTaskAuto::_evaluateTriplets() { // TODO: fix the issues mentioned below diff --git a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp index d68c26571a09..316f3851ff12 100644 --- a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp +++ b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp @@ -91,6 +91,7 @@ class FlightTaskAuto : public FlightTask matrix::Vector2f _getTargetVelocityXY(); /**< only used for follow-me and only here because of legacy reason.*/ void _updateInternalWaypoints(); /**< Depending on state of vehicle, the internal waypoints might differ from target (for instance if offtrack). */ bool _compute_heading_from_2D_vector(float &heading, matrix::Vector2f v); /**< Computes and sets heading a 2D vector */ + virtual bool _checkTakeoff(); /**< determines when to trigger a takeoff (ignored in flight) */ matrix::Vector3f _prev_prev_wp{}; /**< Pre-previous waypoint (local frame). This will be used for smoothing trajectories -> not used yet. */ matrix::Vector3f _prev_wp{}; /**< Previous waypoint (local frame). If no previous triplet is available, the prev_wp is set to current position. */ diff --git a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp index 4517644d105a..073649d32f3d 100644 --- a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp +++ b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp @@ -143,4 +143,5 @@ void FlightTask::_setDefaultConstraints() _constraints.tilt = math::radians(_param_mpc_tiltmax_air.get()); _constraints.min_distance_to_ground = NAN; _constraints.max_distance_to_ground = NAN; + _constraints.want_takeoff = false; } diff --git a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp index 38fe00c7015b..8106bd731851 100644 --- a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp +++ b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp @@ -198,7 +198,7 @@ class FlightTask : public ModuleParams /** * Set constraints to default values */ - virtual void _setDefaultConstraints(); + virtual void _setDefaultConstraints(); /* Time abstraction */ static constexpr uint64_t _timeout = 500000; /**< maximal time in us before a loop or data times out */ diff --git a/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp index deae1751d5b5..5fdd58ce3b30 100644 --- a/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp +++ b/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp @@ -345,10 +345,17 @@ void FlightTaskManualAltitude::_updateSetpoints() _respectGroundSlowdown(); } +bool FlightTaskManualAltitude::_checkTakeoff() +{ + // stick is deflected above the middle 15% of the range + return _sticks(2) < -0.3f; +} + bool FlightTaskManualAltitude::update() { _scaleSticks(); _updateSetpoints(); + _constraints.want_takeoff = _checkTakeoff(); return true; } diff --git a/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp b/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp index 2993f4d38f73..96593bfb6008 100644 --- a/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp +++ b/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp @@ -56,6 +56,7 @@ class FlightTaskManualAltitude : public FlightTaskManual void _updateHeadingSetpoints(); /**< sets yaw or yaw speed */ virtual void _updateSetpoints(); /**< updates all setpoints */ virtual void _scaleSticks(); /**< scales sticks to velocity in z */ + virtual bool _checkTakeoff(); /**< determines when to trigger a takeoff (ignored in flight) */ /** * rotates vector into local frame 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 4b33d4a4a603..2d1ef1ed30cd 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -232,12 +232,11 @@ class MulticopterPositionControl : public ModuleBase /** * Update smooth takeoff vertical velocity ramp to bring the vehicle off the ground without step - * @param z_sp position setpoint in the z-Direction * @param vz_sp velocity setpoint in the z-Direction - * @param jz_sp jerk setpoint in the z-Direction - * @param min_ground_clearance minimal distance to the ground in which e.g. optical flow works correctly + * @param z_sp position setpoint in the z-Direction + * @param vz_constraint velocity to ramp to when there's only a position setpoint */ - void update_takeoff_ramp(const float z_sp, const float vz_sp, const float jz_sp, const float min_distance_to_ground); + void update_takeoff_ramp(const bool want_takeoff, const float vz_sp, const float z_sp, const float vz_constraint); /** * Adjust the setpoint during landing. @@ -647,7 +646,7 @@ MulticopterPositionControl::run() // do smooth takeoff after delay if there's a valid vertical velocity or position if (_spoolup_time_hysteresis.get_state() && PX4_ISFINITE(_states.position(2)) && PX4_ISFINITE(_states.velocity(2))) { - update_takeoff_ramp(setpoint.z, setpoint.vz, setpoint.jerk_z, constraints.min_distance_to_ground); + update_takeoff_ramp(constraints.want_takeoff, setpoint.vz, setpoint.z, constraints.speed_up); } // disable horizontal / yaw control during smooth takeoff and limit maximum speed upwards @@ -978,62 +977,40 @@ MulticopterPositionControl::start_flight_task() } void -MulticopterPositionControl::update_takeoff_ramp(const float z_sp, const float vz_sp, const float jz_sp, const float min_distance_to_ground) +MulticopterPositionControl::update_takeoff_ramp(const bool want_takeoff, const float vz_sp, const float z_sp, const float vz_constraint) { const float takeoff_ramp_initialization = -0.7f; // check if takeoff is triggered - if (_vehicle_land_detected.landed && !_in_takeoff_ramp) { - // vehicle is still landed and no takeoff was initiated yet, check if takeoff is triggered - // minimum takeoff altitude either 20cm or if valid above minimum altitude specified by flight task/estimator - float min_altitude = 0.2f; - if (PX4_ISFINITE(min_distance_to_ground)) { - min_altitude = min_distance_to_ground + 0.05f; - } - - // upwards velocity setpoint larger than a minimal speed - const bool velocity_triggered_takeoff = PX4_ISFINITE(vz_sp) && vz_sp < -0.3f; - // upwards jerk setpoint - const bool jerk_triggered_takeoff = PX4_ISFINITE(jz_sp) && jz_sp < -FLT_EPSILON; - // position setpoint above the minimum altitude - _position_triggered_takeoff = PX4_ISFINITE(z_sp) && (z_sp < (_states.position(2) - min_altitude)); - - if (velocity_triggered_takeoff || jerk_triggered_takeoff || _position_triggered_takeoff) { - // takeoff was triggered, start velocity ramp - _takeoff_ramp_velocity = takeoff_ramp_initialization; - _in_takeoff_ramp = true; - _takeoff_reference_z = _states.position(2); - } + if (_vehicle_land_detected.landed && !_in_takeoff_ramp && want_takeoff) { + // takeoff was triggered, start velocity ramp + _takeoff_ramp_velocity = takeoff_ramp_initialization; + _in_takeoff_ramp = true; + _takeoff_reference_z = _states.position(2); } // if in smooth takeoff limit upwards velocity setpoint to a smooth ramp if (_in_takeoff_ramp) { float takeoff_desired_velocity = -vz_sp; - // if takeoff was triggered by a position setpoint, ramp to the takeoff speed parameter - if (_position_triggered_takeoff) { - takeoff_desired_velocity = _param_mpc_tko_speed.get(); + // if there's only a position setpoint we go up with the configured takeoff speed + if (!PX4_ISFINITE(vz_sp) && PX4_ISFINITE(z_sp)) { + takeoff_desired_velocity = vz_constraint; } - // ramp up vrtical velocity in TKO_RAMP_T seconds + if (_param_mpc_tko_ramp_t.get() > _dt) { _takeoff_ramp_velocity += (takeoff_desired_velocity - takeoff_ramp_initialization) * _dt / _param_mpc_tko_ramp_t.get(); - } else { - // no ramp, directly go to desired takeoff speed + // no ramp time, directly jump to desired velocity _takeoff_ramp_velocity = takeoff_desired_velocity; } // make sure we cannot overshoot the desired setpoint with the ramp _takeoff_ramp_velocity = math::min(_takeoff_ramp_velocity, takeoff_desired_velocity); - // smooth takeoff is achieved once desired altitude/velocity setpoint is reached - if (_position_triggered_takeoff) { - _in_takeoff_ramp = _states.position(2) - 0.2f > math::max(z_sp, _takeoff_reference_z - _param_mpc_land_alt2.get()); - - } else { - _in_takeoff_ramp = (_takeoff_ramp_velocity < -vz_sp); - } + // smooth takeoff is finished once desired velocity setpoint is reached + _in_takeoff_ramp = (_takeoff_ramp_velocity < takeoff_desired_velocity); } } From 5e0bbc3a25fed722b8e3d9e659d083b0a69ca41f Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 14 May 2019 07:57:13 +0100 Subject: [PATCH 08/18] FlightTasks: fix takeoff trigger for offboard --- .../FlightTasks/tasks/Auto/FlightTaskAuto.cpp | 11 ------- .../FlightTasks/tasks/Auto/FlightTaskAuto.hpp | 1 - .../tasks/FlightTask/FlightTask.cpp | 29 ++++++++++++++++++- .../tasks/FlightTask/FlightTask.hpp | 15 ++++------ .../FlightTaskManualAltitude.hpp | 2 +- .../tasks/Offboard/FlightTaskOffboard.cpp | 15 +++++----- 6 files changed, 42 insertions(+), 31 deletions(-) diff --git a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp index 58e5ab456275..51b4b3aee2f7 100644 --- a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp +++ b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp @@ -129,17 +129,6 @@ void FlightTaskAuto::_limitYawRate() } } -bool FlightTaskAuto::_checkTakeoff() { - // position setpoint above the minimum altitude - float min_altitude = 0.2f; - const float min_distance_to_ground = _sub_vehicle_local_position->get().hagl_min; - if (PX4_ISFINITE(min_distance_to_ground)) { - min_altitude = min_distance_to_ground + 0.05f; - } - - return PX4_ISFINITE(_position_setpoint(2)) && _position_setpoint(2) < (_position(2) - min_altitude); -} - bool FlightTaskAuto::_evaluateTriplets() { // TODO: fix the issues mentioned below diff --git a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp index 316f3851ff12..d68c26571a09 100644 --- a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp +++ b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp @@ -91,7 +91,6 @@ class FlightTaskAuto : public FlightTask matrix::Vector2f _getTargetVelocityXY(); /**< only used for follow-me and only here because of legacy reason.*/ void _updateInternalWaypoints(); /**< Depending on state of vehicle, the internal waypoints might differ from target (for instance if offtrack). */ bool _compute_heading_from_2D_vector(float &heading, matrix::Vector2f v); /**< Computes and sets heading a 2D vector */ - virtual bool _checkTakeoff(); /**< determines when to trigger a takeoff (ignored in flight) */ matrix::Vector3f _prev_prev_wp{}; /**< Pre-previous waypoint (local frame). This will be used for smoothing trajectories -> not used yet. */ matrix::Vector3f _prev_wp{}; /**< Previous waypoint (local frame). If no previous triplet is available, the prev_wp is set to current position. */ diff --git a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp index 073649d32f3d..e79e7d66d1b2 100644 --- a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp +++ b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp @@ -6,7 +6,7 @@ constexpr uint64_t FlightTask::_timeout; // First index of empty_setpoint corresponds to time-stamp and requires a finite number. const vehicle_local_position_setpoint_s FlightTask::empty_setpoint = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, {NAN, NAN, NAN}}; -const vehicle_constraints_s FlightTask::empty_constraints = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, {}}; +const vehicle_constraints_s FlightTask::empty_constraints = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, false, {}}; const landing_gear_s FlightTask::empty_landing_gear_default_keep = {0, landing_gear_s::GEAR_KEEP, {}}; bool FlightTask::initializeSubscriptions(SubscriptionArray &subscription_array) @@ -145,3 +145,30 @@ void FlightTask::_setDefaultConstraints() _constraints.max_distance_to_ground = NAN; _constraints.want_takeoff = false; } + +bool FlightTask::_checkTakeoff() +{ + // position setpoint above the minimum altitude + bool position_triggered_takeoff = false; + + if (PX4_ISFINITE(_position_setpoint(2))) { + //minimal altitude either 20cm or what is necessary for correct estimation e.g. optical flow + float min_altitude = 0.2f; + const float min_distance_to_ground = _sub_vehicle_local_position->get().hagl_min; + + if (PX4_ISFINITE(min_distance_to_ground)) { + min_altitude = min_distance_to_ground + 0.05f; + } + + position_triggered_takeoff = _position_setpoint(2) < (_position(2) - min_altitude); + } + + // upwards velocity setpoint + bool velocity_triggered_takeoff = false; + + if (PX4_ISFINITE(_velocity_setpoint(2))) { + velocity_triggered_takeoff = _velocity_setpoint(2) < -0.3f; + } + + return position_triggered_takeoff || velocity_triggered_takeoff; +} diff --git a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp index 8106bd731851..19984514b859 100644 --- a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp +++ b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp @@ -185,21 +185,18 @@ class FlightTask : public ModuleParams uORB::Subscription *_sub_attitude{nullptr}; uint8_t _heading_reset_counter{0}; /**< estimator heading reset */ - /** - * Reset all setpoints to NAN - */ + /** Reset all setpoints to NAN */ void _resetSetpoints(); - /** - * Check and update local position - */ + /** Check and update local position */ void _evaluateVehicleLocalPosition(); - /** - * Set constraints to default values - */ + /** Set constraints to default values */ virtual void _setDefaultConstraints(); + /** determines when to trigger a takeoff (ignored in flight) */ + virtual bool _checkTakeoff(); + /* Time abstraction */ static constexpr uint64_t _timeout = 500000; /**< maximal time in us before a loop or data times out */ float _time = 0; /**< passed time in seconds since the task was activated */ diff --git a/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp b/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp index 96593bfb6008..a0ea75424c63 100644 --- a/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp +++ b/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp @@ -56,7 +56,7 @@ class FlightTaskManualAltitude : public FlightTaskManual void _updateHeadingSetpoints(); /**< sets yaw or yaw speed */ virtual void _updateSetpoints(); /**< updates all setpoints */ virtual void _scaleSticks(); /**< scales sticks to velocity in z */ - virtual bool _checkTakeoff(); /**< determines when to trigger a takeoff (ignored in flight) */ + bool _checkTakeoff() override; /** * rotates vector into local frame diff --git a/src/lib/FlightTasks/tasks/Offboard/FlightTaskOffboard.cpp b/src/lib/FlightTasks/tasks/Offboard/FlightTaskOffboard.cpp index 866f49f4e159..8b05f17b470e 100644 --- a/src/lib/FlightTasks/tasks/Offboard/FlightTaskOffboard.cpp +++ b/src/lib/FlightTasks/tasks/Offboard/FlightTaskOffboard.cpp @@ -75,17 +75,16 @@ bool FlightTaskOffboard::activate() bool FlightTaskOffboard::update() { + // reset setpoint for every loop + _resetSetpoints(); + if (!_sub_triplet_setpoint->get().current.valid) { - _resetSetpoints(); + _setDefaultConstraints(); _position_setpoint = _position; return false; } - // reset setpoint for every loop - _resetSetpoints(); - // Yaw / Yaw-speed - if (_sub_triplet_setpoint->get().current.yaw_valid) { // yaw control required _yaw_setpoint = _sub_triplet_setpoint->get().current.yaw; @@ -169,7 +168,6 @@ bool FlightTaskOffboard::update() // 2. position setpoint + velocity setpoint (velocity used as feedforward) // 3. velocity setpoint // 4. acceleration setpoint -> this will be mapped to normalized thrust setpoint because acceleration is not supported - const bool position_ctrl_xy = _sub_triplet_setpoint->get().current.position_valid && _sub_vehicle_local_position->get().xy_valid; const bool position_ctrl_z = _sub_triplet_setpoint->get().current.alt_valid @@ -225,7 +223,6 @@ bool FlightTaskOffboard::update() } // Z-direction - if (feedforward_ctrl_z) { _position_setpoint(2) = _sub_triplet_setpoint->get().current.z; _velocity_setpoint(2) = _sub_triplet_setpoint->get().current.vz; @@ -239,12 +236,14 @@ bool FlightTaskOffboard::update() // Acceleration // Note: this is not supported yet and will be mapped to normalized thrust directly. - if (_sub_triplet_setpoint->get().current.acceleration_valid) { _thrust_setpoint(0) = _sub_triplet_setpoint->get().current.a_x; _thrust_setpoint(1) = _sub_triplet_setpoint->get().current.a_y; _thrust_setpoint(2) = _sub_triplet_setpoint->get().current.a_z; } + // use default conditions of upwards position or velocity to take off + _constraints.want_takeoff = _checkTakeoff(); + return true; } From 77562bd268f83c8bcfca1f6e6677f32de002ff04 Mon Sep 17 00:00:00 2001 From: bresch Date: Tue, 14 May 2019 11:07:34 +0200 Subject: [PATCH 09/18] AutoSmoothVel - Override checkTakeoff with task-specific logic and reactivate z axis with downward velocity to takeoff smoothly --- .../FlightTaskAutoLineSmoothVel.cpp | 12 ++++++++---- .../FlightTaskAutoLineSmoothVel.hpp | 7 ++++++- 2 files changed, 14 insertions(+), 5 deletions(-) diff --git a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp index ae1ac19f69ea..bdfb3bf7e572 100644 --- a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp +++ b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp @@ -57,10 +57,12 @@ bool FlightTaskAutoLineSmoothVel::activate() void FlightTaskAutoLineSmoothVel::reActivate() { - // Don't reset during takeoff TODO: Find a proper solution - // The issue here is that with a small increment of velocity setpoint (generated by this flight task), the - // land detector doesn't detect takeoff and without takeoff detection, the - // flight task is always reset. + // On ground, reset acceleration and velocity to zero + for (int i = 0; i < 2; ++i) { + _trajectory[i].reset(0.f, 0.f, _position(i)); + } + + _trajectory[2].reset(0.f, 0.7f, _position(2)); } void FlightTaskAutoLineSmoothVel::_generateSetpoints() @@ -143,6 +145,7 @@ void FlightTaskAutoLineSmoothVel::_prepareSetpoints() // If the position setpoints are set to NAN, the values in the velocity setpoints are used as velocity targets: nothing to do here. _checkEkfResetCounters(); + _want_takeoff = false; if (PX4_ISFINITE(_position_setpoint(0)) && PX4_ISFINITE(_position_setpoint(1))) { @@ -191,6 +194,7 @@ void FlightTaskAutoLineSmoothVel::_prepareSetpoints() _velocity_setpoint(2) = vel_sp_z; } + _want_takeoff = _velocity_setpoint(2) < -0.3f; } } diff --git a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp index 151140250ba1..943ae6a726c6 100644 --- a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp +++ b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp @@ -67,6 +67,9 @@ class FlightTaskAutoLineSmoothVel : public FlightTaskAutoMapper2 void _generateSetpoints() override; /**< Generate setpoints along line. */ + /** determines when to trigger a takeoff (ignored in flight) */ + bool _checkTakeoff() override { return _want_takeoff; }; + inline float _constrainOneSide(float val, float constrain); void _checkEkfResetCounters(); /**< Reset the trajectories when the ekf resets velocity or position */ void _generateHeading(); @@ -75,7 +78,9 @@ class FlightTaskAutoLineSmoothVel : public FlightTaskAutoMapper2 void _prepareSetpoints(); /**< Generate velocity target points for the trajectory generator. */ void _generateTrajectory(); VelocitySmoothing _trajectory[3]; ///< Trajectories in x, y and z directions - float _yaw_sp_prev; + + float _yaw_sp_prev{NAN}; + bool _want_takeoff{false}; /* counters for estimator local position resets */ struct { From ae2588c4af247be04ce6b1323754d3314911f6cc Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 15 May 2019 08:15:38 +0200 Subject: [PATCH 10/18] mc_pos_control: replace takeoff velocity ramp with thrust ramp The velocity ramp had problems with: - different vehicle tunings resulted in the start value of the resulting thrust ramp staring either higher and lower than zero thrust. lower -> delay of beginning higher -> small jump at beginning - when a task set position and velocity at the same time during takeoff (which AutoSmoothVel does) it resulted in a velocity setpoint jump at the end of the ramp because the additional velocity setpoint correction from the position controller was not considered. The thrust ramp should now be very deterministic: - always start at zero - always end at the curreant thrust setpoint output of the complete position controller --- .../mc_pos_control/mc_pos_control_main.cpp | 109 ++++++++---------- 1 file changed, 50 insertions(+), 59 deletions(-) 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 2d1ef1ed30cd..62002c6ca2d7 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -104,8 +104,7 @@ class MulticopterPositionControl : public ModuleBase private: // smooth takeoff vertical velocity ramp state bool _in_takeoff_ramp = false; /**< true while takeoff ramp is applied */ - bool _position_triggered_takeoff = false; /**< true if takeoff was triggered by a position setpoint */ - float _takeoff_ramp_velocity = -1.f; /**< current value of the smooth takeoff ramp */ + float _takeoff_ramp_thrust = 0.f; /**< current value of the smooth takeoff ramp */ float _takeoff_reference_z = 0.f; /**< z-position when takeoff ramp was initiated */ orb_advert_t _att_sp_pub{nullptr}; /**< attitude setpoint publication */ @@ -236,7 +235,7 @@ class MulticopterPositionControl : public ModuleBase * @param z_sp position setpoint in the z-Direction * @param vz_constraint velocity to ramp to when there's only a position setpoint */ - void update_takeoff_ramp(const bool want_takeoff, const float vz_sp, const float z_sp, const float vz_constraint); + void update_takeoff_ramp(const bool want_takeoff, const float thr_sp); /** * Adjust the setpoint during landing. @@ -644,43 +643,6 @@ MulticopterPositionControl::run() // check if all local states are valid and map accordingly set_vehicle_states(setpoint.vz); - // do smooth takeoff after delay if there's a valid vertical velocity or position - if (_spoolup_time_hysteresis.get_state() && PX4_ISFINITE(_states.position(2)) && PX4_ISFINITE(_states.velocity(2))) { - update_takeoff_ramp(constraints.want_takeoff, setpoint.vz, setpoint.z, constraints.speed_up); - } - - // disable horizontal / yaw control during smooth takeoff and limit maximum speed upwards - if (_in_takeoff_ramp) { - - // during smooth takeoff, constrain speed to ramp state - constraints.speed_up = _takeoff_ramp_velocity; - // altitude above reference takeoff - const float alt_above_tko = -(_states.position(2) - _takeoff_reference_z); - - // disable yaw control when close to ground - if (alt_above_tko <= ALTITUDE_THRESHOLD) { - - setpoint.yawspeed = NAN; - - // if there is a valid yaw estimate, just set setpoint to yaw - if (PX4_ISFINITE(_states.yaw)) { - setpoint.yaw = _states.yaw; - } - - // limit tilt during smooth takeoff when still close to ground - constraints.tilt = math::radians(_param_mpc_tiltmax_lnd.get()); - } - } - - if (_vehicle_land_detected.landed && !_in_takeoff_ramp && !PX4_ISFINITE(setpoint.thrust[2])) { - // Keep throttle low when landed and NOT in smooth takeoff - reset_setpoint_to_nan(setpoint); - setpoint.thrust[0] = setpoint.thrust[1] = setpoint.thrust[2] = 0.0f; - setpoint.yaw = _states.yaw; - // reactivate the task which will reset the setpoint to current state - _flight_tasks.reActivate(); - } - // limit altitude only if local position is valid if (PX4_ISFINITE(_states.position(2))) { limit_altitude(setpoint); @@ -719,6 +681,46 @@ MulticopterPositionControl::run() local_pos_sp.vz = PX4_ISFINITE(_control.getVelSp()(2)) ? _control.getVelSp()(2) : setpoint.vz; _control.getThrustSetpoint().copyTo(local_pos_sp.thrust); + // do smooth takeoff after delay if there's a valid vertical velocity or position + if (_spoolup_time_hysteresis.get_state()) { + update_takeoff_ramp(constraints.want_takeoff, local_pos_sp.thrust[2]); + } + + // disable horizontal / yaw control during smooth takeoff and limit maximum speed upwards + if (_in_takeoff_ramp) { + // altitude above reference takeoff + const float alt_above_tko = -(_states.position(2) - _takeoff_reference_z); + + // disable yaw control when close to ground + if (alt_above_tko <= ALTITUDE_THRESHOLD) { + + setpoint.yawspeed = NAN; + + // if there is a valid yaw estimate, just set setpoint to yaw + if (PX4_ISFINITE(_states.yaw)) { + setpoint.yaw = _states.yaw; + } + + // limit tilt during smooth takeoff when still close to ground + constraints.tilt = math::radians(_param_mpc_tiltmax_lnd.get()); + } + } + + if (_in_takeoff_ramp) { + local_pos_sp.thrust[2] = math::max(local_pos_sp.thrust[2], _takeoff_ramp_thrust); + //local_pos_sp.thrust[0] = local_pos_sp.thrust[1] = 0.f; + _control.resetIntegralXY(); + _control.resetIntegralZ(); + } + + if (_vehicle_land_detected.landed && !_in_takeoff_ramp && !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.yaw = _states.yaw; + // reactivate the task which will reset the setpoint to current state + _flight_tasks.reActivate(); + } + // Publish local position setpoint // This message will be used by other modules (such as Landdetector) to determine // vehicle intention. @@ -977,40 +979,29 @@ MulticopterPositionControl::start_flight_task() } void -MulticopterPositionControl::update_takeoff_ramp(const bool want_takeoff, const float vz_sp, const float z_sp, const float vz_constraint) +MulticopterPositionControl::update_takeoff_ramp(const bool want_takeoff, const float thr_sp) { - const float takeoff_ramp_initialization = -0.7f; - // check if takeoff is triggered if (_vehicle_land_detected.landed && !_in_takeoff_ramp && want_takeoff) { // takeoff was triggered, start velocity ramp - _takeoff_ramp_velocity = takeoff_ramp_initialization; + _takeoff_ramp_thrust = 0.0f; _in_takeoff_ramp = true; _takeoff_reference_z = _states.position(2); } - // if in smooth takeoff limit upwards velocity setpoint to a smooth ramp + // if in smooth takeoff limit upwards thrust setpoint to a smooth ramp if (_in_takeoff_ramp) { - float takeoff_desired_velocity = -vz_sp; - - // if there's only a position setpoint we go up with the configured takeoff speed - if (!PX4_ISFINITE(vz_sp) && PX4_ISFINITE(z_sp)) { - takeoff_desired_velocity = vz_constraint; - } - + float takeoff_desired_thrust = thr_sp; if (_param_mpc_tko_ramp_t.get() > _dt) { - _takeoff_ramp_velocity += (takeoff_desired_velocity - takeoff_ramp_initialization) * _dt / _param_mpc_tko_ramp_t.get(); + _takeoff_ramp_thrust += takeoff_desired_thrust * _dt / _param_mpc_tko_ramp_t.get(); } else { // no ramp time, directly jump to desired velocity - _takeoff_ramp_velocity = takeoff_desired_velocity; + _takeoff_ramp_thrust = takeoff_desired_thrust; } - // make sure we cannot overshoot the desired setpoint with the ramp - _takeoff_ramp_velocity = math::min(_takeoff_ramp_velocity, takeoff_desired_velocity); - - // smooth takeoff is finished once desired velocity setpoint is reached - _in_takeoff_ramp = (_takeoff_ramp_velocity < takeoff_desired_velocity); + // smooth takeoff is finished once desired thrust setpoint is reached + _in_takeoff_ramp = (_takeoff_ramp_thrust > takeoff_desired_thrust); } } From 41498712057b2582759a006016f7cccc949307d6 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 15 May 2019 18:53:15 +0200 Subject: [PATCH 11/18] Add a Takeoff class to handle multicopter takeoff In a deterministic way with clear states to go through. --- src/modules/mc_pos_control/CMakeLists.txt | 4 + .../mc_pos_control/Takeoff/CMakeLists.txt | 42 ++++++ .../mc_pos_control/Takeoff/Takeoff.cpp | 121 ++++++++++++++++++ .../mc_pos_control/Takeoff/Takeoff.hpp | 83 ++++++++++++ .../mc_pos_control/Takeoff/TakeoffTest.cpp | 49 +++++++ .../mc_pos_control/mc_pos_control_main.cpp | 91 +++---------- 6 files changed, 318 insertions(+), 72 deletions(-) create mode 100644 src/modules/mc_pos_control/Takeoff/CMakeLists.txt create mode 100644 src/modules/mc_pos_control/Takeoff/Takeoff.cpp create mode 100644 src/modules/mc_pos_control/Takeoff/Takeoff.hpp create mode 100644 src/modules/mc_pos_control/Takeoff/TakeoffTest.cpp diff --git a/src/modules/mc_pos_control/CMakeLists.txt b/src/modules/mc_pos_control/CMakeLists.txt index f649cecbef69..08c93243c292 100644 --- a/src/modules/mc_pos_control/CMakeLists.txt +++ b/src/modules/mc_pos_control/CMakeLists.txt @@ -30,6 +30,9 @@ # POSSIBILITY OF SUCH DAMAGE. # ############################################################################ + +add_subdirectory(Takeoff) + px4_add_module( MODULE modules__mc_pos_control MAIN mc_pos_control @@ -46,4 +49,5 @@ px4_add_module( ecl_geo WeatherVane CollisionPrevention + Takeoff ) diff --git a/src/modules/mc_pos_control/Takeoff/CMakeLists.txt b/src/modules/mc_pos_control/Takeoff/CMakeLists.txt new file mode 100644 index 000000000000..164b175ed979 --- /dev/null +++ b/src/modules/mc_pos_control/Takeoff/CMakeLists.txt @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2019 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_library(Takeoff + Takeoff.cpp +) +target_include_directories(Takeoff + PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR} +) + +px4_add_gtest(SRC TakeoffTest.cpp LINKLIBS Takeoff) diff --git a/src/modules/mc_pos_control/Takeoff/Takeoff.cpp b/src/modules/mc_pos_control/Takeoff/Takeoff.cpp new file mode 100644 index 000000000000..37c9dd37433c --- /dev/null +++ b/src/modules/mc_pos_control/Takeoff/Takeoff.cpp @@ -0,0 +1,121 @@ +/**************************************************************************** + * + * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file Takeoff.cpp + */ + +#include "Takeoff.hpp" +#include + +void Takeoff::updateTakeoffState(const bool armed, const bool landed, const bool want_takeoff, + const float takeoff_desired_thrust, const bool skip_takeoff) +{ + _spoolup_time_hysteresis.set_state_and_update(armed); + + switch (_takeoff_state) { + case TakeoffState::disarmed: + if (armed) { + _takeoff_state = TakeoffState::spoolup; + + } else { + break; + } + + case TakeoffState::spoolup: + if (_spoolup_time_hysteresis.get_state()) { + _takeoff_state = TakeoffState::ready_for_takeoff; + + } else { + break; + } + + case TakeoffState::ready_for_takeoff: + if (want_takeoff) { + _takeoff_state = TakeoffState::rampup; + _takeoff_ramp_thrust = 0.0f; + + } else { + break; + } + + case TakeoffState::rampup: + if (_takeoff_ramp_thrust <= takeoff_desired_thrust) { + _takeoff_state = TakeoffState::flight; + + } else { + break; + } + + case TakeoffState::flight: + if (landed) { + _takeoff_state = TakeoffState::ready_for_takeoff; + } + + break; + + default: + break; + } + + if (armed && skip_takeoff) { + _takeoff_state = TakeoffState::flight; + } + + // TODO: need to consider free fall here + if (!armed) { + _takeoff_state = TakeoffState::disarmed; + } +} + +float Takeoff::updateThrustRamp(const float takeoff_desired_thrust, const float dt) +{ + if (_takeoff_state < TakeoffState::rampup) { + return 0.f; + } + + if (_takeoff_state == TakeoffState::rampup) { + if (_takeoff_ramp_time > FLT_EPSILON) { + _takeoff_ramp_thrust += takeoff_desired_thrust * dt / _takeoff_ramp_time; + + } else { + _takeoff_ramp_thrust = takeoff_desired_thrust; + } + + if (_takeoff_ramp_thrust > takeoff_desired_thrust) { + return _takeoff_ramp_thrust; + } + } + + return takeoff_desired_thrust; +} diff --git a/src/modules/mc_pos_control/Takeoff/Takeoff.hpp b/src/modules/mc_pos_control/Takeoff/Takeoff.hpp new file mode 100644 index 000000000000..6d07f2983b0d --- /dev/null +++ b/src/modules/mc_pos_control/Takeoff/Takeoff.hpp @@ -0,0 +1,83 @@ +/**************************************************************************** + * + * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file Takeoff.hpp + * + * A class handling all takeoff states and a smooth ramp up of the motors. + */ + +#pragma once + +#include +#include + +using namespace time_literals; + +enum class TakeoffState { + disarmed = 0, + spoolup, + ready_for_takeoff, + rampup, + flight +}; + +class Takeoff +{ +public: + + Takeoff() = default; + ~Takeoff() = default; + + /** + * Update the state for the takeoff. + * @param setpoint a vehicle_local_position_setpoint_s structure + * @return true if setpoint has updated correctly + */ + void updateTakeoffState(const bool armed, const bool landed, const bool want_takeoff, + const float takeoff_desired_thrust, const bool skip_takeoff); + float updateThrustRamp(const float dt, const float takeoff_desired_thrust); + + void setTakeoffRampTime(const float seconds) { _takeoff_ramp_time = seconds; } + void setSpoolupTime(const float seconds) { _spoolup_time_hysteresis.set_hysteresis_time_from(false, (hrt_abstime)(seconds * (float)1_s)); } + TakeoffState getTakeoffState() { return _takeoff_state; } + + // TODO: make this private as soon as tasks also run while disarmed and updateTakeoffState gets called all the time + systemlib::Hysteresis _spoolup_time_hysteresis{false}; /**< becomes true MPC_SPOOLUP_TIME seconds after the vehicle was armed */ + +private: + TakeoffState _takeoff_state = TakeoffState::disarmed; + + float _takeoff_ramp_time = 0.f; + float _takeoff_ramp_thrust = 0.f; +}; diff --git a/src/modules/mc_pos_control/Takeoff/TakeoffTest.cpp b/src/modules/mc_pos_control/Takeoff/TakeoffTest.cpp new file mode 100644 index 000000000000..9d8b77699d71 --- /dev/null +++ b/src/modules/mc_pos_control/Takeoff/TakeoffTest.cpp @@ -0,0 +1,49 @@ +/**************************************************************************** + * + * Copyright (C) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include + +TEST(TakeoffTest, Initialization) +{ + Takeoff takeoff; + EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::disarmed); +} + +// TEST(TakeoffTest, Ramp) +// { +// Takeoff takeoff; +// takeoff.updateTakeoffState(true, false, true, 1.f, false); +// takeoff.updateThrustRamp(1.f, 0.1f); +// EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::disarmed); +// } 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 62002c6ca2d7..6ab4909cc2dd 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -67,6 +67,7 @@ #include #include "PositionControl.hpp" #include "Utility/ControlMath.hpp" +#include "Takeoff.hpp" using namespace time_literals; @@ -102,10 +103,7 @@ class MulticopterPositionControl : public ModuleBase int print_status() override; private: - // smooth takeoff vertical velocity ramp state - bool _in_takeoff_ramp = false; /**< true while takeoff ramp is applied */ - float _takeoff_ramp_thrust = 0.f; /**< current value of the smooth takeoff ramp */ - float _takeoff_reference_z = 0.f; /**< z-position when takeoff ramp was initiated */ + Takeoff _takeoff; /**< state machine and ramp to bring the vehicle off the ground without jumps */ orb_advert_t _att_sp_pub{nullptr}; /**< attitude setpoint publication */ orb_advert_t _traj_sp_pub{nullptr}; /**< trajectory setpoints publication */ @@ -177,7 +175,6 @@ class MulticopterPositionControl : public ModuleBase /** During smooth-takeoff, below ALTITUDE_THRESHOLD the yaw-control is turned off ant tilt is limited */ static constexpr float ALTITUDE_THRESHOLD = 0.3f; - systemlib::Hysteresis _spoolup_time_hysteresis{false}; /**< becomes true MPC_SPOOLUP_TIME seconds after the vehicle was armed */ systemlib::Hysteresis _failsafe_land_hysteresis{false}; /**< becomes true if task did not update correctly for LOITER_TIME_BEFORE_DESCEND */ WeatherVane *_wv_controller{nullptr}; @@ -229,14 +226,6 @@ class MulticopterPositionControl : public ModuleBase */ void publish_trajectory_sp(const vehicle_local_position_setpoint_s &traj); - /** - * Update smooth takeoff vertical velocity ramp to bring the vehicle off the ground without step - * @param vz_sp velocity setpoint in the z-Direction - * @param z_sp position setpoint in the z-Direction - * @param vz_constraint velocity to ramp to when there's only a position setpoint - */ - void update_takeoff_ramp(const bool want_takeoff, const float thr_sp); - /** * Adjust the setpoint during landing. * Thrust is adjusted to support the land-detector during detection. @@ -367,7 +356,8 @@ MulticopterPositionControl::parameters_update(bool force) _param_mpc_land_speed.set(math::min(_param_mpc_land_speed.get(), _param_mpc_z_vel_max_dn.get())); // set trigger time for takeoff delay - _spoolup_time_hysteresis.set_hysteresis_time_from(false, (int)(_param_mpc_spoolup_time.get() * (float)1_s)); + _takeoff.setSpoolupTime(_param_mpc_spoolup_time.get()); + _takeoff.setTakeoffRampTime(_param_mpc_tko_ramp_t.get()); if (_wv_controller != nullptr) { _wv_controller->update_parameters(); @@ -592,9 +582,9 @@ MulticopterPositionControl::run() } // takeoff delay for motors to reach idle speed - _spoolup_time_hysteresis.set_state_and_update(_control_mode.flag_armed); + _takeoff._spoolup_time_hysteresis.set_state_and_update(_control_mode.flag_armed); - if (_spoolup_time_hysteresis.get_state()) { + if (_takeoff._spoolup_time_hysteresis.get_state()) { // when vehicle is ready switch to the required flighttask start_flight_task(); @@ -681,39 +671,22 @@ MulticopterPositionControl::run() local_pos_sp.vz = PX4_ISFINITE(_control.getVelSp()(2)) ? _control.getVelSp()(2) : setpoint.vz; _control.getThrustSetpoint().copyTo(local_pos_sp.thrust); - // do smooth takeoff after delay if there's a valid vertical velocity or position - if (_spoolup_time_hysteresis.get_state()) { - update_takeoff_ramp(constraints.want_takeoff, local_pos_sp.thrust[2]); - } - - // disable horizontal / yaw control during smooth takeoff and limit maximum speed upwards - if (_in_takeoff_ramp) { - // altitude above reference takeoff - const float alt_above_tko = -(_states.position(2) - _takeoff_reference_z); - - // disable yaw control when close to ground - if (alt_above_tko <= ALTITUDE_THRESHOLD) { + // handle smooth takeoff + _takeoff.updateTakeoffState(_control_mode.flag_armed, _vehicle_land_detected.landed, constraints.want_takeoff, local_pos_sp.thrust[2], !_control_mode.flag_control_climb_rate_enabled); + local_pos_sp.thrust[2] = _takeoff.updateThrustRamp(local_pos_sp.thrust[2], _dt); - setpoint.yawspeed = NAN; - - // if there is a valid yaw estimate, just set setpoint to yaw - if (PX4_ISFINITE(_states.yaw)) { - setpoint.yaw = _states.yaw; - } - - // limit tilt during smooth takeoff when still close to ground - constraints.tilt = math::radians(_param_mpc_tiltmax_lnd.get()); - } - } - - if (_in_takeoff_ramp) { - local_pos_sp.thrust[2] = math::max(local_pos_sp.thrust[2], _takeoff_ramp_thrust); - //local_pos_sp.thrust[0] = local_pos_sp.thrust[1] = 0.f; + if (_takeoff.getTakeoffState() < TakeoffState::flight) { + // we set thrust to zero, this will help to decide if we are actually landed or not + setpoint.thrust[0] = setpoint.thrust[1] = 0.0f; + // set yaw-sp to current yaw to avoid any corrections + setpoint.yaw = _states.yaw; + setpoint.yawspeed = 0.f; + // prevent any integrator windup _control.resetIntegralXY(); _control.resetIntegralZ(); } - if (_vehicle_land_detected.landed && !_in_takeoff_ramp && !PX4_ISFINITE(setpoint.thrust[2])) { + if (_takeoff.getTakeoffState() < TakeoffState::rampup && !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.yaw = _states.yaw; @@ -733,7 +706,7 @@ MulticopterPositionControl::run() // Part of landing logic: if ground-contact/maybe landed was detected, turn off // controller. This message does not have to be logged as part of the vehicle_local_position_setpoint topic. // Note: only adust thrust output if there was not thrust-setpoint demand in D-direction. - if (!_in_takeoff_ramp && !PX4_ISFINITE(setpoint.thrust[2])) { + if (_takeoff.getTakeoffState() > TakeoffState::rampup && !PX4_ISFINITE(setpoint.thrust[2])) { limit_thrust_during_landing(local_pos_sp); } @@ -978,33 +951,6 @@ MulticopterPositionControl::start_flight_task() } } -void -MulticopterPositionControl::update_takeoff_ramp(const bool want_takeoff, const float thr_sp) -{ - // check if takeoff is triggered - if (_vehicle_land_detected.landed && !_in_takeoff_ramp && want_takeoff) { - // takeoff was triggered, start velocity ramp - _takeoff_ramp_thrust = 0.0f; - _in_takeoff_ramp = true; - _takeoff_reference_z = _states.position(2); - } - - // if in smooth takeoff limit upwards thrust setpoint to a smooth ramp - if (_in_takeoff_ramp) { - float takeoff_desired_thrust = thr_sp; - - if (_param_mpc_tko_ramp_t.get() > _dt) { - _takeoff_ramp_thrust += takeoff_desired_thrust * _dt / _param_mpc_tko_ramp_t.get(); - } else { - // no ramp time, directly jump to desired velocity - _takeoff_ramp_thrust = takeoff_desired_thrust; - } - - // smooth takeoff is finished once desired thrust setpoint is reached - _in_takeoff_ramp = (_takeoff_ramp_thrust > takeoff_desired_thrust); - } -} - void MulticopterPositionControl::limit_thrust_during_landing(vehicle_local_position_setpoint_s &setpoint) { @@ -1014,6 +960,7 @@ MulticopterPositionControl::limit_thrust_during_landing(vehicle_local_position_s setpoint.thrust[0] = setpoint.thrust[1] = setpoint.thrust[2] = 0.0f; // set yaw-sp to current yaw to avoid any corrections setpoint.yaw = _states.yaw; + setpoint.yawspeed = 0.f; // prevent any integrator windup _control.resetIntegralXY(); _control.resetIntegralZ(); From fa813c08c1c96571aaa6885b21494ce2a7255504 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Thu, 16 May 2019 10:23:06 +0100 Subject: [PATCH 12/18] mc_pos_control: fix adjusting the wrong setpoint There are two local_position_setpoint in the position controller. One describing the setpoint the task gives to the position controller and a second one with the output of the position controller. I corrected the wrong one during takeoff because the new takeoff thrust ramp runs after the controller and not before. --- .../mc_pos_control/mc_pos_control_main.cpp | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) 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 6ab4909cc2dd..4918238446ef 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -675,21 +675,20 @@ MulticopterPositionControl::run() _takeoff.updateTakeoffState(_control_mode.flag_armed, _vehicle_land_detected.landed, constraints.want_takeoff, local_pos_sp.thrust[2], !_control_mode.flag_control_climb_rate_enabled); local_pos_sp.thrust[2] = _takeoff.updateThrustRamp(local_pos_sp.thrust[2], _dt); - if (_takeoff.getTakeoffState() < TakeoffState::flight) { - // we set thrust to zero, this will help to decide if we are actually landed or not - setpoint.thrust[0] = setpoint.thrust[1] = 0.0f; + if (_takeoff.getTakeoffState() < TakeoffState::flight && !PX4_ISFINITE(setpoint.thrust[2])) { + // we are not flying yet and need to avoid any corrections + // set the horizontal thrust to zero + local_pos_sp.thrust[0] = local_pos_sp.thrust[1] = 0.0f; // set yaw-sp to current yaw to avoid any corrections - setpoint.yaw = _states.yaw; - setpoint.yawspeed = 0.f; + // TODO: we need a clean way to disable yaw control + local_pos_sp.yaw = _states.yaw; + local_pos_sp.yawspeed = 0.f; // prevent any integrator windup _control.resetIntegralXY(); _control.resetIntegralZ(); } if (_takeoff.getTakeoffState() < TakeoffState::rampup && !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.yaw = _states.yaw; // reactivate the task which will reset the setpoint to current state _flight_tasks.reActivate(); } From 33c7a9cb6bdc005f799f0156ccaf8c276167edc7 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Thu, 16 May 2019 12:49:25 +0100 Subject: [PATCH 13/18] mc_pos_control: fix updating takeoff state when no flight task is running Without always updating the takeoff state it will not get skipped when the takeoff happened manually and when you switch from manual to position mode the drone goes to idle and falls. --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) 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 4918238446ef..5d7316c8c800 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -581,10 +581,11 @@ MulticopterPositionControl::run() _wv_controller->update(matrix::Quatf(_att_sp.q_d), _states.yaw); } - // takeoff delay for motors to reach idle speed - _takeoff._spoolup_time_hysteresis.set_state_and_update(_control_mode.flag_armed); + // an update is necessary here because otherwise the takeoff state doesn't get skiped with non-altitude-controlled modes + _takeoff.updateTakeoffState(_control_mode.flag_armed, _vehicle_land_detected.landed, false, -1.f, !_control_mode.flag_control_climb_rate_enabled); - if (_takeoff._spoolup_time_hysteresis.get_state()) { + // takeoff delay for motors to reach idle speed + if (_takeoff.getTakeoffState() >= TakeoffState::ready_for_takeoff) { // when vehicle is ready switch to the required flighttask start_flight_task(); From dfe355fd037f2f96118f4026e1e1566bcf750593 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Thu, 16 May 2019 19:51:21 +0100 Subject: [PATCH 14/18] mc_pos_control_params: user friendly default spool/rampup times --- src/modules/mc_pos_control/mc_pos_control_params.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) 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 746f930964fb..f6fc4702c84e 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -668,10 +668,10 @@ PARAM_DEFINE_FLOAT(MPC_LAND_ALT2, 5.0f); * A time constant of 0 disables the ramp * * @min 0 - * @max 1 + * @max 5 * @group Multicopter Position Control */ -PARAM_DEFINE_FLOAT(MPC_TKO_RAMP_T, 0.4f); +PARAM_DEFINE_FLOAT(MPC_TKO_RAMP_T, 3.0f); /** * Manual-Position control sub-mode @@ -717,7 +717,7 @@ PARAM_DEFINE_INT32(MPC_AUTO_MODE, 1); * @unit s * @group Multicopter Position Control */ -PARAM_DEFINE_FLOAT(MPC_SPOOLUP_TIME, 0.0f); +PARAM_DEFINE_FLOAT(MPC_SPOOLUP_TIME, 1.0f); /** * Yaw mode. From 0ace0486955d050faebb7e5135b5902b7357eda4 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Fri, 17 May 2019 08:17:24 +0100 Subject: [PATCH 15/18] mc_pos_control: switch back to velocity ramp But fix the two crucial problems: - When to begin the ramp? There's a calculation now for the velocity ramp initial value such that the resulting thrust is zero at the beginning. - When to end the ramp? The ramp is applied to the upwards velocity constraint and it just ramps from the initial value to the velocity constraint which is applied during flight. Slower/going down is always possible. --- .../mc_pos_control/Takeoff/Takeoff.cpp | 30 ++++++----- .../mc_pos_control/Takeoff/Takeoff.hpp | 20 +++---- .../mc_pos_control/mc_pos_control_main.cpp | 53 ++++++++++--------- 3 files changed, 58 insertions(+), 45 deletions(-) diff --git a/src/modules/mc_pos_control/Takeoff/Takeoff.cpp b/src/modules/mc_pos_control/Takeoff/Takeoff.cpp index 37c9dd37433c..77abc78ab10f 100644 --- a/src/modules/mc_pos_control/Takeoff/Takeoff.cpp +++ b/src/modules/mc_pos_control/Takeoff/Takeoff.cpp @@ -36,10 +36,16 @@ */ #include "Takeoff.hpp" -#include +#include + +void Takeoff::generateInitialValue(const float hover_thrust, float velocity_p_gain) +{ + velocity_p_gain = math::max(velocity_p_gain, 0.01f); + _takeoff_ramp_vz_init = -hover_thrust / velocity_p_gain; +} void Takeoff::updateTakeoffState(const bool armed, const bool landed, const bool want_takeoff, - const float takeoff_desired_thrust, const bool skip_takeoff) + const float takeoff_desired_vz, const bool skip_takeoff) { _spoolup_time_hysteresis.set_state_and_update(armed); @@ -63,14 +69,14 @@ void Takeoff::updateTakeoffState(const bool armed, const bool landed, const bool case TakeoffState::ready_for_takeoff: if (want_takeoff) { _takeoff_state = TakeoffState::rampup; - _takeoff_ramp_thrust = 0.0f; + _takeoff_ramp_vz = _takeoff_ramp_vz_init; } else { break; } case TakeoffState::rampup: - if (_takeoff_ramp_thrust <= takeoff_desired_thrust) { + if (_takeoff_ramp_vz >= takeoff_desired_vz) { _takeoff_state = TakeoffState::flight; } else { @@ -98,24 +104,24 @@ void Takeoff::updateTakeoffState(const bool armed, const bool landed, const bool } } -float Takeoff::updateThrustRamp(const float takeoff_desired_thrust, const float dt) +float Takeoff::updateRamp(const float dt, const float takeoff_desired_vz) { if (_takeoff_state < TakeoffState::rampup) { - return 0.f; + return _takeoff_ramp_vz_init; } if (_takeoff_state == TakeoffState::rampup) { - if (_takeoff_ramp_time > FLT_EPSILON) { - _takeoff_ramp_thrust += takeoff_desired_thrust * dt / _takeoff_ramp_time; + if (_takeoff_ramp_time > dt) { + _takeoff_ramp_vz += (takeoff_desired_vz - _takeoff_ramp_vz_init) * dt / _takeoff_ramp_time; } else { - _takeoff_ramp_thrust = takeoff_desired_thrust; + _takeoff_ramp_vz = takeoff_desired_vz; } - if (_takeoff_ramp_thrust > takeoff_desired_thrust) { - return _takeoff_ramp_thrust; + if (_takeoff_ramp_vz < takeoff_desired_vz) { + return _takeoff_ramp_vz; } } - return takeoff_desired_thrust; + return takeoff_desired_vz; } diff --git a/src/modules/mc_pos_control/Takeoff/Takeoff.hpp b/src/modules/mc_pos_control/Takeoff/Takeoff.hpp index 6d07f2983b0d..957562e1b756 100644 --- a/src/modules/mc_pos_control/Takeoff/Takeoff.hpp +++ b/src/modules/mc_pos_control/Takeoff/Takeoff.hpp @@ -55,29 +55,31 @@ enum class TakeoffState { class Takeoff { public: - Takeoff() = default; ~Takeoff() = default; + // initialize parameters + void setTakeoffRampTime(const float seconds) { _takeoff_ramp_time = seconds; } + void setSpoolupTime(const float seconds) { _spoolup_time_hysteresis.set_hysteresis_time_from(false, (hrt_abstime)(seconds * (float)1_s)); } + void generateInitialValue(const float hover_thrust, const float velocity_p_gain); + /** * Update the state for the takeoff. * @param setpoint a vehicle_local_position_setpoint_s structure * @return true if setpoint has updated correctly */ void updateTakeoffState(const bool armed, const bool landed, const bool want_takeoff, - const float takeoff_desired_thrust, const bool skip_takeoff); - float updateThrustRamp(const float dt, const float takeoff_desired_thrust); + const float takeoff_desired_vz, const bool skip_takeoff); + float updateRamp(const float dt, const float takeoff_desired_vz); - void setTakeoffRampTime(const float seconds) { _takeoff_ramp_time = seconds; } - void setSpoolupTime(const float seconds) { _spoolup_time_hysteresis.set_hysteresis_time_from(false, (hrt_abstime)(seconds * (float)1_s)); } TakeoffState getTakeoffState() { return _takeoff_state; } - // TODO: make this private as soon as tasks also run while disarmed and updateTakeoffState gets called all the time - systemlib::Hysteresis _spoolup_time_hysteresis{false}; /**< becomes true MPC_SPOOLUP_TIME seconds after the vehicle was armed */ - private: TakeoffState _takeoff_state = TakeoffState::disarmed; float _takeoff_ramp_time = 0.f; - float _takeoff_ramp_thrust = 0.f; + float _takeoff_ramp_vz_init = 0.f; + float _takeoff_ramp_vz = 0.f; + + systemlib::Hysteresis _spoolup_time_hysteresis{false}; /**< becomes true MPC_SPOOLUP_TIME seconds after the vehicle was armed */ }; 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 5d7316c8c800..82690e4b04b8 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -151,7 +151,9 @@ class MulticopterPositionControl : public ModuleBase (ParamInt) _param_mpc_auto_mode, (ParamInt) _param_mpc_alt_mode, (ParamFloat) _param_mpc_spoolup_time, /**< time to let motors spool up after arming */ - (ParamFloat) _param_mpc_tiltmax_lnd /**< maximum tilt for landing and smooth takeoff */ + (ParamFloat) _param_mpc_tiltmax_lnd, /**< maximum tilt for landing and smooth takeoff */ + (ParamFloat)_param_mpc_thr_hover, + (ParamFloat)_param_mpc_z_vel_p ); control::BlockDerivative _vel_x_deriv; /**< velocity derivative in x */ @@ -358,6 +360,7 @@ MulticopterPositionControl::parameters_update(bool force) // set trigger time for takeoff delay _takeoff.setSpoolupTime(_param_mpc_spoolup_time.get()); _takeoff.setTakeoffRampTime(_param_mpc_tko_ramp_t.get()); + _takeoff.generateInitialValue(_param_mpc_thr_hover.get(), _param_mpc_z_vel_p.get()); if (_wv_controller != nullptr) { _wv_controller->update_parameters(); @@ -582,7 +585,7 @@ MulticopterPositionControl::run() } // an update is necessary here because otherwise the takeoff state doesn't get skiped with non-altitude-controlled modes - _takeoff.updateTakeoffState(_control_mode.flag_armed, _vehicle_land_detected.landed, false, -1.f, !_control_mode.flag_control_climb_rate_enabled); + _takeoff.updateTakeoffState(_control_mode.flag_armed, _vehicle_land_detected.landed, false, 10.f, !_control_mode.flag_control_climb_rate_enabled); // takeoff delay for motors to reach idle speed if (_takeoff.getTakeoffState() >= TakeoffState::ready_for_takeoff) { @@ -634,6 +637,30 @@ MulticopterPositionControl::run() // check if all local states are valid and map accordingly set_vehicle_states(setpoint.vz); + // handle smooth takeoff + _takeoff.updateTakeoffState(_control_mode.flag_armed, _vehicle_land_detected.landed, constraints.want_takeoff, constraints.speed_up, !_control_mode.flag_control_climb_rate_enabled); + constraints.speed_up = _takeoff.updateRamp(_dt, constraints.speed_up); + + if (_takeoff.getTakeoffState() < TakeoffState::rampup && !PX4_ISFINITE(setpoint.thrust[2])) { + // we are not flying yet and need to avoid any corrections + reset_setpoint_to_nan(setpoint); + setpoint.thrust[0] = setpoint.thrust[1] = setpoint.thrust[2] = 0.0f; + // set yaw-sp to current yaw + // TODO: we need a clean way to disable yaw control + setpoint.yaw = _states.yaw; + setpoint.yawspeed = 0.f; + // prevent any integrator windup + _control.resetIntegralXY(); + _control.resetIntegralZ(); + // reactivate the task which will reset the setpoint to current state + _flight_tasks.reActivate(); + } + + if (_takeoff.getTakeoffState() < TakeoffState::flight && !PX4_ISFINITE(setpoint.thrust[2])) { + constraints.tilt = math::radians(_param_mpc_tiltmax_lnd.get()); + } + + // limit altitude only if local position is valid if (PX4_ISFINITE(_states.position(2))) { limit_altitude(setpoint); @@ -672,28 +699,6 @@ MulticopterPositionControl::run() local_pos_sp.vz = PX4_ISFINITE(_control.getVelSp()(2)) ? _control.getVelSp()(2) : setpoint.vz; _control.getThrustSetpoint().copyTo(local_pos_sp.thrust); - // handle smooth takeoff - _takeoff.updateTakeoffState(_control_mode.flag_armed, _vehicle_land_detected.landed, constraints.want_takeoff, local_pos_sp.thrust[2], !_control_mode.flag_control_climb_rate_enabled); - local_pos_sp.thrust[2] = _takeoff.updateThrustRamp(local_pos_sp.thrust[2], _dt); - - if (_takeoff.getTakeoffState() < TakeoffState::flight && !PX4_ISFINITE(setpoint.thrust[2])) { - // we are not flying yet and need to avoid any corrections - // set the horizontal thrust to zero - local_pos_sp.thrust[0] = local_pos_sp.thrust[1] = 0.0f; - // set yaw-sp to current yaw to avoid any corrections - // TODO: we need a clean way to disable yaw control - local_pos_sp.yaw = _states.yaw; - local_pos_sp.yawspeed = 0.f; - // prevent any integrator windup - _control.resetIntegralXY(); - _control.resetIntegralZ(); - } - - if (_takeoff.getTakeoffState() < TakeoffState::rampup && !PX4_ISFINITE(setpoint.thrust[2])) { - // reactivate the task which will reset the setpoint to current state - _flight_tasks.reActivate(); - } - // Publish local position setpoint // This message will be used by other modules (such as Landdetector) to determine // vehicle intention. From 78c8b2c57f936b30a9e314ebcde916e0342d6470 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 21 May 2019 13:21:25 +0100 Subject: [PATCH 16/18] Revert "mc_att_control: fix having high thrust when disarmed" This reverts commit 0c81a19decde6ddfe4ce87c34c762ea15fd3ab09. --- src/modules/mc_att_control/mc_att_control_main.cpp | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 2e5e84fc4bc9..b12bdaed6e83 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -500,10 +500,7 @@ MulticopterAttitudeControl::generate_attitude_setpoint(float dt, bool reset_yaw_ q_sp.copyTo(attitude_setpoint.q_d); attitude_setpoint.q_d_valid = true; - // hotfix: make sure to leave a zero thrust setpoint for position controller to take over after initialization in manual mode - // without this the actuator_controls thrust stays like it was set here when switching to a position controlled mode and the - // land detector will immediately detect takeoff when arming - attitude_setpoint.thrust_body[2] = _v_control_mode.flag_armed ? -throttle_curve(_manual_control_sp.z) : 0.0f; + attitude_setpoint.thrust_body[2] = -throttle_curve(_manual_control_sp.z); attitude_setpoint.timestamp = hrt_absolute_time(); if (_attitude_sp_id != nullptr) { orb_publish_auto(_attitude_sp_id, &_vehicle_attitude_setpoint_pub, &attitude_setpoint, nullptr, ORB_PRIO_DEFAULT); From fce056a5db16a1774b70314353032ec8651f9f07 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 22 May 2019 12:44:47 +0100 Subject: [PATCH 17/18] mc_att_control: fix applying not updated thrust setpoint --- src/modules/mc_att_control/mc_att_control_main.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index b12bdaed6e83..295ae8729643 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -521,6 +521,12 @@ MulticopterAttitudeControl::control_attitude() { vehicle_attitude_setpoint_poll(); + // reinitialize the setpoint while not armed to make sure no value from the last flight is still kept + if (!_v_control_mode.flag_armed) { + Quatf().copyTo(_v_att_sp.q_d); + Vector3f().copyTo(_v_att_sp.thrust_body); + } + // physical thrust axis is the negative of body z axis _thrust_sp = -_v_att_sp.thrust_body[2]; From 36736f966a3dd3ff4133c32622df538d2eb3c196 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 22 May 2019 15:38:00 +0100 Subject: [PATCH 18/18] Takeoff: address @RomanBapst's review comments --- .../tasks/FlightTask/FlightTask.cpp | 2 +- .../FlightTaskManualAltitude.cpp | 2 +- src/modules/mc_pos_control/Takeoff/Takeoff.cpp | 2 +- src/modules/mc_pos_control/Takeoff/Takeoff.hpp | 18 +++++++++++++++++- .../mc_pos_control/mc_pos_control_main.cpp | 2 +- 5 files changed, 21 insertions(+), 5 deletions(-) diff --git a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp index e79e7d66d1b2..21155786f5fd 100644 --- a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp +++ b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp @@ -152,7 +152,7 @@ bool FlightTask::_checkTakeoff() bool position_triggered_takeoff = false; if (PX4_ISFINITE(_position_setpoint(2))) { - //minimal altitude either 20cm or what is necessary for correct estimation e.g. optical flow + // minimal altitude either 20cm or what is necessary for correct estimation e.g. optical flow float min_altitude = 0.2f; const float min_distance_to_ground = _sub_vehicle_local_position->get().hagl_min; diff --git a/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp index 5fdd58ce3b30..433f0956e53a 100644 --- a/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp +++ b/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp @@ -347,7 +347,7 @@ void FlightTaskManualAltitude::_updateSetpoints() bool FlightTaskManualAltitude::_checkTakeoff() { - // stick is deflected above the middle 15% of the range + // stick is deflected above 65% throttle (_sticks(2) is in the range [-1,1]) return _sticks(2) < -0.3f; } diff --git a/src/modules/mc_pos_control/Takeoff/Takeoff.cpp b/src/modules/mc_pos_control/Takeoff/Takeoff.cpp index 77abc78ab10f..70af19134327 100644 --- a/src/modules/mc_pos_control/Takeoff/Takeoff.cpp +++ b/src/modules/mc_pos_control/Takeoff/Takeoff.cpp @@ -38,7 +38,7 @@ #include "Takeoff.hpp" #include -void Takeoff::generateInitialValue(const float hover_thrust, float velocity_p_gain) +void Takeoff::generateInitialRampValue(const float hover_thrust, float velocity_p_gain) { velocity_p_gain = math::max(velocity_p_gain, 0.01f); _takeoff_ramp_vz_init = -hover_thrust / velocity_p_gain; diff --git a/src/modules/mc_pos_control/Takeoff/Takeoff.hpp b/src/modules/mc_pos_control/Takeoff/Takeoff.hpp index 957562e1b756..d3bb433d20f4 100644 --- a/src/modules/mc_pos_control/Takeoff/Takeoff.hpp +++ b/src/modules/mc_pos_control/Takeoff/Takeoff.hpp @@ -61,7 +61,14 @@ class Takeoff // initialize parameters void setTakeoffRampTime(const float seconds) { _takeoff_ramp_time = seconds; } void setSpoolupTime(const float seconds) { _spoolup_time_hysteresis.set_hysteresis_time_from(false, (hrt_abstime)(seconds * (float)1_s)); } - void generateInitialValue(const float hover_thrust, const float velocity_p_gain); + + /** + * Calculate a vertical velocity to initialize the takeoff ramp + * that when passed to the velocity controller results in a zero throttle setpoint. + * @param hover_thrust normalized thrsut value with which the vehicle hovers + * @param velocity_p_gain proportional gain of the velocity controller to calculate the thrust + */ + void generateInitialRampValue(const float hover_thrust, const float velocity_p_gain); /** * Update the state for the takeoff. @@ -70,6 +77,15 @@ class Takeoff */ void updateTakeoffState(const bool armed, const bool landed, const bool want_takeoff, const float takeoff_desired_vz, const bool skip_takeoff); + + /** + * Update and return the velocity constraint ramp value during takeoff. + * By ramping up _takeoff_ramp_vz during the takeoff and using it to constain the maximum climb rate a smooth takeoff behavior is achieved. + * Returns zero on the ground and takeoff_desired_vz in flight. + * @param dt time in seconds since the last call/loop iteration + * @param takeoff_desired_vz end value for the velocity ramp + * @return true if setpoint has updated correctly + */ float updateRamp(const float dt, const float takeoff_desired_vz); TakeoffState getTakeoffState() { return _takeoff_state; } 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 82690e4b04b8..1580aedce1db 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -360,7 +360,7 @@ MulticopterPositionControl::parameters_update(bool force) // set trigger time for takeoff delay _takeoff.setSpoolupTime(_param_mpc_spoolup_time.get()); _takeoff.setTakeoffRampTime(_param_mpc_tko_ramp_t.get()); - _takeoff.generateInitialValue(_param_mpc_thr_hover.get(), _param_mpc_z_vel_p.get()); + _takeoff.generateInitialRampValue(_param_mpc_thr_hover.get(), _param_mpc_z_vel_p.get()); if (_wv_controller != nullptr) { _wv_controller->update_parameters();