diff --git a/libraries/AP_InertialNav/AP_InertialNav3D.cpp b/libraries/AP_InertialNav/AP_InertialNav3D.cpp new file mode 100644 index 0000000000..7d2d3f01d3 --- /dev/null +++ b/libraries/AP_InertialNav/AP_InertialNav3D.cpp @@ -0,0 +1,299 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#include +#include + +#if defined(ARDUINO) && ARDUINO >= 100 + #include "Arduino.h" +#else + #include +#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; +} \ No newline at end of file diff --git a/libraries/AP_InertialNav/AP_InertialNav3D.h b/libraries/AP_InertialNav/AP_InertialNav3D.h new file mode 100644 index 0000000000..6178365e48 --- /dev/null +++ b/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 +#include // ArduPilot Mega IMU Library +#include // ArduPilot Mega Barometer Library +#include // 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__ diff --git a/libraries/AP_InertialNav/examples/AP_InertialNav_test/AP_InertialNav_test.pde b/libraries/AP_InertialNav/examples/AP_InertialNav_test/AP_InertialNav_test.pde new file mode 100644 index 0000000000..c0a99d90d9 --- /dev/null +++ b/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 +#include +#include +#include // ArduPilot GPS library +#include // Arduino I2C lib +#include // Arduino SPI lib +#include // SPI3 library +#include // ArduPilot Mega Analog to Digital Converter Library +#include +#include // ArduPilot Mega Barometer Library +#include +#include // ArduPilot Mega Magnetometer Library +#include // ArduPilot Mega Vector/Matrix math Library +#include // ArduPilot Mega Inertial Sensor (accel & gyro) Library +#include // ArduPilot Mega IMU Library +#include // Parent header of Timer +#include // TimerProcess is the scheduler for MPU6000 reads. +#include +#include +#include // PID library +#include // PID library +#include +#include // Complementary filter for combining barometer altitude with accelerometers +#include // 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); +} diff --git a/libraries/AP_InertialNav/examples/AP_InertialNav_test/Makefile b/libraries/AP_InertialNav/examples/AP_InertialNav_test/Makefile new file mode 100644 index 0000000000..d1f40fd90f --- /dev/null +++ b/libraries/AP_InertialNav/examples/AP_InertialNav_test/Makefile @@ -0,0 +1 @@ +include ../../../AP_Common/Arduino.mk