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

Improved along track calculation for APO.

This commit is contained in:
James Goppert 2011-12-04 01:25:14 -05:00
parent f8fab5f7d6
commit 98a21a70c0
3 changed files with 5 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
libraries/APO/AP_Guide.cpp
View File

@ -170,7 +170,6 @@ void MavlinkGuide::handleCommand() {
// check for along track next waypoint requirement
float alongTrack = _command.alongTrack(_previousCommand,
_command,
_nav->getLat_degInt(),
_nav->getLon_degInt());
float segmentLength = _previousCommand.distanceTo(_command);

12
libraries/APO/AP_MavlinkCommand.cpp
View File

@ -194,16 +194,12 @@ float AP_MavlinkCommand::crossTrack(const AP_MavlinkCommand & previous,
}
// calculates along track distance of a current location
float AP_MavlinkCommand::alongTrack(const AP_MavlinkCommand & previous, const AP_MavlinkCommand & next,
float AP_MavlinkCommand::alongTrack(const AP_MavlinkCommand & previous,
int32_t lat_degInt, int32_t lon_degInt) const {
// ignores lat/lon since single prec.
float t1N = previous.getPN(lat_degInt, lon_degInt);
float t1E = previous.getPE(lat_degInt, lon_degInt);
float t2N = previous.getPN(next.getLat_degInt(), next.getLon_degInt());
float t2E = previous.getPE(next.getLat_degInt(), next.getLon_degInt());
float segmentLength = previous.distanceTo(next);
if (segmentLength == 0) return 0;
return (t1N*t2N + t1E*t2E)/segmentLength;
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;
}

2
libraries/APO/AP_MavlinkCommand.h
View File

@ -370,7 +370,7 @@ public:
float crossTrack(const AP_MavlinkCommand & previous, int32_t lat_degInt, int32_t lon_degInt) const;
// calculates along track distance of a current location
float alongTrack(const AP_MavlinkCommand & previous, const AP_MavlinkCommand & next, int32_t lat_degInt, int32_t lon_degInt) const;
float alongTrack(const AP_MavlinkCommand & previous, int32_t lat_degInt, int32_t lon_degInt) const;
};
} // namespace apo