2014-01-03 07:57:50 -04:00
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
2015-08-11 03:28:43 -03:00
# include <AP_HAL/AP_HAL.h>
2014-01-03 07:57:50 -04:00
# 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
*/
2014-01-03 20:16:07 -04:00
/**
update internal state
*/
void AP_InertialNav_NavEKF : : update ( float dt )
{
2015-10-12 09:13:32 -03:00
// get the position relative to the local earth frame origin
_ahrs_ekf . get_relative_position_NED ( _relpos_cm ) ;
2014-02-08 22:57:08 -04:00
_relpos_cm * = 100 ; // convert to cm
2015-10-12 09:13:32 -03:00
// get the absolute WGS-84 position
2015-03-12 23:02:01 -03:00
_haveabspos = _ahrs_ekf . get_position ( _abspos ) ;
2014-02-08 22:57:08 -04:00
2015-10-12 09:13:32 -03:00
// get the velocity relative to the local earth frame
_ahrs_ekf . get_velocity_NED ( _velocity_cm ) ;
2014-02-08 22:57:08 -04:00
_velocity_cm * = 100 ; // convert to cm/s
2014-01-03 20:16:07 -04:00
2015-10-12 09:13:32 -03:00
// Get a derivative of the vertical position which is kinematically consistent with the vertical position is required by some control loops.
2015-10-12 05:13:37 -03:00
// This is different to the vertical velocity from the EKF which is not always consistent with the verical position due to the various errors that are being corrected for.
2015-10-12 09:13:32 -03:00
_ahrs_ekf . get_vert_pos_rate ( _pos_z_rate ) ;
2015-10-12 05:13:37 -03:00
_pos_z_rate * = 100 ; // convert to cm/s
2014-01-03 20:16:07 -04:00
// InertialNav is NEU
_relpos_cm . z = - _relpos_cm . z ;
_velocity_cm . z = - _velocity_cm . z ;
2015-10-12 05:13:37 -03:00
_pos_z_rate = - _pos_z_rate ;
2014-01-03 20:16:07 -04:00
}
2014-01-03 07:57:50 -04:00
/**
2015-01-02 04:06:22 -04:00
* get_filter_status : returns filter status as a series of flags
2014-01-03 07:57:50 -04:00
*/
2015-01-02 04:06:22 -04:00
nav_filter_status AP_InertialNav_NavEKF : : get_filter_status ( ) const
2014-01-03 07:57:50 -04:00
{
2015-10-12 09:13:32 -03:00
nav_filter_status status ;
_ahrs_ekf . get_filter_status ( status ) ;
return status ;
2014-01-03 07:57:50 -04:00
}
2015-02-02 23:05:24 -04:00
/**
* get_origin - returns the inertial navigation origin in lat / lon / alt
*/
2015-03-12 10:22:54 -03:00
struct Location AP_InertialNav_NavEKF : : get_origin ( ) const
{
struct Location ret ;
2015-10-15 02:10:44 -03:00
if ( ! _ahrs_ekf . get_origin ( ret ) ) {
// initialise location to all zeros if EKF1 origin not yet set
memset ( & ret , 0 , sizeof ( ret ) ) ;
}
2015-03-12 10:22:54 -03:00
return ret ;
2015-02-02 22:11:48 -04:00
}
2014-01-03 07:57:50 -04:00
/**
* get_position - returns the current position relative to the home location in cm .
*
* @ return
*/
const Vector3f & AP_InertialNav_NavEKF : : get_position ( void ) const
{
2015-03-12 10:22:54 -03:00
return _relpos_cm ;
2014-01-03 07:57:50 -04:00
}
2015-02-25 10:13:17 -04:00
/**
* get_location - updates the provided location with the latest calculated locatoin
* 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
{
2015-10-12 09:13:32 -03:00
return _ahrs_ekf . get_location ( loc ) ;
2015-02-25 10:13:17 -04:00
}
2014-01-03 07:57:50 -04:00
/**
* 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
{
2015-03-12 10:22:54 -03:00
return _abspos . lat ;
2014-01-03 07:57:50 -04:00
}
/**
* 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
{
2015-03-12 10:22:54 -03:00
return _abspos . lng ;
2014-01-03 07:57:50 -04:00
}
/**
* 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
{
2015-03-12 10:22:54 -03:00
return _velocity_cm ;
2014-01-03 07:57:50 -04:00
}
/**
* get_velocity_xy - returns the current horizontal velocity in cm / s
*
* @ returns the current horizontal velocity in cm / s
*/
2014-04-21 09:58:42 -03:00
float AP_InertialNav_NavEKF : : get_velocity_xy ( ) const
2014-01-03 07:57:50 -04:00
{
2015-03-12 10:22:54 -03:00
return pythagorous2 ( _velocity_cm . x , _velocity_cm . y ) ;
2014-01-03 07:57:50 -04:00
}
2015-10-12 05:13:37 -03:00
/**
* get_pos_z_derivative - returns the derivative of the z position in cm / s
*/
float AP_InertialNav_NavEKF : : get_pos_z_derivative ( ) const
{
return _pos_z_rate ;
}
2014-01-03 07:57:50 -04:00
/**
* get_altitude - get latest altitude estimate in cm
* @ return
*/
float AP_InertialNav_NavEKF : : get_altitude ( ) const
{
2015-03-12 10:22:54 -03:00
return _relpos_cm . z ;
2014-01-03 07:57:50 -04:00
}
2015-04-06 04:19:53 -03:00
/**
* getHgtAboveGnd - get latest height above ground level estimate in cm and a validity flag
*
* @ return
*/
2015-07-09 20:42:52 -03:00
bool AP_InertialNav_NavEKF : : get_hagl ( float & height ) const
2015-04-06 04:19:53 -03:00
{
// true when estimate is valid
2015-10-12 09:13:32 -03:00
bool valid = _ahrs_ekf . get_hagl ( height ) ;
2015-04-06 04:19:53 -03:00
// convert height from m to cm
height * = 100.0f ;
return valid ;
}
2015-04-23 00:27:51 -03:00
/**
* get_hgt_ctrl_limit - get maximum height to be observed by the control loops in cm and a validity flag
* this is used to limit height during optical flow navigation
* it will return invalid when no limiting is required
* @ return
*/
2015-04-23 02:16:28 -03:00
bool AP_InertialNav_NavEKF : : get_hgt_ctrl_limit ( float & limit ) const
2015-04-23 00:27:51 -03:00
{
// true when estimate is valid
2015-10-12 09:13:32 -03:00
if ( _ahrs_ekf . get_hgt_ctrl_limit ( limit ) ) {
2015-04-23 02:16:28 -03:00
// convert height from m to cm
limit * = 100.0f ;
return true ;
}
return false ;
2015-04-23 00:27:51 -03:00
}
2014-01-03 07:57:50 -04:00
/**
* get_velocity_z - returns the current climbrate .
*
* @ see get_velocity ( ) . z
*
* @ return climbrate in cm / s
*/
float AP_InertialNav_NavEKF : : get_velocity_z ( ) const
{
2015-03-12 10:22:54 -03:00
return _velocity_cm . z ;
2014-01-03 07:57:50 -04:00
}
# endif // AP_AHRS_NAVEKF_AVAILABLE