mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-22 08:38:36 -04:00
AP_Mount: fix extreme gimbal movements before TX is turned on
This commit is contained in:
parent
9ac8e98161
commit
6a0b56ee77
@ -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);
|
||||
}
|
||||
}
|
||||
|
Loading…
Reference in New Issue
Block a user