zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Mount_Backend: move RC target handling to backend

mission-4.1.18
Randy Mackay 10 years ago committed by Andrew Tridgell
parent
b3044ced1f
commit
9d4210b82a
4 changed files with 61 additions and 51 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 50
      libraries/AP_Mount/AP_Mount_Backend.cpp
  2. 8
      libraries/AP_Mount/AP_Mount_Backend.h
  3. 47
      libraries/AP_Mount/AP_Mount_Servo.cpp
  4. 7
      libraries/AP_Mount/AP_Mount_Servo.h

50
libraries/AP_Mount/AP_Mount_Backend.cpp
Unescape View File

@ -2,6 +2,56 @@ @@ -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
Unescape View File

@ -63,11 +63,19 @@ public: @@ -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
Unescape View File

@ -62,37 +62,8 @@ void AP_Mount_Servo::update() @@ -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() @@ -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
Unescape View File

@ -73,10 +73,6 @@ private: @@ -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: @@ -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
};

Write Preview
Loading…
Cancel
Save