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

Tracker: roll compensated slewing

This commit is contained in:
stefanlynka 2016-07-28 11:19:54 +09:00 committed by Randy Mackay
parent 16c9023cf0
commit 33abb619b9
4 changed files with 130 additions and 105 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
AntennaTracker/Tracker.h
View File

@ -132,6 +132,12 @@ private:
RC_Channel channel_yaw{CH_YAW};
RC_Channel channel_pitch{CH_PITCH};
LowPassFilterFloat yaw_servo_out_filt;
LowPassFilterFloat pitch_servo_out_filt;
bool yaw_servo_out_filt_init = false;
bool pitch_servo_out_filt_init = false;
AP_SerialManager serial_manager;
const uint8_t num_gcs = MAVLINK_COMM_NUM_BUFFERS;
GCS_MAVLINK_Tracker gcs[MAVLINK_COMM_NUM_BUFFERS];
@ -175,8 +181,6 @@ private:
uint8_t one_second_counter = 0;
bool target_set = false;
int8_t slew_dir = 0;
uint32_t slew_start_ms = 0;
// use this to prevent recursion during sensor init
bool in_mavlink_delay = false;
@ -254,8 +258,10 @@ private:
void start_logging();
void log_init(void);
bool should_log(uint32_t mask);
void calc_angle_error(float pitch, float yaw);
void calc_angle_error(float pitch, float yaw, bool direction_reversed);
void calc_body_frame_target(float pitch, float yaw, float& bf_pitch, float& bf_yaw);
bool convert_bf_to_ef(float pitch, float yaw, float& ef_pitch, float& ef_yaw);
bool get_ef_yaw_direction();
public:
void mavlink_snoop(const mavlink_message_t* msg);

90
AntennaTracker/control_auto.cpp
View File

