AP_MotorsMulticopter: add HOVER_LEARN param

This commit is contained in:
Randy Mackay 2016-06-09 14:21:04 +09:00
parent 4f0db2bc36
commit 25778a24e2
4 changed files with 31 additions and 3 deletions

View File

@ -115,6 +115,13 @@ const AP_Param::GroupInfo AP_MotorsMulticopter::var_info[] = {
// @User: Advanced
AP_GROUPINFO("THST_HOVER", 21, AP_MotorsMulticopter, _throttle_hover, AP_MOTORS_THST_HOVER_DEFAULT),
// @Param: HOVER_LEARN
// @DisplayName: Hover Value Learning
// @Description: Enable/Disable automatic learning of hover throttle
// @Values: 0:Disabled, 1:Enabled
// @User: Advanced
AP_GROUPINFO("HOVER_LEARN", 22, AP_MotorsMulticopter, _throttle_hover_learn, 1),
AP_GROUPEND
};
@ -341,8 +348,10 @@ void AP_MotorsMulticopter::set_throttle_range(uint16_t min_throttle, int16_t rad
// update the throttle input filter. should be called at 100hz
void AP_MotorsMulticopter::update_throttle_hover(float dt)
{
if (_throttle_hover_learn > 0) {
_throttle_hover = _throttle_hover + (dt/(dt+AP_MOTORS_THST_HOVER_TC))*(_throttle_in-_throttle_hover);
}
}
void AP_MotorsMulticopter::output_logic()
{
@ -529,3 +538,12 @@ void AP_MotorsMulticopter::output_motor_mask(float thrust, uint8_t mask)
}
hal.rcout->push();
}
// save parameters as part of disarming
void AP_MotorsMulticopter::save_params_on_disarm()
{
// save hover throttle
if (_throttle_hover_learn > 0) {
_throttle_hover.save();
}
}

View File

@ -131,6 +131,9 @@ protected:
// apply any thrust compensation for the frame
virtual void thrust_compensation(void) {}
// save parameters as part of disarming
void save_params_on_disarm();
// flag bitmask
struct {
spool_up_down_mode spool_mode : 3; // motor's current spool mode
@ -148,6 +151,7 @@ protected:
AP_Int16 _pwm_min; // minimum PWM value that will ever be output to the motors (if 0, vehicle's throttle input channel's min pwm used)
AP_Int16 _pwm_max; // maximum PWM value that will ever be output to the motors (if 0, vehicle's throttle input channel's max pwm used)
AP_Float _throttle_hover; // estimated throttle required to hover throttle in the range 0 ~ 1
AP_Int8 _throttle_hover_learn; // enable/disabled hover thrust learning
// internal variables
bool motor_enabled[AP_MOTORS_MAX_NUM_MOTORS]; // true if motor is enabled

View File

@ -59,8 +59,11 @@ AP_Motors::AP_Motors(uint16_t loop_rate, uint16_t speed_hz) :
void AP_Motors::armed(bool arm)
{
if (_flags.armed != arm) {
_flags.armed = arm;
AP_Notify::flags.armed = arm;
save_params_on_disarm();
}
};
// pilot input in the -1 ~ +1 range for roll, pitch and yaw. 0~1 range for throttle

View File

@ -147,6 +147,9 @@ protected:
// update the throttle input filter
virtual void update_throttle_filter() = 0;
// save parameters as part of disarming
virtual void save_params_on_disarm() {}
// convert input in -1 to +1 range to pwm output
int16_t calc_pwm_output_1to1(float input, const RC_Channel& servo);