|
|
|
@ -134,9 +134,9 @@ const AP_Param::GroupInfo AP_MotorsMulticopter::var_info[] = {
@@ -134,9 +134,9 @@ const AP_Param::GroupInfo AP_MotorsMulticopter::var_info[] = {
|
|
|
|
|
// @Param: HOVER_LEARN
|
|
|
|
|
// @DisplayName: Hover Value Learning
|
|
|
|
|
// @Description: Enable/Disable automatic learning of hover throttle
|
|
|
|
|
// @Values: 0:Disabled, 1:Enabled
|
|
|
|
|
// @Values: 0:Disabled, 1:Learn, 2:LearnAndSave
|
|
|
|
|
// @User: Advanced
|
|
|
|
|
AP_GROUPINFO("HOVER_LEARN", 22, AP_MotorsMulticopter, _throttle_hover_learn, 1), |
|
|
|
|
AP_GROUPINFO("HOVER_LEARN", 22, AP_MotorsMulticopter, _throttle_hover_learn, HOVER_LEARN_AND_SAVE), |
|
|
|
|
|
|
|
|
|
AP_GROUPEND |
|
|
|
|
}; |
|
|
|
@ -374,7 +374,7 @@ void AP_MotorsMulticopter::set_throttle_range(int16_t radio_min, int16_t radio_m
@@ -374,7 +374,7 @@ void AP_MotorsMulticopter::set_throttle_range(int16_t radio_min, int16_t radio_m
|
|
|
|
|
// update the throttle input filter. should be called at 100hz
|
|
|
|
|
void AP_MotorsMulticopter::update_throttle_hover(float dt) |
|
|
|
|
{ |
|
|
|
|
if (_throttle_hover_learn > 0) { |
|
|
|
|
if (_throttle_hover_learn != HOVER_LEARN_DISABLED) { |
|
|
|
|
_throttle_hover = _throttle_hover + (dt/(dt+AP_MOTORS_THST_HOVER_TC))*(_throttle_in-_throttle_hover); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
@ -570,7 +570,7 @@ void AP_MotorsMulticopter::output_motor_mask(float thrust, uint8_t mask)
@@ -570,7 +570,7 @@ void AP_MotorsMulticopter::output_motor_mask(float thrust, uint8_t mask)
|
|
|
|
|
void AP_MotorsMulticopter::save_params_on_disarm() |
|
|
|
|
{ |
|
|
|
|
// save hover throttle
|
|
|
|
|
if (_throttle_hover_learn > 0) { |
|
|
|
|
if (_throttle_hover_learn == HOVER_LEARN_AND_SAVE) { |
|
|
|
|
_throttle_hover.save(); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|