Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-21 16:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_AHRS: add methods for returning location for origin offsets

Browse Source
This commit is contained in:
Peter Barker 2023-09-06 14:02:28 +10:00 committed by Andrew Tridgell
parent e872a8fd6f
commit 6e1e1f6596
2 changed files with 30 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
libraries/AP_AHRS/AP_AHRS.cpp
View File

@ -3191,6 +3191,31 @@ bool AP_AHRS::get_velocity_NED(Vector3f &vec) const
return state.velocity_NED_ok;
}
// return location corresponding to vector relative to the
// vehicle's origin
bool AP_AHRS::get_location_from_origin_offset(Location &loc, const Vector3p &offset_ned) const
{
if (!get_origin(loc)) {
return false;
}
loc.offset(offset_ned);
return true;
}
// return location corresponding to vector relative to the
// vehicle's home location
bool AP_AHRS::get_location_from_home_offset(Location &loc, const Vector3p &offset_ned) const
{
if (!home_is_set()) {
return false;
}
loc = get_home();
loc.offset(offset_ned);
return true;
}
// singleton instance
AP_AHRS *AP_AHRS::_singleton;

5
libraries/AP_AHRS/AP_AHRS.h
View File

@ -244,6 +244,11 @@ public:
void get_relative_position_D_home(float &posD) const;
bool get_relative_position_D_origin(float &posD) const WARN_IF_UNUSED;
// return location corresponding to vector relative to the
// vehicle's origin
bool get_location_from_origin_offset(Location &loc, const Vector3p &offset_ned) const WARN_IF_UNUSED;
bool get_location_from_home_offset(Location &loc, const Vector3p &offset_ned) const WARN_IF_UNUSED;
// Get a derivative of the vertical position in m/s which is kinematically consistent with the vertical position is required by some control loops.
// This is different to the vertical velocity from the EKF which is not always consistent with the vertical position due to the various errors that are being corrected for.
bool get_vert_pos_rate_D(float &velocity) const;