From b59cd4cbe5f8112723acbf1f6ca3aa25861167e1 Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 4 Sep 2017 12:39:56 +0200 Subject: [PATCH 01/17] Multicopter mixer - Always unsaturate low-saturated motors when possible --- src/lib/mixer/mixer_multirotor.cpp | 10 +--------- 1 file changed, 1 insertion(+), 9 deletions(-) diff --git a/src/lib/mixer/mixer_multirotor.cpp b/src/lib/mixer/mixer_multirotor.cpp index 8bd087d9a29d..35b561c4e68e 100644 --- a/src/lib/mixer/mixer_multirotor.cpp +++ b/src/lib/mixer/mixer_multirotor.cpp @@ -210,15 +210,7 @@ MultirotorMixer::mix(float *outputs, unsigned space) // TODO: revise the saturation/boosting strategy if (min_out < 0.0f && max_out < 1.0f && -min_out <= 1.0f - max_out) { - float max_thrust_diff = thrust * thrust_increase_factor - thrust; - - if (max_thrust_diff >= -min_out) { - boost = -min_out; - - } else { - boost = max_thrust_diff; - roll_pitch_scale = (thrust + boost) / (thrust - min_out); - } + boost = -min_out; } else if (max_out > 1.0f && min_out > 0.0f && min_out >= max_out - 1.0f) { float max_thrust_diff = thrust - thrust_decrease_factor * thrust; From 5e02cbfbf64486bfe4c8476cbbe4e7d985f6423a Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 4 Sep 2017 12:43:35 +0200 Subject: [PATCH 02/17] Multicopter mixer - Rewrite unnecessarily complicated conditions --- src/lib/mixer/mixer_multirotor.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/lib/mixer/mixer_multirotor.cpp b/src/lib/mixer/mixer_multirotor.cpp index 35b561c4e68e..4134dd0c51f6 100644 --- a/src/lib/mixer/mixer_multirotor.cpp +++ b/src/lib/mixer/mixer_multirotor.cpp @@ -209,10 +209,10 @@ MultirotorMixer::mix(float *outputs, unsigned space) // which may result in motor saturation after thrust boost is applied // TODO: revise the saturation/boosting strategy - if (min_out < 0.0f && max_out < 1.0f && -min_out <= 1.0f - max_out) { + if (min_out < 0.0f && max_out < 1.0f && max_out - min_out <= 1.0f) { boost = -min_out; - } else if (max_out > 1.0f && min_out > 0.0f && min_out >= max_out - 1.0f) { + } else if (max_out > 1.0f && min_out > 0.0f && max_out - min_out <= 1.0f) { float max_thrust_diff = thrust - thrust_decrease_factor * thrust; if (max_thrust_diff >= max_out - 1.0f) { @@ -223,12 +223,12 @@ MultirotorMixer::mix(float *outputs, unsigned space) roll_pitch_scale = (1 - (thrust + boost)) / (max_out - thrust); } - } else if (min_out < 0.0f && max_out < 1.0f && -min_out > 1.0f - max_out) { + } else if (min_out < 0.0f && max_out < 1.0f && max_out - min_out > 1.0f) { float max_thrust_diff = thrust * thrust_increase_factor - thrust; boost = math::constrain(-min_out - (1.0f - max_out) / 2.0f, 0.0f, max_thrust_diff); roll_pitch_scale = (thrust + boost) / (thrust - min_out); - } else if (max_out > 1.0f && min_out > 0.0f && min_out < max_out - 1.0f) { + } else if (max_out > 1.0f && min_out > 0.0f && max_out - min_out > 1.0f) { float max_thrust_diff = thrust - thrust_decrease_factor * thrust; boost = math::constrain(-(max_out - 1.0f - min_out) / 2.0f, -max_thrust_diff, 0.0f); roll_pitch_scale = (1 - (thrust + boost)) / (max_out - thrust); From 50d521e476826098a402710d1ed17572432a0bb4 Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 4 Sep 2017 12:51:48 +0200 Subject: [PATCH 03/17] Multicopter mixer - Always unsaturate high-saturated motors when possible --- src/lib/mixer/mixer_multirotor.cpp | 10 +--------- 1 file changed, 1 insertion(+), 9 deletions(-) diff --git a/src/lib/mixer/mixer_multirotor.cpp b/src/lib/mixer/mixer_multirotor.cpp index 4134dd0c51f6..c452c109dbda 100644 --- a/src/lib/mixer/mixer_multirotor.cpp +++ b/src/lib/mixer/mixer_multirotor.cpp @@ -213,15 +213,7 @@ MultirotorMixer::mix(float *outputs, unsigned space) boost = -min_out; } else if (max_out > 1.0f && min_out > 0.0f && max_out - min_out <= 1.0f) { - float max_thrust_diff = thrust - thrust_decrease_factor * thrust; - - if (max_thrust_diff >= max_out - 1.0f) { - boost = -(max_out - 1.0f); - - } else { - boost = -max_thrust_diff; - roll_pitch_scale = (1 - (thrust + boost)) / (max_out - thrust); - } + boost = -(max_out - 1.0f); } else if (min_out < 0.0f && max_out < 1.0f && max_out - min_out > 1.0f) { float max_thrust_diff = thrust * thrust_increase_factor - thrust; From 1d1f37516488a1a2ce9bd4706824d461874bbb14 Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 4 Sep 2017 14:11:05 +0200 Subject: [PATCH 04/17] Multicopter mixer - Remove arbitraty boost gain during saturation --- src/lib/mixer/mixer_multirotor.cpp | 13 +++---------- 1 file changed, 3 insertions(+), 10 deletions(-) diff --git a/src/lib/mixer/mixer_multirotor.cpp b/src/lib/mixer/mixer_multirotor.cpp index c452c109dbda..1322f563f18f 100644 --- a/src/lib/mixer/mixer_multirotor.cpp +++ b/src/lib/mixer/mixer_multirotor.cpp @@ -179,10 +179,6 @@ MultirotorMixer::mix(float *outputs, unsigned space) // clean out class variable used to capture saturation _saturation_status.value = 0; - // thrust boost parameters - float thrust_increase_factor = 1.5f; - float thrust_decrease_factor = 0.6f; - /* perform initial mix pass yielding unbounded outputs, ignore yaw */ for (unsigned i = 0; i < _rotor_count; i++) { float out = roll * _rotors[i].roll_scale + @@ -216,18 +212,15 @@ MultirotorMixer::mix(float *outputs, unsigned space) boost = -(max_out - 1.0f); } else if (min_out < 0.0f && max_out < 1.0f && max_out - min_out > 1.0f) { - float max_thrust_diff = thrust * thrust_increase_factor - thrust; - boost = math::constrain(-min_out - (1.0f - max_out) / 2.0f, 0.0f, max_thrust_diff); + boost = -min_out - (1.0f - max_out) / 2.0f; roll_pitch_scale = (thrust + boost) / (thrust - min_out); } else if (max_out > 1.0f && min_out > 0.0f && max_out - min_out > 1.0f) { - float max_thrust_diff = thrust - thrust_decrease_factor * thrust; - boost = math::constrain(-(max_out - 1.0f - min_out) / 2.0f, -max_thrust_diff, 0.0f); + boost = -(max_out - min_out - 1.0f) / 2.0f; roll_pitch_scale = (1 - (thrust + boost)) / (max_out - thrust); } else if (min_out < 0.0f && max_out > 1.0f) { - boost = math::constrain(-(max_out - 1.0f + min_out) / 2.0f, thrust_decrease_factor * thrust - thrust, - thrust_increase_factor * thrust - thrust); + boost = -(max_out - 1.0f + min_out) / 2.0f; roll_pitch_scale = (thrust + boost) / (thrust - min_out); } From 0728034474032a137ed9d114d315fbf005a559c2 Mon Sep 17 00:00:00 2001 From: bresch Date: Tue, 5 Sep 2017 11:26:06 +0200 Subject: [PATCH 05/17] Multicopter mixer - Simplify and correct mistakes of roll-pitch motor saturation handling --- src/lib/mixer/mixer_multirotor.cpp | 31 +++++++++++++++--------------- 1 file changed, 16 insertions(+), 15 deletions(-) diff --git a/src/lib/mixer/mixer_multirotor.cpp b/src/lib/mixer/mixer_multirotor.cpp index 1322f563f18f..68d471156370 100644 --- a/src/lib/mixer/mixer_multirotor.cpp +++ b/src/lib/mixer/mixer_multirotor.cpp @@ -199,31 +199,32 @@ MultirotorMixer::mix(float *outputs, unsigned space) float boost = 0.0f; // value added to demanded thrust (can also be negative) float roll_pitch_scale = 1.0f; // scale for demanded roll and pitch + float delta_out_max = max_out - min_out; // distance between the two extrema + // If the difference between the to extrema is smaller than 1.0, the boost can safely unsaturate a motor if needed + // without saturating another one. + // Otherwise, a scaler is computed to make the distance between the two extrema exacly 1.0 and the boost + // value is computed to maximize the roll-pitch control. + // // Note: thrust boost is computed assuming thrust_gain==1 for all motors. // On asymmetric platforms, some motors have thrust_gain<1, // which may result in motor saturation after thrust boost is applied // TODO: revise the saturation/boosting strategy + if (delta_out_max <= 1.0f) { + if (min_out < 0.0f) { + boost = -min_out; - if (min_out < 0.0f && max_out < 1.0f && max_out - min_out <= 1.0f) { - boost = -min_out; + } else if (max_out > 1.0f) { + boost = -(max_out - 1.0f); - } else if (max_out > 1.0f && min_out > 0.0f && max_out - min_out <= 1.0f) { - boost = -(max_out - 1.0f); - - } else if (min_out < 0.0f && max_out < 1.0f && max_out - min_out > 1.0f) { - boost = -min_out - (1.0f - max_out) / 2.0f; - roll_pitch_scale = (thrust + boost) / (thrust - min_out); - - } else if (max_out > 1.0f && min_out > 0.0f && max_out - min_out > 1.0f) { - boost = -(max_out - min_out - 1.0f) / 2.0f; - roll_pitch_scale = (1 - (thrust + boost)) / (max_out - thrust); + } - } else if (min_out < 0.0f && max_out > 1.0f) { - boost = -(max_out - 1.0f + min_out) / 2.0f; - roll_pitch_scale = (thrust + boost) / (thrust - min_out); + } else { + roll_pitch_scale = 1 / (max_out - min_out); + boost = 1.0f - ((max_out - thrust) * roll_pitch_scale + thrust); } + // capture saturation if (min_out < 0.0f) { _saturation_status.flags.motor_neg = true; From 077ca2f416c51d75ffc878217082ba9420dd86fc Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 20 Sep 2017 19:16:01 +0200 Subject: [PATCH 06/17] Multicopter mixer - Use already computed value instead of recomputing it --- src/lib/mixer/mixer_multirotor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/mixer/mixer_multirotor.cpp b/src/lib/mixer/mixer_multirotor.cpp index 68d471156370..59730013e351 100644 --- a/src/lib/mixer/mixer_multirotor.cpp +++ b/src/lib/mixer/mixer_multirotor.cpp @@ -220,7 +220,7 @@ MultirotorMixer::mix(float *outputs, unsigned space) } } else { - roll_pitch_scale = 1 / (max_out - min_out); + roll_pitch_scale = 1 / (delta_out_max); boost = 1.0f - ((max_out - thrust) * roll_pitch_scale + thrust); } From 9332881ff98981d6a39fe0327e0f63f8f9dc3f07 Mon Sep 17 00:00:00 2001 From: bresch Date: Tue, 13 Feb 2018 11:53:54 +0100 Subject: [PATCH 07/17] Airmode - Add airmode parameter for multicopter mixer --- src/drivers/px4fmu/fmu.cpp | 13 +++++++++++++ src/drivers/px4io/px4io.cpp | 8 ++++++++ src/lib/mixer/mixer.h | 13 +++++++++++++ src/lib/mixer/mixer_group.cpp | 10 ++++++++++ src/lib/mixer/mixer_multirotor.cpp | 10 ++++++++++ .../mc_att_control/mc_att_control_params.c | 16 +++++++++++++++- src/modules/px4iofirmware/mixer.cpp | 5 +++++ src/modules/px4iofirmware/protocol.h | 4 ++++ src/modules/px4iofirmware/px4io.h | 1 + src/modules/px4iofirmware/registers.c | 5 +++++ 10 files changed, 84 insertions(+), 1 deletion(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index fa83c751e0ed..e3a55852a8b4 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -275,6 +275,7 @@ class PX4FMU : public device::CDev, public ModuleBase float _mot_t_max; // maximum rise time for motor (slew rate limiting) float _thr_mdl_fac; // thrust to pwm modelling factor + bool _airmode; // multicopter air-mode perf_counter_t _ctl_latency; @@ -385,6 +386,7 @@ PX4FMU::PX4FMU(bool run_as_task) : _to_mixer_status(nullptr), _mot_t_max(0.0f), _thr_mdl_fac(0.0f), + _airmode(false), _ctl_latency(perf_alloc(PC_ELAPSED, "ctl_lat")) { for (unsigned i = 0; i < _max_actuators; i++) { @@ -1292,6 +1294,7 @@ PX4FMU::cycle() // factor 2 is needed because actuator outputs are in the range [-1,1] const float delta_out_max = 2.0f * 1000.0f * dt / (_max_pwm[0] - _min_pwm[0]) / _mot_t_max; _mixers->set_max_delta_out_once(delta_out_max); + _mixers->set_airmode(_airmode); } if (_thr_mdl_fac > FLT_EPSILON) { @@ -1783,6 +1786,16 @@ void PX4FMU::update_params() if (param_handle != PARAM_INVALID) { param_get(param_handle, &_thr_mdl_fac); } + + // multicopter air-mode + param_handle = param_find("MC_AIRMODE"); + + if (param_handle != PARAM_INVALID) { + int val; + param_get(param_handle, &val); + _airmode = val > 0; + PX4_DEBUG("%s: %d", "MC_AIRMODE", _airmode); + } } diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index ce913cf4c309..3b835809093d 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1221,6 +1221,14 @@ PX4IO::task_main() param_get(parm_handle, ¶m_val); (void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_MOTOR_SLEW_MAX, FLOAT_TO_REG(param_val)); } + + /* air-mode */ + parm_handle = param_find("MC_AIRMODE"); + + if (parm_handle != PARAM_INVALID) { + param_get(parm_handle, ¶m_val); + (void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_AIRMODE, SIGNED_TO_REG(param_val)); + } } } diff --git a/src/lib/mixer/mixer.h b/src/lib/mixer/mixer.h index 989a59e43ffd..2def433d9e75 100644 --- a/src/lib/mixer/mixer.h +++ b/src/lib/mixer/mixer.h @@ -218,6 +218,13 @@ class __EXPORT Mixer */ virtual void set_thrust_factor(float val) {} + /** + * @brief Set airmode. Airmode allows the mixer to increase the total thrust in order to unsaturate the motors. + * + * @param[in] airmode Dis/-activate airmode by setting it to false/true + */ + virtual void set_airmode(bool airmode) {}; + protected: /** client-supplied callback used when fetching control values */ ControlCallback _control_cb; @@ -403,6 +410,8 @@ class __EXPORT MixerGroup : public Mixer */ virtual void set_thrust_factor(float val); + virtual void set_airmode(bool airmode); + private: Mixer *_first; /**< linked list of mixers */ @@ -650,6 +659,8 @@ class __EXPORT MultirotorMixer : public Mixer */ virtual void set_thrust_factor(float val) {_thrust_factor = val;} + virtual void set_airmode(bool airmode); + union saturation_status { struct { uint16_t valid : 1; // 0 - true when the saturation status is used @@ -675,6 +686,8 @@ class __EXPORT MultirotorMixer : public Mixer float _delta_out_max; float _thrust_factor; + bool _airmode; + void update_saturation_status(unsigned index, bool clipping_high, bool clipping_low); saturation_status _saturation_status; diff --git a/src/lib/mixer/mixer_group.cpp b/src/lib/mixer/mixer_group.cpp index e4fb98fbff7c..200be683baf4 100644 --- a/src/lib/mixer/mixer_group.cpp +++ b/src/lib/mixer/mixer_group.cpp @@ -185,7 +185,17 @@ MixerGroup::set_thrust_factor(float val) mixer->set_thrust_factor(val); mixer = mixer->_next; } +} + +void +MixerGroup::set_airmode(bool airmode) +{ + Mixer *mixer = _first; + while (mixer != nullptr) { + mixer->set_airmode(airmode); + mixer = mixer->_next; + } } uint16_t diff --git a/src/lib/mixer/mixer_multirotor.cpp b/src/lib/mixer/mixer_multirotor.cpp index 59730013e351..ed14bf48ec52 100644 --- a/src/lib/mixer/mixer_multirotor.cpp +++ b/src/lib/mixer/mixer_multirotor.cpp @@ -84,6 +84,7 @@ MultirotorMixer::MultirotorMixer(ControlCallback control_cb, _idle_speed(-1.0f + idle_speed * 2.0f), /* shift to output range here to avoid runtime calculation */ _delta_out_max(0.0f), _thrust_factor(0.0f), + _airmode(false), _rotor_count(_config_rotor_count[(MultirotorGeometryUnderlyingType)geometry]), _rotors(_config_index[(MultirotorGeometryUnderlyingType)geometry]), _outputs_prev(new float[_rotor_count]) @@ -224,6 +225,9 @@ MultirotorMixer::mix(float *outputs, unsigned space) boost = 1.0f - ((max_out - thrust) * roll_pitch_scale + thrust); } + if (!_airmode) { + boost = math::min(boost, 0.0f); // Disable positive boosting if not in air-mode + } // capture saturation if (min_out < 0.0f) { @@ -425,6 +429,12 @@ MultirotorMixer::update_saturation_status(unsigned index, bool clipping_high, bo _saturation_status.flags.valid = true; } +void +MultirotorMixer::set_airmode(bool airmode) +{ + _airmode = airmode; +} + void MultirotorMixer::groups_required(uint32_t &groups) { diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c index 89202e086e4d..60fd8d35e7fc 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -542,4 +542,18 @@ PARAM_DEFINE_FLOAT(MC_TPA_RATE_D, 0.0f); * @increment 10 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_DTERM_CUTOFF, 30.f); +PARAM_DEFINE_FLOAT(MC_DTERM_CUTOFF, 0.f); + +/** + * Multicopter air-mode + * + * The air-mode enables the mixer to increase or decrease the total thrust of the multirotor + * in order to keep attitude and rate control even at low and high throttle. + * This function should be disabled during tuning as it will help the controller + * to diverge if the closed-loop is unstable. + * + * @boolean + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_INT32(MC_AIRMODE, 0); + diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 5a167a447f39..b874c8cac236 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -254,6 +254,11 @@ mixer_tick(void) */ mixer_group.set_trims(r_page_servo_control_trim, PX4IO_SERVO_COUNT); + /* + * Update air-mode parameter + */ + mixer_group.set_airmode(REG_TO_BOOL(r_setup_airmode)); + /* * Run the mixers. diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 04fd0712fc38..8e6e425e35a3 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -78,6 +78,8 @@ #define REG_TO_FLOAT(_reg) ((float)REG_TO_SIGNED(_reg) / 10000.0f) #define FLOAT_TO_REG(_float) SIGNED_TO_REG((int16_t)floorf((_float + 0.00005f) * 10000.0f)) +#define REG_TO_BOOL(_reg) ((bool)(_reg)) + #define PX4IO_PROTOCOL_VERSION 4 /* maximum allowable sizes on this protocol version */ @@ -232,6 +234,8 @@ enum { /* DSM bind states */ #define PX4IO_P_SETUP_MOTOR_SLEW_MAX 24 /* max motor slew rate */ +#define PX4IO_P_SETUP_AIRMODE 27 /* air-mode */ + #define PX4IO_P_SETUP_THR_MDL_FAC 25 /* factor for modelling static pwm output to thrust relationship */ #define PX4IO_P_SETUP_THERMAL 26 /* thermal management */ diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 00b27372a8d2..10db31a277ff 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -132,6 +132,7 @@ extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */ #define r_setup_sbus_rate r_page_setup[PX4IO_P_SETUP_SBUS_RATE] #define r_setup_thr_fac r_page_setup[PX4IO_P_SETUP_THR_MDL_FAC] #define r_setup_slew_max r_page_setup[PX4IO_P_SETUP_MOTOR_SLEW_MAX] +#define r_setup_airmode r_page_setup[PX4IO_P_SETUP_AIRMODE] #define r_control_values (&r_page_controls[0]) diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index fc6919863579..462398cf0859 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -178,6 +178,7 @@ volatile uint16_t r_page_setup[] = { [PX4IO_P_SETUP_SCALE_PITCH] = 10000, [PX4IO_P_SETUP_SCALE_YAW] = 10000, [PX4IO_P_SETUP_MOTOR_SLEW_MAX] = 0, + [PX4IO_P_SETUP_AIRMODE] = 0, [PX4IO_P_SETUP_THR_MDL_FAC] = 0, [PX4IO_P_SETUP_THERMAL] = PX4IO_THERMAL_IGNORE }; @@ -704,6 +705,10 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) sbus1_set_output_rate_hz(value); break; + case PX4IO_P_SETUP_AIRMODE: + r_page_setup[PX4IO_P_SETUP_AIRMODE] = value; + break; + case PX4IO_P_SETUP_THR_MDL_FAC: update_mc_thrust_param = true; r_page_setup[offset] = value; From 57edc89ad3195f1b89c9bf8345a2d23db4cffd89 Mon Sep 17 00:00:00 2001 From: bresch Date: Tue, 13 Feb 2018 15:18:40 +0100 Subject: [PATCH 08/17] Airmode - Add support for UAVCAN --- src/modules/uavcan/uavcan_main.cpp | 18 ++++++++++++++++++ src/modules/uavcan/uavcan_main.hpp | 3 +++ 2 files changed, 21 insertions(+) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 286f3b9ed0b1..c67db46dbe57 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -417,6 +417,20 @@ int UavcanNode::get_param(int remote_node_id, const char *name) return rv; } +void UavcanNode::update_params() +{ + param_t param_handle; + + // multicopter air-mode + param_handle = param_find("MC_AIRMODE"); + + if (param_handle != PARAM_INVALID) { + int val; + param_get(param_handle, &val); + _airmode = val > 0; + } +} + int UavcanNode::start_fw_server() { int rv = -1; @@ -821,6 +835,8 @@ int UavcanNode::run() while (!_task_should_exit) { + update_params(); + switch (_fw_server_action) { case Start: _fw_server_status = start_fw_server(); @@ -908,6 +924,8 @@ int UavcanNode::run() // but this driver could well serve multiple groups. unsigned num_outputs_max = 8; + _mixers->set_airmode(_airmode); + // Do mixing _outputs.noutputs = _mixers->mix(&_outputs.output[0], num_outputs_max); diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 4c0575e5c0be..69cc732a87d5 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -152,6 +152,7 @@ class UavcanNode : public device::CDev int request_fw_check(); int print_params(uavcan::protocol::param::GetSet::Response &resp); int get_set_param(int nodeid, const char *name, uavcan::protocol::param::GetSet::Request &req); + void update_params(); void set_setget_response(uavcan::protocol::param::GetSet::Response *resp) { _setget_response = resp; @@ -207,6 +208,8 @@ class UavcanNode : public device::CDev actuator_outputs_s _outputs = {}; + bool _airmode = false; + // index into _poll_fds for each _control_subs handle uint8_t _poll_ids[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN]; From 7552f4d2f92cddf5ccc3d00a9548ff24daff7b20 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Thu, 15 Feb 2018 21:31:23 +0100 Subject: [PATCH 09/17] fmu + px4io: fixes for MC_AIRMODE parameter - fmu: in case of _mot_t_max==0 _airmode was not set in the mixer - px4io: param_val is a float --- src/drivers/px4fmu/fmu.cpp | 5 +++-- src/drivers/px4io/px4io.cpp | 5 +++-- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index e3a55852a8b4..4c2ac5ddded7 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -1294,7 +1294,6 @@ PX4FMU::cycle() // factor 2 is needed because actuator outputs are in the range [-1,1] const float delta_out_max = 2.0f * 1000.0f * dt / (_max_pwm[0] - _min_pwm[0]) / _mot_t_max; _mixers->set_max_delta_out_once(delta_out_max); - _mixers->set_airmode(_airmode); } if (_thr_mdl_fac > FLT_EPSILON) { @@ -1363,6 +1362,8 @@ PX4FMU::cycle() ORB_PRIO_DEFAULT); } + _mixers->set_airmode(_airmode); + perf_end(_ctl_latency); } } @@ -1791,7 +1792,7 @@ void PX4FMU::update_params() param_handle = param_find("MC_AIRMODE"); if (param_handle != PARAM_INVALID) { - int val; + int32_t val; param_get(param_handle, &val); _airmode = val > 0; PX4_DEBUG("%s: %d", "MC_AIRMODE", _airmode); diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 3b835809093d..164d798c2d3f 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1226,8 +1226,9 @@ PX4IO::task_main() parm_handle = param_find("MC_AIRMODE"); if (parm_handle != PARAM_INVALID) { - param_get(parm_handle, ¶m_val); - (void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_AIRMODE, SIGNED_TO_REG(param_val)); + int32_t param_val_int; + param_get(parm_handle, ¶m_val_int); + (void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_AIRMODE, SIGNED_TO_REG(param_val_int)); } } From d7166376e705a176d80b9468f6087251e938ddbd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Mon, 19 Feb 2018 15:03:56 +0100 Subject: [PATCH 10/17] mixer_multirotor: fix comment thrust_gain -> thrust_scale Added in 262d9c790bf0d3cd652. --- src/lib/mixer/mixer_multirotor.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/lib/mixer/mixer_multirotor.cpp b/src/lib/mixer/mixer_multirotor.cpp index ed14bf48ec52..42d97e8e659b 100644 --- a/src/lib/mixer/mixer_multirotor.cpp +++ b/src/lib/mixer/mixer_multirotor.cpp @@ -207,8 +207,8 @@ MultirotorMixer::mix(float *outputs, unsigned space) // Otherwise, a scaler is computed to make the distance between the two extrema exacly 1.0 and the boost // value is computed to maximize the roll-pitch control. // - // Note: thrust boost is computed assuming thrust_gain==1 for all motors. - // On asymmetric platforms, some motors have thrust_gain<1, + // Note: thrust boost is computed assuming thrust_scale==1 for all motors. + // On asymmetric platforms, some motors have thrust_scale<1, // which may result in motor saturation after thrust boost is applied // TODO: revise the saturation/boosting strategy if (delta_out_max <= 1.0f) { From c988ba2e80ff668bbd5b7f7156cc6f5a511347d2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Mon, 19 Feb 2018 15:05:46 +0100 Subject: [PATCH 11/17] cleanup uavcan_main: replace warnx with PX4_{INFO,ERR,DEBUG} --- src/modules/uavcan/uavcan_main.cpp | 58 +++++++++++++----------------- 1 file changed, 24 insertions(+), 34 deletions(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index c67db46dbe57..f42978d7721a 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -526,7 +526,7 @@ int UavcanNode::fw_server(eServerAction action) int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) { if (_instance != nullptr) { - warnx("Already started"); + PX4_WARN("Already started"); return -1; } @@ -560,14 +560,14 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) can = new CanInitHelper(); if (can == nullptr) { // We don't have exceptions so bad_alloc cannot be thrown - warnx("Out of memory"); + PX4_ERR("Out of memory"); return -1; } const int can_init_res = can->init(bitrate); if (can_init_res < 0) { - warnx("CAN driver init failed %i", can_init_res); + PX4_ERR("CAN driver init failed %i", can_init_res); return can_init_res; } } @@ -578,12 +578,7 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) _instance = new UavcanNode(can->driver, uavcan_stm32::SystemClock::instance()); if (_instance == nullptr) { - warnx("Out of memory"); - return -1; - } - - if (_instance == nullptr) { - warnx("Out of memory"); + PX4_ERR("Out of memory"); return -1; } @@ -592,7 +587,7 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) if (node_init_res < 0) { delete _instance; _instance = nullptr; - warnx("Node init failed %i", node_init_res); + PX4_ERR("Node init failed %i", node_init_res); return node_init_res; } @@ -604,7 +599,7 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) static_cast(run_trampoline), nullptr); if (_instance->_task < 0) { - warnx("start failed: %d", errno); + PX4_ERR("start failed: %d", errno); return -errno; } @@ -624,7 +619,7 @@ void UavcanNode::fill_node_info() swver.optional_field_flags |= swver.OPTIONAL_FIELD_FLAG_VCS_COMMIT; // Too verbose for normal operation - //warnx("SW version vcs_commit: 0x%08x", unsigned(swver.vcs_commit)); + //PX4_INFO("SW version vcs_commit: 0x%08x", unsigned(swver.vcs_commit)); _node.setSoftwareVersion(swver); @@ -678,11 +673,11 @@ int UavcanNode::init(uavcan::NodeID node_id) ret = br->init(); if (ret < 0) { - warnx("cannot init sensor bridge '%s' (%d)", br->get_name(), ret); + PX4_ERR("cannot init sensor bridge '%s' (%d)", br->get_name(), ret); return ret; } - warnx("sensor bridge '%s' init ok", br->get_name()); + PX4_INFO("sensor bridge '%s' init ok", br->get_name()); br = br->getSibling(); } @@ -697,7 +692,7 @@ void UavcanNode::node_spin_once() const int spin_res = _node.spinOnce(); if (spin_res < 0) { - warnx("node spin error %i", spin_res); + PX4_ERR("node spin error %i", spin_res); } @@ -788,7 +783,7 @@ int UavcanNode::run() const int slave_init_res = _time_sync_slave.start(); if (slave_init_res < 0) { - warnx("Failed to start time_sync_slave"); + PX4_ERR("Failed to start time_sync_slave"); _task_should_exit = true; } @@ -806,7 +801,7 @@ int UavcanNode::run() const int busevent_fd = ::open(uavcan_stm32::BusEvent::DevName, 0); if (busevent_fd < 0) { - warnx("Failed to open %s", uavcan_stm32::BusEvent::DevName); + PX4_ERR("Failed to open %s", uavcan_stm32::BusEvent::DevName); _task_should_exit = true; } @@ -999,7 +994,6 @@ int UavcanNode::run() (void)::close(busevent_fd); teardown(); - warnx("exiting."); exit(0); } @@ -1046,12 +1040,12 @@ UavcanNode::subscribe() // the first fd used by CAN for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (sub_groups & (1 << i)) { - warnx("subscribe to actuator_controls_%d", i); + PX4_DEBUG("subscribe to actuator_controls_%d", i); _control_subs[i] = orb_subscribe(_control_topics[i]); } if (unsub_groups & (1 << i)) { - warnx("unsubscribe from actuator_controls_%d", i); + PX4_DEBUG("unsubscribe from actuator_controls_%d", i); orb_unsubscribe(_control_subs[i]); _control_subs[i] = -1; } @@ -1114,7 +1108,7 @@ UavcanNode::ioctl(file *filp, int cmd, unsigned long arg) ret = _mixers->load_from_buf(buf, buflen); if (ret != 0) { - warnx("mixer load failed with %d", ret); + PX4_ERR("mixer load failed with %d", ret); delete _mixers; _mixers = nullptr; _groups_required = 0; @@ -1172,10 +1166,6 @@ UavcanNode::ioctl(file *filp, int cmd, unsigned long arg) void UavcanNode::print_info() { - if (!_instance) { - warnx("not running, start first"); - } - (void)pthread_mutex_lock(&_node_mutex); // Memory status @@ -1288,10 +1278,10 @@ void UavcanNode::hardpoint_controller_set(uint8_t hardpoint_id, uint16_t command */ static void print_usage() { - warnx("usage: \n" - "\tuavcan {start [fw]|status|stop [all|fw]|shrink|arm|disarm|update fw|\n" - "\t param [set|get|list|save] |reset |\n" - "\t hardpoint set }"); + PX4_INFO("usage: \n" + "\tuavcan {start [fw]|status|stop [all|fw]|shrink|arm|disarm|update fw|\n" + "\t param [set|get|list|save] |reset |\n" + "\t hardpoint set }"); } extern "C" __EXPORT int uavcan_main(int argc, char *argv[]); @@ -1311,7 +1301,7 @@ int uavcan_main(int argc, char *argv[]) int rv = UavcanNode::instance()->fw_server(UavcanNode::Start); if (rv < 0) { - warnx("Firmware Server Failed to Start %d", rv); + PX4_ERR("Firmware Server Failed to Start %d", rv); ::exit(rv); } @@ -1319,7 +1309,7 @@ int uavcan_main(int argc, char *argv[]) } // Already running, no error - warnx("already started"); + PX4_INFO("already started"); ::exit(0); } @@ -1328,7 +1318,7 @@ int uavcan_main(int argc, char *argv[]) (void)param_get(param_find("UAVCAN_NODE_ID"), &node_id); if (node_id < 0 || node_id > uavcan::NodeID::Max || !uavcan::NodeID(node_id).isUnicast()) { - warnx("Invalid Node ID %i", node_id); + PX4_ERR("Invalid Node ID %i", node_id); ::exit(1); } @@ -1337,7 +1327,7 @@ int uavcan_main(int argc, char *argv[]) (void)param_get(param_find("UAVCAN_BITRATE"), &bitrate); // Start - warnx("Node ID %u, bitrate %u", node_id, bitrate); + PX4_INFO("Node ID %u, bitrate %u", node_id, bitrate); return UavcanNode::start(node_id, bitrate); } @@ -1467,7 +1457,7 @@ int uavcan_main(int argc, char *argv[]) inst->shrink(); if (rv < 0) { - warnx("Firmware Server Failed to Stop %d", rv); + PX4_ERR("Firmware Server Failed to Stop %d", rv); ::exit(rv); } From 57c5bddd3e0670deefabddc7fcadcf270f0c464f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Mon, 19 Feb 2018 15:09:07 +0100 Subject: [PATCH 12/17] uavcan_main: use parameter_update to check for param updates this avoids calling param_get() on every loop iteration. --- src/modules/uavcan/uavcan_main.cpp | 33 +++++++++++++++++++++--------- 1 file changed, 23 insertions(+), 10 deletions(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index f42978d7721a..d9137b41c2c7 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -58,6 +58,7 @@ #include #include +#include #include #include @@ -419,16 +420,14 @@ int UavcanNode::get_param(int remote_node_id, const char *name) void UavcanNode::update_params() { - param_t param_handle; + // multicopter air-mode + param_t param_handle = param_find("MC_AIRMODE"); - // multicopter air-mode - param_handle = param_find("MC_AIRMODE"); - - if (param_handle != PARAM_INVALID) { - int val; - param_get(param_handle, &val); - _airmode = val > 0; - } + if (param_handle != PARAM_INVALID) { + int32_t val; + param_get(param_handle, &val); + _airmode = val > 0; + } } int UavcanNode::start_fw_server() @@ -828,9 +827,21 @@ int UavcanNode::run() _actuator_direct_poll_fd_num = add_poll_fd(_actuator_direct_sub); } + update_params(); + + int params_sub = orb_subscribe(ORB_ID(parameter_update)); + while (!_task_should_exit) { - update_params(); + /* check for parameter updates */ + bool param_updated = false; + orb_check(params_sub, ¶m_updated); + + if (param_updated) { + struct parameter_update_s update; + orb_copy(ORB_ID(parameter_update), params_sub, &update); + update_params(); + } switch (_fw_server_action) { case Start: @@ -991,6 +1002,8 @@ int UavcanNode::run() } } + orb_unsubscribe(params_sub); + (void)::close(busevent_fd); teardown(); From abc9502d8bbf039ceb3431c3875c801bca76fd54 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Mon, 19 Feb 2018 17:09:42 +0100 Subject: [PATCH 13/17] snapdragon_pwm_out: add MC_AIRMODE support --- .../snapdragon_pwm_out/snapdragon_pwm_out.cpp | 50 +++++++++++++++---- 1 file changed, 41 insertions(+), 9 deletions(-) diff --git a/src/drivers/snapdragon_pwm_out/snapdragon_pwm_out.cpp b/src/drivers/snapdragon_pwm_out/snapdragon_pwm_out.cpp index 7727cd2a6cf4..04b9f1602768 100644 --- a/src/drivers/snapdragon_pwm_out/snapdragon_pwm_out.cpp +++ b/src/drivers/snapdragon_pwm_out/snapdragon_pwm_out.cpp @@ -44,6 +44,7 @@ #include #include #include +#include #include #include @@ -117,23 +118,25 @@ MultirotorMixer *_mixer = nullptr; * forward declaration */ -void usage(); +static void usage(); -void start(); +static void start(); -void stop(); +static void stop(); -int pwm_initialize(const char *device); +static int pwm_initialize(const char *device); -void pwm_deinitialize(); +static void pwm_deinitialize(); -void send_outputs_pwm(const uint16_t *pwm); +static void send_outputs_pwm(const uint16_t *pwm); -void task_main_trampoline(int argc, char *argv[]); +static void task_main_trampoline(int argc, char *argv[]); -void subscribe(); +static void subscribe(); -void task_main(int argc, char *argv[]); +static void task_main(int argc, char *argv[]); + +static void update_params(bool &airmode); int initialize_mixer(const char *mixer_filename); @@ -157,6 +160,17 @@ int mixer_control_callback(uintptr_t handle, return 0; } +void update_params(bool &airmode) +{ + // multicopter air-mode + param_t param_handle = param_find("MC_AIRMODE"); + + if (param_handle != PARAM_INVALID) { + int32_t val; + param_get(param_handle, &val); + airmode = val > 0; + } +} int initialize_mixer(const char *mixer_filename) { @@ -343,6 +357,10 @@ void task_main(int argc, char *argv[]) // subscribe and set up polling subscribe(); + bool airmode = false; + update_params(airmode); + int params_sub = orb_subscribe(ORB_ID(parameter_update)); + // Start disarmed _armed.armed = false; _armed.prearmed = false; @@ -355,6 +373,10 @@ void task_main(int argc, char *argv[]) // Main loop while (!_task_should_exit) { + if (_mixer) { + _mixer->set_airmode(airmode); + } + /* wait up to 10ms for data */ int pret = px4_poll(_poll_fds, _poll_fds_num, 10); @@ -438,6 +460,15 @@ void task_main(int argc, char *argv[]) _outputs_pub = orb_advertise(ORB_ID(actuator_outputs), &_outputs); } + /* check for parameter updates */ + bool param_updated = false; + orb_check(params_sub, ¶m_updated); + + if (param_updated) { + struct parameter_update_s update; + orb_copy(ORB_ID(parameter_update), params_sub, &update); + update_params(airmode); + } } pwm_deinitialize(); @@ -449,6 +480,7 @@ void task_main(int argc, char *argv[]) } orb_unsubscribe(_armed_sub); + orb_unsubscribe(params_sub); _is_running = false; } From e78872e45bd561290ce6f293f1c796f25c2a772b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Mon, 19 Feb 2018 17:10:50 +0100 Subject: [PATCH 14/17] linux_pwm_out: add MC_AIRMODE support --- src/drivers/linux_pwm_out/linux_pwm_out.cpp | 51 ++++++++++++++++++--- 1 file changed, 45 insertions(+), 6 deletions(-) diff --git a/src/drivers/linux_pwm_out/linux_pwm_out.cpp b/src/drivers/linux_pwm_out/linux_pwm_out.cpp index 7dc812931945..8d1b3d090ed3 100644 --- a/src/drivers/linux_pwm_out/linux_pwm_out.cpp +++ b/src/drivers/linux_pwm_out/linux_pwm_out.cpp @@ -44,6 +44,7 @@ #include #include #include +#include #include #include @@ -99,17 +100,19 @@ int32_t _pwm_max; MixerGroup *_mixer_group = nullptr; -void usage(); +static void usage(); -void start(); +static void start(); -void stop(); +static void stop(); -void task_main_trampoline(int argc, char *argv[]); +static void task_main_trampoline(int argc, char *argv[]); -void subscribe(); +static void subscribe(); -void task_main(int argc, char *argv[]); +static void task_main(int argc, char *argv[]); + +static void update_params(bool &airmode); /* mixer initialization */ int initialize_mixer(const char *mixer_filename); @@ -128,6 +131,19 @@ int mixer_control_callback(uintptr_t handle, } +void update_params(bool &airmode) +{ + // multicopter air-mode + param_t param_handle = param_find("MC_AIRMODE"); + + if (param_handle != PARAM_INVALID) { + int32_t val; + param_get(param_handle, &val); + airmode = val > 0; + } +} + + int initialize_mixer(const char *mixer_filename) { char buf[4096]; @@ -227,6 +243,10 @@ void task_main(int argc, char *argv[]) // subscribe and set up polling subscribe(); + bool airmode = false; + update_params(airmode); + int params_sub = orb_subscribe(ORB_ID(parameter_update)); + int rc_channels_sub = -1; // Start disarmed @@ -244,6 +264,10 @@ void task_main(int argc, char *argv[]) orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); } + if (_mixer_group) { + _mixer_group->set_airmode(airmode); + } + int pret = px4_poll(_poll_fds, _poll_fds_num, 10); /* Timed out, do a periodic check for _task_should_exit. */ @@ -366,6 +390,17 @@ void task_main(int argc, char *argv[]) PX4_ERR("Could not mix output! Exiting..."); _task_should_exit = true; } + + /* check for parameter updates */ + bool param_updated = false; + orb_check(params_sub, ¶m_updated); + + if (param_updated) { + struct parameter_update_s update; + orb_copy(ORB_ID(parameter_update), params_sub, &update); + update_params(airmode); + } + } delete pwm_out; @@ -385,6 +420,10 @@ void task_main(int argc, char *argv[]) orb_unsubscribe(rc_channels_sub); } + if (params_sub != -1) { + orb_unsubscribe(params_sub); + } + _is_running = false; } From 4410bd4f0014d0670bab902d42341ef39dccc875 Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 12 Mar 2018 11:49:53 +0100 Subject: [PATCH 15/17] Multicopter mixer - Recompute safe roll_pitch_scale if not in air-mode If not in air-mode the mixer is not able to apply positive boosting and roll_pitch_scale is recomputed to apply symmetric - reduced - thrust. This has the consequence to cut completely the outputs when the thrust is set to zero. --- src/lib/mixer/mixer_multirotor.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/lib/mixer/mixer_multirotor.cpp b/src/lib/mixer/mixer_multirotor.cpp index 42d97e8e659b..398f30a73b2a 100644 --- a/src/lib/mixer/mixer_multirotor.cpp +++ b/src/lib/mixer/mixer_multirotor.cpp @@ -217,7 +217,6 @@ MultirotorMixer::mix(float *outputs, unsigned space) } else if (max_out > 1.0f) { boost = -(max_out - 1.0f); - } } else { @@ -226,7 +225,13 @@ MultirotorMixer::mix(float *outputs, unsigned space) } if (!_airmode) { - boost = math::min(boost, 0.0f); // Disable positive boosting if not in air-mode + // disable positive boosting if not in air-mode + // boosting is positive when min_out < 0.0 + // roll_pitch_scale is reduced accordingly + if (boost > 0.0f) { + roll_pitch_scale = thrust / (thrust - min_out); + boost = 0.0f; + } } // capture saturation From 67ea6365c5659990a097058bc782e4954a6f5fdc Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 12 Mar 2018 14:41:30 +0100 Subject: [PATCH 16/17] PWMSim : add MC_AIRMODE support --- src/drivers/pwm_out_sim/PWMSim.cpp | 30 ++++++++++++++++++++++++++++++ src/drivers/pwm_out_sim/PWMSim.hpp | 4 ++++ 2 files changed, 34 insertions(+) diff --git a/src/drivers/pwm_out_sim/PWMSim.cpp b/src/drivers/pwm_out_sim/PWMSim.cpp index 7224a494f998..ddbd49cde6c1 100644 --- a/src/drivers/pwm_out_sim/PWMSim.cpp +++ b/src/drivers/pwm_out_sim/PWMSim.cpp @@ -135,6 +135,18 @@ PWMSim::subscribe() } } +void PWMSim::update_params() +{ + // multicopter air-mode + param_t param_handle = param_find("MC_AIRMODE"); + + if (param_handle != PARAM_INVALID) { + int32_t val; + param_get(param_handle, &val); + _airmode = val > 0; + } +} + void PWMSim::run() { @@ -146,6 +158,9 @@ PWMSim::run() /* advertise the mixed control outputs, insist on the first group output */ _outputs_pub = orb_advertise(ORB_ID(actuator_outputs), &_actuator_outputs); + update_params(); + int params_sub = orb_subscribe(ORB_ID(parameter_update)); + /* loop until killed */ while (!should_exit()) { @@ -172,6 +187,10 @@ PWMSim::run() _current_update_rate = _update_rate; } + if (_mixers) { + _mixers->set_airmode(_airmode); + } + /* this can happen during boot, but after the sleep its likely resolved */ if (_poll_fds_num == 0) { usleep(1000 * 1000); @@ -283,6 +302,16 @@ PWMSim::run() _lockdown = aa.manual_lockdown; } } + + /* check for parameter updates */ + bool param_updated = false; + orb_check(params_sub, ¶m_updated); + + if (param_updated) { + struct parameter_update_s update; + orb_copy(ORB_ID(parameter_update), params_sub, &update); + update_params(); + } } for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { @@ -292,6 +321,7 @@ PWMSim::run() } orb_unsubscribe(_armed_sub); + orb_unsubscribe(params_sub); } int diff --git a/src/drivers/pwm_out_sim/PWMSim.hpp b/src/drivers/pwm_out_sim/PWMSim.hpp index 5331ac18e596..732636d129ff 100644 --- a/src/drivers/pwm_out_sim/PWMSim.hpp +++ b/src/drivers/pwm_out_sim/PWMSim.hpp @@ -49,6 +49,7 @@ #include #include #include +#include class PWMSim : public device::CDev, public ModuleBase { @@ -129,10 +130,13 @@ class PWMSim : public device::CDev, public ModuleBase actuator_controls_s _controls[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS] {}; orb_id_t _control_topics[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS] {}; + bool _airmode{false}; ///< multicopter air-mode + static int control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input); void subscribe(); + void update_params(); }; #endif /* DRIVERS_PWM_OUT_SIM_PWMSIM_HPP_ */ From 6821696a16bd680d6a280bf69e6f246ac6f41ee8 Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 23 Mar 2018 09:44:50 +0100 Subject: [PATCH 17/17] Airmode - Minor rewording --- src/lib/mixer/mixer.h | 2 +- src/lib/mixer/mixer_multirotor.cpp | 4 ++-- src/modules/mc_att_control/mc_att_control_params.c | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/lib/mixer/mixer.h b/src/lib/mixer/mixer.h index 2def433d9e75..bf7203a19277 100644 --- a/src/lib/mixer/mixer.h +++ b/src/lib/mixer/mixer.h @@ -221,7 +221,7 @@ class __EXPORT Mixer /** * @brief Set airmode. Airmode allows the mixer to increase the total thrust in order to unsaturate the motors. * - * @param[in] airmode Dis/-activate airmode by setting it to false/true + * @param[in] airmode De/-activate airmode by setting it to false/true */ virtual void set_airmode(bool airmode) {}; diff --git a/src/lib/mixer/mixer_multirotor.cpp b/src/lib/mixer/mixer_multirotor.cpp index 398f30a73b2a..4463ea38fba2 100644 --- a/src/lib/mixer/mixer_multirotor.cpp +++ b/src/lib/mixer/mixer_multirotor.cpp @@ -220,13 +220,13 @@ MultirotorMixer::mix(float *outputs, unsigned space) } } else { - roll_pitch_scale = 1 / (delta_out_max); + roll_pitch_scale = 1.0f / (delta_out_max); boost = 1.0f - ((max_out - thrust) * roll_pitch_scale + thrust); } if (!_airmode) { // disable positive boosting if not in air-mode - // boosting is positive when min_out < 0.0 + // boosting can only be positive when min_out < 0.0 // roll_pitch_scale is reduced accordingly if (boost > 0.0f) { roll_pitch_scale = thrust / (thrust - min_out); diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c index 60fd8d35e7fc..08f6d9f4236e 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -547,7 +547,7 @@ PARAM_DEFINE_FLOAT(MC_DTERM_CUTOFF, 0.f); /** * Multicopter air-mode * - * The air-mode enables the mixer to increase or decrease the total thrust of the multirotor + * The air-mode enables the mixer to increase the total thrust of the multirotor * in order to keep attitude and rate control even at low and high throttle. * This function should be disabled during tuning as it will help the controller * to diverge if the closed-loop is unstable.