You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
144 lines
3.5 KiB
144 lines
3.5 KiB
#include <AP_HAL/AP_HAL.h> |
|
#include "AP_InertialNav.h" |
|
|
|
#if AP_AHRS_NAVEKF_AVAILABLE |
|
|
|
/* |
|
A wrapper around the AP_InertialNav class which uses the NavEKF |
|
filter if available, and falls back to the AP_InertialNav filter |
|
when EKF is not available |
|
*/ |
|
|
|
/** |
|
update internal state |
|
*/ |
|
void AP_InertialNav_NavEKF::update(float dt) |
|
{ |
|
// get the NE position relative to the local earth frame origin |
|
Vector2f posNE; |
|
if (_ahrs_ekf.get_relative_position_NE_origin(posNE)) { |
|
_relpos_cm.x = posNE.x * 100; // convert from m to cm |
|
_relpos_cm.y = posNE.y * 100; // convert from m to cm |
|
} |
|
|
|
// get the D position relative to the local earth frame origin |
|
float posD; |
|
if (_ahrs_ekf.get_relative_position_D_origin(posD)) { |
|
_relpos_cm.z = - posD * 100; // convert from m in NED to cm in NEU |
|
} |
|
|
|
// get the absolute WGS-84 position |
|
_haveabspos = _ahrs_ekf.get_position(_abspos); |
|
|
|
// get the velocity relative to the local earth frame |
|
Vector3f velNED; |
|
if (_ahrs_ekf.get_velocity_NED(velNED)) { |
|
_velocity_cm = velNED * 100; // convert to cm/s |
|
_velocity_cm.z = -_velocity_cm.z; // convert from NED to NEU |
|
} |
|
} |
|
|
|
/** |
|
* get_filter_status : returns filter status as a series of flags |
|
*/ |
|
nav_filter_status AP_InertialNav_NavEKF::get_filter_status() const |
|
{ |
|
nav_filter_status status; |
|
_ahrs_ekf.get_filter_status(status); |
|
return status; |
|
} |
|
|
|
/** |
|
* get_origin - returns the inertial navigation origin in lat/lon/alt |
|
*/ |
|
struct Location AP_InertialNav_NavEKF::get_origin() const |
|
{ |
|
struct Location ret; |
|
if (!_ahrs_ekf.get_origin(ret)) { |
|
// initialise location to all zeros if EKF origin not yet set |
|
ret.zero(); |
|
} |
|
return ret; |
|
} |
|
|
|
/** |
|
* get_position - returns the current position relative to the home location in cm. |
|
* |
|
* @return |
|
*/ |
|
const Vector3f &AP_InertialNav_NavEKF::get_position(void) const |
|
{ |
|
return _relpos_cm; |
|
} |
|
|
|
/** |
|
* get_location - updates the provided location with the latest calculated location |
|
* returns true on success (i.e. the EKF knows it's latest position), false on failure |
|
*/ |
|
bool AP_InertialNav_NavEKF::get_location(struct Location &loc) const |
|
{ |
|
return _ahrs_ekf.get_location(loc); |
|
} |
|
|
|
/** |
|
* get_latitude - returns the latitude of the current position estimation in 100 nano degrees (i.e. degree value multiplied by 10,000,000) |
|
*/ |
|
int32_t AP_InertialNav_NavEKF::get_latitude() const |
|
{ |
|
return _abspos.lat; |
|
} |
|
|
|
/** |
|
* get_longitude - returns the longitude of the current position estimation in 100 nano degrees (i.e. degree value multiplied by 10,000,000) |
|
* @return |
|
*/ |
|
int32_t AP_InertialNav_NavEKF::get_longitude() const |
|
{ |
|
return _abspos.lng; |
|
} |
|
|
|
/** |
|
* get_velocity - returns the current velocity in cm/s |
|
* |
|
* @return velocity vector: |
|
* .x : latitude velocity in cm/s |
|
* .y : longitude velocity in cm/s |
|
* .z : vertical velocity in cm/s |
|
*/ |
|
const Vector3f &AP_InertialNav_NavEKF::get_velocity() const |
|
{ |
|
return _velocity_cm; |
|
} |
|
|
|
/** |
|
* get_speed_xy - returns the current horizontal speed in cm/s |
|
* |
|
* @returns the current horizontal speed in cm/s |
|
*/ |
|
float AP_InertialNav_NavEKF::get_speed_xy() const |
|
{ |
|
return norm(_velocity_cm.x, _velocity_cm.y); |
|
} |
|
|
|
/** |
|
* get_altitude - get latest altitude estimate in cm |
|
* @return |
|
*/ |
|
float AP_InertialNav_NavEKF::get_altitude() const |
|
{ |
|
return _relpos_cm.z; |
|
} |
|
|
|
/** |
|
* get_velocity_z - returns the current climbrate. |
|
* |
|
* @see get_velocity().z |
|
* |
|
* @return climbrate in cm/s |
|
*/ |
|
float AP_InertialNav_NavEKF::get_velocity_z() const |
|
{ |
|
return _velocity_cm.z; |
|
} |
|
|
|
#endif // AP_AHRS_NAVEKF_AVAILABLE
|
|
|