aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Motors: add optional motor output throttle filter

Browse Source
This commit is contained in:
Jonathan Challinger 2015-03-31 20:16:43 -07:00 committed by Randy Mackay
parent f93df8997b
commit a2c69fe90d
2 changed files with 41 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
libraries/AP_Motors/AP_Motors_Class.cpp
View File

@ -93,6 +93,14 @@ const AP_Param::GroupInfo AP_Motors::var_info[] PROGMEM = {
// @User: Advanced
AP_GROUPINFO("CURR_MAX", 12, AP_Motors, _batt_current_max, AP_MOTORS_CURR_MAX_DEFAULT),
// @Param: THR_FILT
// @DisplayName: Throttle output filter
// @Description: Frequency cutoff (in hz) of throttle output filter
// @Range: 2 5
// @Units: Hz
// @User: Advanced
AP_GROUPINFO("THR_FILT", 13, AP_Motors, _throttle_filt_hz, 0.0f),
AP_GROUPEND
};
@ -117,7 +125,9 @@ AP_Motors::AP_Motors(RC_Channel& rc_roll, RC_Channel& rc_pitch, RC_Channel& rc_t
_batt_resistance(0.0f),
_batt_timer(0),
_lift_max(1.0f),
_throttle_limit(1.0f)
_throttle_limit(1.0f),
_throttle_in(0.0f),
_throttle_filter()
{
AP_Param::setup_object_defaults(this, var_info);
@ -127,6 +137,9 @@ AP_Motors::AP_Motors(RC_Channel& rc_roll, RC_Channel& rc_pitch, RC_Channel& rc_t
// setup battery voltage filtering
_batt_voltage_filt.set_cutoff_frequency(1.0f/_loop_rate,AP_MOTORS_BATT_VOLT_FILT_HZ);
_batt_voltage_filt.reset(1.0f);
_throttle_filter.set_cutoff_frequency(1.0f/_loop_rate,_throttle_filt_hz);
_throttle_filter.reset(0.0f);
};
void AP_Motors::armed(bool arm)
@ -167,6 +180,9 @@ void AP_Motors::throttle_pass_through(int16_t pwm)
// output - sends commands to the motors
void AP_Motors::output()
{
// update throttle filter
update_throttle_filter();
// update max throttle
update_max_throttle();
@ -198,6 +214,23 @@ void AP_Motors::slow_start(bool true_false)
_max_throttle = constrain_int16(_rc_throttle.servo_out, 0, AP_MOTORS_DEFAULT_MAX_THROTTLE);
}
// update the throttle input filter
void AP_Motors::update_throttle_filter()
{
if (_throttle_filter.get_cutoff_frequency() != _throttle_filt_hz) {
_throttle_filter.set_cutoff_frequency(1.0f/_loop_rate,_throttle_filt_hz);
}
if (_flags.armed) {
_throttle_filter.apply(_throttle_in);
} else {
_throttle_filter.reset(0.0f);
}
// prevent _rc_throttle.servo_out from wrapping at int16 max or min
_rc_throttle.servo_out = constrain_float(_throttle_filter.get(),-32000,32000);
}
// update_max_throttle - updates the limits on _max_throttle if necessary taking into account slow_start_throttle flag
void AP_Motors::update_max_throttle()
{

8
libraries/AP_Motors/AP_Motors_Class.h
View File

@ -121,7 +121,7 @@ public:
void set_roll(int16_t roll_in) { _rc_roll.servo_out = roll_in; }; // range -4500 ~ 4500
void set_pitch(int16_t pitch_in) { _rc_pitch.servo_out = pitch_in; }; // range -4500 ~ 4500
void set_yaw(int16_t yaw_in) { _rc_yaw.servo_out = yaw_in; }; // range -4500 ~ 4500
void set_throttle(int16_t throttle_in) { _rc_throttle.servo_out = throttle_in; }; // range 0 ~ 1000
void set_throttle(int16_t throttle_in) { _throttle_in = throttle_in; }; // range 0 ~ 1000
// accessors for roll, pitch, yaw and throttle inputs to motors
int16_t get_roll() const { return _rc_roll.servo_out; }
@ -201,6 +201,9 @@ protected:
virtual void output_armed()=0;
virtual void output_disarmed()=0;
// update the throttle input filter
void update_throttle_filter();
// update_max_throttle - updates the limits on _max_throttle for slow_start and current limiting flag
void update_max_throttle();
@ -242,6 +245,7 @@ protected:
AP_Float _batt_voltage_max; // maximum voltage used to scale lift
AP_Float _batt_voltage_min; // minimum voltage used to scale lift
AP_Float _batt_current_max; // current over which maximum throttle is limited
AP_Float _throttle_filt_hz; // throttle output filter time constant in hz
// internal variables
RC_Channel& _rc_roll; // roll input in from users is held in servo_out
@ -267,5 +271,7 @@ protected:
int16_t _batt_timer; // timer used in battery resistance calcs
float _lift_max; // maximum lift ratio from battery voltage
float _throttle_limit; // ratio of throttle limit between hover and maximum
float _throttle_in; // last throttle input from set_throttle caller
LowPassFilterFloat _throttle_filter; // throttle input filter
};
#endif // __AP_MOTORS_CLASS_H__