Skip to content

Commit

Permalink
AP_AHRS: fixed comments on position functions
Browse files Browse the repository at this point in the history
  • Loading branch information
tridge committed Jul 31, 2023
1 parent 3784841 commit e9cb826
Show file tree
Hide file tree
Showing 2 changed files with 21 additions and 15 deletions.
30 changes: 18 additions & 12 deletions libraries/AP_AHRS/AP_AHRS.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -1462,8 +1462,9 @@ bool AP_AHRS::get_hagl(float &height) const
return false;
}

// return a relative ground position to the origin in meters
// North/East/Down order.
/*
return a relative NED position from the origin in meters
*/
bool AP_AHRS::get_relative_position_NED_origin(Vector3f &vec) const
{
switch (active_EKF_type()) {
Expand Down Expand Up @@ -1514,8 +1515,9 @@ bool AP_AHRS::get_relative_position_NED_origin(Vector3f &vec) const
return false;
}

// return a relative ground position to the home in meters
// North/East/Down order.
/*
return a relative ground position from home in meters
*/
bool AP_AHRS::get_relative_position_NED_home(Vector3f &vec) const
{
Location originLLH;
Expand All @@ -1533,8 +1535,9 @@ bool AP_AHRS::get_relative_position_NED_home(Vector3f &vec) const
return true;
}

// write a relative ground position estimate to the origin in meters, North/East order
// return true if estimate is valid
/*
return a relative position estimate from the origin in meters
*/
bool AP_AHRS::get_relative_position_NE_origin(Vector2f &posNE) const
{
switch (active_EKF_type()) {
Expand Down Expand Up @@ -1569,8 +1572,9 @@ bool AP_AHRS::get_relative_position_NE_origin(Vector2f &posNE) const
return false;
}

// return a relative ground position to the home in meters
// North/East order.
/*
return a relative ground position from home in meters North/East
*/
bool AP_AHRS::get_relative_position_NE_home(Vector2f &posNE) const
{
Location originLLH;
Expand All @@ -1590,8 +1594,9 @@ bool AP_AHRS::get_relative_position_NE_home(Vector2f &posNE) const
// write a relative ground position estimate to the origin in meters, North/East order


// write a relative ground position to the origin in meters, Down
// return true if the estimate is valid
/*
return a relative ground position from the origin in meters, down
*/
bool AP_AHRS::get_relative_position_D_origin(float &posD) const
{
switch (active_EKF_type()) {
Expand Down Expand Up @@ -1625,8 +1630,9 @@ bool AP_AHRS::get_relative_position_D_origin(float &posD) const
return false;
}

// write a relative ground position to home in meters, Down
// will use the barometer if the EKF isn't available
/*
return relative position from home in meters
*/
void AP_AHRS::get_relative_position_D_home(float &posD) const
{
if (!_home_is_set) {
Expand Down
6 changes: 3 additions & 3 deletions libraries/AP_AHRS/AP_AHRS.h
Original file line number Diff line number Diff line change
Expand Up @@ -214,17 +214,17 @@ class AP_AHRS {
// order. Must only be called if have_inertial_nav() is true
bool get_velocity_NED(Vector3f &vec) const WARN_IF_UNUSED;

// return the relative position NED to either home or origin
// return the relative position NED from either home or origin
// return true if the estimate is valid
bool get_relative_position_NED_home(Vector3f &vec) const WARN_IF_UNUSED;
bool get_relative_position_NED_origin(Vector3f &vec) const WARN_IF_UNUSED;

// return the relative position NE to either home or origin
// return the relative position NE from home or origin
// return true if the estimate is valid
bool get_relative_position_NE_home(Vector2f &posNE) const WARN_IF_UNUSED;
bool get_relative_position_NE_origin(Vector2f &posNE) const WARN_IF_UNUSED;

// return the relative position down to either home or origin
// return the relative position down from home or origin
// baro will be used for the _home relative one if the EKF isn't
void get_relative_position_D_home(float &posD) const;
bool get_relative_position_D_origin(float &posD) const WARN_IF_UNUSED;
Expand Down

0 comments on commit e9cb826

Please sign in to comment.