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

Tracker: limit yaw to YAW_RANGE

This commit is contained in:
Randy Mackay 2014-10-06 15:28:49 +09:00
parent e7e0dd3509
commit f4d45c9b6d
1 changed files with 25 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
AntennaTracker/servos.pde
View File

@ -163,6 +163,7 @@ static void update_yaw_position_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 yaw_limit_cd = g.yaw_range*100/2;
const int16_t margin = 500; // 5 degrees slop
// Antenna as Ballerina. Use with antenna that do not have continuously rotating servos, ie at some point in rotation
@ -238,6 +239,10 @@ static void update_yaw_position_servo(float yaw)
slew_dir = new_slew_dir;
// initialise limit flags
servo_limit.yaw_lower = false;
servo_limit.yaw_upper = false;
int16_t new_servo_out;
if (slew_dir != 0) {
new_servo_out = slew_dir * 18000;
@ -248,18 +253,32 @@ static void update_yaw_position_servo(float yaw)
new_servo_out = constrain_float(channel_yaw.servo_out - servo_change, -18000, 18000);
}
// add slew rate limit
// rate limit yaw servo
if (g.yaw_slew_time > 0.02f) {
uint16_t max_change = 0.02f * 100.0f * g.yaw_range / g.yaw_slew_time;
uint16_t max_change = 0.02f * yaw_limit_cd / g.yaw_slew_time;
if (max_change < 1) {
max_change = 1;
}
new_servo_out = constrain_float(new_servo_out,
channel_yaw.servo_out - max_change,
channel_yaw.servo_out + max_change);
if (new_servo_out <= channel_yaw.servo_out - max_change) {
new_servo_out = channel_yaw.servo_out - max_change;
servo_limit.yaw_lower = true;
}
if (new_servo_out >= channel_yaw.servo_out + max_change) {
new_servo_out = channel_yaw.servo_out + max_change;
servo_limit.yaw_upper = true;
}
}
channel_yaw.servo_out = new_servo_out;
// position limit pitch servo
if (channel_yaw.servo_out <= -yaw_limit_cd) {
channel_yaw.servo_out = -yaw_limit_cd;
servo_limit.yaw_lower = true;
}
if (channel_yaw.servo_out >= yaw_limit_cd) {
channel_yaw.servo_out = yaw_limit_cd;
servo_limit.yaw_upper = true;
}
}