From cdb6aca6bd8bd033324ba16466547515f36e47b8 Mon Sep 17 00:00:00 2001
From: Matthias Grob <maetugr@gmail.com>
Date: Mon, 28 Jan 2019 15:00:33 +0100
Subject: [PATCH] mc_pos_control: refactor spoolup time naming
---
.../mc_pos_control/mc_pos_control_main.cpp | 31 +++++++------------
.../mc_pos_control/mc_pos_control_params.c | 22 ++++++-------
2 files changed, 22 insertions(+), 31 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 f92e585386..bfd9ae3fa8 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -159,7 +159,7 @@ private:
(ParamInt<px4::params::MPC_POS_MODE>) MPC_POS_MODE,
(ParamInt<px4::params::MPC_AUTO_MODE>) MPC_AUTO_MODE,
(ParamInt<px4::params::MPC_ALT_MODE>) MPC_ALT_MODE,
- (ParamFloat<px4::params::MPC_IDLE_TKO>) MPC_IDLE_TKO, /**< time constant for smooth takeoff ramp */
+ (ParamFloat<px4::params::MPC_SPOOLUP_TIME>) MPC_SPOOLUP_TIME, /**< time to let motors spool up after arming */
(ParamInt<px4::params::MPC_OBS_AVOID>) MPC_OBS_AVOID, /**< enable obstacle avoidance */
(ParamFloat<px4::params::MPC_TILTMAX_LND>) MPC_TILTMAX_LND /**< maximum tilt for landing and smooth takeoff */
);
@@ -185,14 +185,7 @@ private:
/** During smooth-takeoff, below ALTITUDE_THRESHOLD the yaw-control is turned off ant tilt is limited */
static constexpr float ALTITUDE_THRESHOLD = 0.3f;
- /**
- * Hysteresis that turns true once vehicle is armed for MPC_IDLE_TKO seconds.
- * A real vehicle requires some time to accelerates the propellers to IDLE speed. To ensure
- * that the propellers reach idle speed before initiating a takeoff, a delay of MPC_IDLE_TKO
- * is added.
- */
- systemlib::Hysteresis _arm_hysteresis{false}; /**< becomes true once vehicle is armed for MPC_IDLE_TKO seconds */
-
+ 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};
@@ -411,8 +404,8 @@ MulticopterPositionControl::parameters_update(bool force)
_tko_speed.set(math::min(_tko_speed.get(), _vel_max_up.get()));
_land_speed.set(math::min(_land_speed.get(), _vel_max_down.get()));
- // set trigger time for arm hysteresis
- _arm_hysteresis.set_hysteresis_time_from(false, (int)(MPC_IDLE_TKO.get() * (float)1_s));
+ // set trigger time for takeoff delay
+ _spoolup_time_hysteresis.set_hysteresis_time_from(false, (int)(MPC_SPOOLUP_TIME.get() * (float)1_s));
if (_wv_controller != nullptr) {
_wv_controller->update_parameters();
@@ -649,16 +642,15 @@ MulticopterPositionControl::run()
_wv_controller->update(matrix::Quatf(_att_sp.q_d), _states.yaw);
}
- // arm hysteresis prevents vehicle to takeoff
- // before propeller reached idle speed.
- _arm_hysteresis.set_state_and_update(_control_mode.flag_armed);
+ // start takeoff after delay to allow motors to reach idle speed
+ _spoolup_time_hysteresis.set_state_and_update(_control_mode.flag_armed);
- if (_arm_hysteresis.get_state()) {
- // as soon vehicle is armed check for flighttask
+ if (_spoolup_time_hysteresis.get_state()) {
+ // when vehicle is ready switch to the required flighttask
start_flight_task();
} else {
- // disable flighttask
+ // stop flighttask while disarmed
_flight_tasks.switchTask(FlightTaskIndex::None);
}
@@ -706,9 +698,8 @@ MulticopterPositionControl::run()
// check if all local states are valid and map accordingly
set_vehicle_states(setpoint.vz);
- // we can only do a smooth takeoff if a valid velocity or position is available and are
- // armed long enough
- if (_arm_hysteresis.get_state() && PX4_ISFINITE(_states.position(2)) && PX4_ISFINITE(_states.velocity(2))) {
+ // 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, constraints);
update_smooth_takeoff(setpoint.z, setpoint.vz);
}
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 9b91cf426a..f87debef19 100644
--- a/src/modules/mc_pos_control/mc_pos_control_params.c
+++ b/src/modules/mc_pos_control/mc_pos_control_params.c
@@ -658,7 +658,7 @@ PARAM_DEFINE_FLOAT(MPC_LAND_ALT2, 5.0f);
PARAM_DEFINE_FLOAT(MPC_TKO_RAMP_T, 0.4f);
/**
- * Manual-Position control sub-mode.
+ * Manual-Position control sub-mode
*
* The supported sub-modes are:
* 0 Default position control where sticks map to position/velocity directly. Maximum speeds
@@ -679,7 +679,7 @@ PARAM_DEFINE_FLOAT(MPC_TKO_RAMP_T, 0.4f);
PARAM_DEFINE_INT32(MPC_POS_MODE, 1);
/**
- * Auto sub-mode.
+ * Auto sub-mode
*
* @value 0 Default line tracking
* @value 1 Jerk-limited trajectory
@@ -688,20 +688,20 @@ PARAM_DEFINE_INT32(MPC_POS_MODE, 1);
PARAM_DEFINE_INT32(MPC_AUTO_MODE, 1);
/**
- * Delay from idle state to arming state.
+ * Enforced delay between arming and takeoff
*
- * For altitude controlled modes, the transition from
- * idle to armed state is delayed by MPC_IDLE_TKO time to ensure
- * that the propellers have reached idle speed before attempting a
- * takeoff. This delay is particularly useful for vehicles with large
- * propellers.
+ * For altitude controlled modes the time from arming the motors until
+ * a takeoff is possible gets forced to be at least MPC_SPOOLUP_TIME seconds
+ * to ensure the motors and propellers can sppol up and reach idle speed before
+ * getting commanded to spin faster. This delay is particularly useful for vehicles
+ * with slow motor spin-up e.g. because of large propellers.
*
* @min 0
* @max 10
- * @unit sec
+ * @unit s
* @group Multicopter Position Control
*/
-PARAM_DEFINE_FLOAT(MPC_IDLE_TKO, 0.0f);
+PARAM_DEFINE_FLOAT(MPC_SPOOLUP_TIME, 0.0f);
/**
* Flag to enable obstacle avoidance
@@ -713,7 +713,7 @@ PARAM_DEFINE_FLOAT(MPC_IDLE_TKO, 0.0f);
PARAM_DEFINE_INT32(MPC_OBS_AVOID, 0);
/**
- * Yaw mode.
+ * Yaw mode
*
* Specifies the heading in Auto.
*
--
GitLab