AP_Mount: fix extreme gimbal movements before TX is turned on

This commit is contained in:
Randy Mackay 2018-12-19 14:43:30 +09:00
parent 9ac8e98161
commit 6a0b56ee77

View File

@ -92,27 +92,27 @@ void AP_Mount_Backend::update_targets_from_rc()
// if joystick_speed is defined then pilot input defines a rate of change of the angle
if (_frontend._joystick_speed) {
// allow pilot speed position input to come directly from an RC_Channel
if (roll_rc_in && rc_ch(roll_rc_in)) {
if (roll_rc_in && (rc_ch(roll_rc_in) != nullptr) && (rc_ch(roll_rc_in)->get_radio_in() > 0)) {
_angle_ef_target_rad.x += rc_ch(roll_rc_in)->norm_input_dz() * 0.0001f * _frontend._joystick_speed;
_angle_ef_target_rad.x = constrain_float(_angle_ef_target_rad.x, radians(_state._roll_angle_min*0.01f), radians(_state._roll_angle_max*0.01f));
}
if (tilt_rc_in && (rc_ch(tilt_rc_in))) {
if (tilt_rc_in && (rc_ch(tilt_rc_in) != nullptr) && (rc_ch(tilt_rc_in)->get_radio_in() > 0)) {
_angle_ef_target_rad.y += rc_ch(tilt_rc_in)->norm_input_dz() * 0.0001f * _frontend._joystick_speed;
_angle_ef_target_rad.y = constrain_float(_angle_ef_target_rad.y, radians(_state._tilt_angle_min*0.01f), radians(_state._tilt_angle_max*0.01f));
}
if (pan_rc_in && (rc_ch(pan_rc_in))) {
if (pan_rc_in && (rc_ch(pan_rc_in) != nullptr) && (rc_ch(pan_rc_in)->get_radio_in() > 0)) {
_angle_ef_target_rad.z += rc_ch(pan_rc_in)->norm_input_dz() * 0.0001f * _frontend._joystick_speed;
_angle_ef_target_rad.z = constrain_float(_angle_ef_target_rad.z, radians(_state._pan_angle_min*0.01f), radians(_state._pan_angle_max*0.01f));
}
} else {
// allow pilot position input to come directly from an RC_Channel
if (roll_rc_in && (rc_ch(roll_rc_in))) {
if (roll_rc_in && (rc_ch(roll_rc_in) != nullptr) && (rc_ch(roll_rc_in)->get_radio_in() > 0)) {
_angle_ef_target_rad.x = angle_input_rad(rc_ch(roll_rc_in), _state._roll_angle_min, _state._roll_angle_max);
}
if (tilt_rc_in && (rc_ch(tilt_rc_in))) {
if (tilt_rc_in && (rc_ch(tilt_rc_in) != nullptr) && (rc_ch(tilt_rc_in)->get_radio_in() > 0)) {
_angle_ef_target_rad.y = angle_input_rad(rc_ch(tilt_rc_in), _state._tilt_angle_min, _state._tilt_angle_max);
}
if (pan_rc_in && (rc_ch(pan_rc_in))) {
if (pan_rc_in && (rc_ch(pan_rc_in) != nullptr) && (rc_ch(pan_rc_in)->get_radio_in() > 0)) {
_angle_ef_target_rad.z = angle_input_rad(rc_ch(pan_rc_in), _state._pan_angle_min, _state._pan_angle_max);
}
}