aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions 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 98a21a70c0
commit 7fae764c55
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;
}