Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-05 07:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Rally: Added one getter method and made a utility method public.

Browse Source
This commit is contained in:
Michael Day 2014-04-21 14:30:06 -07:00 committed by Andrew Tridgell
parent 0e065e4894
commit 96173bfb8a
2 changed files with 8 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_Rally/AP_Rally.cpp
View File

@ -80,7 +80,7 @@ bool AP_Rally::set_rally_point_with_index(uint8_t i, const RallyLocation &rallyL
}
// helper function to translate a RallyLocation to a Location
Location AP_Rally::rally_location_to_location(const RallyLocation &rally_loc, const Location &home_loc) const
Location AP_Rally::rally_location_to_location(const RallyLocation &rally_loc) const
{
Location ret = {};
@ -90,7 +90,7 @@ Location AP_Rally::rally_location_to_location(const RallyLocation &rally_loc, co
//Currently can't do true AGL on the APM. Relative altitudes are
//relative to HOME point's altitude. Terrain on the board is inbound
//for the PX4, though. This line will need to be updated when that happens:
ret.alt = (rally_loc.alt*100UL) + home_loc.alt;
ret.alt = (rally_loc.alt*100UL) + _ahrs.get_home().alt;
ret.lat = rally_loc.lat;
ret.lng = rally_loc.lng;
@ -109,7 +109,7 @@ bool AP_Rally::find_nearest_rally_point(const Location &current_loc, RallyLocati
if (!get_rally_point_with_index(i, next_rally)) {
continue;
}
Location rally_loc = rally_location_to_location(next_rally, home_loc);
Location rally_loc = rally_location_to_location(next_rally);
float dis = get_distance(current_loc, rally_loc);
if (dis < min_dis || min_dis < 0) {
@ -134,7 +134,7 @@ Location AP_Rally::calc_best_rally_or_home_location(const Location &current_loc,
if (find_nearest_rally_point(current_loc, ral_loc)) {
// valid rally point found
return_loc = rally_location_to_location(ral_loc, home_loc);
return_loc = rally_location_to_location(ral_loc);
} else {
// no valid rally point, return home position
return_loc = home_loc;

7
libraries/AP_Rally/AP_Rally.h
View File

@ -46,6 +46,10 @@ public:
bool set_rally_point_with_index(uint8_t i, const RallyLocation &rallyLoc);
uint8_t get_rally_total() const { return _rally_point_total_count; }
float get_rally_limit_km() const { return _rally_limit_km; }
Location rally_location_to_location(const RallyLocation &ret) const;
// logic handling
Location calc_best_rally_or_home_location(const Location &current_loc, float rtl_home_alt) const;
bool find_nearest_rally_point(const Location &myloc, RallyLocation &ret) const;
@ -53,9 +57,6 @@ public:
// parameter block
static const struct AP_Param::GroupInfo var_info[];
private:
Location rally_location_to_location(const RallyLocation &rally_loc, const Location &home_loc) const;
private:
// internal variables
const AP_AHRS& _ahrs; // used only for home position