Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-21 15:23:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialNav: implement InertialNav in terms of AHRS when available

Browse Source
This commit is contained in:
Andrew Tridgell 2014-01-04 11:16:07 +11:00
parent 5d43a1d704
commit 47f541e143
3 changed files with 82 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
libraries/AP_InertialNav/AP_InertialNav.h
View File

@ -34,7 +34,7 @@ class AP_InertialNav
public:
// Constructor
AP_InertialNav( const AP_AHRS &ahrs, AP_Baro &baro, GPS*& gps, GPS_Glitch& gps_glitch ) :
AP_InertialNav(AP_AHRS &ahrs, AP_Baro &baro, GPS*& gps, GPS_Glitch& gps_glitch ) :
_ahrs(ahrs),
_baro(baro),
_gps(gps),
@ -52,7 +52,7 @@ public:
/**
* initializes the object.
*/
void init();
virtual void init();
/**
* update - updates velocity and position estimates using latest info from accelerometers
@ -60,7 +60,7 @@ public:
*
* @param dt : time since last update in seconds
*/
void update(float dt);
virtual void update(float dt);
//
// XY Axis specific methods
@ -80,7 +80,7 @@ public:
* position_ok - true if inertial based altitude and position can be trusted
* @return
*/
bool position_ok() const;
virtual bool position_ok() const;
/**
* check_gps - checks if new gps readings have arrived and calls correct_with_gps to
@ -103,33 +103,33 @@ public:
*
* @return
*/
const Vector3f& get_position() const { return _position; }
virtual const Vector3f& get_position() const { return _position; }
/**
* get_latitude - returns the latitude of the current position estimation in 100 nano degrees (i.e. degree value multiplied by 10,000,000)
* @return
*/
int32_t get_latitude() const;
virtual int32_t get_latitude() const;
/**
* 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 get_longitude() const;
virtual int32_t get_longitude() const;
/**
* 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 get_latitude_diff() const;
virtual float get_latitude_diff() const;
/**
* 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 get_longitude_diff() const;
virtual float get_longitude_diff() const;
/**
* get_velocity - returns the current velocity in cm/s
@ -139,14 +139,14 @@ public:
* .y : longitude velocity in cm/s
* .z : vertical velocity in cm/s
*/
const Vector3f& get_velocity() const { return _velocity; }
virtual const Vector3f& get_velocity() const { return _velocity; }
/**
* get_velocity_xy - returns the current horizontal velocity in cm/s
*
* @returns the current horizontal velocity in cm/s
*/
float get_velocity_xy();
virtual float get_velocity_xy();
/**
* set_velocity_xy - overwrites the current horizontal velocity in cm/s
@ -179,7 +179,7 @@ public:
* altitude_ok - returns true if inertial based altitude and position can be trusted
* @return
*/
bool altitude_ok() const { return true; }
virtual bool altitude_ok() const { return true; }
/**
* check_baro - checks if new baro readings have arrived and calls correct_with_baro to
@ -198,10 +198,11 @@ public:
void correct_with_baro(float baro_alt, float dt);
/**
* get_altitude - get latest altitude estimate in cm
* get_altitude - get latest altitude estimate in cm above the
* reference position
* @return
*/
float get_altitude() const { return _position.z; }
virtual float get_altitude() const { return _position.z; }
/**
* set_altitude - overwrites the current altitude value.
@ -215,9 +216,9 @@ public:
*
* @see get_velocity().z
*
* @return climbrate in cm/s
* @return climbrate in cm/s (positive up)
*/
float get_velocity_z() const { return _velocity.z; }
virtual float get_velocity_z() const { return _velocity.z; }
/**
* set_velocity_z - overwrites the current climbrate.
@ -229,7 +230,7 @@ public:
/**
* error_count - returns number of missed updates from GPS
*/
uint8_t error_count() const { return _error_count; }
virtual uint8_t error_count() const { return _error_count; }
/**
* ignore_next_error - the next error (if it occurs immediately) will not be added to the error count
@ -270,7 +271,7 @@ protected:
uint8_t ignore_error : 3; // the number of iterations for which we should ignore errors
} _flags;
const AP_AHRS &_ahrs; // reference to ahrs object
AP_AHRS &_ahrs; // reference to ahrs object
AP_Baro &_baro; // reference to barometer
GPS*& _gps; // pointer to gps

51
libraries/AP_InertialNav/AP_InertialNav_NavEKF.cpp
View File

@ -1,7 +1,10 @@
/// -*- 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
/*
@ -18,12 +21,30 @@ void AP_InertialNav_NavEKF::init()
AP_InertialNav::init();
}
/**
update internal state
*/
void AP_InertialNav_NavEKF::update(float dt)
{
AP_InertialNav::update(dt);
_relpos_cm = _ahrs.get_relative_position_NED() * 100;
_haveabspos = _ahrs.get_position(_abspos);
_velocity_cm = _ahrs.get_velocity_NED() * 100;
// 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();
}
@ -36,6 +57,9 @@ bool AP_InertialNav_NavEKF::position_ok() const
*/
const Vector3f &AP_InertialNav_NavEKF::get_position(void) const
{
if (_ahrs.have_inertial_nav()) {
return _relpos_cm;
}
return AP_InertialNav::get_position();
}
@ -44,6 +68,9 @@ const Vector3f &AP_InertialNav_NavEKF::get_position(void) const
*/
int32_t AP_InertialNav_NavEKF::get_latitude() const
{
if (_ahrs.have_inertial_nav() && _haveabspos) {
return _abspos.lat;
}
return AP_InertialNav::get_latitude();
}
@ -53,6 +80,9 @@ int32_t AP_InertialNav_NavEKF::get_latitude() const
*/
int32_t AP_InertialNav_NavEKF::get_longitude() const
{
if (_ahrs.have_inertial_nav() && _haveabspos) {
return _abspos.lng;
}
return AP_InertialNav::get_longitude();
}
@ -63,6 +93,9 @@ int32_t AP_InertialNav_NavEKF::get_longitude() const
*/
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();
}
@ -73,6 +106,9 @@ float AP_InertialNav_NavEKF::get_latitude_diff() const
*/
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();
}
@ -86,6 +122,9 @@ float AP_InertialNav_NavEKF::get_longitude_diff() const
*/
const Vector3f &AP_InertialNav_NavEKF::get_velocity() const
{
if (_ahrs.have_inertial_nav()) {
return _velocity_cm;
}
return AP_InertialNav::get_velocity();
}
@ -96,6 +135,9 @@ const Vector3f &AP_InertialNav_NavEKF::get_velocity() const
*/
float AP_InertialNav_NavEKF::get_velocity_xy()
{
if (_ahrs.have_inertial_nav()) {
return pythagorous2(_velocity_cm.x, _velocity_cm.y);
}
return AP_InertialNav::get_velocity_xy();
}
@ -105,6 +147,9 @@ float AP_InertialNav_NavEKF::get_velocity_xy()
*/
bool AP_InertialNav_NavEKF::altitude_ok() const
{
if (_ahrs.have_inertial_nav() && _haveabspos) {
return true;
}
return AP_InertialNav::altitude_ok();
}
@ -114,6 +159,9 @@ bool AP_InertialNav_NavEKF::altitude_ok() const
*/
float AP_InertialNav_NavEKF::get_altitude() const
{
if (_ahrs.have_inertial_nav()) {
return _relpos_cm.z;
}
return AP_InertialNav::get_altitude();
}
@ -126,6 +174,9 @@ float AP_InertialNav_NavEKF::get_altitude() const
*/
float AP_InertialNav_NavEKF::get_velocity_z() const
{
if (_ahrs.have_inertial_nav()) {
return _velocity_cm.z;
}
return AP_InertialNav::get_velocity_z();
}

13
libraries/AP_InertialNav/AP_InertialNav_NavEKF.h
View File

@ -14,7 +14,7 @@ class AP_InertialNav_NavEKF : public AP_InertialNav
{
public:
// Constructor
AP_InertialNav_NavEKF(const AP_AHRS &ahrs, AP_Baro &baro, GPS*& gps, GPS_Glitch& gps_glitch ) :
AP_InertialNav_NavEKF(AP_AHRS &ahrs, AP_Baro &baro, GPS*& gps, GPS_Glitch& gps_glitch ) :
AP_InertialNav(ahrs, baro, gps, gps_glitch)
{
}
@ -24,6 +24,11 @@ public:
*/
void init();
/**
update internal state
*/
void update(float dt);
/**
* position_ok - true if inertial based altitude and position can be trusted
* @return
@ -101,6 +106,12 @@ public:
* @return climbrate in cm/s
*/
float get_velocity_z() const;
private:
Vector3f _relpos_cm; // NEU
Vector3f _velocity_cm; // NEU
struct Location _abspos;
bool _haveabspos;
};
#endif // __AP_INERTIALNAV_NAVEKF_H__