From 033c512fbf45cacc59262a6cb1cad9c437270122 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 18 Jul 2015 11:53:58 +0200 Subject: [PATCH 1/5] Add separate params for manual throttle control --- .../mc_pos_control/mc_pos_control_params.c | 33 ++++++++++++++++--- 1 file changed, 29 insertions(+), 4 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 a3793b0cc0..d29d11c65d 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -41,7 +41,32 @@ #include /** - * Minimum thrust + * Minimum thrust in auto thrust control + * + * It's recommended to set it > 0 to avoid free fall with zero thrust. + * + * @min 0.05 + * @max 1.0 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.12f); + +/** + * Maximum thrust in auto thrust control + * + * Limit max allowed thrust. Setting a value of one can put + * the system into actuator saturation as no spread between + * the motors is possible any more. A value of 0.8 - 0.9 + * is recommended. + * + * @min 0.0 + * @max 0.95 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_THR_MAX, 0.9f); + +/** + * Minimum manual thrust * * Minimum vertical thrust. It's recommended to set it > 0 to avoid free fall with zero thrust. * @@ -49,10 +74,10 @@ * @max 1.0 * @group Multicopter Position Control */ -PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.12f); +PARAM_DEFINE_FLOAT(MPC_MANTHR_MIN, 0.12f); /** - * Maximum thrust + * Maximum manual thrust * * Limit max allowed thrust. Setting a value of one can put * the system into actuator saturation as no spread between @@ -63,7 +88,7 @@ PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.12f); * @max 1.0 * @group Multicopter Position Control */ -PARAM_DEFINE_FLOAT(MPC_THR_MAX, 0.9f); +PARAM_DEFINE_FLOAT(MPC_MANTHR_MAX, 0.9f); /** * Proportional gain for vertical position error From d94f2aa407741a86256e1971197e5efb3ec93ddc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 18 Jul 2015 11:54:35 +0200 Subject: [PATCH 2/5] MC pos control: Use separate params for manual control --- .../mc_pos_control/mc_pos_control_main.cpp | 20 ++++++++++++++----- 1 file changed, 15 insertions(+), 5 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 59caa27ef7..ae41685178 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -80,6 +80,9 @@ #include #include +#include +#include + #define TILT_COS_MAX 0.7f #define SIGMA 0.000001f #define MIN_DIST 0.01f @@ -92,7 +95,7 @@ */ extern "C" __EXPORT int mc_pos_control_main(int argc, char *argv[]); -class MulticopterPositionControl +class MulticopterPositionControl : public control::SuperBlock { public: /** @@ -146,6 +149,8 @@ private: struct vehicle_local_position_setpoint_s _local_pos_sp; /**< vehicle local position setpoint */ struct vehicle_global_velocity_setpoint_s _global_vel_sp; /**< vehicle global velocity setpoint */ + control::BlockParamFloat _manual_thr_min; + control::BlockParamFloat _manual_thr_max; struct { param_t thr_min; @@ -290,7 +295,7 @@ MulticopterPositionControl *g_control; } MulticopterPositionControl::MulticopterPositionControl() : - + SuperBlock(NULL, "MPC"), _task_should_exit(false), _control_task(-1), _mavlink_fd(-1), @@ -310,7 +315,8 @@ MulticopterPositionControl::MulticopterPositionControl() : _att_sp_pub(-1), _local_pos_sp_pub(-1), _global_vel_sp_pub(-1), - + _manual_thr_min(this, "MANTHR_MIN"), + _manual_thr_max(this, "MANTHR_MAX"), _ref_alt(0.0f), _ref_timestamp(0), @@ -410,6 +416,10 @@ MulticopterPositionControl::parameters_update(bool force) } if (updated || force) { + /* update C++ param system */ + updateParams(); + + /* update legacy C interface params */ param_get(_params_handles.thr_min, &_params.thr_min); param_get(_params_handles.thr_max, &_params.thr_max); param_get(_params_handles.tilt_max_air, &_params.tilt_max_air); @@ -1413,11 +1423,11 @@ MulticopterPositionControl::task_main() /* control throttle directly if no climb rate controller is active */ if (!_control_mode.flag_control_climb_rate_enabled) { - _att_sp.thrust = math::min(_manual.z, _params.thr_max); + _att_sp.thrust = math::min(_manual.z, _manual_thr_max.get()); /* enforce minimum throttle if not landed */ if (!_vehicle_status.condition_landed) { - _att_sp.thrust = math::max(_att_sp.thrust, _params.thr_min); + _att_sp.thrust = math::max(_att_sp.thrust, _manual_thr_min.get()); } } From 7b7ec672b9e5023d60168a53a1d3ecc94709004e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 22 Jul 2015 12:42:29 +0200 Subject: [PATCH 3/5] Add prearm state to actuator armed topic --- msg/actuator_armed.msg | 1 + 1 file changed, 1 insertion(+) diff --git a/msg/actuator_armed.msg b/msg/actuator_armed.msg index c8098724e8..f307e366ae 100644 --- a/msg/actuator_armed.msg +++ b/msg/actuator_armed.msg @@ -1,6 +1,7 @@ uint64 timestamp # Microseconds since system boot bool armed # Set to true if system is armed +bool prearmed # Set to true if the actuator safety is disabled but motors are not armed bool ready_to_arm # Set to true if system is ready to be armed bool lockdown # Set to true if actuators are forced to being disabled (due to emergency or HIL) bool force_failsafe # Set to true if the actuators are forced to the failsafe position From fb9dcfc4a16bcf31f2a863756e37c05443b3831b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 22 Jul 2015 12:42:50 +0200 Subject: [PATCH 4/5] commander: populate prearmed field --- src/modules/commander/commander.cpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index f7ef1ec8e2..f045cb4b04 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2144,6 +2144,19 @@ int commander_thread_main(int argc, char *argv[]) orb_publish(ORB_ID(vehicle_status), status_pub, &status); armed.timestamp = now; + + /* set prearmed state if safety is off, or safety is not present and 5 seconds passed */ + if (safety.safety_switch_available) { + + /* safety is off, go into prearmed */ + armed.prearmed = safety.safety_off; + } else { + /* safety is not present, go into prearmed + * (all output drivers should be started / unlocked last in the boot process + * when the rest of the system is fully initialized) + */ + armed.prearmed = (hrt_elapsed_time(&commander_boot_timestamp) > 5 * 1000 * 1000); + } orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); } From f94e2c8136375440c7398e51379252ee936534ef Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 22 Jul 2015 12:43:21 +0200 Subject: [PATCH 5/5] FMU driver: Rely on prearmed field to pre-arm --- src/drivers/px4fmu/fmu.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index b1766f7390..c55289fc54 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -165,7 +165,7 @@ private: unsigned _num_failsafe_set; unsigned _num_disarmed_set; - static bool arm_nothrottle() { return (_armed.ready_to_arm && !_armed.armed); } + static bool arm_nothrottle() { return (_armed.prearmed && !_armed.armed); } static void task_main_trampoline(int argc, char *argv[]); void task_main(); @@ -733,7 +733,7 @@ PX4FMU::task_main() orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); /* update the armed status and check that we're not locked down */ - bool set_armed = (_armed.armed || _armed.ready_to_arm) && !_armed.lockdown; + bool set_armed = (_armed.armed || _armed.prearmed) && !_armed.lockdown; if (_servo_armed != set_armed) { _servo_armed = set_armed;