diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index f54daac440..b0cb518d6b 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -105,8 +105,8 @@ #include // Photo or video camera #include // Camera/Antenna mount #include // needed for AHRS build -#include // ArduPilot Mega inertial navigation library -#include // Complementary filter for combining barometer altitude with accelerometers +#include // ArduPilot Mega inertial navigation library +#include // Complementary filter for combining barometer altitude with accelerometers #include // Configuration @@ -857,7 +857,7 @@ static float G_Dt = 0.02; // Inertial Navigation //////////////////////////////////////////////////////////////////////////////// #if INERTIAL_NAV == ENABLED -AP_InertialNav3D inertial_nav(&ahrs, &ins, &barometer, &g_gps); +AP_InertialNav inertial_nav(&ahrs, &ins, &barometer, &g_gps); #endif //////////////////////////////////////////////////////////////////////////////// diff --git a/ArduCopter/Log.pde b/ArduCopter/Log.pde index 7b30ed981c..fb6d02d4f4 100644 --- a/ArduCopter/Log.pde +++ b/ArduCopter/Log.pde @@ -790,10 +790,10 @@ static void Log_Write_INAV(float delta_t) DataFlash.WriteInt((int16_t)inertial_nav._position.z); // 2 accel + baro filtered altitude DataFlash.WriteInt((int16_t)climb_rate_actual); // 3 barometer based climb rate DataFlash.WriteInt((int16_t)inertial_nav._velocity.z); // 4 accel + baro based climb rate - DataFlash.WriteLong(get_int(inertial_nav._comp_filter3D._comp_k1o.x)); // 5 accel correction x-axis - DataFlash.WriteLong(get_int(inertial_nav._comp_filter3D._comp_k1o.y)); // 6 accel correction y-axis - DataFlash.WriteLong(get_int(inertial_nav._comp_filter3D._comp_k1o.z)); // 7 accel correction z-axis - DataFlash.WriteLong(get_int(inertial_nav._comp_filter3D.comp_k1o_ef.z));// 8 accel correction earth frame + DataFlash.WriteLong(get_int(inertial_nav._comp_filter._comp_k1o.x)); // 5 accel correction x-axis + DataFlash.WriteLong(get_int(inertial_nav._comp_filter._comp_k1o.y)); // 6 accel correction y-axis + DataFlash.WriteLong(get_int(inertial_nav._comp_filter._comp_k1o.z)); // 7 accel correction z-axis + DataFlash.WriteLong(get_int(inertial_nav._comp_filter.comp_k1o_ef.z));// 8 accel correction earth frame DataFlash.WriteLong(get_int(inertial_nav._accel_ef.x)); // 9 accel earth frame x-axis DataFlash.WriteLong(get_int(inertial_nav._accel_ef.y)); // 10 accel earth frame y-axis DataFlash.WriteLong(get_int(inertial_nav._accel_ef.z)); // 11 accel earth frame z-axis diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index db7903fa4b..de26fae1af 100644 --- a/ArduCopter/Parameters.pde +++ b/ArduCopter/Parameters.pde @@ -371,7 +371,7 @@ const AP_Param::Info var_info[] PROGMEM = { #if INERTIAL_NAV == ENABLED // @Group: INAV_ // @Path: ../libraries/AP_InertialNav/AP_InertialNav.cpp - GOBJECT(inertial_nav, "INAV_", AP_InertialNav3D), + GOBJECT(inertial_nav, "INAV_", AP_InertialNav), #endif GOBJECT(gcs0, "SR0_", GCS_MAVLINK), diff --git a/libraries/AP_InertialNav/AP_InertialNav3D.cpp b/libraries/AP_InertialNav/AP_InertialNav.cpp similarity index 79% rename from libraries/AP_InertialNav/AP_InertialNav3D.cpp rename to libraries/AP_InertialNav/AP_InertialNav.cpp index 7d2d3f01d3..3d81ec35ff 100644 --- a/libraries/AP_InertialNav/AP_InertialNav3D.cpp +++ b/libraries/AP_InertialNav/AP_InertialNav.cpp @@ -1,6 +1,6 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- #include -#include +#include #if defined(ARDUINO) && ARDUINO >= 100 #include "Arduino.h" @@ -9,36 +9,36 @@ #endif // table of user settable parameters -const AP_Param::GroupInfo AP_InertialNav3D::var_info[] PROGMEM = { +const AP_Param::GroupInfo AP_InertialNav::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), + AP_GROUPINFO("ACORR", 0, AP_InertialNav, _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), + AP_GROUPINFO("TC_XY", 1, AP_InertialNav, _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_GROUPINFO("TC_Z", 2, AP_InertialNav, _time_constant_z, AP_INTERTIALNAV_TC_Z), AP_GROUPEND }; // save_params - save all parameters to eeprom -void AP_InertialNav3D::save_params() +void AP_InertialNav::save_params() { - Vector3f accel_corr = _comp_filter3D.get_1st_order_correction(); + Vector3f accel_corr = _comp_filter.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); @@ -46,27 +46,27 @@ void AP_InertialNav3D::save_params() } // set time constant - set timeconstant used by complementary filter -void AP_InertialNav3D::set_time_constant_xy( float time_constant_in_seconds ) +void AP_InertialNav::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); + _comp_filter.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 ) +void AP_InertialNav::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); + _comp_filter.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() +void AP_InertialNav::check_baro() { uint32_t baro_update_time; @@ -85,7 +85,7 @@ void AP_InertialNav3D::check_baro() // 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) +void AP_InertialNav::correct_with_baro(float baro_alt, float dt) { static uint8_t first_reads = 0; @@ -96,8 +96,8 @@ void AP_InertialNav3D::correct_with_baro(float baro_alt, float dt) // 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); + _comp_filter.set_3rd_order_z(baro_alt); + //_comp_filter.set_2nd_order_z(climb_rate); first_reads++; } @@ -105,11 +105,11 @@ void AP_InertialNav3D::correct_with_baro(float baro_alt, float dt) Matrix3f dcm = _ahrs->get_dcm_matrix(); // provide baro alt to filter - _comp_filter3D.correct_3rd_order_z(baro_alt, dcm, dt); + _comp_filter.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) +void AP_InertialNav::set_current_position(int32_t lon, int32_t lat) { // set base location _base_lon = lon; @@ -120,14 +120,14 @@ void AP_InertialNav3D::set_current_position(int32_t lon, int32_t lat) _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); + _comp_filter.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() +void AP_InertialNav::check_gps() { uint32_t gps_time; uint32_t now; @@ -155,7 +155,7 @@ void AP_InertialNav3D::check_gps() } // 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) +void AP_InertialNav::correct_with_gps(int32_t lon, int32_t lat, float dt) { float x,y; @@ -174,12 +174,12 @@ void AP_InertialNav3D::correct_with_gps(int32_t lon, int32_t lat, float dt) Matrix3f dcm = _ahrs->get_dcm_matrix(); // call comp filter's correct xy - _comp_filter3D.correct_3rd_order_xy(-x, -y, dcm, dt); + _comp_filter.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) +void AP_InertialNav::update(float dt) { // discard samples where dt is too large if( dt > 0.1 ) { @@ -210,24 +210,24 @@ void AP_InertialNav3D::update(float dt) } // provide accelerometer values to filter - _comp_filter3D.add_1st_order_sample(_accel_ef); + _comp_filter.add_1st_order_sample(_accel_ef); // recalculate estimates - _comp_filter3D.calculate(dt, dcm); + _comp_filter.calculate(dt, dcm); // get position and velocity estimates - _position = _comp_filter3D.get_3rd_order_estimate(); - _velocity = _comp_filter3D.get_2nd_order_estimate(); + _position = _comp_filter.get_3rd_order_estimate(); + _velocity = _comp_filter.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() +bool AP_InertialNav::position_ok() { return _xy_enabled && (millis() - _gps_last_update < 3000); } // get accel based latitude -int32_t AP_InertialNav3D::get_latitude() +int32_t AP_InertialNav::get_latitude() { // make sure we've been initialised if( !_xy_enabled ) { @@ -239,7 +239,7 @@ int32_t AP_InertialNav3D::get_latitude() } // get accel based longitude -int32_t AP_InertialNav3D::get_longitude() +int32_t AP_InertialNav::get_longitude() { // make sure we've been initialised if( !_xy_enabled ) { @@ -251,7 +251,7 @@ int32_t AP_InertialNav3D::get_longitude() } // get accel based latitude -float AP_InertialNav3D::get_latitude_diff() +float AP_InertialNav::get_latitude_diff() { // make sure we've been initialised if( !_xy_enabled ) { @@ -264,7 +264,7 @@ float AP_InertialNav3D::get_latitude_diff() } // get accel based longitude -float AP_InertialNav3D::get_longitude_diff() +float AP_InertialNav::get_longitude_diff() { // make sure we've been initialised if( !_xy_enabled ) { @@ -277,7 +277,7 @@ float AP_InertialNav3D::get_longitude_diff() } // get velocity in latitude & longitude directions -float AP_InertialNav3D::get_latitude_velocity() +float AP_InertialNav::get_latitude_velocity() { // make sure we've been initialised if( !_xy_enabled ) { @@ -288,7 +288,7 @@ float AP_InertialNav3D::get_latitude_velocity() // Note: is +_velocity.x the output velocity in logs is in reverse direction from accel lat } -float AP_InertialNav3D::get_longitude_velocity() +float AP_InertialNav::get_longitude_velocity() { // make sure we've been initialised if( !_xy_enabled ) { diff --git a/libraries/AP_InertialNav/AP_InertialNav3D.h b/libraries/AP_InertialNav/AP_InertialNav.h similarity index 86% rename from libraries/AP_InertialNav/AP_InertialNav3D.h rename to libraries/AP_InertialNav/AP_InertialNav.h index 6178365e48..b19155d5d7 100644 --- a/libraries/AP_InertialNav/AP_InertialNav3D.h +++ b/libraries/AP_InertialNav/AP_InertialNav.h @@ -1,26 +1,26 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#ifndef __AP_INERTIALNAV3D_H__ -#define __AP_INERTIALNAV3D_H__ +#ifndef __AP_INERTIALNAV_H__ +#define __AP_INERTIALNAV_H__ #include #include // ArduPilot Mega IMU Library #include // ArduPilot Mega Barometer Library -#include // Complementary filter for combining barometer altitude with accelerometers +#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 + * AP_InertialNav is an attempt to use accelerometers to augment other sensors to improve altitud e position hold */ -class AP_InertialNav3D +class AP_InertialNav { public: // Constructor - AP_InertialNav3D( AP_AHRS* ahrs, AP_InertialSensor* ins, AP_Baro* baro, GPS** gps_ptr ) : + AP_InertialNav( AP_AHRS* ahrs, AP_InertialSensor* ins, AP_Baro* baro, GPS** gps_ptr ) : _ahrs(ahrs), _ins(ins), _baro(baro), @@ -28,7 +28,7 @@ public: _baro_last_update(0), _gps_last_update(0), _xy_enabled(false), - _comp_filter3D(AP_INTERTIALNAV_TC_XY, AP_INTERTIALNAV_TC_Z) + _comp_filter(AP_INTERTIALNAV_TC_XY, AP_INTERTIALNAV_TC_Z) {} // Initialisation @@ -70,11 +70,11 @@ public: // 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); } + virtual void set_altitude( int32_t new_altitude) { _comp_filter.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); } + virtual void set_velocity_z( int32_t new_velocity ) { _comp_filter.set_2nd_order_z(new_velocity); } // get latitude & longitude positions virtual int32_t get_latitude(); @@ -114,8 +114,8 @@ public: 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 + ThirdOrderCompFilter _comp_filter; // 3rd order complementary filter for combining baro readings with accelerometers }; -#endif // __AP_INERTIALNAV3D_H__ +#endif // __AP_INERTIALNAV_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 index caf27d3df0..c66059eb01 100644 --- a/libraries/AP_InertialNav/examples/AP_InertialNav_test/AP_InertialNav_test.pde +++ b/libraries/AP_InertialNav/examples/AP_InertialNav_test/AP_InertialNav_test.pde @@ -24,8 +24,8 @@ #include #include // PID library #include // PID library -#include -#include // Complementary filter for combining barometer altitude with accelerometers +#include +#include // Complementary filter for combining barometer altitude with accelerometers #include // ArduPilot general purpose FIFO buffer FastSerialPort(Serial, 0); diff --git a/libraries/Filter/ThirdOrderCompFilter3D.cpp b/libraries/Filter/ThirdOrderCompFilter.cpp similarity index 87% rename from libraries/Filter/ThirdOrderCompFilter3D.cpp rename to libraries/Filter/ThirdOrderCompFilter.cpp index 13199dfecb..8f68dc8f59 100644 --- a/libraries/Filter/ThirdOrderCompFilter3D.cpp +++ b/libraries/Filter/ThirdOrderCompFilter.cpp @@ -1,6 +1,6 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- #include -#include +#include #if defined(ARDUINO) && ARDUINO >= 100 #include "Arduino.h" @@ -11,7 +11,7 @@ // Public Methods ////////////////////////////////////////////////////////////// // update_gains - update gains from time constant (given in seconds) -void ThirdOrderCompFilter3D::update_gains(float time_constant_seconds_xy, float time_constant_seconds_z) +void ThirdOrderCompFilter::update_gains(float time_constant_seconds_xy, float time_constant_seconds_z) { static float last_time_constant_xy = 0; static float last_time_constant_z = 0; @@ -42,7 +42,7 @@ void ThirdOrderCompFilter3D::update_gains(float time_constant_seconds_xy, float } // set_3rd_order - resets the first order value (i.e. position) -void ThirdOrderCompFilter3D::set_3rd_order_xy(float x, float y) +void ThirdOrderCompFilter::set_3rd_order_xy(float x, float y) { _comp_h.x = x; _comp_h.y = y; @@ -55,27 +55,27 @@ void ThirdOrderCompFilter3D::set_3rd_order_xy(float x, float y) } // set_3rd_order - resets the first order value (i.e. position) -void ThirdOrderCompFilter3D::set_3rd_order_z(float z ) +void ThirdOrderCompFilter::set_3rd_order_z(float z ) { _comp_h.z = z; _comp_h_correction.z = 0; } // set_2nd_order - resets the second order value (i.e. velocity) -void ThirdOrderCompFilter3D::set_2nd_order_xy(float x, float y) +void ThirdOrderCompFilter::set_2nd_order_xy(float x, float y) { _comp_v.x = x; _comp_v.y = y; } // set_2nd_order - resets the second order value (i.e. velocity) -void ThirdOrderCompFilter3D::set_2nd_order_z(float z ) +void ThirdOrderCompFilter::set_2nd_order_z(float z ) { _comp_v.z = z; } // correct_3rd_order_z - correct accelerometer offsets using barometer or gps -void ThirdOrderCompFilter3D::correct_3rd_order_xy(float x, float y, Matrix3f& dcm_matrix, float deltat) +void ThirdOrderCompFilter::correct_3rd_order_xy(float x, float y, Matrix3f& dcm_matrix, float deltat) { float hist_comp_h_x, hist_comp_h_y; @@ -106,7 +106,7 @@ void ThirdOrderCompFilter3D::correct_3rd_order_xy(float x, float y, Matrix3f& dc } // correct_3rd_order_z - correct accelerometer offsets using barometer or gps -void ThirdOrderCompFilter3D::correct_3rd_order_z(float third_order_sample, Matrix3f& dcm_matrix, float deltat) +void ThirdOrderCompFilter::correct_3rd_order_z(float third_order_sample, Matrix3f& dcm_matrix, float deltat) { float hist_comp_h_z; @@ -133,7 +133,7 @@ void ThirdOrderCompFilter3D::correct_3rd_order_z(float third_order_sample, Matri } // recalculate the 2nd and 3rd order estimates -void ThirdOrderCompFilter3D::calculate(float deltat, Matrix3f& dcm_matrix) +void ThirdOrderCompFilter::calculate(float deltat, Matrix3f& dcm_matrix) { // get earth frame accelerometer correction comp_k1o_ef = dcm_matrix * _comp_k1o; diff --git a/libraries/Filter/ThirdOrderCompFilter3D.h b/libraries/Filter/ThirdOrderCompFilter.h similarity index 94% rename from libraries/Filter/ThirdOrderCompFilter3D.h rename to libraries/Filter/ThirdOrderCompFilter.h index 8f0ab48628..d0b1e0adc8 100644 --- a/libraries/Filter/ThirdOrderCompFilter3D.h +++ b/libraries/Filter/ThirdOrderCompFilter.h @@ -6,12 +6,12 @@ // your option) any later version. // -/// @file ThirdOrderCompFilter3D.h +/// @file ThirdOrderCompFilter.h /// @brief A class to implement third order complementary filter (for combining barometer and GPS with accelerometer data) /// math provided by Jonathan Challenger -#ifndef __THIRDORDERCOMPFILTER3D_H__ -#define __THIRDORDERCOMPFILTER3D_H__ +#ifndef __THIRD_ORDER_COMP_FILTER_H__ +#define __THIRD_ORDER_COMP_FILTER_H__ #include #include // Math library for matrix and vector math @@ -24,11 +24,11 @@ #define THIRD_ORDER_COMP_FILTER_HISTORIC_XY_SAVE_COUNTER_DEFAULT THIRD_ORDER_SAVE_POS_10HZ -class ThirdOrderCompFilter3D +class ThirdOrderCompFilter { public: // constructor - ThirdOrderCompFilter3D(float time_constant_seconds_xy, float time_constant_seconds_z) + ThirdOrderCompFilter(float time_constant_seconds_xy, float time_constant_seconds_z) { update_gains(time_constant_seconds_xy, time_constant_seconds_z); }; @@ -86,4 +86,4 @@ public: Vector3f comp_k1o_ef; // accelerometer correction in earth frame (only z element is used). here for debug purposes }; -#endif // __THIRDORDERCOMPFILTER3D_H__ +#endif // __THIRD_ORDER_COMP_FILTER_H__