5
0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-05 23:48:31 -04:00

Tracker: compensate for tilt while tracking

This adds an earth frame to body frame conversion which is required
to convert the earth-frame angles to the target vehicle into body-frame
movements of the servos
This commit is contained in:
stefanlynka 2016-07-14 11:40:49 +09:00 committed by Randy Mackay
parent c6d3f07b5f
commit a693f9ead6
3 changed files with 72 additions and 60 deletions

View File

@ -158,6 +158,8 @@ private:
float bearing; // bearing to vehicle in centi-degrees float bearing; // bearing to vehicle in centi-degrees
float distance; // distance to vehicle in meters float distance; // distance to vehicle in meters
float pitch; // pitch to vehicle in degrees (positive means vehicle is above tracker, negative means below) float pitch; // pitch to vehicle in degrees (positive means vehicle is above tracker, negative means below)
float angle_error_pitch; // angle error between target and current pitch in centi-degrees
float angle_error_yaw; // angle error between target and current yaw in centi-degrees
float alt_difference_baro; // altitude difference between tracker and vehicle in meters according to the barometer. positive value means vehicle is above tracker float alt_difference_baro; // altitude difference between tracker and vehicle in meters according to the barometer. positive value means vehicle is above tracker
float alt_difference_gps; // altitude difference between tracker and vehicle in meters according to the gps. positive value means vehicle is above tracker float alt_difference_gps; // altitude difference between tracker and vehicle in meters according to the gps. positive value means vehicle is above tracker
float altitude_offset; // offset in meters which is added to tracker altitude to align altitude measurements with vehicle's barometer float altitude_offset; // offset in meters which is added to tracker altitude to align altitude measurements with vehicle's barometer
@ -166,7 +168,7 @@ private:
bool need_altitude_calibration : 1;// true if tracker altitude has not been determined (true after startup) bool need_altitude_calibration : 1;// true if tracker altitude has not been determined (true after startup)
bool scan_reverse_pitch : 1;// controls direction of pitch movement in SCAN mode bool scan_reverse_pitch : 1;// controls direction of pitch movement in SCAN mode
bool scan_reverse_yaw : 1;// controls direction of yaw movement in SCAN mode bool scan_reverse_yaw : 1;// controls direction of yaw movement in SCAN mode
} nav_status = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, false, false, true, false, false}; } nav_status = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, false, false, true, false, false};
// setup the var_info table // setup the var_info table
AP_Param param_loader{var_info}; AP_Param param_loader{var_info};
@ -215,11 +217,11 @@ private:
void update_GPS(void); void update_GPS(void);
void init_servos(); void init_servos();
void update_pitch_servo(float pitch); void update_pitch_servo(float pitch);
void update_pitch_position_servo(float pitch); void update_pitch_position_servo(void);
void update_pitch_cr_servo(float pitch); void update_pitch_cr_servo(float pitch);
void update_pitch_onoff_servo(float pitch); void update_pitch_onoff_servo(float pitch);
void update_yaw_servo(float yaw); void update_yaw_servo(float yaw);
void update_yaw_position_servo(float yaw); void update_yaw_position_servo(void);
void update_yaw_cr_servo(float yaw); void update_yaw_cr_servo(float yaw);
void update_yaw_onoff_servo(float yaw); void update_yaw_onoff_servo(float yaw);
void init_tracker(); void init_tracker();
@ -252,6 +254,8 @@ private:
void start_logging(); void start_logging();
void log_init(void); void log_init(void);
bool should_log(uint32_t mask); bool should_log(uint32_t mask);
void calc_angle_error(float pitch, float yaw);
void calc_body_frame_target(float pitch, float yaw, float& bf_pitch, float& bf_yaw);
public: public:
void mavlink_snoop(const mavlink_message_t* msg); void mavlink_snoop(const mavlink_message_t* msg);

View File

