diff --git a/libraries/AP_Rally/AP_Rally.cpp b/libraries/AP_Rally/AP_Rally.cpp index afb5f2d0ad..21d6a4ffb5 100644 --- a/libraries/AP_Rally/AP_Rally.cpp +++ b/libraries/AP_Rally/AP_Rally.cpp @@ -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 ¤t_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 ¤t_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; diff --git a/libraries/AP_Rally/AP_Rally.h b/libraries/AP_Rally/AP_Rally.h index a1646ed947..6491788d17 100644 --- a/libraries/AP_Rally/AP_Rally.h +++ b/libraries/AP_Rally/AP_Rally.h @@ -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 ¤t_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