From 55563eba49f87624d7b73356ea3d421fee7ec081 Mon Sep 17 00:00:00 2001 From: Martina Rivizzigno Date: Wed, 11 Nov 2020 16:59:13 +0100 Subject: [PATCH] MPC_SPOOLUP_TIME -> COM_SPOOLUP_TIME --- .../init.d/airframes/13014_vtol_babyshark | 2 +- src/lib/parameters/param_translation.cpp | 9 +++++++++ src/modules/commander/Commander.cpp | 4 ++-- src/modules/commander/Commander.hpp | 4 +++- src/modules/commander/commander_params.c | 16 ++++++++++++++++ .../failure_detector/FailureDetector.cpp | 2 +- .../MulticopterPositionControl.cpp | 2 +- .../MulticopterPositionControl.hpp | 2 +- src/modules/mc_pos_control/Takeoff/Takeoff.hpp | 2 +- .../mc_pos_control/mc_pos_control_params.c | 16 ---------------- 10 files changed, 35 insertions(+), 24 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/airframes/13014_vtol_babyshark b/ROMFS/px4fmu_common/init.d/airframes/13014_vtol_babyshark index 5d1be9b55f..14578b70f2 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/13014_vtol_babyshark +++ b/ROMFS/px4fmu_common/init.d/airframes/13014_vtol_babyshark @@ -62,7 +62,7 @@ param set-default MPC_JERK_AUTO 4 param set-default MPC_LAND_SPEED 1 param set-default MPC_MAN_TILT_MAX 25 param set-default MPC_MAN_Y_MAX 40 -param set-default MPC_SPOOLUP_TIME 1.5 +param set-default COM_SPOOLUP_TIME 1.5 param set-default MPC_THR_HOVER 0.45 param set-default MPC_TILTMAX_AIR 25 param set-default MPC_TKO_RAMP_T 1.8 diff --git a/src/lib/parameters/param_translation.cpp b/src/lib/parameters/param_translation.cpp index 2b537899c0..740517852b 100644 --- a/src/lib/parameters/param_translation.cpp +++ b/src/lib/parameters/param_translation.cpp @@ -42,6 +42,15 @@ bool param_modify_on_import(bson_node_t node) { + // migrate MPC_SPOOLUP_TIME -> COM_SPOOLUP_TIME (2020-12-03). This can be removed after the next release (current release=1.11) + if (node->type == BSON_DOUBLE) { + if (strcmp("MPC_SPOOLUP_TIME", node->name) == 0) { + strcpy(node->name, "COM_SPOOLUP_TIME"); + PX4_INFO("param migrating MPC_SPOOLUP_TIME (removed) -> COM_SPOOLUP_TIME: value=%.3f", node->d); + return true; + } + } + // migrate COM_ARM_AUTH -> COM_ARM_AUTH_ID, COM_ARM_AUTH_MET and COM_ARM_AUTH_TO (2020-11-06). This can be removed after the next release (current release=1.11) if (node->type == BSON_INT32) { if (strcmp("COM_ARM_AUTH", node->name) == 0) { diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 8bddedc8e5..d3fdf38332 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -2721,8 +2721,8 @@ Commander::run() if (_arm_state_machine.isArmed()) { if (fd_status_flags.arm_escs) { - // 500ms is the PWM spoolup time. Within this timeframe controllers are not affecting actuator_outputs - if (hrt_elapsed_time(&_vehicle_status.armed_time) < 500_ms) { + // Checks have to pass within the spool up time + if (hrt_elapsed_time(&_vehicle_status.armed_time) < _param_com_spoolup_time.get() * 1_s) { disarm(arm_disarm_reason_t::failure_detector); mavlink_log_critical(&_mavlink_log_pub, "ESCs did not respond to arm request\t"); events::send(events::ID("commander_fd_escs_not_arming"), events::Log::Critical, "ESCs did not respond to arm request"); diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index e70c40c384..221307b847 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -258,7 +258,9 @@ private: (ParamInt) _param_cbrk_vtolarming, (ParamInt) _param_com_flt_time_max, - (ParamFloat) _param_com_wind_max + (ParamFloat) _param_com_wind_max, + + (ParamFloat) _param_com_spoolup_time ) // optional parameters diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index c22c489987..15c98f8c09 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -1023,6 +1023,22 @@ PARAM_DEFINE_INT32(COM_ARM_ARSP_EN, 1); */ PARAM_DEFINE_INT32(COM_ARM_SDCARD, 1); +/** + * Enforced delay between arming and further navigation + * + * The minimal time from arming the motors until moving the vehicle is possible is COM_SPOOLUP_TIME seconds. + * Goal: + * - Motors and propellers spool up to idle speed before getting commanded to spin faster + * - Timeout for ESCs and smart batteries to successfulyy do failure checks + * e.g. for stuck rotors before the vehicle is off the ground + * + * @group Commander + * @min 0 + * @max 5 + * @unit s + */ +PARAM_DEFINE_FLOAT(COM_SPOOLUP_TIME, 1.0f); + /** * Wind speed warning threshold * diff --git a/src/modules/commander/failure_detector/FailureDetector.cpp b/src/modules/commander/failure_detector/FailureDetector.cpp index 19f2996e2b..6581fcdca1 100644 --- a/src/modules/commander/failure_detector/FailureDetector.cpp +++ b/src/modules/commander/failure_detector/FailureDetector.cpp @@ -269,7 +269,7 @@ void FailureDetector::updateEscsStatus(const vehicle_status_s &vehicle_status, c bool is_esc_failure = !is_all_escs_armed; for (int i = 0; i < limited_esc_count; i++) { - is_esc_failure = is_esc_failure | (esc_status.esc[i].failures > 0); + is_esc_failure = is_esc_failure || (esc_status.esc[i].failures > 0); } _esc_failure_hysteresis.set_hysteresis_time_from(false, 300_ms); diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp index af69de733a..8dfb9e3f34 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.cpp @@ -234,7 +234,7 @@ void MulticopterPositionControl::parameters_update(bool force) _param_mpc_tko_speed.set(math::min(_param_mpc_tko_speed.get(), _param_mpc_z_vel_max_up.get())); _param_mpc_land_speed.set(math::min(_param_mpc_land_speed.get(), _param_mpc_z_vel_max_dn.get())); - _takeoff.setSpoolupTime(_param_mpc_spoolup_time.get()); + _takeoff.setSpoolupTime(_param_com_spoolup_time.get()); _takeoff.setTakeoffRampTime(_param_mpc_tko_ramp_t.get()); _takeoff.generateInitialRampValue(_param_mpc_z_vel_p_acc.get()); } diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.hpp b/src/modules/mc_pos_control/MulticopterPositionControl.hpp index 815d08e66f..d3103dd584 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.hpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.hpp @@ -147,7 +147,7 @@ private: (ParamBool) _param_mpc_use_hte, // Takeoff / Land - (ParamFloat) _param_mpc_spoolup_time, /**< time to let motors spool up after arming */ + (ParamFloat) _param_com_spoolup_time, /**< time to let motors spool up after arming */ (ParamFloat) _param_mpc_tko_ramp_t, /**< time constant for smooth takeoff ramp */ (ParamFloat) _param_mpc_tko_speed, (ParamFloat) _param_mpc_land_speed, diff --git a/src/modules/mc_pos_control/Takeoff/Takeoff.hpp b/src/modules/mc_pos_control/Takeoff/Takeoff.hpp index 560b0f6ef8..4fd29bffb1 100644 --- a/src/modules/mc_pos_control/Takeoff/Takeoff.hpp +++ b/src/modules/mc_pos_control/Takeoff/Takeoff.hpp @@ -94,7 +94,7 @@ public: private: TakeoffState _takeoff_state = TakeoffState::disarmed; - systemlib::Hysteresis _spoolup_time_hysteresis{false}; ///< becomes true MPC_SPOOLUP_TIME seconds after the vehicle was armed + systemlib::Hysteresis _spoolup_time_hysteresis{false}; ///< becomes true COM_SPOOLUP_TIME seconds after the vehicle was armed float _takeoff_ramp_time{0.f}; float _takeoff_ramp_vz_init{0.f}; ///< verticval velocity resulting in zero thrust 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 ac7f99cd2c..69d910b475 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -780,22 +780,6 @@ PARAM_DEFINE_FLOAT(MPC_TKO_RAMP_T, 3.0f); */ PARAM_DEFINE_INT32(MPC_POS_MODE, 4); -/** - * Enforced delay between arming and takeoff - * - * 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 s - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_SPOOLUP_TIME, 1.0f); - /** * Yaw mode. *