@ -17,12 +17,44 @@ void Tracker::update_auto(void)
return; return;
} }
float yaw = wrap_180_cd((nav_status.bearing+g.yaw_trim)*100) * 0.01f; float yaw = wrap_180_cd((nav_status.bearing+g.yaw_trim)*100); // target yaw in centidegrees
float pitch = constrain_float(nav_status.pitch+g.pitch_trim, -90, 90); float pitch = constrain_float(nav_status.pitch+g.pitch_trim, -90, 90) * 100; // target pitch in centidegrees
calc_angle_error(pitch, yaw);
float bf_pitch;
float bf_yaw;
calc_body_frame_target(pitch, yaw, bf_pitch, bf_yaw);
// only move servos if target is at least distance_min away // only move servos if target is at least distance_min away
if ((g.distance_min <= 0) || (nav_status.distance >= g.distance_min)) { if ((g.distance_min <= 0) || (nav_status.distance >= g.distance_min)) {
update_pitch_servo(pitch); update_pitch_servo(bf_pitch);
update_yaw_servo(yaw); update_yaw_servo(bf_yaw);
} }
} }
void Tracker::calc_angle_error( float pitch, float yaw)
{
// Pitch angle error in centidegrees
// Positive error means the target is above current pitch
// Negative error means the target is below current pitch
float ahrs_pitch = ahrs.pitch_sensor;
nav_status.angle_error_pitch = pitch - ahrs_pitch;
// Yaw angle error in centidegrees
// Positive error means the target is right of current yaw
// Negative error means the target is left of current yaw
int32_t ahrs_yaw_cd = wrap_180_cd(ahrs.yaw_sensor);
nav_status.angle_error_yaw = wrap_180_cd(yaw - ahrs_yaw_cd);
// earth frame to body frame angle error conversion
float bf_pitch_err = ahrs.cos_roll() * nav_status.angle_error_pitch + ahrs.sin_roll() * ahrs.cos_pitch() * nav_status.angle_error_yaw;
float bf_yaw_err = -ahrs.sin_roll() * nav_status.angle_error_pitch + ahrs.cos_pitch() * ahrs.cos_roll() * nav_status.angle_error_yaw;
nav_status.angle_error_pitch = bf_pitch_err;
nav_status.angle_error_yaw = bf_yaw_err;
}
void Tracker::calc_body_frame_target(float pitch, float yaw, float& bf_pitch, float& bf_yaw)
{
// earth frame to body frame pitch and yaw conversion
bf_pitch = ahrs.cos_roll() * pitch + ahrs.sin_roll() * ahrs.cos_pitch() * yaw;
bf_yaw = -ahrs.sin_roll() * pitch + ahrs.cos_pitch() * ahrs.cos_roll() * yaw;
}

View File

