From d221313dfb7579268f39346bc10db7bc53fc7bad Mon Sep 17 00:00:00 2001 From: Roman Date: Sun, 9 Oct 2016 14:21:13 +0200 Subject: [PATCH] implemented mapping between desired thrust and applied pwm in multirotor mixer. Signed-off-by: Roman --- src/drivers/px4io/px4io.cpp | 8 +++++ src/modules/px4iofirmware/mixer.cpp | 12 ++++++++ src/modules/px4iofirmware/protocol.h | 4 ++- src/modules/px4iofirmware/px4io.h | 2 ++ src/modules/px4iofirmware/registers.c | 6 +++- src/modules/sensors/sensor_params.c | 12 ++++++++ src/modules/systemlib/mixer/mixer.h | 29 ++++++++++++++++++- src/modules/systemlib/mixer/mixer_group.cpp | 12 ++++++++ .../systemlib/mixer/mixer_multirotor.cpp | 13 +++++++++ 9 files changed, 95 insertions(+), 3 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index db2cb34905..d66544e820 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1295,6 +1295,14 @@ PX4IO::task_main() } } + /* thrust to pwm modelling factor */ + parm_handle = param_find("THR_MDL_FAC"); + + if (parm_handle != PARAM_INVALID) { + param_get(parm_handle, ¶m_val); + (void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_THR_MDL_FAC, FLOAT_TO_REG(param_val)); + } + /* maximum motor pwm slew rate */ parm_handle = param_find("MOT_SLEW_MAX"); diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 909ce3d2a0..b6ab3f3989 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -248,7 +248,13 @@ mixer_tick(void) } /* mix */ + /* update parameter for mc thrust model if it updated */ + if (update_mc_thrust_param) { + mixer_group.set_thrust_factor(REG_TO_FLOAT(r_setup_thr_fac)); + update_mc_thrust_param = false; + } + /* mix */ /* poor mans mutex */ in_mixer = true; mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT, &r_mixer_limits); @@ -536,6 +542,12 @@ mixer_set_failsafe() mixer_group.set_max_delta_out_once(delta_out_max); } + /* update parameter for mc thrust model if it updated */ + if (update_mc_thrust_param) { + mixer_group.set_thrust_factor(REG_TO_FLOAT(r_setup_thr_fac)); + update_mc_thrust_param = false; + } + /* mix */ mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT, &r_mixer_limits); diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 7b90aaa8ed..7559da9b75 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -241,7 +241,9 @@ enum { /* DSM bind states */ #define PX4IO_P_SETUP_MOTOR_SLEW_MAX 24 /* max motor slew rate */ -#define PX4IO_P_SETUP_THERMAL 25 /* thermal management */ +#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 */ #define PX4IO_THERMAL_IGNORE UINT16_MAX #define PX4IO_THERMAL_OFF 0 #define PX4IO_THERMAL_FULL 10000 diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index a1bb804dd2..e50315415d 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -127,6 +127,7 @@ extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */ #define r_setup_scale_pitch r_page_setup[PX4IO_P_SETUP_SCALE_PITCH] #define r_setup_scale_yaw r_page_setup[PX4IO_P_SETUP_SCALE_YAW] #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_control_values (&r_page_controls[0]) @@ -148,6 +149,7 @@ struct sys_state_s { extern struct sys_state_s system_state; extern float dt; +extern bool update_mc_thrust_param; /* * PWM limit structure diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index fdb689cd2e..ecb7beaeaa 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -56,6 +56,7 @@ static int registers_set_one(uint8_t page, uint8_t offset, uint16_t value); static void pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t altrate); +bool update_mc_thrust_param; /** * PAGE 0 * @@ -182,6 +183,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_THR_MDL_FAC] = 0, [PX4IO_P_SETUP_THERMAL] = PX4IO_THERMAL_IGNORE }; @@ -709,12 +711,14 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) case PX4IO_P_SETUP_SCALE_ROLL: case PX4IO_P_SETUP_SCALE_PITCH: case PX4IO_P_SETUP_SCALE_YAW: + case PX4IO_P_SETUP_MOTOR_SLEW_MAX: case PX4IO_P_SETUP_SBUS_RATE: r_page_setup[offset] = value; sbus1_set_output_rate_hz(value); break; - case PX4IO_P_SETUP_MOTOR_SLEW_MAX: + case PX4IO_P_SETUP_THR_MDL_FAC: + update_mc_thrust_param = true; r_page_setup[offset] = value; break; diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 92222ea89b..a2bfdde31c 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -3277,6 +3277,18 @@ PARAM_DEFINE_INT32(PWM_AUX_MAX, 2000); */ PARAM_DEFINE_INT32(PWM_AUX_DISARMED, 1500); +/** + * Thrust to PWM model parameter + * + * Parameter used to model the relationship between static thrust and motor + * input PWM. Model is: thrust = (1-factor)*PWM + factor * PWM^2 + * + * @min 0.0 + * @max 1.0 + * @group PWM Outputs + */ +PARAM_DEFINE_FLOAT(THR_MDL_FAC, 0.65f); + /** * Minimum motor rise time (slew rate limit). * diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h index d285a88f86..9ade606eaa 100644 --- a/src/modules/systemlib/mixer/mixer.h +++ b/src/modules/systemlib/mixer/mixer.h @@ -199,8 +199,20 @@ public: */ virtual void set_max_delta_out_once(float delta_out_max) {}; + /** + * @brief Set trim offset for this mixer + * + * @return the number of outputs this mixer feeds to + */ virtual unsigned set_trim(float trim) = 0; + /* + * @brief Sets the thrust factor used to calculate mapping from desired thrust to pwm. + * + * @param[in] val The value + */ + virtual void set_thrust_factor(float val) {}; + protected: /** client-supplied callback used when fetching control values */ ControlCallback _control_cb; @@ -367,6 +379,13 @@ public: return 0; } + /** + * @brief Sets the thrust factor used to calculate mapping from desired thrust to pwm. + * + * @param[in] val The value + */ + virtual void set_thrust_factor(float val); + private: Mixer *_first; /**< linked list of mixers */ @@ -601,13 +620,21 @@ public: return _rotor_count; } + /** + * @brief Sets the thrust factor used to calculate mapping from desired thrust to pwm. + * + * @param[in] val The value + */ + virtual void set_thrust_factor(float val) {_thrust_factor = val;} + private: float _roll_scale; float _pitch_scale; float _yaw_scale; float _idle_speed; - float _delta_out_max; + float _thrust_factor; + orb_advert_t _limits_pub; multirotor_motor_limits_s _limits; diff --git a/src/modules/systemlib/mixer/mixer_group.cpp b/src/modules/systemlib/mixer/mixer_group.cpp index f5f103b45c..ba7d6d1b93 100644 --- a/src/modules/systemlib/mixer/mixer_group.cpp +++ b/src/modules/systemlib/mixer/mixer_group.cpp @@ -141,6 +141,18 @@ MixerGroup::set_trims(int16_t *values, unsigned n) return index; } +void +MixerGroup::set_thrust_factor(float val) +{ + Mixer *mixer = _first; + + while (mixer != nullptr) { + mixer->set_thrust_factor(val); + mixer = mixer->_next; + } + +} + uint16_t MixerGroup::get_saturation_status() { diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index 0024917b76..c84abe5ad9 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -91,6 +91,7 @@ MultirotorMixer::MultirotorMixer(ControlCallback control_cb, _yaw_scale(yaw_scale), _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), _limits_pub(), _rotor_count(_config_rotor_count[(MultirotorGeometryUnderlyingType)geometry]), _rotors(_config_index[(MultirotorGeometryUnderlyingType)geometry]), @@ -358,6 +359,18 @@ MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg) yaw * _rotors[i].yaw_scale + thrust + boost; + /* + implement simple model for static relationship between applied motor pwm and motor thrust + model: thrust = (1 - _thrust_factor) * PWM + _thrust_factor * PWM^2 + this model assumes normalized input / output in the range [0,1] so this is the right place + to do it as at this stage the outputs are in that range. + */ + if (_thrust_factor > 0.0f) { + outputs[i] = -(1.0f - _thrust_factor) / (2.0f * _thrust_factor) + sqrtf((1.0f - _thrust_factor) * + (1.0f - _thrust_factor) / (4.0f * _thrust_factor * _thrust_factor) + (outputs[i] < 0.0f ? 0.0f : outputs[i] / + _thrust_factor)); + } + outputs[i] = constrain(_idle_speed + (outputs[i] * (1.0f - _idle_speed)), _idle_speed, 1.0f); }