Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 18:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

Along track fix for negative.

Browse Source
This commit is contained in:
James Goppert 2011-12-04 01:36:53 -05:00
parent 28b835713a
commit fbbf1aa4a2
1 changed files with 5 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/APO/AP_MavlinkCommand.cpp
View File

@ -196,10 +196,13 @@ float AP_MavlinkCommand::crossTrack(const AP_MavlinkCommand & previous,
// calculates along track distance of a current location
float AP_MavlinkCommand::alongTrack(const AP_MavlinkCommand & previous,
int32_t lat_degInt, int32_t lon_degInt) const {
// ignores lat/lon since single prec.
float dXt = crossTrack(previous,lat_degInt,lon_degInt);
float d = previous.distanceTo(getLat_degInt(),getLon_degInt());
return acos(cos(d/rEarth)/cos(dXt/rEarth))*rEarth;
float bCurrent = previous.bearingTo(lat_degInt, lon_degInt);
float bNext = previous.bearingTo(*this);
float y = acos(cos(d/rEarth)/cos(dXt/rEarth))*rEarth;
if (cos(bCurrent-bNext) < 0) y = -y;
return y;
}