From 8fc16d5cdfc88d1282ac8e432aabaa917d8b4dbe Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 25 Oct 2013 22:55:31 +0900 Subject: [PATCH] AP_InertialNav: add comments, make ahrs const from neurocopter 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 --- libraries/AP_InertialNav/AP_InertialNav.cpp | 36 +-- libraries/AP_InertialNav/AP_InertialNav.h | 238 ++++++++++++++++---- 2 files changed, 216 insertions(+), 58 deletions(-) diff --git a/libraries/AP_InertialNav/AP_InertialNav.cpp b/libraries/AP_InertialNav/AP_InertialNav.cpp index e016b97491..e0e7db178d 100644 --- a/libraries/AP_InertialNav/AP_InertialNav.cpp +++ b/libraries/AP_InertialNav/AP_InertialNav.cpp @@ -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; diff --git a/libraries/AP_InertialNav/AP_InertialNav.h b/libraries/AP_InertialNav/AP_InertialNav.h index 86ff599649..338a831579 100644 --- a/libraries/AP_InertialNav/AP_InertialNav.h +++ b/libraries/AP_InertialNav/AP_InertialNav.h @@ -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;