fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-23 09:17:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

implemented mapping between desired thrust and applied pwm in

Browse Source 
multirotor mixer.

Signed-off-by: Roman <bapstroman@gmail.com>
This commit is contained in:
Roman
2016-10-09 14:21:13 +02:00
committed by Lorenz Meier
parent 22733b1501
commit d221313dfb
9 changed files with 95 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/px4iofirmware/mixer.cpp
+12
View File
@@ -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);
src/modules/px4iofirmware/protocol.h
+3 -1
View File
@@ -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
src/modules/px4iofirmware/px4io.h
+2
View File
@@ -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
src/modules/px4iofirmware/registers.c
+5 -1
View File
@@ -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;
src/modules/sensors/sensor_params.c
+12
View File
@@ -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).
*
src/modules/systemlib/mixer/mixer.h
+28 -1
View File
@@ -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;
src/modules/systemlib/mixer/mixer_group.cpp
+12
View File
@@ -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()
{
src/modules/systemlib/mixer/mixer_multirotor.cpp
+13
View File
@@ -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);
}