diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp index 0ff8868d77..253b988891 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.cpp +++ b/libraries/AP_Mount/AP_Mount_Backend.cpp @@ -2,6 +2,56 @@ #include +// update_targets_from_rc - updates angle targets using input from receiver +void AP_Mount_Backend::update_targets_from_rc() +{ +#define rc_ch(i) RC_Channel::rc_channel(i-1) + + uint8_t roll_rc_in = _frontend.state[_instance]._roll_rc_in; + uint8_t tilt_rc_in = _frontend.state[_instance]._tilt_rc_in; + uint8_t pan_rc_in = _frontend.state[_instance]._pan_rc_in; + + // 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)) { + _angle_ef_target_rad.x += rc_ch(roll_rc_in)->norm_input_dz() * 0.0001f * _frontend._joystick_speed; + constrain_float(_angle_ef_target_rad.x, radians(_frontend.state[_instance]._roll_angle_min*0.01f), radians(_frontend.state[_instance]._roll_angle_max*0.01f)); + } + if (tilt_rc_in && (rc_ch(tilt_rc_in))) { + _angle_ef_target_rad.y += rc_ch(tilt_rc_in)->norm_input_dz() * 0.0001f * _frontend._joystick_speed; + constrain_float(_angle_ef_target_rad.y, radians(_frontend.state[_instance]._tilt_angle_min*0.01f), radians(_frontend.state[_instance]._tilt_angle_max*0.01f)); + } + if (pan_rc_in && (rc_ch(pan_rc_in))) { + _angle_ef_target_rad.z += rc_ch(pan_rc_in)->norm_input_dz() * 0.0001f * _frontend._joystick_speed; + constrain_float(_angle_ef_target_rad.z, radians(_frontend.state[_instance]._pan_angle_min*0.01f), radians(_frontend.state[_instance]._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))) { + _angle_ef_target_rad.x = angle_input_rad(rc_ch(roll_rc_in), _frontend.state[_instance]._roll_angle_min, _frontend.state[_instance]._roll_angle_max); + } + if (tilt_rc_in && (rc_ch(tilt_rc_in))) { + _angle_ef_target_rad.y = angle_input_rad(rc_ch(tilt_rc_in), _frontend.state[_instance]._tilt_angle_min, _frontend.state[_instance]._tilt_angle_max); + } + if (pan_rc_in && (rc_ch(pan_rc_in))) { + _angle_ef_target_rad.z = angle_input_rad(rc_ch(pan_rc_in), _frontend.state[_instance]._pan_angle_min, _frontend.state[_instance]._pan_angle_max); + } + } +} + +// returns the angle (degrees*100) that the RC_Channel input is receiving +int32_t AP_Mount_Backend::angle_input(RC_Channel* rc, int16_t angle_min, int16_t angle_max) +{ + return (rc->get_reverse() ? -1 : 1) * (rc->radio_in - rc->radio_min) * (int32_t)(angle_max - angle_min) / (rc->radio_max - rc->radio_min) + (rc->get_reverse() ? angle_max : angle_min); +} + +// returns the angle (radians) that the RC_Channel input is receiving +float AP_Mount_Backend::angle_input_rad(RC_Channel* rc, int16_t angle_min, int16_t angle_max) +{ + return radians(angle_input(rc, angle_min, angle_max)*0.01f); +} + // calc_angle_to_location - calculates the earth-frame roll, tilt and pan angles (and radians) to point at the given target void AP_Mount_Backend::calc_angle_to_location(const struct Location &target, Vector3f& angles_to_target_rad, bool calc_tilt, bool calc_pan) { diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index 908b28e2b7..61edf4550c 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -63,11 +63,19 @@ public: protected: + // update_targets_from_rc - updates angle targets (i.e. _angle_ef_target_rad) using input from receiver + void update_targets_from_rc(); + + // angle_input, angle_input_rad - convert RC input into an earth-frame target angle + int32_t angle_input(RC_Channel* rc, int16_t angle_min, int16_t angle_max); + float angle_input_rad(RC_Channel* rc, int16_t angle_min, int16_t angle_max); + // calc_angle_to_location - calculates the earth-frame roll, tilt and pan angles (and radians) to point at the given target void calc_angle_to_location(const struct Location &target, Vector3f& angles_to_target_rad, bool calc_tilt, bool calc_pan); AP_Mount &_frontend; // reference to the front end which holds parameters uint8_t _instance; // this instance's number + Vector3f _angle_ef_target_rad; // desired earth-frame roll, tilt and pan angles in radians }; #endif // __AP_MOUNT_BACKEND_H__ diff --git a/libraries/AP_Mount/AP_Mount_Servo.cpp b/libraries/AP_Mount/AP_Mount_Servo.cpp index 8eec373a5e..40ba8c64f7 100644 --- a/libraries/AP_Mount/AP_Mount_Servo.cpp +++ b/libraries/AP_Mount/AP_Mount_Servo.cpp @@ -62,37 +62,8 @@ void AP_Mount_Servo::update() // RC radio manual angle control, but with stabilization from the AHRS case MAV_MOUNT_MODE_RC_TARGETING: { -#define rc_ch(i) RC_Channel::rc_channel(i-1) - uint8_t roll_rc_in = _frontend.state[_instance]._roll_rc_in; - uint8_t tilt_rc_in = _frontend.state[_instance]._tilt_rc_in; - uint8_t pan_rc_in = _frontend.state[_instance]._pan_rc_in; - - if (_frontend._joystick_speed) { // for spring loaded joysticks - // allow pilot speed position input to come directly from an RC_Channel - if (roll_rc_in && rc_ch(roll_rc_in)) { - _angle_ef_target_rad.x += rc_ch(roll_rc_in)->norm_input_dz() * 0.0001f * _frontend._joystick_speed; - constrain_float(_angle_ef_target_rad.x, radians(_frontend.state[_instance]._roll_angle_min*0.01f), radians(_frontend.state[_instance]._roll_angle_max*0.01f)); - } - if (tilt_rc_in && (rc_ch(tilt_rc_in))) { - _angle_ef_target_rad.y += rc_ch(tilt_rc_in)->norm_input_dz() * 0.0001f * _frontend._joystick_speed; - constrain_float(_angle_ef_target_rad.y, radians(_frontend.state[_instance]._tilt_angle_min*0.01f), radians(_frontend.state[_instance]._tilt_angle_max*0.01f)); - } - if (pan_rc_in && (rc_ch(pan_rc_in))) { - _angle_ef_target_rad.z += rc_ch(pan_rc_in)->norm_input_dz() * 0.0001f * _frontend._joystick_speed; - constrain_float(_angle_ef_target_rad.z, radians(_frontend.state[_instance]._pan_angle_min*0.01f), radians(_frontend.state[_instance]._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))) { - _angle_ef_target_rad.x = angle_input_rad(rc_ch(roll_rc_in), _frontend.state[_instance]._roll_angle_min, _frontend.state[_instance]._roll_angle_max); - } - if (tilt_rc_in && (rc_ch(tilt_rc_in))) { - _angle_ef_target_rad.y = angle_input_rad(rc_ch(tilt_rc_in), _frontend.state[_instance]._tilt_angle_min, _frontend.state[_instance]._tilt_angle_max); - } - if (pan_rc_in && (rc_ch(pan_rc_in))) { - _angle_ef_target_rad.z = angle_input_rad(rc_ch(pan_rc_in), _frontend.state[_instance]._pan_angle_min, _frontend.state[_instance]._pan_angle_max); - } - } + // update targets using pilot's rc inputs + update_targets_from_rc(); stabilize(); break; } @@ -263,20 +234,6 @@ void AP_Mount_Servo::stabilize() } } -// returns the angle (degrees*100) that the RC_Channel input is receiving -int32_t -AP_Mount_Servo::angle_input(RC_Channel* rc, int16_t angle_min, int16_t angle_max) -{ - return (rc->get_reverse() ? -1 : 1) * (rc->radio_in - rc->radio_min) * (int32_t)(angle_max - angle_min) / (rc->radio_max - rc->radio_min) + (rc->get_reverse() ? angle_max : angle_min); -} - -// returns the angle (radians) that the RC_Channel input is receiving -float -AP_Mount_Servo::angle_input_rad(RC_Channel* rc, int16_t angle_min, int16_t angle_max) -{ - return radians(angle_input(rc, angle_min, angle_max)*0.01f); -} - // closest_limit - returns closest angle to 'angle' taking into account limits. all angles are in degrees * 10 int16_t AP_Mount_Servo::closest_limit(int16_t angle, int16_t angle_min, int16_t angle_max) { diff --git a/libraries/AP_Mount/AP_Mount_Servo.h b/libraries/AP_Mount/AP_Mount_Servo.h index 6facb57f31..5b9d57e374 100644 --- a/libraries/AP_Mount/AP_Mount_Servo.h +++ b/libraries/AP_Mount/AP_Mount_Servo.h @@ -73,10 +73,6 @@ private: // stabilize - stabilizes the mount relative to the Earth's frame void stabilize(); - // angle_input, angle_input_rad - convert RC input into an earth-frame target angle - int32_t angle_input(RC_Channel* rc, int16_t angle_min, int16_t angle_max); - float angle_input_rad(RC_Channel* rc, int16_t angle_min, int16_t angle_max); - // closest_limit - returns closest angle to 'angle' taking into account limits. all angles are in degrees * 10 int16_t closest_limit(int16_t angle, int16_t angle_min, int16_t angle_max); @@ -89,8 +85,7 @@ private: RC_Channel_aux::Aux_servo_function_t _pan_idx; // RC_Channel_aux mount pan function index RC_Channel_aux::Aux_servo_function_t _open_idx; // RC_Channel_aux mount open function index - Vector3f _angle_ef_target_rad; // desired earth-frame roll, tilt and pan angles in radians - Vector3f _angle_bf_output_deg; // final body frame output angle in degres + Vector3f _angle_bf_output_deg; // final body frame output angle in degrees uint32_t _last_check_servo_map_ms; // system time of latest call to check_servo_map function };