Browse Source

AP_Mount: angle_input uses norm_input

master
Randy Mackay 6 years ago
parent
commit
9767c74311
  1. 4
      libraries/AP_Mount/AP_Mount_Backend.cpp

4
libraries/AP_Mount/AP_Mount_Backend.cpp

@ -125,9 +125,7 @@ void AP_Mount_Backend::update_targets_from_rc()
// returns the angle (degrees*100) that the RC_Channel input is receiving // returns the angle (degrees*100) that the RC_Channel input is receiving
int32_t AP_Mount_Backend::angle_input(const RC_Channel* rc, int16_t angle_min, int16_t angle_max) int32_t AP_Mount_Backend::angle_input(const RC_Channel* rc, int16_t angle_min, int16_t angle_max)
{ {
int16_t radio_in = constrain_int16(rc->get_radio_in(), rc->get_radio_min(), rc->get_radio_max()); return (rc->norm_input() + 1.0f) * 0.5f * (angle_max - angle_min) + angle_min;
return (rc->get_reverse() ? -1 : 1) * (radio_in - rc->get_radio_min())
* (int32_t)(angle_max - angle_min) / (rc->get_radio_max() - rc->get_radio_min()) + (rc->get_reverse() ? angle_max : angle_min);
} }
// returns the angle (radians) that the RC_Channel input is receiving // returns the angle (radians) that the RC_Channel input is receiving

Loading…
Cancel
Save