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

@@ -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.x = x;
-    _position_base.y = pos_y;
+    _position_base.y = y;
     _position_correction.x = 0;
     _position_correction.y = 0;
 

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_update;           // system time of last gps update in ms
-    uint32_t                _gps_last_time;             // time of last gps update according to the gps itself
+    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_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 lag
+    AP_BufferFloat_Size5    _hist_position_estimate_y;  // buffer of historic accel based position to account for gps lag
-    int32_t                 _base_lat;                  // base latitude
+    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
+    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
+    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 lag
+    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_base;             // (uncorrected) position estimate in cm - relative to the home location (_base_lat, _base_lon, 0)
-    Vector3f                _position_correction;       // sum of correction to _comp_h from delayed 1st order samples    
+    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)
+    Vector3f                _velocity;                  // latest velocity estimate (integrated from accelerometer values) in cm/s
-    Vector3f                _position_error;
+    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;