@ -20,7 +20,9 @@ void Tracker::update_auto(void)
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);
bool direction_reversed = get_ef_yaw_direction();
calc_angle_error(pitch, yaw, direction_reversed);
float bf_pitch;
float bf_yaw;
@ -32,23 +34,31 @@ void Tracker::update_auto(void)
update_yaw_servo(bf_yaw);
}
}
void Tracker::calc_angle_error( float pitch, float yaw)
void Tracker::calc_angle_error(float pitch, float yaw, bool direction_reversed)
{
// Pitch angle error in centidegrees
// Positive error means the target is above current pitch
// 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;
int32_t ef_pitch_angle_error = pitch - ahrs_pitch;
// Yaw angle error in centidegrees
// Positive error means the target is right of current yaw
// 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);
int32_t ef_yaw_angle_error = wrap_180_cd(yaw - ahrs_yaw_cd);
if (direction_reversed) {
if (ef_yaw_angle_error > 0) {
ef_yaw_angle_error = (yaw - ahrs_yaw_cd) - 36000;
} else {
ef_yaw_angle_error = 36000 + (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;
float bf_pitch_err;
float bf_yaw_err;
calc_body_frame_target(ef_pitch_angle_error, ef_yaw_angle_error, bf_pitch_err, bf_yaw_err);
nav_status.angle_error_pitch = bf_pitch_err;
nav_status.angle_error_yaw = bf_yaw_err;
}
@ -58,3 +68,65 @@ void Tracker::calc_body_frame_target(float pitch, float yaw, float& bf_pitch, fl
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;
}
bool Tracker::convert_bf_to_ef(float pitch, float yaw, float& ef_pitch, float& ef_yaw)
{
// avoid divide by zero
if (is_zero(ahrs.cos_pitch())) {
return false;
}
// convert earth frame angle or rates to body frame
ef_pitch = ahrs.cos_roll() * pitch - ahrs.sin_roll() * yaw; //pitch
ef_yaw = (ahrs.sin_roll() / ahrs.cos_pitch()) * pitch + (ahrs.cos_roll() / ahrs.cos_pitch()) * yaw; //yaw
return true;
}
// return value is true if takign the long road to the target, false if normal, shortest direction should be used
bool Tracker::get_ef_yaw_direction()
{
// calculating distances from current pitch/yaw to lower and upper limits in centi-degrees
float yaw_angle_limit_lower = (-g.yaw_range * 100.0f / 2.0f) - yaw_servo_out_filt.get();
float yaw_angle_limit_upper = (g.yaw_range * 100.0f / 2.0f) - yaw_servo_out_filt.get();
float pitch_angle_limit_lower = (g.pitch_min * 100.0f) - pitch_servo_out_filt.get();
float pitch_angle_limit_upper = (g.pitch_max * 100.0f) - pitch_servo_out_filt.get();
// distances to earthframe angle limits in centi-degrees
float ef_yaw_limit_lower = yaw_angle_limit_lower;
float ef_yaw_limit_upper = yaw_angle_limit_upper;
float ef_pitch_limit_lower = pitch_angle_limit_lower;
float ef_pitch_limit_upper = pitch_angle_limit_upper;
convert_bf_to_ef(pitch_angle_limit_lower, yaw_angle_limit_lower, ef_pitch_limit_lower, ef_yaw_limit_lower);
convert_bf_to_ef(pitch_angle_limit_upper, yaw_angle_limit_upper, ef_pitch_limit_upper, ef_yaw_limit_upper);
float ef_yaw_current = wrap_180_cd(ahrs.yaw_sensor);
float ef_yaw_target = wrap_180_cd((nav_status.bearing+g.yaw_trim)*100);
float ef_yaw_angle_error = wrap_180_cd(ef_yaw_target - ef_yaw_current);
// calculate angle error to target in both directions (i.e. moving up/right or lower/left)
float yaw_angle_error_upper;
float yaw_angle_error_lower;
if (ef_yaw_angle_error >= 0) {
yaw_angle_error_upper = ef_yaw_angle_error;
yaw_angle_error_lower = ef_yaw_angle_error - 36000;
} else {
yaw_angle_error_upper = 36000 + ef_yaw_angle_error;
yaw_angle_error_lower = ef_yaw_angle_error;
}
// checks that the vehicle is outside the tracker's range
if ((yaw_angle_error_lower < ef_yaw_limit_lower) && (yaw_angle_error_upper > ef_yaw_limit_upper)) {
// if the tracker is trying to move clockwise to reach the vehicle,
// but the tracker coudl get closer to the vehicle by moving counter-clockwise then set direction_reversed to true
if (ef_yaw_angle_error>0 && ((ef_yaw_limit_lower - yaw_angle_error_lower) < (yaw_angle_error_upper - ef_yaw_limit_upper))) {
return true;
}
// if the tracker is trying to move counter-clockwise to reach the vehicle,
// but the tracker coudl get closer to the vehicle by moving then set direction_reversed to true
if (ef_yaw_angle_error<0 && ((ef_yaw_limit_lower - yaw_angle_error_lower) > (yaw_angle_error_upper - ef_yaw_limit_upper))) {
return true;
}
}
// if we get this far we can use the regular, shortest path to the target
return false;
}

4
AntennaTracker/defines.h
View File

@ -30,6 +30,10 @@ enum AltSource {
ALT_SOURCE_GPS_VEH_ONLY=2
};
// Filter
#define SERVO_OUT_FILT_HZ 0.1f
#define G_Dt 0.02f
// Logging parameters
#define MASK_LOG_ATTITUDE (1<<0)
#define MASK_LOG_GPS (1<<1)

129
AntennaTracker/servos.cpp
View File

@ -20,6 +20,9 @@ void Tracker::init_servos()
// initialise output to servos
channel_yaw.calc_pwm();
channel_pitch.calc_pwm();
yaw_servo_out_filt.set_cutoff_frequency(SERVO_OUT_FILT_HZ);
pitch_servo_out_filt.set_cutoff_frequency(SERVO_OUT_FILT_HZ);
}
/**
@ -77,29 +80,23 @@ void Tracker::update_pitch_position_servo()
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
if (g.pitch_slew_time > 0.02f) {
uint16_t max_change = 0.02f * ((-pitch_min_cd+pitch_max_cd)/2) / g.pitch_slew_time;
if (max_change < 1) {
max_change = 1;
}
if (new_servo_out <= channel_pitch.get_servo_out() - max_change) {
new_servo_out = channel_pitch.get_servo_out() - max_change;
}
if (new_servo_out >= channel_pitch.get_servo_out() + max_change) {
new_servo_out = channel_pitch.get_servo_out() + max_change;
}
// position limit pitch servo
if (new_servo_out <= pitch_min_cd) {
new_servo_out = pitch_min_cd;
g.pidPitch2Srv.reset_I();
}
if (new_servo_out >= pitch_max_cd) {
new_servo_out = pitch_max_cd;
g.pidPitch2Srv.reset_I();
}
// rate limit pitch servo
channel_pitch.set_servo_out(new_servo_out);
// position limit pitch servo
if (channel_pitch.get_servo_out() <= pitch_min_cd) {
channel_pitch.set_servo_out(pitch_min_cd);
g.pidPitch2Srv.reset_I();
}
if (channel_pitch.get_servo_out() >= pitch_max_cd) {
channel_pitch.set_servo_out(pitch_max_cd);
g.pidPitch2Srv.reset_I();
if (pitch_servo_out_filt_init) {
pitch_servo_out_filt.apply(new_servo_out, G_Dt);
} else {
pitch_servo_out_filt.reset(new_servo_out);
pitch_servo_out_filt_init = true;
}
}
@ -120,7 +117,7 @@ void Tracker::update_pitch_onoff_servo(float pitch)
// 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){
} else if (pitch*100<pitch_max_cd) {
// negative error means we are pointing too high, so push the
// servo down
channel_pitch.set_servo_out(9000);
@ -134,7 +131,7 @@ void Tracker::update_pitch_cr_servo(float pitch)
{
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)){
if ((pitch>pitch_min_cd) && (pitch<pitch_max_cd)) {
g.pidPitch2Srv.set_input_filter_all(nav_status.angle_error_pitch);
channel_pitch.set_servo_out(g.pidPitch2Srv.get_pid());
}
@ -173,7 +170,6 @@ void Tracker::update_yaw_servo(float yaw)
void Tracker::update_yaw_position_servo()
{
int32_t yaw_limit_cd = g.yaw_range*100/2;
const int16_t margin = MAX(500, wrap_360_cd(36000-yaw_limit_cd)/2);
// Antenna as Ballerina. Use with antenna that do not have continuously rotating servos, ie at some point in rotation
// the servo limits are reached and the servo has to slew 360 degrees to the 'other side' to keep tracking.
@ -181,7 +177,6 @@ void Tracker::update_yaw_position_servo()
// This algorithm accounts for the fact that the antenna mount may not be aligned with North
// (in fact, any alignment is permissible), and that the alignment may change (possibly rapidly) over time
// (as when the antenna is mounted on a moving, turning vehicle)
// When the servo is being forced beyond its limits, it rapidly slews to the 'other side' then normal tracking takes over.
//
// With my antenna mount, large pwm output drives the antenna anticlockise, so need:
// param set RC1_REV -1
@ -203,82 +198,30 @@ void Tracker::update_yaw_position_servo()
Use our current yawspeed to determine if we are moving in the
right direction
*/
int8_t new_slew_dir = slew_dir;
// get earth frame z-axis rotation rate in radians
Vector3f earth_rotation = ahrs.get_gyro() * ahrs.get_rotation_body_to_ned();
bool making_progress;
if (slew_dir != 0) {
making_progress = (-slew_dir * earth_rotation.z >= 0);
} else {
making_progress = (nav_status.angle_error_yaw * earth_rotation.z <= 0);
}
// handle hitting servo limits
if (abs(channel_yaw.get_servo_out()) == 18000 &&
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
// right direction, so slew the other way
new_slew_dir = -channel_yaw.get_servo_out() / 18000;
slew_start_ms = AP_HAL::millis();
}
/*
stop slewing and revert to normal control when normal control
should move us in the right direction
*/
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)nav_status.angle_error_yaw,
(long)channel_yaw.get_servo_out(),
(double)degrees(ahrs.get_gyro().z));
}
slew_dir = new_slew_dir;
int16_t new_servo_out;
if (slew_dir != 0) {
new_servo_out = slew_dir * 18000;
g.pidYaw2Srv.reset_I();
} else {
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);
}
// rate limit yaw servo
if (g.yaw_slew_time > 0.02f) {
uint16_t max_change = 0.02f * yaw_limit_cd / g.yaw_slew_time;
if (max_change < 1) {
max_change = 1;
}
if (new_servo_out <= channel_yaw.get_servo_out() - max_change) {
new_servo_out = channel_yaw.get_servo_out() - max_change;
}
if (new_servo_out >= channel_yaw.get_servo_out() + max_change) {
new_servo_out = channel_yaw.get_servo_out() + max_change;
}
}
channel_yaw.set_servo_out(new_servo_out);
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);
float new_servo_out = constrain_float(channel_yaw.get_servo_out() + servo_change, -18000, 18000);
// position limit yaw servo
if (channel_yaw.get_servo_out() <= -yaw_limit_cd) {
channel_yaw.set_servo_out(-yaw_limit_cd);
if (new_servo_out <= -yaw_limit_cd) {
new_servo_out = -yaw_limit_cd;
g.pidYaw2Srv.reset_I();
}
if (channel_yaw.get_servo_out() >= yaw_limit_cd) {
channel_yaw.set_servo_out(yaw_limit_cd);
if (new_servo_out >= yaw_limit_cd) {
new_servo_out = yaw_limit_cd;
g.pidYaw2Srv.reset_I();
}
channel_yaw.set_servo_out(new_servo_out);
if(yaw_servo_out_filt_init){
yaw_servo_out_filt.apply(new_servo_out, G_Dt);
} else {
yaw_servo_out_filt.reset(new_servo_out);
yaw_servo_out_filt_init = true;
}
}