AP_SmartRTL: use ahrs singleton

This commit is contained in:
Peter Barker 2018-03-31 23:31:51 +11:00 committed by Francisco Ferreira
parent e33ce5eb48
commit 3781d2f2eb
3 changed files with 5 additions and 9 deletions

View File

@ -72,8 +72,7 @@ const AP_Param::GroupInfo AP_SmartRTL::var_info[] = {
* seconds before initiating the return journey.
*/
AP_SmartRTL::AP_SmartRTL(const AP_AHRS& ahrs, bool example_mode) :
_ahrs(ahrs),
AP_SmartRTL::AP_SmartRTL(bool example_mode) :
_example_mode(example_mode)
{
AP_Param::setup_object_defaults(this, var_info);
@ -170,7 +169,7 @@ bool AP_SmartRTL::pop_point(Vector3f& point)
void AP_SmartRTL::set_home(bool position_ok)
{
Vector3f current_pos;
position_ok &= _ahrs.get_relative_position_NED_origin(current_pos);
position_ok &= AP::ahrs().get_relative_position_NED_origin(current_pos);
set_home(position_ok, current_pos);
}
@ -217,7 +216,7 @@ void AP_SmartRTL::update(bool position_ok, bool save_position)
}
Vector3f current_pos;
position_ok &= _ahrs.get_relative_position_NED_origin(current_pos);
position_ok &= AP::ahrs().get_relative_position_NED_origin(current_pos);
update(position_ok, current_pos);
}

View File

@ -30,7 +30,7 @@ class AP_SmartRTL {
public:
// constructor, destructor
AP_SmartRTL(const AP_AHRS& ahrs, bool example_mode = false);
AP_SmartRTL(bool example_mode = false);
// initialise safe rtl including setting up background processes
void init();
@ -165,9 +165,6 @@ private:
// logging
void log_action(SRTL_Actions action, const Vector3f &point = Vector3f());
// external references
const AP_AHRS& _ahrs;
// parameters
AP_Float _accuracy;
AP_Int16 _points_max;

View File

@ -29,7 +29,7 @@ public:
static DummyVehicle vehicle;
AP_AHRS_NavEKF &ahrs(vehicle.ahrs);
AP_SmartRTL smart_rtl{ahrs, true};
AP_SmartRTL smart_rtl{true};
AP_BoardConfig board_config;
void setup();