mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
AP_Follow: use ahrs singleton
This commit is contained in:
parent
ac99d37bc1
commit
e33ce5eb48
@ -114,8 +114,7 @@ const AP_Param::GroupInfo AP_Follow::var_info[] = {
|
|||||||
constructor is not called until detect() returns true, so we
|
constructor is not called until detect() returns true, so we
|
||||||
already know that we should setup the proximity sensor
|
already know that we should setup the proximity sensor
|
||||||
*/
|
*/
|
||||||
AP_Follow::AP_Follow(const AP_AHRS &ahrs) :
|
AP_Follow::AP_Follow() :
|
||||||
_ahrs(ahrs),
|
|
||||||
_p_pos(AP_FOLLOW_POS_P_DEFAULT)
|
_p_pos(AP_FOLLOW_POS_P_DEFAULT)
|
||||||
{
|
{
|
||||||
AP_Param::setup_object_defaults(this, var_info);
|
AP_Param::setup_object_defaults(this, var_info);
|
||||||
@ -158,7 +157,7 @@ bool AP_Follow::get_target_dist_and_vel_ned(Vector3f &dist_ned, Vector3f &dist_w
|
|||||||
{
|
{
|
||||||
// get our location
|
// get our location
|
||||||
Location current_loc;
|
Location current_loc;
|
||||||
if (!_ahrs.get_position(current_loc)) {
|
if (!AP::ahrs().get_position(current_loc)) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -36,7 +36,7 @@ public:
|
|||||||
};
|
};
|
||||||
|
|
||||||
// constructor
|
// constructor
|
||||||
AP_Follow(const AP_AHRS &ahrs);
|
AP_Follow();
|
||||||
|
|
||||||
// set which target to follow
|
// set which target to follow
|
||||||
void set_target_sysid(uint8_t sysid) { _sysid = sysid; }
|
void set_target_sysid(uint8_t sysid) { _sysid = sysid; }
|
||||||
@ -87,9 +87,6 @@ private:
|
|||||||
// get offsets in meters in NED frame
|
// get offsets in meters in NED frame
|
||||||
bool get_offsets_ned(Vector3f &offsets) const;
|
bool get_offsets_ned(Vector3f &offsets) const;
|
||||||
|
|
||||||
// references
|
|
||||||
const AP_AHRS &_ahrs;
|
|
||||||
|
|
||||||
// parameters
|
// parameters
|
||||||
AP_Int8 _enabled; // 1 if this subsystem is enabled
|
AP_Int8 _enabled; // 1 if this subsystem is enabled
|
||||||
AP_Int16 _sysid; // target's mavlink system id (0 to use first sysid seen)
|
AP_Int16 _sysid; // target's mavlink system id (0 to use first sysid seen)
|
||||||
|
Loading…
Reference in New Issue
Block a user