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

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
AntennaTracker/Tracker.h
View File

@ -158,6 +158,8 @@ private:
float bearing; // bearing to vehicle in centi-degrees
float distance; // distance to vehicle in meters
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_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
@ -166,7 +168,7 @@ private:
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_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
AP_Param param_loader{var_info};
@ -215,11 +217,11 @@ private:
void update_GPS(void);
void init_servos();
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_onoff_servo(float pitch);
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_onoff_servo(float yaw);
void init_tracker();
@ -252,6 +254,8 @@ private:
void start_logging();
void log_init(void);
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:
void mavlink_snoop(const mavlink_message_t* msg);

40
AntennaTracker/control_auto.cpp
View File

@ -17,12 +17,44 @@ void Tracker::update_auto(void)
return;
}
float yaw = wrap_180_cd((nav_status.bearing+g.yaw_trim)*100) * 0.01f;
float pitch = constrain_float(nav_status.pitch+g.pitch_trim, -90, 90);
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) * 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
if ((g.distance_min <= 0) || (nav_status.distance >= g.distance_min)) {
update_pitch_servo(pitch);
update_yaw_servo(yaw);
update_pitch_servo(bf_pitch);
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;
}

82
AntennaTracker/servos.cpp
View File

@ -39,7 +39,7 @@ void Tracker::update_pitch_servo(float pitch)
case SERVO_TYPE_POSITION:
default:
update_pitch_position_servo(pitch);
update_pitch_position_servo();
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
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_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,
@ -79,7 +74,7 @@ void Tracker::update_pitch_position_servo(float pitch)
// PITCH2SRV_IMAX 4000.000000
// 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();
// rate limit pitch servo
@ -115,24 +110,19 @@ void Tracker::update_pitch_position_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_max_cd = g.pitch_max*100;
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);
} else if ((err > 0) && (pitch*100>pitch_min_cd)) {
// positive error means we are pointing too high, so push the
// servo down
} else if ((nav_status.angle_error_pitch > 0) && (pitch*100>pitch_min_cd)) {
// positive error means we are pointing too low, so push the
// servo up
channel_pitch.set_servo_out(-9000);
} else if (pitch*100<pitch_max_cd){
// negative error means we are pointing too low, so push the
// servo up
// negative error means we are pointing too high, so push the
// servo down
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)
{
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_max_cd = g.pitch_max*100;
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());
}
}
@ -168,7 +156,7 @@ void Tracker::update_yaw_servo(float yaw)
case SERVO_TYPE_POSITION:
default:
update_yaw_position_servo(yaw);
update_yaw_position_servo();
break;
}
@ -182,10 +170,8 @@ void Tracker::update_yaw_servo(float yaw)
yaw to the requested yaw, so the board (and therefore the antenna)
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;
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
// 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
int32_t angle_err = wrap_180_cd(ahrs_yaw_cd - yaw_cd);
/*
a positive error means that we need to rotate counter-clockwise
a negative error means that we need to rotate clockwise
a positive 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
right direction
@ -228,12 +213,12 @@ void Tracker::update_yaw_position_servo(float yaw)
if (slew_dir != 0) {
making_progress = (-slew_dir * earth_rotation.z >= 0);
} else {
making_progress = (angle_err * earth_rotation.z >= 0);
making_progress = (nav_status.angle_error_yaw * earth_rotation.z <= 0);
}
// handle hitting servo limits
if (abs(channel_yaw.get_servo_out()) == 18000 &&
labs(angle_err) > margin &&
labs(nav_status.angle_error_yaw) > margin &&
making_progress &&
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
@ -246,14 +231,14 @@ void Tracker::update_yaw_position_servo(float yaw)
stop slewing and revert to normal control when normal control
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;
}
if (new_slew_dir != slew_dir) {
tracker.gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Slew: %d/%d err=%ld servo=%ld ez=%.3f",
(int)slew_dir, (int)new_slew_dir,
(long)angle_err,
(long)nav_status.angle_error_yaw,
(long)channel_yaw.get_servo_out(),
(double)degrees(ahrs.get_gyro().z));
}
@ -264,10 +249,10 @@ void Tracker::update_yaw_position_servo(float yaw)
new_servo_out = slew_dir * 18000;
g.pidYaw2Srv.reset_I();
} 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();
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
@ -304,22 +289,17 @@ void Tracker::update_yaw_position_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;
if (fabsf(err) < acceptable_error) {
if (fabsf(nav_status.angle_error_yaw * 0.01f) < acceptable_error) {
channel_yaw.set_servo_out(0);
} else if (err > 0) {
// positive error means we are 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
} else if (nav_status.angle_error_yaw * 0.01f > 0) {
// positive error means we are counter-clockwise of the target, so
// move clockwise
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)
{
int32_t ahrs_yaw_cd = wrap_180_cd(ahrs.yaw_sensor);
float yaw_cd = wrap_180_cd(yaw*100.0f);
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());
g.pidYaw2Srv.set_input_filter_all(nav_status.angle_error_yaw);
channel_yaw.set_servo_out(-g.pidYaw2Srv.get_pid());
}