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

Rover: use new angle wrap code

This commit is contained in:
Andrew Tridgell 2013-03-29 13:14:14 +11:00
parent a165f0d138
commit 3a1e9e43a1
2 changed files with 4 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
APMrover2/navigation.pde
View File

@ -42,32 +42,18 @@ static void calc_bearing_error()
{
static butter10hz1_6 butter;
bearing_error_cd = wrap_180(nav_bearing - ahrs.yaw_sensor);
bearing_error_cd = wrap_180_cd(nav_bearing - ahrs.yaw_sensor);
bearing_error_cd = butter.filter(bearing_error_cd);
}
static long wrap_360(long error)
{
if (error > 36000) error -= 36000;
if (error < 0) error += 36000;
return error;
}
static long wrap_180(long error)
{
if (error > 18000) error -= 36000;
if (error < -18000) error += 36000;
return error;
}
static void update_crosstrack(void)
{
// Crosstrack Error
// ----------------
if (abs(wrap_180(target_bearing - crosstrack_bearing)) < 4500) { // If we are too far off or too close we don't do track following
if (abs(wrap_180_cd(target_bearing - crosstrack_bearing)) < 4500) { // If we are too far off or too close we don't do track following
crosstrack_error = sinf(radians((target_bearing - crosstrack_bearing) / (float)100)) * wp_distance; // Meters we are off track line
nav_bearing += constrain(crosstrack_error * g.crosstrack_gain, -g.crosstrack_entry_angle.get(), g.crosstrack_entry_angle.get());
nav_bearing = wrap_360(nav_bearing);
nav_bearing = wrap_360_cd(nav_bearing);
}
}

2
APMrover2/test.pde
View File

@ -517,7 +517,7 @@ test_mag(uint8_t argc, const Menu::arg *argv)
if (compass.healthy) {
Vector3f maggy = compass.get_offsets();
cliSerial->printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d,\tXYZoff: %6.2f, %6.2f, %6.2f\n"),
(wrap_360(ToDeg(heading) * 100)) /100,
(wrap_360_cd(ToDeg(heading) * 100)) /100,
(int)compass.mag_x,
(int)compass.mag_y,
(int)compass.mag_z,