AP_InertialNav: first implementation of inertial nav library

libraries/AP_InertialNav/AP_InertialNav3D.cpp

@@ -0,0 +1,299 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+#include <FastSerial.h>
+#include <AP_InertialNav3D.h>
+
+#if defined(ARDUINO) && ARDUINO >= 100
+ #include "Arduino.h"
+#else
+ #include <wiring.h>
+#endif
+
+// table of user settable parameters
+const AP_Param::GroupInfo AP_InertialNav3D::var_info[] PROGMEM = {
+	// index 0 and 1 are for old parameters that are no longer used
+
+    // @Param: ACORR
+    // @DisplayName: Inertial Nav calculated accelerometer corrections
+    // @Description: calculated accelerometer corrections
+    // @Increment: 1
+    AP_GROUPINFO("ACORR",   0, AP_InertialNav3D, _accel_correction, 0),
+
+    // @Param: TC_XY
+    // @DisplayName: Horizontal Time Constant
+    // @Description: Time constnat for GPS and accel mixing. Higher TC increases GPS impact on position
+    // @Range: 0 10
+    // @Increment: 0.1
+    AP_GROUPINFO("TC_XY",   1, AP_InertialNav3D, _time_constant_xy, AP_INTERTIALNAV_TC_XY),
+
+    // @Param: TC_Z
+    // @DisplayName: Vertical Time Constant
+    // @Description: Time constnat for baro and accel mixing. Higher TC increases barometers impact on altitude
+    // @Range: 0 10
+    // @Increment: 0.1
+    AP_GROUPINFO("TC_Z",    2, AP_InertialNav3D, _time_constant_z, AP_INTERTIALNAV_TC_Z),
+
+    AP_GROUPEND
+};
+
+// save_params - save all parameters to eeprom
+void AP_InertialNav3D::save_params()
+{
+    Vector3f accel_corr = _comp_filter3D.get_1st_order_correction();
+    accel_corr.x = constrain(accel_corr.x,-200,200);
+    accel_corr.y = constrain(accel_corr.y,-200,200);
+    accel_corr.z = constrain(accel_corr.z,-200,200);
+    _accel_correction.set_and_save(accel_corr);
+}
+
+// set time constant - set timeconstant used by complementary filter
+void AP_InertialNav3D::set_time_constant_xy( float time_constant_in_seconds )
+{
+    // ensure it's a reasonable value
+    if( time_constant_in_seconds > 0 && time_constant_in_seconds < 30 ) {
+        _time_constant_xy = time_constant_in_seconds;
+        _comp_filter3D.update_gains(_time_constant_xy, _time_constant_z);
+    }
+}
+
+// set time constant - set timeconstant used by complementary filter
+void AP_InertialNav3D::set_time_constant_z( float time_constant_in_seconds )
+{
+    // ensure it's a reasonable value
+    if( time_constant_in_seconds > 0 && time_constant_in_seconds < 30 ) {
+        _time_constant_z = time_constant_in_seconds;
+        _comp_filter3D.update_gains(_time_constant_xy, _time_constant_z);
+    }
+}
+
+// check_baro - check if new baro readings have arrived and use them to correct vertical accelerometer offsets
+void AP_InertialNav3D::check_baro()
+{
+    uint32_t baro_update_time;
+
+    if( _baro == NULL )
+        return;
+
+    // calculate time since last baro reading
+    baro_update_time = _baro->get_last_update();
+    if( baro_update_time != _baro_last_update ) {
+        float dt = (float)(baro_update_time - _baro_last_update) / 1000.0;
+        // call correction method
+        correct_with_baro(_baro->get_altitude()*100, dt);
+        _baro_last_update = baro_update_time;
+    }
+}
+
+
+// correct_with_baro - modifies accelerometer offsets using barometer.  dt is time since last baro reading
+void AP_InertialNav3D::correct_with_baro(float baro_alt, float dt)
+{
+    static uint8_t first_reads = 0;
+
+    // discard samples where dt is too large
+    if( dt > 0.2 ) {
+        return;
+    }
+
+    // discard first 10 reads but perform some initialisation
+    if( first_reads <= 10 ) {
+        _comp_filter3D.set_3rd_order_z(baro_alt);
+        //_comp_filter3D.set_2nd_order_z(climb_rate);
+        first_reads++;
+    }
+
+    // get dcm matrix
+    Matrix3f dcm = _ahrs->get_dcm_matrix();
+
+    // provide baro alt to filter
+    _comp_filter3D.correct_3rd_order_z(baro_alt, dcm, dt);
+}
+
+// set_current_position - all internal calculations are recorded as the distances from this point
+void AP_InertialNav3D::set_current_position(int32_t lon, int32_t lat)
+{
+    // set base location
+    _base_lon = lon;
+    _base_lat = lat;
+
+    // set longitude->meters scaling
+    // this is used to offset the shrinking longitude as we go towards the poles
+    _lon_to_m_scaling = cos((fabs((float)lat)/10000000.0) * 0.0174532925);
+
+    // set estimated position to this position
+    _comp_filter3D.set_3rd_order_xy(0,0);
+
+    // set xy as enabled
+    _xy_enabled = true;
+}
+
+// check_gps - check if new gps readings have arrived and use them to correct position estimates
+void AP_InertialNav3D::check_gps()
+{
+    uint32_t gps_time;
+    uint32_t now;
+
+    if( _gps_ptr == NULL || *_gps_ptr == NULL )
+        return;
+
+    // get time according to the gps
+    gps_time = (*_gps_ptr)->time;
+
+    // compare gps time to previous reading
+    if( gps_time != _gps_last_time ) {
+
+        // calculate time since last gps reading
+        now = millis();
+        float dt = (float)(now - _gps_last_update) / 1000.0;
+
+        // call position correction method
+        correct_with_gps((*_gps_ptr)->longitude, (*_gps_ptr)->latitude, dt);
+
+        // record gps time and system time of this update
+        _gps_last_time = gps_time;
+        _gps_last_update = now;
+    }
+}
+
+// correct_with_gps - modifies accelerometer offsets using gps.  dt is time since last gps update
+void AP_InertialNav3D::correct_with_gps(int32_t lon, int32_t lat, float dt)
+{
+    float x,y;
+
+    // discard samples where dt is too large
+    if( dt > 1.0 || dt == 0 || !_xy_enabled) {
+        return;
+    }
+
+    // calculate distance from home
+    //x = (float)(lat - _base_lat) * 1.113195;
+    //y = (float)(lon - _base_lon) * _lon_to_m_scaling * 1.113195;
+    x = (float)(lat - _base_lat);
+    y = (float)(lon - _base_lon) * _lon_to_m_scaling;
+
+    // convert accelerometer readings to earth frame
+    Matrix3f dcm = _ahrs->get_dcm_matrix();
+
+    // call comp filter's correct xy
+    _comp_filter3D.correct_3rd_order_xy(-x, -y, dcm, dt);
+    //Notes: with +x above, accel lat comes out reversed
+}
+
+// update - updates velocities and positions using latest info from ahrs, ins and barometer if new data is available;
+void AP_InertialNav3D::update(float dt)
+{
+    // discard samples where dt is too large
+    if( dt > 0.1 ) {
+        return;
+    }
+
+    // check barometer
+    check_baro();
+
+    // check gps
+    check_gps();
+
+    // read acclerometer values
+    _accel_bf = _ins->get_accel();
+
+    // convert accelerometer readings to earth frame
+    Matrix3f dcm = _ahrs->get_dcm_matrix();
+    _accel_ef = dcm * _accel_bf;
+
+    // remove influence of gravity
+    _accel_ef.z += AP_INTERTIALNAV_GRAVITY;
+    _accel_ef *= 100;
+
+    // remove xy if not enabled
+    if( !_xy_enabled ) {
+        _accel_ef.x = 0;
+        _accel_ef.y = 0;
+    }
+
+    // provide accelerometer values to filter
+    _comp_filter3D.add_1st_order_sample(_accel_ef);
+
+    // recalculate estimates
+    _comp_filter3D.calculate(dt, dcm);
+    
+    // get position and velocity estimates
+    _position = _comp_filter3D.get_3rd_order_estimate();
+    _velocity = _comp_filter3D.get_2nd_order_estimate();
+}
+
+// position_ok - return true if position has been initialised and have received gps data within 3 seconds
+bool AP_InertialNav3D::position_ok()
+{
+    return _xy_enabled && (millis() - _gps_last_update < 3000);
+}
+
+// get accel based latitude
+int32_t AP_InertialNav3D::get_latitude()
+{
+    // make sure we've been initialised
+    if( !_xy_enabled ) {
+        return 0;
+    }
+
+    //return _base_lat - (int32_t)(_position.x / 1.113195);
+    return _base_lat - (int32_t)_position.x;
+}
+
+// get accel based longitude
+int32_t AP_InertialNav3D::get_longitude()
+{
+    // make sure we've been initialised
+    if( !_xy_enabled ) {
+        return 0;
+    }
+
+    //return _base_lon - (int32_t)(_position.y / (_lon_to_m_scaling * 1.113195) );
+    return _base_lon - (int32_t)(_position.y / _lon_to_m_scaling );
+}
+
+// get accel based latitude
+float AP_InertialNav3D::get_latitude_diff()
+{
+    // make sure we've been initialised
+    if( !_xy_enabled ) {
+        return 0;
+    }
+
+    //return _base_lat + (int32_t)_position.x;
+    //return -_position.x / 1.113195;
+    return -_position.x;
+}
+
+// get accel based longitude
+float AP_InertialNav3D::get_longitude_diff()
+{
+    // make sure we've been initialised
+    if( !_xy_enabled ) {
+        return 0;
+    }
+
+    //return _base_lon - (int32_t)(_position.x / _lon_to_m_scaling);
+    //return -_position.y / (_lon_to_m_scaling * 1.113195);
+    return -_position.y / _lon_to_m_scaling;
+}
+
+// get velocity in latitude & longitude directions
+float AP_InertialNav3D::get_latitude_velocity()
+{
+    // make sure we've been initialised
+    if( !_xy_enabled ) {
+        return 0;
+    }
+
+    return -_velocity.x;
+    // Note: is +_velocity.x the output velocity in logs is in reverse direction from accel lat
+}
+
+float AP_InertialNav3D::get_longitude_velocity()
+{
+    // make sure we've been initialised
+    if( !_xy_enabled ) {
+        return 0;
+    }
+
+    return -_velocity.y;
+}

