mirror of https://github.com/ArduPilot/ardupilot
188 lines
4.5 KiB
C++
188 lines
4.5 KiB
C++
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
|
|
#include <AP_HAL.h>
|
|
#include "AP_InertialNav.h"
|
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
#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
|
|
*/
|
|
|
|
/**
|
|
* initializes the object.
|
|
*/
|
|
void AP_InertialNav_NavEKF::init()
|
|
{
|
|
AP_InertialNav::init();
|
|
}
|
|
|
|
/**
|
|
update internal state
|
|
*/
|
|
void AP_InertialNav_NavEKF::update(float dt)
|
|
{
|
|
AP_InertialNav::update(dt);
|
|
_ahrs.get_relative_position_NED(_relpos_cm);
|
|
_relpos_cm *= 100; // convert to cm
|
|
|
|
_haveabspos = _ahrs.get_position(_abspos);
|
|
|
|
_ahrs.get_velocity_NED(_velocity_cm);
|
|
_velocity_cm *= 100; // convert to cm/s
|
|
|
|
// InertialNav is NEU
|
|
_relpos_cm.z = - _relpos_cm.z;
|
|
_velocity_cm.z = -_velocity_cm.z;
|
|
}
|
|
|
|
/**
|
|
* position_ok - true if inertial based altitude and position can be trusted
|
|
* @return
|
|
*/
|
|
bool AP_InertialNav_NavEKF::position_ok() const
|
|
{
|
|
if (_ahrs.have_inertial_nav() && _haveabspos) {
|
|
return true;
|
|
}
|
|
return AP_InertialNav::position_ok();
|
|
}
|
|
|
|
/**
|
|
* get_position - returns the current position relative to the home location in cm.
|
|
*
|
|
* the home location was set with AP_InertialNav::set_home_position(int32_t, int32_t)
|
|
*
|
|
* @return
|
|
*/
|
|
const Vector3f &AP_InertialNav_NavEKF::get_position(void) const
|
|
{
|
|
if (_ahrs.have_inertial_nav()) {
|
|
return _relpos_cm;
|
|
}
|
|
return AP_InertialNav::get_position();
|
|
}
|
|
|
|
/**
|
|
* 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
|
|
{
|
|
if (_ahrs.have_inertial_nav() && _haveabspos) {
|
|
return _abspos.lat;
|
|
}
|
|
return AP_InertialNav::get_latitude();
|
|
}
|
|
|
|
/**
|
|
* 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
|
|
{
|
|
if (_ahrs.have_inertial_nav() && _haveabspos) {
|
|
return _abspos.lng;
|
|
}
|
|
return AP_InertialNav::get_longitude();
|
|
}
|
|
|
|
/**
|
|
* get_latitude_diff - returns the current latitude difference from the home location.
|
|
*
|
|
* @return difference in 100 nano degrees (i.e. degree value multiplied by 10,000,000)
|
|
*/
|
|
float AP_InertialNav_NavEKF::get_latitude_diff() const
|
|
{
|
|
if (_ahrs.have_inertial_nav()) {
|
|
return _relpos_cm.x / LATLON_TO_CM;
|
|
}
|
|
return AP_InertialNav::get_latitude_diff();
|
|
}
|
|
|
|
/**
|
|
* get_longitude_diff - returns the current longitude difference from the home location.
|
|
*
|
|
* @return difference in 100 nano degrees (i.e. degree value multiplied by 10,000,000)
|
|
*/
|
|
float AP_InertialNav_NavEKF::get_longitude_diff() const
|
|
{
|
|
if (_ahrs.have_inertial_nav()) {
|
|
return _relpos_cm.y / _lon_to_cm_scaling;
|
|
}
|
|
return AP_InertialNav::get_longitude_diff();
|
|
}
|
|
|
|
/**
|
|
* 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
|
|
{
|
|
if (_ahrs.have_inertial_nav()) {
|
|
return _velocity_cm;
|
|
}
|
|
return AP_InertialNav::get_velocity();
|
|
}
|
|
|
|
/**
|
|
* get_velocity_xy - returns the current horizontal velocity in cm/s
|
|
*
|
|
* @returns the current horizontal velocity in cm/s
|
|
*/
|
|
float AP_InertialNav_NavEKF::get_velocity_xy() const
|
|
{
|
|
if (_ahrs.have_inertial_nav()) {
|
|
return pythagorous2(_velocity_cm.x, _velocity_cm.y);
|
|
}
|
|
return AP_InertialNav::get_velocity_xy();
|
|
}
|
|
|
|
/**
|
|
* altitude_ok - returns true if inertial based altitude and position can be trusted
|
|
* @return
|
|
*/
|
|
bool AP_InertialNav_NavEKF::altitude_ok() const
|
|
{
|
|
if (_ahrs.have_inertial_nav() && _haveabspos) {
|
|
return true;
|
|
}
|
|
return AP_InertialNav::altitude_ok();
|
|
}
|
|
|
|
/**
|
|
* get_altitude - get latest altitude estimate in cm
|
|
* @return
|
|
*/
|
|
float AP_InertialNav_NavEKF::get_altitude() const
|
|
{
|
|
if (_ahrs.have_inertial_nav()) {
|
|
return _relpos_cm.z;
|
|
}
|
|
return AP_InertialNav::get_altitude();
|
|
}
|
|
|
|
/**
|
|
* get_velocity_z - returns the current climbrate.
|
|
*
|
|
* @see get_velocity().z
|
|
*
|
|
* @return climbrate in cm/s
|
|
*/
|
|
float AP_InertialNav_NavEKF::get_velocity_z() const
|
|
{
|
|
if (_ahrs.have_inertial_nav()) {
|
|
return _velocity_cm.z;
|
|
}
|
|
return AP_InertialNav::get_velocity_z();
|
|
}
|
|
|
|
#endif // AP_AHRS_NAVEKF_AVAILABLE
|