diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index 2bb2dd1131..75e84e6c88 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -951,7 +951,7 @@ get_throttle_rate(int16_t z_target_speed) // calculate rate error #if INERTIAL_NAV_Z == ENABLED - z_rate_error = z_target_speed - inertial_nav._velocity.z; // calc the speed error + z_rate_error = z_target_speed - inertial_nav.get_velocity_z(); // calc the speed error #else z_rate_error = z_target_speed - climb_rate; // calc the speed error #endif @@ -1071,7 +1071,7 @@ get_throttle_land() // detect whether we have landed by watching for minimum throttle and now movement #if INERTIAL_NAV_Z == ENABLED - if (abs(inertial_nav._velocity.z) < 20 && (g.rc_3.servo_out <= get_angle_boost(g.throttle_min) || g.pid_throttle_accel.get_integrator() <= -150)) { + if (abs(inertial_nav.get_velocity_z()) < 20 && (g.rc_3.servo_out <= get_angle_boost(g.throttle_min) || g.pid_throttle_accel.get_integrator() <= -150)) { #else if (abs(climb_rate) < 20 && (g.rc_3.servo_out <= get_angle_boost(g.throttle_min) || g.pid_throttle_accel.get_integrator() <= -150)) { #endif diff --git a/ArduCopter/Log.pde b/ArduCopter/Log.pde index 90ffc52f76..778ce142a3 100644 --- a/ArduCopter/Log.pde +++ b/ArduCopter/Log.pde @@ -786,32 +786,30 @@ static void Log_Read_Attitude() (unsigned)temp7); } -// Write an INAV packet. Total length : 36 Bytes +// Write an INAV packet. Total length : 52 Bytes static void Log_Write_INAV(float delta_t) { #if INERTIAL_NAV_XY == ENABLED || INERTIAL_NAV_Z == ENABLED + Vector3f accel_corr = inertial_nav.accel_correction.get(); + DataFlash.WriteByte(HEAD_BYTE1); DataFlash.WriteByte(HEAD_BYTE2); DataFlash.WriteByte(LOG_INAV_MSG); DataFlash.WriteInt((int16_t)baro_alt); // 1 barometer altitude - DataFlash.WriteInt((int16_t)inertial_nav._position.z); // 2 accel + baro filtered altitude + DataFlash.WriteInt((int16_t)inertial_nav.get_altitude()); // 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_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 - DataFlash.WriteLong(get_int(delta_t)); // 12 time delta of samples - DataFlash.WriteLong(g_gps->latitude-home.lat); // 13 lat from home - DataFlash.WriteLong(g_gps->longitude-home.lng); // 14 lon from home - DataFlash.WriteLong(get_int(inertial_nav.get_latitude_diff())); // 15 accel based lat from home - DataFlash.WriteLong(get_int(inertial_nav.get_longitude_diff())); // 16 accel based lon from home - DataFlash.WriteLong(get_int(inertial_nav.get_latitude_velocity())); // 17 accel based lat velocity - DataFlash.WriteLong(get_int(inertial_nav.get_longitude_velocity())); // 18 accel based lon velocity + DataFlash.WriteInt((int16_t)inertial_nav.get_velocity_z()); // 4 accel + baro based climb rate + DataFlash.WriteLong(get_int(accel_corr.x)); // 5 accel correction x-axis + DataFlash.WriteLong(get_int(accel_corr.y)); // 6 accel correction y-axis + DataFlash.WriteLong(get_int(accel_corr.z)); // 7 accel correction z-axis + DataFlash.WriteLong(get_int(inertial_nav.accel_correction_ef.z)); // 8 accel correction earth frame + DataFlash.WriteLong(g_gps->latitude-home.lat); // 9 lat from home + DataFlash.WriteLong(g_gps->longitude-home.lng); // 10 lon from home + DataFlash.WriteLong(get_int(inertial_nav.get_latitude_diff())); // 11 accel based lat from home + DataFlash.WriteLong(get_int(inertial_nav.get_longitude_diff())); // 12 accel based lon from home + DataFlash.WriteLong(get_int(inertial_nav.get_latitude_velocity())); // 13 accel based lat velocity + DataFlash.WriteLong(get_int(inertial_nav.get_longitude_velocity())); // 14 accel based lon velocity DataFlash.WriteByte(END_BYTE); #endif @@ -828,18 +826,14 @@ static void Log_Read_INAV() float temp6 = get_float(DataFlash.ReadLong()); // 6 accel correction y-axis float temp7 = get_float(DataFlash.ReadLong()); // 7 accel correction z-axis float temp8 = get_float(DataFlash.ReadLong()); // 8 accel correction earth frame - float temp9 = get_float(DataFlash.ReadLong()); // 9 accel earth frame x-axis - float temp10 = get_float(DataFlash.ReadLong()); // 10 accel earth frame y-axis - float temp11 = get_float(DataFlash.ReadLong()); // 11 accel earth frame z-axis - float temp12 = get_float(DataFlash.ReadLong()); // 12 time delta of samples - int32_t temp13 = DataFlash.ReadLong(); // 13 lat from home - int32_t temp14 = DataFlash.ReadLong(); // 14 lon from home - float temp15 = get_float(DataFlash.ReadLong()); // 15 accel based lat from home - float temp16 = get_float(DataFlash.ReadLong()); // 16 accel based lon from home - float temp17 = get_float(DataFlash.ReadLong()); // 17 accel based lat velocity - float temp18 = get_float(DataFlash.ReadLong()); // 18 accel based lon velocity - // 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 - cliSerial->printf_P(PSTR("INAV, %d, %d, %d, %d, %6.4f, %6.4f, %6.4f, %6.4f, %6.4f, %6.4f, %6.4f, %6.4f, %ld, %ld, %6.4f, %6.4f, %6.4f, %6.4f\n"), + int32_t temp9 = DataFlash.ReadLong(); // 9 lat from home + int32_t temp10 = DataFlash.ReadLong(); // 10 lon from home + float temp11 = get_float(DataFlash.ReadLong()); // 11 accel based lat from home + float temp12 = get_float(DataFlash.ReadLong()); // 12 accel based lon from home + float temp13 = get_float(DataFlash.ReadLong()); // 13 accel based lat velocity + float temp14 = get_float(DataFlash.ReadLong()); // 14 accel based lon velocity + // 1 2 3 4 5 6 7 8 9 10 11 12 13 14 + cliSerial->printf_P(PSTR("INAV, %d, %d, %d, %d, %6.4f, %6.4f, %6.4f, %6.4f, %ld, %ld, %6.4f, %6.4f, %6.4f, %6.4f\n"), (int)temp1, (int)temp2, (int)temp3, @@ -853,11 +847,7 @@ static void Log_Read_INAV() temp11, temp12, temp13, - temp14, - temp15, - temp16, - temp17, - temp18); + temp14); } // Write a mode packet. Total length : 7 bytes diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index 05e8a68cdd..98e32ca83e 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -541,7 +541,7 @@ static int32_t get_altitude_error() #if INERTIAL_NAV_Z == ENABLED // use inertial nav for altitude error - return next_WP.alt - inertial_nav._position.z; + return next_WP.alt - inertial_nav.get_altitude(); #else return next_WP.alt - current_loc.alt; #endif diff --git a/libraries/AP_InertialNav/AP_InertialNav.cpp b/libraries/AP_InertialNav/AP_InertialNav.cpp index 3d81ec35ff..957932d10c 100644 --- a/libraries/AP_InertialNav/AP_InertialNav.cpp +++ b/libraries/AP_InertialNav/AP_InertialNav.cpp @@ -10,24 +10,22 @@ // table of user settable parameters 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_InertialNav, _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 + // @Description: Time constant for GPS and accel mixing. Higher TC decreases GPS impact on position estimate // @Range: 0 10 // @Increment: 0.1 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 + // @Description: Time constant for baro and accel mixing. Higher TC decreases barometers impact on altitude estimate // @Range: 0 10 // @Increment: 0.1 AP_GROUPINFO("TC_Z", 2, AP_InertialNav, _time_constant_z, AP_INTERTIALNAV_TC_Z), @@ -35,95 +33,93 @@ const AP_Param::GroupInfo AP_InertialNav::var_info[] PROGMEM = { AP_GROUPEND }; +// init - initialise library +void AP_InertialNav::init() +{ + // recalculate the gains + update_gains(); +} + // save_params - save all parameters to eeprom void AP_InertialNav::save_params() { - 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); - _accel_correction.set_and_save(accel_corr); + Vector3f accel_corr = accel_correction.get(); + accel_corr.x = constrain(accel_corr.x,-100,100); + accel_corr.y = constrain(accel_corr.y,-100,100); + accel_corr.z = constrain(accel_corr.z,-100,100); + accel_correction.set_and_save(accel_corr); } +// update - updates velocities and positions using latest info from ahrs, ins and barometer if new data is available; +void AP_InertialNav::update(float dt) +{ + Vector3f acc_corr = accel_correction.get(); + Vector3f accel_ef; + + // discard samples where dt is too large + if( dt > 0.1 ) { + return; + } + + // check barometer + check_baro(); + + // check gps + check_gps(); + + // convert accelerometer readings to earth frame + Matrix3f dcm = _ahrs->get_dcm_matrix(); + accel_ef = dcm * _ins->get_accel(); + + // 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; + } + + // get earth frame accelerometer correction + accel_correction_ef = dcm * acc_corr; + + // calculate velocity by adding new acceleration from accelerometers + _velocity += (-accel_ef + accel_correction_ef) * dt; + + // calculate new estimate of position + _position_base += _velocity * dt; + + // store 3rd order estimate (i.e. estimated vertical position) for future use + _hist_position_estimate_z.add(_position_base.z); + + // store 3rd order estimate (i.e. horizontal position) for future use at 10hz + _historic_xy_counter++; + if( _historic_xy_counter >= AP_INTERTIALNAV_SAVE_POS_AFTER_ITERATIONS ) { + _historic_xy_counter = 0; + _hist_position_estimate_x.add(_position_base.x); + _hist_position_estimate_y.add(_position_base.y); + } +} + +// +// XY Axis specific methods +// + // set time constant - set timeconstant used by complementary filter 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_filter.update_gains(_time_constant_xy, _time_constant_z); + update_gains(); } } -// set time constant - set timeconstant used by complementary filter -void AP_InertialNav::set_time_constant_z( float time_constant_in_seconds ) +// position_ok - return true if position has been initialised and have received gps data within 3 seconds +bool AP_InertialNav::position_ok() { - // 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_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_InertialNav::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_InertialNav::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_filter.set_3rd_order_z(baro_alt); - //_comp_filter.set_2nd_order_z(climb_rate); - first_reads++; - } - - // get dcm matrix - Matrix3f dcm = _ahrs->get_dcm_matrix(); - - // provide baro alt to filter - _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_InertialNav::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_filter.set_3rd_order_xy(0,0); - - // set xy as enabled - _xy_enabled = true; + return _xy_enabled && (millis() - _gps_last_update < 3000); } // check_gps - check if new gps readings have arrived and use them to correct position estimates @@ -158,13 +154,14 @@ void AP_InertialNav::check_gps() void AP_InertialNav::correct_with_gps(int32_t lon, int32_t lat, float dt) { float x,y; + float hist_position_base_x, hist_position_base_y; // discard samples where dt is too large if( dt > 1.0 || dt == 0 || !_xy_enabled) { return; } - // calculate distance from home + // calculate distance from base location //x = (float)(lat - _base_lat) * 1.113195; //y = (float)(lon - _base_lon) * _lon_to_m_scaling * 1.113195; x = (float)(lat - _base_lat); @@ -173,57 +170,35 @@ void AP_InertialNav::correct_with_gps(int32_t lon, int32_t lat, float dt) // convert accelerometer readings to earth frame Matrix3f dcm = _ahrs->get_dcm_matrix(); - // call comp filter's correct xy - _comp_filter.correct_3rd_order_xy(-x, -y, dcm, dt); - //Notes: with +x above, accel lat comes out reversed -} + // correct accelerometer offsets using gps -// update - updates velocities and positions using latest info from ahrs, ins and barometer if new data is available; -void AP_InertialNav::update(float dt) -{ - // discard samples where dt is too large - if( dt > 0.1 ) { - return; + // ublox gps positions are delayed by 400ms + // we store historical position at 10hz so 4 iterations ago + if( _hist_position_estimate_x.num_items() >= AP_INTERTIALNAV_GPS_LAG_IN_10HZ_INCREMENTS ) { + hist_position_base_x = _hist_position_estimate_x.peek(AP_INTERTIALNAV_GPS_LAG_IN_10HZ_INCREMENTS-1); + hist_position_base_y = _hist_position_estimate_y.peek(AP_INTERTIALNAV_GPS_LAG_IN_10HZ_INCREMENTS-1); + }else{ + hist_position_base_x = _position_base.x; + hist_position_base_y = _position_base.y; } - // check barometer - check_baro(); + // calculate error in position from gps with our historical estimate + // To-Do: check why x and y are reversed + float err_x = -x - (hist_position_base_x + _position_correction.x); + float err_y = -y - (hist_position_base_y + _position_correction.y); - // check gps - check_gps(); + // calculate correction to accelerometers and apply in the body frame + Vector3f accel_corr = accel_correction.get(); + accel_corr += dcm.mul_transpose(Vector3f((err_x*_k3_xy)*dt,(err_y*_k3_xy)*dt,0)); + accel_correction.set(accel_corr); - // read acclerometer values - _accel_bf = _ins->get_accel(); + // correct velocity + _velocity.x += (err_x*_k2_xy) * dt; + _velocity.y += (err_y*_k2_xy) * dt; - // 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_filter.add_1st_order_sample(_accel_ef); - - // recalculate estimates - _comp_filter.calculate(dt, dcm); - - // get position and velocity estimates - _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_InertialNav::position_ok() -{ - return _xy_enabled && (millis() - _gps_last_update < 3000); + // correct position + _position_correction.x += err_x*_k1_xy * dt; + _position_correction.y += err_y*_k1_xy * dt; } // get accel based latitude @@ -235,7 +210,7 @@ int32_t AP_InertialNav::get_latitude() } //return _base_lat - (int32_t)(_position.x / 1.113195); - return _base_lat - (int32_t)_position.x; + return _base_lat - (int32_t)(_position_base.x + _position_correction.x); } // get accel based longitude @@ -247,7 +222,32 @@ int32_t AP_InertialNav::get_longitude() } //return _base_lon - (int32_t)(_position.y / (_lon_to_m_scaling * 1.113195) ); - return _base_lon - (int32_t)(_position.y / _lon_to_m_scaling ); + return _base_lon - (int32_t)((_position_base.y+_position_correction.y) / _lon_to_m_scaling ); +} + +// set_current_position - all internal calculations are recorded as the distances from this point +void AP_InertialNav::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); + + // reset corrections to base position to zero + _position_base.x = 0; + _position_base.y = 0; + _position_correction.x = 0; + _position_correction.y = 0; + + // clear historic estimates + _hist_position_estimate_x.clear(); + _hist_position_estimate_y.clear(); + + // set xy as enabled + _xy_enabled = true; } // get accel based latitude @@ -260,7 +260,7 @@ float AP_InertialNav::get_latitude_diff() //return _base_lat + (int32_t)_position.x; //return -_position.x / 1.113195; - return -_position.x; + return -(_position_base.x+_position_correction.x); } // get accel based longitude @@ -273,7 +273,7 @@ float AP_InertialNav::get_longitude_diff() //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; + return -(_position_base.y+_position_correction.y) / _lon_to_m_scaling; } // get velocity in latitude & longitude directions @@ -296,4 +296,126 @@ float AP_InertialNav::get_longitude_velocity() } return -_velocity.y; -} \ No newline at end of file +} + +// set_velocity_xy - set velocity in latitude & longitude directions (in cm/s) +void AP_InertialNav::set_velocity_xy(float x, float y) +{ + _velocity.x = x; + _velocity.y = y; +} + +// +// Z Axis methods +// + +// set time constant - set timeconstant used by complementary filter +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; + update_gains(); + } +} + +// check_baro - check if new baro readings have arrived and use them to correct vertical accelerometer offsets +void AP_InertialNav::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_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.2 ) { + return; + } + + // discard first 10 reads but perform some initialisation + if( first_reads <= 10 ) { + set_altitude(baro_alt); + first_reads++; + } + + // get dcm matrix + Matrix3f dcm = _ahrs->get_dcm_matrix(); + + // 3rd order samples (i.e. position from baro) are delayed by 150ms (15 iterations at 100hz) + // so we should calculate error using historical estimates + if( _hist_position_estimate_z.num_items() >= 15 ) { + hist_position_base_z = _hist_position_estimate_z.peek(14); + }else{ + hist_position_base_z = _position_base.z; + } + + // calculate error in position from baro with our estimate + float err = baro_alt - (hist_position_base_z + _position_correction.z); + + // calculate correction to accelerometers and apply in the body frame + Vector3f accel_corr = accel_correction.get(); + accel_corr += dcm.mul_transpose(Vector3f(0,0,(err*_k3_z) * dt)); + accel_correction.set(accel_corr); + + // correct velocity + _velocity.z += (err*_k2_z) * dt; + + // correct position + _position_correction.z += err*_k1_z * dt; +} + +// set_altitude - set base altitude estimate in cm +void AP_InertialNav::set_altitude( float new_altitude) +{ + _position_base.z = new_altitude; + _position_correction.z = 0; +} + +// +// Private methods +// + +// update_gains - update gains from time constant (given in seconds) +void AP_InertialNav::update_gains() +{ + // X & Y axis time constant + if( _time_constant_xy == 0 ) { + _k1_xy = _k2_xy = _k3_xy = 0; + }else{ + _k1_xy = 3 / _time_constant_xy; + _k2_xy = 3 / (_time_constant_xy*_time_constant_xy); + _k3_xy = 1 / (_time_constant_xy*_time_constant_xy*_time_constant_xy); + } + + // Z axis time constant + if( _time_constant_z == 0 ) { + _k1_z = _k2_z = _k3_z = 0; + }else{ + _k1_z = 3 / _time_constant_z; + _k2_z = 3 / (_time_constant_z*_time_constant_z); + _k3_z = 1 / (_time_constant_z*_time_constant_z*_time_constant_z); + } +} + +// set_velocity_z - get latest climb rate (in cm/s) +void AP_InertialNav::set_velocity_z(float z ) +{ + _velocity.z = z; +} diff --git a/libraries/AP_InertialNav/AP_InertialNav.h b/libraries/AP_InertialNav/AP_InertialNav.h index a69e06ca7a..c033a9bd11 100644 --- a/libraries/AP_InertialNav/AP_InertialNav.h +++ b/libraries/AP_InertialNav/AP_InertialNav.h @@ -6,12 +6,17 @@ #include #include // ArduPilot Mega IMU Library #include // ArduPilot Mega Barometer Library -#include // Complementary filter for combining barometer altitude with accelerometers +#include // FIFO buffer library #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 3.0 // default time constant for complementary filter's Z axis +// #defines to control how often historical accel based positions are saved +// so they can later be compared to laggy gps readings +#define AP_INTERTIALNAV_SAVE_POS_AFTER_ITERATIONS 10 +#define AP_INTERTIALNAV_GPS_LAG_IN_10HZ_INCREMENTS 4 // must not be larger than size of _hist_position_estimate_x and _hist_position_estimate_y + /* * AP_InertialNav is an attempt to use accelerometers to augment other sensors to improve altitud e position hold */ @@ -25,35 +30,29 @@ public: _ins(ins), _baro(baro), _gps_ptr(gps_ptr), - _baro_last_update(0), - _gps_last_update(0), _xy_enabled(false), - _comp_filter(AP_INTERTIALNAV_TC_XY, AP_INTERTIALNAV_TC_Z) + _gps_last_update(0), + _baro_last_update(0) {} // Initialisation - virtual void init() { - set_time_constant_xy(_time_constant_xy); - set_time_constant_z(_time_constant_z); - } + virtual void init(); // save_params - save all parameters to eeprom virtual void save_params(); + // update - updates velocities and positions using latest info from accelerometers; + virtual void update(float dt); + + // + // XY Axis specific methods + // + // 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); + // altitude_ok, position_ok - true if inertial based altitude and position can be trusted + virtual bool position_ok(); // check_gps - check if new gps readings have arrived and use them to correct position estimates virtual void check_gps(); @@ -61,61 +60,91 @@ public: // 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_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_filter.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 + // 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); + + // get latitude & longitude positions from base location (in cm) virtual float get_latitude_diff(); virtual float get_longitude_diff(); - // get velocity in latitude & longitude directions + // get velocity in latitude & longitude directions (in cm/s) virtual float get_latitude_velocity(); virtual float get_longitude_velocity(); + // set velocity in latitude & longitude directions (in cm/s) + virtual void set_velocity_xy(float x, float y); + + // + // Z Axis methods + // + + // set time constant - set timeconstant used by complementary filter + virtual void set_time_constant_z( float time_constant_in_seconds ); + + // altitude_ok, position_ok - true if inertial based altitude and position can be trusted + virtual bool altitude_ok() { return true; } + + // 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); + + // get_altitude - get latest altitude estimate in cm + virtual float get_altitude() { return _position_base.z + _position_correction.z; } + virtual void set_altitude( float 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( float new_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; + // public variables + AP_Vector3f accel_correction; // acceleration corrections + Vector3f accel_correction_ef; // earth frame accelerometer corrections. here for logging purposes only - 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 +protected: - bool _xy_enabled; + virtual void update_gains(); // update_gains - update gains from time constant (given in seconds) - 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 + AP_AHRS* _ahrs; // pointer to ahrs object + AP_InertialSensor* _ins; // pointer to inertial sensor + AP_Baro* _baro; // pointer to barometer + GPS** _gps_ptr; // pointer to pointer to gps - ThirdOrderCompFilter _comp_filter; // 3rd order complementary filter for combining baro readings with accelerometers + // XY Axis specific variables + bool _xy_enabled; // xy position estimates enabled + AP_Float _time_constant_xy; // time constant for horizontal corrections + 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 + 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 + float _lon_to_m_scaling; // conversion of longitude to meters + + // Z Axis specific variables + AP_Float _time_constant_z; // time constant for vertical corrections + 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 + // 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) }; #endif // __AP_INERTIALNAV_H__