Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Mount_Backend: move RC target handling to backend

This commit is contained in:
Randy Mackay 2015-01-15 14:11:17 +09:00 committed by Andrew Tridgell
parent b3044ced1f
commit 9d4210b82a
4 changed files with 61 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
libraries/AP_Mount/AP_Mount_Backend.cpp
View File

@ -2,6 +2,56 @@
#include <AP_Mount_Backend.h>
// 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)
{

8
libraries/AP_Mount/AP_Mount_Backend.h
View File

@ -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__

47
libraries/AP_Mount/AP_Mount_Servo.cpp
View File

@ -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)
{

7
libraries/AP_Mount/AP_Mount_Servo.h
View File

@ -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
};