@ -39,7 +39,7 @@ void Tracker::update_pitch_servo(float pitch)
case SERVO_TYPE_POSITION: case SERVO_TYPE_POSITION:
default: default:
update_pitch_position_servo(pitch); update_pitch_position_servo();
break; break;
} }
@ -52,13 +52,8 @@ void Tracker::update_pitch_servo(float pitch)
update the pitch (elevation) servo. The aim is to drive the boards ahrs pitch to the update the pitch (elevation) servo. The aim is to drive the boards ahrs pitch to the
requested pitch, so the board (and therefore the antenna) will be pointing at the target requested pitch, so the board (and therefore the antenna) will be pointing at the target
*/ */
void Tracker::update_pitch_position_servo(float pitch) void Tracker::update_pitch_position_servo()
{ {
// degrees(ahrs.pitch) is -90 to 90, where 0 is horizontal
// pitch argument is -90 to 90, where 0 is horizontal
// servo_out is in 100ths of a degree
float ahrs_pitch = ahrs.pitch_sensor*0.01f;
int32_t angle_err = -(ahrs_pitch - pitch) * 100.0f;
int32_t pitch_min_cd = g.pitch_min*100; int32_t pitch_min_cd = g.pitch_min*100;
int32_t pitch_max_cd = g.pitch_max*100; int32_t pitch_max_cd = g.pitch_max*100;
// Need to configure your servo so that increasing servo_out causes increase in pitch/elevation (ie pointing higher into the sky, // Need to configure your servo so that increasing servo_out causes increase in pitch/elevation (ie pointing higher into the sky,
@ -79,7 +74,7 @@ void Tracker::update_pitch_position_servo(float pitch)
// PITCH2SRV_IMAX 4000.000000 // PITCH2SRV_IMAX 4000.000000
// calculate new servo position // calculate new servo position
g.pidPitch2Srv.set_input_filter_all(angle_err); g.pidPitch2Srv.set_input_filter_all(nav_status.angle_error_pitch);
int32_t new_servo_out = channel_pitch.get_servo_out() + g.pidPitch2Srv.get_pid(); int32_t new_servo_out = channel_pitch.get_servo_out() + g.pidPitch2Srv.get_pid();
// rate limit pitch servo // rate limit pitch servo
@ -115,24 +110,19 @@ void Tracker::update_pitch_position_servo(float pitch)
*/ */
void Tracker::update_pitch_onoff_servo(float pitch) void Tracker::update_pitch_onoff_servo(float pitch)
{ {
// degrees(ahrs.pitch) is -90 to 90, where 0 is horizontal
// pitch argument is -90 to 90, where 0 is horizontal
// get_servo_out() is in 100ths of a degree
float ahrs_pitch = degrees(ahrs.pitch);
float err = ahrs_pitch - pitch;
int32_t pitch_min_cd = g.pitch_min*100; int32_t pitch_min_cd = g.pitch_min*100;
int32_t pitch_max_cd = g.pitch_max*100; int32_t pitch_max_cd = g.pitch_max*100;
float acceptable_error = g.onoff_pitch_rate * g.onoff_pitch_mintime; float acceptable_error = g.onoff_pitch_rate * g.onoff_pitch_mintime;
if (fabsf(err) < acceptable_error) { if (fabsf(nav_status.angle_error_pitch) < acceptable_error) {
channel_pitch.set_servo_out(0); channel_pitch.set_servo_out(0);
} else if ((err > 0) && (pitch*100>pitch_min_cd)) { } else if ((nav_status.angle_error_pitch > 0) && (pitch*100>pitch_min_cd)) {
// positive error means we are pointing too high, so push the // positive error means we are pointing too low, so push the
// servo down // servo up
channel_pitch.set_servo_out(-9000); channel_pitch.set_servo_out(-9000);
} else if (pitch*100<pitch_max_cd){ } else if (pitch*100<pitch_max_cd){
// negative error means we are pointing too low, so push the // negative error means we are pointing too high, so push the
// servo up // servo down
channel_pitch.set_servo_out(9000); channel_pitch.set_servo_out(9000);
} }
} }
@ -142,12 +132,10 @@ void Tracker::update_pitch_onoff_servo(float pitch)
*/ */
void Tracker::update_pitch_cr_servo(float pitch) void Tracker::update_pitch_cr_servo(float pitch)
{ {
float ahrs_pitch = degrees(ahrs.pitch);
float err_cd = (pitch - ahrs_pitch) * 100.0f;
int32_t pitch_min_cd = g.pitch_min*100; int32_t pitch_min_cd = g.pitch_min*100;
int32_t pitch_max_cd = g.pitch_max*100; int32_t pitch_max_cd = g.pitch_max*100;
if ((pitch>pitch_min_cd) && (pitch<pitch_max_cd)){ if ((pitch>pitch_min_cd) && (pitch<pitch_max_cd)){
g.pidPitch2Srv.set_input_filter_all(err_cd); g.pidPitch2Srv.set_input_filter_all(nav_status.angle_error_pitch);
channel_pitch.set_servo_out(g.pidPitch2Srv.get_pid()); channel_pitch.set_servo_out(g.pidPitch2Srv.get_pid());
} }
} }
@ -168,7 +156,7 @@ void Tracker::update_yaw_servo(float yaw)
case SERVO_TYPE_POSITION: case SERVO_TYPE_POSITION:
default: default:
update_yaw_position_servo(yaw); update_yaw_position_servo();
break; break;
} }
@ -182,10 +170,8 @@ void Tracker::update_yaw_servo(float yaw)
yaw to the requested yaw, so the board (and therefore the antenna) yaw to the requested yaw, so the board (and therefore the antenna)
will be pointing at the target will be pointing at the target
*/ */
void Tracker::update_yaw_position_servo(float yaw) void Tracker::update_yaw_position_servo()
{ {
int32_t ahrs_yaw_cd = wrap_180_cd(ahrs.yaw_sensor);
int32_t yaw_cd = wrap_180_cd(yaw*100);
int32_t yaw_limit_cd = g.yaw_range*100/2; int32_t yaw_limit_cd = g.yaw_range*100/2;
const int16_t margin = MAX(500, wrap_360_cd(36000-yaw_limit_cd)/2); const int16_t margin = MAX(500, wrap_360_cd(36000-yaw_limit_cd)/2);
@ -209,11 +195,10 @@ void Tracker::update_yaw_position_servo(float yaw)
// param set RC1_MIN 680 // param set RC1_MIN 680
// According to the specs at https://www.servocity.com/html/hs-645mg_ultra_torque.html, // According to the specs at https://www.servocity.com/html/hs-645mg_ultra_torque.html,
// that should be 600 through 2400, but the azimuth gearing in my antenna pointer is not exactly 2:1 // that should be 600 through 2400, but the azimuth gearing in my antenna pointer is not exactly 2:1
int32_t angle_err = wrap_180_cd(ahrs_yaw_cd - yaw_cd);
/* /*
a positive error means that we need to rotate counter-clockwise a positive error means that we need to rotate clockwise
a negative error means that we need to rotate clockwise a negative error means that we need to rotate counter-clockwise
Use our current yawspeed to determine if we are moving in the Use our current yawspeed to determine if we are moving in the
right direction right direction
@ -228,12 +213,12 @@ void Tracker::update_yaw_position_servo(float yaw)
if (slew_dir != 0) { if (slew_dir != 0) {
making_progress = (-slew_dir * earth_rotation.z >= 0); making_progress = (-slew_dir * earth_rotation.z >= 0);
} else { } else {
making_progress = (angle_err * earth_rotation.z >= 0); making_progress = (nav_status.angle_error_yaw * earth_rotation.z <= 0);
} }
// handle hitting servo limits // handle hitting servo limits
if (abs(channel_yaw.get_servo_out()) == 18000 && if (abs(channel_yaw.get_servo_out()) == 18000 &&
labs(angle_err) > margin && labs(nav_status.angle_error_yaw) > margin &&
making_progress && making_progress &&
AP_HAL::millis() - slew_start_ms > g.min_reverse_time*1000) { AP_HAL::millis() - slew_start_ms > g.min_reverse_time*1000) {
// we are at the limit of the servo and are not moving in the // we are at the limit of the servo and are not moving in the
@ -246,14 +231,14 @@ void Tracker::update_yaw_position_servo(float yaw)
stop slewing and revert to normal control when normal control stop slewing and revert to normal control when normal control
should move us in the right direction should move us in the right direction
*/ */
if (slew_dir * angle_err < -margin) { if (slew_dir * nav_status.angle_error_yaw > margin) {
new_slew_dir = 0; new_slew_dir = 0;
} }
if (new_slew_dir != slew_dir) { if (new_slew_dir != slew_dir) {
tracker.gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Slew: %d/%d err=%ld servo=%ld ez=%.3f", tracker.gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Slew: %d/%d err=%ld servo=%ld ez=%.3f",
(int)slew_dir, (int)new_slew_dir, (int)slew_dir, (int)new_slew_dir,
(long)angle_err, (long)nav_status.angle_error_yaw,
(long)channel_yaw.get_servo_out(), (long)channel_yaw.get_servo_out(),
(double)degrees(ahrs.get_gyro().z)); (double)degrees(ahrs.get_gyro().z));
} }
@ -264,10 +249,10 @@ void Tracker::update_yaw_position_servo(float yaw)
new_servo_out = slew_dir * 18000; new_servo_out = slew_dir * 18000;
g.pidYaw2Srv.reset_I(); g.pidYaw2Srv.reset_I();
} else { } else {
g.pidYaw2Srv.set_input_filter_all(angle_err); g.pidYaw2Srv.set_input_filter_all(nav_status.angle_error_yaw);
float servo_change = g.pidYaw2Srv.get_pid(); float servo_change = g.pidYaw2Srv.get_pid();
servo_change = constrain_float(servo_change, -18000, 18000); servo_change = constrain_float(servo_change, -18000, 18000);
new_servo_out = constrain_float(channel_yaw.get_servo_out() - servo_change, -18000, 18000); new_servo_out = constrain_float(channel_yaw.get_servo_out() + servo_change, -18000, 18000);
} }
// rate limit yaw servo // rate limit yaw servo
@ -304,22 +289,17 @@ void Tracker::update_yaw_position_servo(float yaw)
*/ */
void Tracker::update_yaw_onoff_servo(float yaw) void Tracker::update_yaw_onoff_servo(float yaw)
{ {
int32_t ahrs_yaw_cd = wrap_180_cd(ahrs.yaw_sensor);
int32_t yaw_cd = wrap_180_cd(yaw*100);
int32_t err_cd = wrap_180_cd(ahrs_yaw_cd - yaw_cd);
float err = err_cd * 0.01f;
float acceptable_error = g.onoff_yaw_rate * g.onoff_yaw_mintime; float acceptable_error = g.onoff_yaw_rate * g.onoff_yaw_mintime;
if (fabsf(err) < acceptable_error) { if (fabsf(nav_status.angle_error_yaw * 0.01f) < acceptable_error) {
channel_yaw.set_servo_out(0); channel_yaw.set_servo_out(0);
} else if (err > 0) { } else if (nav_status.angle_error_yaw * 0.01f > 0) {
// positive error means we are clockwise of the target, so // positive error means we are counter-clockwise of the target, so
// move anti-clockwise
channel_yaw.set_servo_out(-18000);
} else {
// negative error means we are anti-clockwise of the target, so
// move clockwise // move clockwise
channel_yaw.set_servo_out(18000); channel_yaw.set_servo_out(18000);
} else {
// negative error means we are clockwise of the target, so
// move counter-clockwise
channel_yaw.set_servo_out(-18000);
} }
} }
@ -328,10 +308,6 @@ void Tracker::update_yaw_onoff_servo(float yaw)
*/ */
void Tracker::update_yaw_cr_servo(float yaw) void Tracker::update_yaw_cr_servo(float yaw)
{ {
int32_t ahrs_yaw_cd = wrap_180_cd(ahrs.yaw_sensor); g.pidYaw2Srv.set_input_filter_all(nav_status.angle_error_yaw);
float yaw_cd = wrap_180_cd(yaw*100.0f); channel_yaw.set_servo_out(-g.pidYaw2Srv.get_pid());
float err_cd = wrap_180_cd(yaw_cd - (float)ahrs_yaw_cd);
g.pidYaw2Srv.set_input_filter_all(err_cd);
channel_yaw.set_servo_out(g.pidYaw2Srv.get_pid());
} }