libraries/AP_InertialNav/AP_InertialNav3D.h

@@ -0,0 +1,121 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#ifndef __AP_INERTIALNAV3D_H__
+#define __AP_INERTIALNAV3D_H__
+
+#include <AP_AHRS.h>
+#include <AP_InertialSensor.h>          // ArduPilot Mega IMU Library
+#include <AP_Baro.h>                    // ArduPilot Mega Barometer Library
+#include <ThirdOrderCompFilter3D.h>     // Complementary filter for combining barometer altitude with accelerometers
+
+#define AP_INTERTIALNAV_GRAVITY 9.80665
+#define AP_INTERTIALNAV_TC_XY   3.0 // default time constant for complementary filter's X & Y axis
+#define AP_INTERTIALNAV_TC_Z    1.5 // default time constant for complementary filter's Z axis
+
+/*
+ * AP_InertialNav3D is an attempt to use accelerometers to augment other sensors to improve altitud e position hold
+ */
+class AP_InertialNav3D
+{
+public:
+
+    // Constructor
+    AP_InertialNav3D( AP_AHRS* ahrs, AP_InertialSensor* ins, AP_Baro* baro, GPS** gps_ptr ) :
+        _ahrs(ahrs),
+        _ins(ins),
+        _baro(baro),
+        _gps_ptr(gps_ptr),
+        _baro_last_update(0),
+        _gps_last_update(0),
+        _xy_enabled(false),
+        _comp_filter3D(AP_INTERTIALNAV_TC_XY, AP_INTERTIALNAV_TC_Z)
+        {}
+
+    // Initialisation
+    virtual void        init() { 
+                            set_time_constant_xy(_time_constant_xy);
+                            set_time_constant_z(_time_constant_z); 
+        }
+
+    // save_params - save all parameters to eeprom
+    virtual void        save_params();
+
+    // set time constant - set timeconstant used by complementary filter
+    virtual void        set_time_constant_xy( float time_constant_in_seconds );
+
+    // set time constant - set timeconstant used by complementary filter
+    virtual void        set_time_constant_z( float time_constant_in_seconds );
+
+    // check_baro - check if new baro readings have arrived and use them to correct vertical accelerometer offsets
+    virtual void        check_baro();
+
+    // correct_with_baro - modifies accelerometer offsets using barometer.  dt is time since last baro reading
+    virtual void        correct_with_baro(float baro_alt, float dt);
+
+    // set_current_position - all internal calculations are recorded as the distances from this point
+    virtual void        set_current_position(int32_t lon, int32_t lat);
+
+    // check_gps - check if new gps readings have arrived and use them to correct position estimates
+    virtual void        check_gps();
+
+    // correct_with_gps - modifies accelerometer offsets using gps.  dt is time since last gps update
+    virtual void        correct_with_gps(int32_t lon, int32_t lat, float dt);
+
+    // update - updates velocities and positions using latest info from accelerometers;
+    virtual void        update(float dt);
+
+    // altitude_ok, position_ok - true if inertial based altitude and position can be trusted
+    virtual bool        altitude_ok() { return true; }
+    virtual bool        position_ok();
+
+    // get_altitude - get latest altitude estimate in cm
+    virtual float       get_altitude() { return _position.z; }
+    virtual void        set_altitude( int32_t new_altitude) { _comp_filter3D.set_3rd_order_z(new_altitude); }
+
+    // get_velocity_z - get latest climb rate (in cm/s)
+    virtual float       get_velocity_z() { return _velocity.z; }
+    virtual void        set_velocity_z( int32_t new_velocity ) { _comp_filter3D.set_2nd_order_z(new_velocity); }
+
+    // get latitude & longitude positions
+    virtual int32_t     get_latitude();
+    virtual int32_t     get_longitude();
+
+        // get latitude & longitude positions from base location
+    virtual float       get_latitude_diff();
+    virtual float       get_longitude_diff();
+    
+    // get velocity in latitude & longitude directions
+    virtual float       get_latitude_velocity();
+    virtual float       get_longitude_velocity();
+
+    // class level parameters
+    static const struct AP_Param::GroupInfo var_info[];
+
+//protected:
+    AP_AHRS*        _ahrs;
+    AP_InertialSensor*  _ins;
+    AP_Baro*        _baro;
+    GPS**           _gps_ptr;
+
+    uint32_t        _baro_last_update;
+    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
+
+    bool            _xy_enabled;
+
+    AP_Float        _time_constant_xy;      // time constant for complementary filter's horizontal corrections
+    AP_Float        _time_constant_z;       // time constant for complementary filter's vertical corrections
+    Vector3f        _accel_bf;              // latest accelerometer values
+    Vector3f        _accel_ef;              // accelerometer values converted from body to earth frame
+    AP_Vector3f     _accel_correction;      // accelerometer corrections calculated by complementary filter
+    Vector3f        _velocity;              // latest velocity estimate (integrated from accelerometer values)
+    Vector3f        _position;              // latest position estimate
+    int32_t         _base_lat;              // base latitude
+    int32_t         _base_lon;              // base longitude
+    float           _lon_to_m_scaling;      // conversion of longitude to meters
+
+    ThirdOrderCompFilter3D   _comp_filter3D;   // 3rd order complementary filter for combining baro readings with accelerometers
+
+};
+
+#endif // __AP_INERTIALNAV3D_H__

