aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_InertialNav: add comments, make ahrs const from neurocopter

Browse Source 
These fixes are provided by neurocopter but with my name because of
merge conflicts
_position and _velocity vectors added to save some floating point add
operations
unused reference to ins in constructor removed
This commit is contained in:
Randy Mackay 2013-10-25 22:55:31 +09:00
parent a7e7084f27
commit 8fc16d5cdf
2 changed files with 216 additions and 58 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
libraries/AP_InertialNav/AP_InertialNav.cpp
View File

@ -31,7 +31,7 @@ void AP_InertialNav::init()
update_gains();
}
// update - updates velocities and positions using latest info from ahrs, ins and barometer if new data is available;
// update - updates velocities and positions using latest info from ahrs and barometer if new data is available;
void AP_InertialNav::update(float dt)
{
// discard samples where dt is too large
@ -39,10 +39,10 @@ void AP_InertialNav::update(float dt)
return;
}
// check barometer
// check if new baro readings have arrived and use them to correct vertical accelerometer offsets.
check_baro();
// check gps
// check if new gps readings have arrived and use them to correct position estimates
check_gps();
Vector3f accel_ef = _ahrs->get_accel_ef();
@ -76,11 +76,14 @@ void AP_InertialNav::update(float dt)
_position_correction.z += _position_error.z * _k1_z * dt;
// calculate velocity increase adding new acceleration from accelerometers
Vector3f velocity_increase = (accel_ef + accel_correction_ef) * dt;
const Vector3f &velocity_increase = (accel_ef + accel_correction_ef) * dt;
// calculate new estimate of position
_position_base += (_velocity + velocity_increase*0.5) * dt;
// update the corrected position estimate
_position = _position_base + _position_correction;
// calculate new velocity
_velocity += velocity_increase;
@ -119,7 +122,7 @@ bool AP_InertialNav::position_ok() const
// check_gps - check if new gps readings have arrived and use them to correct position estimates
void AP_InertialNav::check_gps()
{
uint32_t now = hal.scheduler->millis();
const uint32_t now = hal.scheduler->millis();
// compare gps time to previous reading
if( _gps != NULL && _gps->last_fix_time != _gps_last_time ) {
@ -201,7 +204,7 @@ int32_t AP_InertialNav::get_latitude() const
return 0;
}
return _base_lat + (int32_t)((_position_base.x + _position_correction.x)/LATLON_TO_CM);
return _base_lat + (int32_t)(_position.x/LATLON_TO_CM);
}
// get accel based longitude
@ -212,7 +215,7 @@ int32_t AP_InertialNav::get_longitude() const
return 0;
}
return _base_lon + (int32_t)((_position_base.y+_position_correction.y) / _lon_to_cm_scaling);
return _base_lon + (int32_t)(_position.y / _lon_to_cm_scaling);
}
// set_home_position - all internal calculations are recorded as the distances from this point
@ -233,6 +236,8 @@ void AP_InertialNav::set_home_position(int32_t lon, int32_t lat)
_position_base.y = 0;
_position_correction.x = 0;
_position_correction.y = 0;
_position.x = 0;
_position.y = 0;
// clear historic estimates
_hist_position_estimate_x.clear();
@ -250,7 +255,7 @@ float AP_InertialNav::get_latitude_diff() const
return 0;
}
return ((_position_base.x+_position_correction.x)/LATLON_TO_CM);
return (_position.x/LATLON_TO_CM);
}
// get accel based longitude
@ -261,7 +266,7 @@ float AP_InertialNav::get_longitude_diff() const
return 0;
}
return (_position_base.y+_position_correction.y) / _lon_to_cm_scaling;
return (_position.y / _lon_to_cm_scaling);
}
// get velocity in latitude & longitude directions
@ -315,10 +320,10 @@ void AP_InertialNav::check_baro()
if( _baro == NULL )
return;
// calculate time since last baro reading
// calculate time since last baro reading (in ms)
baro_update_time = _baro->get_last_update();
if( baro_update_time != _baro_last_update ) {
float dt = (float)(baro_update_time - _baro_last_update) * 0.001f;
const float dt = (float)(baro_update_time - _baro_last_update) * 0.001f; // in seconds
// call correction method
correct_with_baro(_baro->get_altitude()*100, dt);
_baro_last_update = baro_update_time;
@ -330,7 +335,6 @@ void AP_InertialNav::check_baro()
void AP_InertialNav::correct_with_baro(float baro_alt, float dt)
{
static uint8_t first_reads = 0;
float hist_position_base_z;
// discard samples where dt is too large
if( dt > 0.5f ) {
@ -345,6 +349,7 @@ void AP_InertialNav::correct_with_baro(float baro_alt, float dt)
// 3rd order samples (i.e. position from baro) are delayed by 150ms (15 iterations at 100hz)
// so we should calculate error using historical estimates
float hist_position_base_z;
if( _hist_position_estimate_z.is_full() ) {
hist_position_base_z = _hist_position_estimate_z.front();
}else{
@ -360,6 +365,7 @@ void AP_InertialNav::set_altitude( float new_altitude)
{
_position_base.z = new_altitude;
_position_correction.z = 0;
_position.z = new_altitude; // _position = _position_base + _position_correction
}
//
@ -395,11 +401,11 @@ void AP_InertialNav::set_velocity_z(float z )
}
// set_position_xy - sets inertial navigation position to given xy coordinates from home
void AP_InertialNav::set_position_xy(float pos_x, float pos_y)
void AP_InertialNav::set_position_xy(float x, float y)
{
// reset position from home
_position_base.x = pos_x;
_position_base.y = pos_y;
_position_base.x = x;
_position_base.y = y;
_position_correction.x = 0;
_position_correction.y = 0;

238
libraries/AP_InertialNav/AP_InertialNav.h
View File

@ -19,14 +19,22 @@
#define AP_INTERTIALNAV_GPS_TIMEOUT_MS 300 // timeout after which position error from GPS will fall to zero
/*
* AP_InertialNav is an attempt to use accelerometers to augment other sensors to improve altitud e position hold
* AP_InertialNav blends accelerometer data with gps and barometer data to improve altitude and position hold.
*
* Most of the functions have to be called at 100Hz. (see defines above)
*
* The accelerometer values are integrated over time to approximate velocity and position.
* The inaccurcy of these estimates grows over time due to noisy sensor data.
* To improve the accuracy, baro and gps readings are used:
* An error value is calculated as the difference between the sensor's measurement and the last position estimation.
* This value is weighted with a gain factor and incorporated into the new estimation
*/
class AP_InertialNav
{
public:
// Constructor
AP_InertialNav( AP_AHRS* ahrs, AP_InertialSensor* ins, AP_Baro* baro, GPS*& gps, GPS_Glitch& gps_glitch ) :
AP_InertialNav( const AP_AHRS* ahrs, AP_Baro* baro, GPS*& gps, GPS_Glitch& gps_glitch ) :
_ahrs(ahrs),
_baro(baro),
_gps(gps),
@ -40,74 +48,202 @@ public:
AP_Param::setup_object_defaults(this, var_info);
}
// Initialisation
/**
* initializes the object.
*
* AP_InertialNav::set_home_position(int32_t, int32_t) should be called later,
* to enable all "horizontal related" getter-methods.
*/
void init();
// update - updates velocities and positions using latest info from accelerometers;
/**
* update - updates velocity and position estimates using latest info from accelerometers
* augmented with gps and baro readings
*
* @param dt : time since last update in seconds
*/
void update(float dt);
//
// XY Axis specific methods
//
// set time constant - set timeconstant used by complementary filter
/**
* set_time_constant_xy - sets time constant used by complementary filter for horizontal position estimate
*
* smaller values means higher influence of gps on position estimation
* bigger values favor the integrated accelerometer data for position estimation
*
* @param time_constant_in_seconds : constant in seconds; 0 < constant < 30 must hold
*/
void set_time_constant_xy( float time_constant_in_seconds );
// altitude_ok, position_ok - true if inertial based altitude and position can be trusted
/**
* position_ok - true if inertial based altitude and position can be trusted
* @return
*/
bool position_ok() const;
// check_gps - check if new gps readings have arrived and use them to correct position estimates
/**
* check_gps - checks if new gps readings have arrived and calls correct_with_gps to
* calculate the horizontal position error
* @see correct_with_gps(int32_t lon, int32_t lat, float dt);
*/
void check_gps();
// correct_with_gps - modifies accelerometer offsets using gps.
/**
* correct_with_gps - calculates horizontal position error using gps
*
* @param now : current time since boot in milliseconds
* @param lon : longitude in 100 nano degrees (i.e. degree value multiplied by 10,000,000)
* @param lat : latitude in 100 nano degrees (i.e. degree value multiplied by 10,000,000)
*/
void correct_with_gps(uint32_t now, int32_t lon, int32_t lat);
// get_position - returns current position from home in cm
Vector3f get_position() const { return _position_base + _position_correction; }
/**
* 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& get_position() const { return _position; }
// get latitude & longitude positions in degrees * 10,000,000
/**
* 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;
/**
* 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;
// set_home_position - all internal calculations are recorded as the distances from this point
/**
* set_home_position - sets home position
*
* all internal calculations are recorded as the distances from this point.
*
* @param lon : longitude in 100 nano degrees (i.e. degree value multiplied by 10,000,000)
* @param lat : latitude in 100 nano degrees (i.e. degree value multiplied by 10,000,000)
*/
void set_home_position(int32_t lon, int32_t lat);
// get latitude & longitude positions from base location (in cm)
/**
* 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;
/**
* 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;
// get velocity in latitude & longitude directions (in cm/s)
/**
* get_latitude_velocity - returns the current velocity in latitude direction.
*
* @see get_velocity().x
*
* @return : latitude velocity in cm/s
*/
float get_latitude_velocity() const;
/**
* get_longitude_velocity - returns the current velocity in longitude direction.
*
* @see get_velocity().y
*
* @return : longitude velocity in cm/s
*/
float get_longitude_velocity() const;
// get_velocity - returns current velocity in cm/s
Vector3f get_velocity() const { return _velocity; }
/**
* 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& get_velocity() const { return _velocity; }
// set velocity in latitude & longitude directions (in cm/s)
/**
* set_velocity_xy - overwrites the current horizontal velocity in cm/s
*
* @param x : latitude velocity in cm/s
* @param y : longitude velocity in cm/s
*/
void set_velocity_xy(float x, float y);
//
// Z Axis methods
//
// set time constant - set timeconstant used by complementary filter
/**
* set_time_constant_z - sets timeconstant used by complementary filter for vertical position estimation
*
* smaller values means higher influence of barometer in altitude estimation
* bigger values favor the integrated accelerometer data for altitude estimation
*
* @param time_constant_in_seconds : constant in s; 0 < constant < 30 must hold
*/
void set_time_constant_z( float time_constant_in_seconds );
// altitude_ok, position_ok - true if inertial based altitude and position can be trusted
/**
* altitude_ok - returns true if inertial based altitude and position can be trusted
* @return
*/
bool altitude_ok() const { return true; }
// check_baro - check if new baro readings have arrived and use them to correct vertical accelerometer offsets
/**
* check_baro - checks if new baro readings have arrived and calls correct_with_baro to
* calculate the vertical position error
*
* @see correct_with_baro(float baro_alt, float dt);
*/
void check_baro();
// correct_with_baro - modifies accelerometer offsets using barometer. dt is time since last baro reading
/**
* correct_with_baro - calculates vertical position error using barometer.
*
* @param baro_alt : altitude in cm
* @param dt : time since last baro reading in s
*/
void correct_with_baro(float baro_alt, float dt);
// get_altitude - get latest altitude estimate in cm
float get_altitude() const { return _position_base.z + _position_correction.z; }
/**
* get_altitude - get latest altitude estimate in cm
* @return
*/
float get_altitude() const { return _position.z; }
/**
* set_altitude - overwrites the current altitude value.
*
* @param new_altitude : altitude in cm
*/
void set_altitude( float new_altitude);
// get_velocity_z - get latest climb rate (in cm/s)
/**
* get_velocity_z - returns the current climbrate.
*
* @see get_velocity().z
*
* @return climbrate in cm/s
*/
float get_velocity_z() const { return _velocity.z; }
/**
* set_velocity_z - overwrites the current climbrate.
*
* @param new_velocity : climbrate in cm/s
*/
void set_velocity_z( float new_velocity );
// class level parameters
@ -116,50 +252,66 @@ public:
// public variables
Vector3f accel_correction_ef; // earth frame accelerometer corrections. here for logging purposes only
// set_position_xy - sets inertial navigation position to given xy coordinates from home
void set_position_xy(float pos_x, float pos_y);
protected:
void update_gains(); // update_gains - update gains from time constant (given in seconds)
/**
* update gains from time constant.
*
* The time constants (in s) can be set with the following methods:
*
* @see: AP_InertialNav::set_time_constant_xy(float)
* @see: AP_InertialNav::set_time_constant_z(float)
*/
void update_gains();
/**
* set_position_xy - overwrites the current position relative to the home location in cm
*
* the home location was set with AP_InertialNav::set_home_location(int32_t, int32_t)
*
* @param x : relative latitude position in cm
* @param y : relative longitude position in cm
*/
void set_position_xy(float x, float y);
// structure for holding flags
struct InertialNav_flags {
uint8_t gps_glitching : 1; // 1 if glitch detector was previously indicating a gps glitch
} _flags;
AP_AHRS* _ahrs; // pointer to ahrs object
const AP_AHRS* const _ahrs; // pointer to ahrs object
AP_Baro* _baro; // pointer to barometer
GPS*& _gps; // pointer to gps
// XY Axis specific variables
bool _xy_enabled; // xy position estimates enabled
AP_Float _time_constant_xy; // time constant for horizontal corrections
AP_Float _time_constant_xy; // time constant for horizontal corrections in s
float _k1_xy; // gain for horizontal position correction
float _k2_xy; // gain for horizontal velocity correction
float _k3_xy; // gain for horizontal accelerometer offset correction
uint32_t _gps_last_update; // system time of last gps update
uint32_t _gps_last_time; // time of last gps update according to the gps itself
uint32_t _gps_last_update; // system time of last gps update in ms
uint32_t _gps_last_time; // time of last gps update according to the gps itself in ms
uint8_t _historic_xy_counter; // counter used to slow saving of position estimates for later comparison to gps
AP_BufferFloat_Size5 _hist_position_estimate_x; // buffer of historic accel based position to account for lag
AP_BufferFloat_Size5 _hist_position_estimate_y; // buffer of historic accel based position to account for lag
int32_t _base_lat; // base latitude
int32_t _base_lon; // base longitude
AP_BufferFloat_Size5 _hist_position_estimate_x; // buffer of historic accel based position to account for gpslag
AP_BufferFloat_Size5 _hist_position_estimate_y; // buffer of historic accel based position to account for gps lag
int32_t _base_lat; // base latitude (home location) in 100 nano degrees (i.e. degree value multiplied by 10,000,000)
int32_t _base_lon; // base longitude (home location) in 100 nano degrees (i.e. degree value multiplied by 10,000,000)
float _lon_to_cm_scaling; // conversion of longitude to centimeters
// Z Axis specific variables
AP_Float _time_constant_z; // time constant for vertical corrections
AP_Float _time_constant_z; // time constant for vertical corrections in s
float _k1_z; // gain for vertical position correction
float _k2_z; // gain for vertical velocity correction
float _k3_z; // gain for vertical accelerometer offset correction
uint32_t _baro_last_update; // time of last barometer update
AP_BufferFloat_Size15 _hist_position_estimate_z; // buffer of historic accel based altitudes to account for lag
uint32_t _baro_last_update; // time of last barometer update in ms
AP_BufferFloat_Size15 _hist_position_estimate_z; // buffer of historic accel based altitudes to account for barometer lag
// general variables
Vector3f _position_base; // position estimate
Vector3f _position_correction; // sum of correction to _comp_h from delayed 1st order samples
Vector3f _velocity; // latest velocity estimate (integrated from accelerometer values)
Vector3f _position_error;
Vector3f _position_base; // (uncorrected) position estimate in cm - relative to the home location (_base_lat, _base_lon, 0)
Vector3f _position_correction; // sum of corrections to _position_base from delayed 1st order samples in cm
Vector3f _velocity; // latest velocity estimate (integrated from accelerometer values) in cm/s
Vector3f _position_error; // current position error in cm - is set by the check_* methods and used by update method to calculate the correction terms
Vector3f _position; // sum(_position_base, _position_correction) - corrected position estimate in cm - relative to the home location (_base_lat, _base_lon, 0)
// GPS Glitch detector
GPS_Glitch& _glitch_detector;