From 6976232a205050ed69347d1802ab789eaa96c31c Mon Sep 17 00:00:00 2001 From: bresch Date: Tue, 13 Feb 2018 11:53:54 +0100 Subject: [PATCH] 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 fa83c751e0..e3a55852a8 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -275,6 +275,7 @@ private: 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 ce913cf4c3..3b83580909 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 989a59e43f..2def433d9e 100644 --- a/src/lib/mixer/mixer.h +++ b/src/lib/mixer/mixer.h @@ -218,6 +218,13 @@ public: */ 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 @@ public: */ virtual void set_thrust_factor(float val); + virtual void set_airmode(bool airmode); + private: Mixer *_first; /**< linked list of mixers */ @@ -650,6 +659,8 @@ public: */ 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 @@ private: 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 e4fb98fbff..200be683ba 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 59730013e3..ed14bf48ec 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 89202e086e..60fd8d35e7 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 5a167a447f..b874c8cac2 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 04fd0712fc..8e6e425e35 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 00b27372a8..10db31a277 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 fc69198635..462398cf08 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;