libraries/AP_InertialNav/examples/AP_InertialNav_test/AP_InertialNav_test.pde

@@ -0,0 +1,70 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
+
+//
+// Simple test for the AP_InertialSensor MPU6000 driver.
+//
+
+#include <FastSerial.h>
+#include <AP_Common.h>
+#include <Arduino_Mega_ISR_Registry.h>
+#include <AP_GPS.h>             // ArduPilot GPS library
+#include <I2C.h>                // Arduino I2C lib
+#include <SPI.h>                // Arduino SPI lib
+#include <SPI3.h>               // SPI3 library
+#include <AP_ADC.h>             // ArduPilot Mega Analog to Digital Converter Library
+#include <AP_AnalogSource.h>
+#include <AP_Baro.h>            // ArduPilot Mega Barometer Library
+#include <Filter.h>
+#include <AP_Compass.h>         // ArduPilot Mega Magnetometer Library
+#include <AP_Math.h>            // ArduPilot Mega Vector/Matrix math Library
+#include <AP_InertialSensor.h>  // ArduPilot Mega Inertial Sensor (accel & gyro) Library
+#include <AP_IMU.h>             // ArduPilot Mega IMU Library
+#include <AP_PeriodicProcess.h> // Parent header of Timer
+#include <AP_TimerProcess.h>    // TimerProcess is the scheduler for MPU6000 reads.
+#include <AP_AHRS.h>
+#include <AP_Airspeed.h>
+#include <AC_PID.h>             // PID library
+#include <APM_PI.h>             // PID library
+#include <AP_InertialNav3D.h>
+#include <ThirdOrderCompFilter3D.h>   // Complementary filter for combining barometer altitude with accelerometers
+#include <AP_Buffer.h>          // ArduPilot general purpose FIFO buffer
+
+FastSerialPort(Serial, 0);
+
+Arduino_Mega_ISR_Registry isr_registry;
+AP_TimerProcess scheduler;
+AP_InertialSensor_MPU6000 ins;
+
+void setup(void)
+{
+    Serial.begin(115200);
+    Serial.println("Doing INS startup...");
+
+    SPI.begin();
+    SPI.setClockDivider(SPI_CLOCK_DIV16); // 1MHZ SPI rate
+
+    isr_registry.init();
+    scheduler.init(&isr_registry);
+
+    // we need to stop the barometer from holding the SPI bus
+    pinMode(40, OUTPUT);
+    digitalWrite(40, HIGH);
+
+    ins.init(AP_InertialSensor::COLD_START, delay, NULL, &scheduler);
+}
+
+void loop(void)
+{
+    Vector3f accel;
+    Vector3f gyro;
+    float temperature;
+
+    delay(20);
+    ins.update();
+    gyro = ins.get_gyro();
+    accel = ins.get_accel();
+    temperature = ins.temperature();
+
+    Serial.printf("AX: %f  AY: %f  AZ: %f  GX: %f  GY: %f  GZ: %f T=%f\n",
+                  accel.x, accel.y, accel.z, gyro.x, gyro.y, gyro.z, temperature);
+}

libraries/AP_InertialNav/examples/AP_InertialNav_test/Makefile

@@ -0,0 +1 @@
+include ../../../AP_Common/Arduino.mk