Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

ArduCopter, AP_InertialNav: consolidated ThirdOrderComplementaryFilter into AP_InertialNav to save about 200bytes of RAM

This commit is contained in:
rmackay9 2012-12-10 00:43:11 +09:00
parent a84d9110bd
commit 154e3c33f7
5 changed files with 364 additions and 223 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduCopter/Attitude.pde
View File

@ -951,7 +951,7 @@ get_throttle_rate(int16_t z_target_speed)
// calculate rate error // calculate rate error
#if INERTIAL_NAV_Z == ENABLED #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 #else
z_rate_error = z_target_speed - climb_rate; // calc the speed error z_rate_error = z_target_speed - climb_rate; // calc the speed error
#endif #endif
@ -1071,7 +1071,7 @@ get_throttle_land()
// detect whether we have landed by watching for minimum throttle and now movement // detect whether we have landed by watching for minimum throttle and now movement
#if INERTIAL_NAV_Z == ENABLED #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 #else
if (abs(climb_rate) < 20 && (g.rc_3.servo_out <= get_angle_boost(g.throttle_min) || g.pid_throttle_accel.get_integrator() <= -150)) { if (abs(climb_rate) < 20 && (g.rc_3.servo_out <= get_angle_boost(g.throttle_min) || g.pid_throttle_accel.get_integrator() <= -150)) {
#endif #endif

58
ArduCopter/Log.pde
View File

@ -786,32 +786,30 @@ static void Log_Read_Attitude()
(unsigned)temp7); (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) static void Log_Write_INAV(float delta_t)
{ {
#if INERTIAL_NAV_XY == ENABLED || INERTIAL_NAV_Z == ENABLED #if INERTIAL_NAV_XY == ENABLED || INERTIAL_NAV_Z == ENABLED
Vector3f accel_corr = inertial_nav.accel_correction.get();
DataFlash.WriteByte(HEAD_BYTE1); DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2); DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_INAV_MSG); DataFlash.WriteByte(LOG_INAV_MSG);
DataFlash.WriteInt((int16_t)baro_alt); // 1 barometer altitude 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)climb_rate_actual); // 3 barometer based climb rate
DataFlash.WriteInt((int16_t)inertial_nav._velocity.z); // 4 accel + baro based climb rate DataFlash.WriteInt((int16_t)inertial_nav.get_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(accel_corr.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(accel_corr.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(accel_corr.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_correction_ef.z)); // 8 accel correction earth frame
DataFlash.WriteLong(get_int(inertial_nav._accel_ef.x)); // 9 accel earth frame x-axis DataFlash.WriteLong(g_gps->latitude-home.lat); // 9 lat from home
DataFlash.WriteLong(get_int(inertial_nav._accel_ef.y)); // 10 accel earth frame y-axis DataFlash.WriteLong(g_gps->longitude-home.lng); // 10 lon from home
DataFlash.WriteLong(get_int(inertial_nav._accel_ef.z)); // 11 accel earth frame z-axis DataFlash.WriteLong(get_int(inertial_nav.get_latitude_diff())); // 11 accel based lat from home
DataFlash.WriteLong(get_int(delta_t)); // 12 time delta of samples DataFlash.WriteLong(get_int(inertial_nav.get_longitude_diff())); // 12 accel based lon from home
DataFlash.WriteLong(g_gps->latitude-home.lat); // 13 lat from home DataFlash.WriteLong(get_int(inertial_nav.get_latitude_velocity())); // 13 accel based lat velocity
DataFlash.WriteLong(g_gps->longitude-home.lng); // 14 lon from home DataFlash.WriteLong(get_int(inertial_nav.get_longitude_velocity())); // 14 accel based lon velocity
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.WriteByte(END_BYTE); DataFlash.WriteByte(END_BYTE);
#endif #endif
@ -828,18 +826,14 @@ static void Log_Read_INAV()
float temp6 = get_float(DataFlash.ReadLong()); // 6 accel correction y-axis float temp6 = get_float(DataFlash.ReadLong()); // 6 accel correction y-axis
float temp7 = get_float(DataFlash.ReadLong()); // 7 accel correction z-axis float temp7 = get_float(DataFlash.ReadLong()); // 7 accel correction z-axis
float temp8 = get_float(DataFlash.ReadLong()); // 8 accel correction earth frame float temp8 = get_float(DataFlash.ReadLong()); // 8 accel correction earth frame
float temp9 = get_float(DataFlash.ReadLong()); // 9 accel earth frame x-axis int32_t temp9 = DataFlash.ReadLong(); // 9 lat from home
float temp10 = get_float(DataFlash.ReadLong()); // 10 accel earth frame y-axis int32_t temp10 = DataFlash.ReadLong(); // 10 lon from home
float temp11 = get_float(DataFlash.ReadLong()); // 11 accel earth frame z-axis float temp11 = get_float(DataFlash.ReadLong()); // 11 accel based lat from home
float temp12 = get_float(DataFlash.ReadLong()); // 12 time delta of samples float temp12 = get_float(DataFlash.ReadLong()); // 12 accel based lon from home
int32_t temp13 = DataFlash.ReadLong(); // 13 lat from home float temp13 = get_float(DataFlash.ReadLong()); // 13 accel based lat velocity
int32_t temp14 = DataFlash.ReadLong(); // 14 lon from home float temp14 = get_float(DataFlash.ReadLong()); // 14 accel based lon velocity
float temp15 = get_float(DataFlash.ReadLong()); // 15 accel based lat from home // 1 2 3 4 5 6 7 8 9 10 11 12 13 14
float temp16 = get_float(DataFlash.ReadLong()); // 16 accel based lon from home 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"),
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"),
(int)temp1, (int)temp1,
(int)temp2, (int)temp2,
(int)temp3, (int)temp3,
@ -853,11 +847,7 @@ static void Log_Read_INAV()
temp11, temp11,
temp12, temp12,
temp13, temp13,
temp14, temp14);
temp15,
temp16,
temp17,
temp18);
} }
// Write a mode packet. Total length : 7 bytes // Write a mode packet. Total length : 7 bytes

2
ArduCopter/navigation.pde
View File

@ -541,7 +541,7 @@ static int32_t get_altitude_error()
#if INERTIAL_NAV_Z == ENABLED #if INERTIAL_NAV_Z == ENABLED
// use inertial nav for altitude error // use inertial nav for altitude error
return next_WP.alt - inertial_nav._position.z; return next_WP.alt - inertial_nav.get_altitude();
#else #else
return next_WP.alt - current_loc.alt; return next_WP.alt - current_loc.alt;
#endif #endif

382
libraries/AP_InertialNav/AP_InertialNav.cpp
View File

@ -10,24 +10,22 @@
// table of user settable parameters // table of user settable parameters
const AP_Param::GroupInfo AP_InertialNav::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 // @Param: ACORR
// @DisplayName: Inertial Nav calculated accelerometer corrections // @DisplayName: Inertial Nav calculated accelerometer corrections
// @Description: calculated accelerometer corrections // @Description: calculated accelerometer corrections
// @Increment: 1 // @Increment: 1
AP_GROUPINFO("ACORR", 0, AP_InertialNav, _accel_correction, 0), AP_GROUPINFO("ACORR", 0, AP_InertialNav, accel_correction, 0),
// @Param: TC_XY // @Param: TC_XY
// @DisplayName: Horizontal Time Constant // @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 // @Range: 0 10
// @Increment: 0.1 // @Increment: 0.1
AP_GROUPINFO("TC_XY", 1, AP_InertialNav, _time_constant_xy, AP_INTERTIALNAV_TC_XY), AP_GROUPINFO("TC_XY", 1, AP_InertialNav, _time_constant_xy, AP_INTERTIALNAV_TC_XY),
// @Param: TC_Z // @Param: TC_Z
// @DisplayName: Vertical Time Constant // @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 // @Range: 0 10
// @Increment: 0.1 // @Increment: 0.1
AP_GROUPINFO("TC_Z", 2, AP_InertialNav, _time_constant_z, AP_INTERTIALNAV_TC_Z), 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 AP_GROUPEND
}; };
// init - initialise library
void AP_InertialNav::init()
{
// recalculate the gains
update_gains();
}
// save_params - save all parameters to eeprom // save_params - save all parameters to eeprom
void AP_InertialNav::save_params() void AP_InertialNav::save_params()
{ {
Vector3f accel_corr = _comp_filter.get_1st_order_correction(); Vector3f accel_corr = accel_correction.get();
accel_corr.x = constrain(accel_corr.x,-200,200); accel_corr.x = constrain(accel_corr.x,-100,100);
accel_corr.y = constrain(accel_corr.y,-200,200); accel_corr.y = constrain(accel_corr.y,-100,100);
accel_corr.z = constrain(accel_corr.z,-200,200); accel_corr.z = constrain(accel_corr.z,-100,100);
_accel_correction.set_and_save(accel_corr); 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 // set time constant - set timeconstant used by complementary filter
void AP_InertialNav::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 // ensure it's a reasonable value
if( time_constant_in_seconds > 0 && time_constant_in_seconds < 30 ) { if( time_constant_in_seconds > 0 && time_constant_in_seconds < 30 ) {
_time_constant_xy = time_constant_in_seconds; _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 // position_ok - return true if position has been initialised and have received gps data within 3 seconds
void AP_InertialNav::set_time_constant_z( float time_constant_in_seconds ) bool AP_InertialNav::position_ok()
{ {
// ensure it's a reasonable value return _xy_enabled && (millis() - _gps_last_update < 3000);
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;
} }
// check_gps - check if new gps readings have arrived and use them to correct position estimates // 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) void AP_InertialNav::correct_with_gps(int32_t lon, int32_t lat, float dt)
{ {
float x,y; float x,y;
float hist_position_base_x, hist_position_base_y;
// discard samples where dt is too large // discard samples where dt is too large
if( dt > 1.0 || dt == 0 || !_xy_enabled) { if( dt > 1.0 || dt == 0 || !_xy_enabled) {
return; return;
} }
// calculate distance from home // calculate distance from base location
//x = (float)(lat - _base_lat) * 1.113195; //x = (float)(lat - _base_lat) * 1.113195;
//y = (float)(lon - _base_lon) * _lon_to_m_scaling * 1.113195; //y = (float)(lon - _base_lon) * _lon_to_m_scaling * 1.113195;
x = (float)(lat - _base_lat); 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 // convert accelerometer readings to earth frame
Matrix3f dcm = _ahrs->get_dcm_matrix(); Matrix3f dcm = _ahrs->get_dcm_matrix();
// call comp filter's correct xy // correct accelerometer offsets using gps
_comp_filter.correct_3rd_order_xy(-x, -y, dcm, dt);
//Notes: with +x above, accel lat comes out reversed // 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;
} }
// update - updates velocities and positions using latest info from ahrs, ins and barometer if new data is available; // calculate error in position from gps with our historical estimate
void AP_InertialNav::update(float dt) // To-Do: check why x and y are reversed
{ float err_x = -x - (hist_position_base_x + _position_correction.x);
// discard samples where dt is too large float err_y = -y - (hist_position_base_y + _position_correction.y);
if( dt > 0.1 ) {
return;
}
// check barometer // calculate correction to accelerometers and apply in the body frame
check_baro(); 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);
// check gps // correct velocity
check_gps(); _velocity.x += (err_x*_k2_xy) * dt;
_velocity.y += (err_y*_k2_xy) * dt;
// read acclerometer values // correct position
_accel_bf = _ins->get_accel(); _position_correction.x += err_x*_k1_xy * dt;
_position_correction.y += err_y*_k1_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);
} }
// get accel based latitude // 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 / 1.113195);
return _base_lat - (int32_t)_position.x; return _base_lat - (int32_t)(_position_base.x + _position_correction.x);
} }
// get accel based longitude // 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 * 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 // get accel based latitude
@ -260,7 +260,7 @@ float AP_InertialNav::get_latitude_diff()
//return _base_lat + (int32_t)_position.x; //return _base_lat + (int32_t)_position.x;
//return -_position.x / 1.113195; //return -_position.x / 1.113195;
return -_position.x; return -(_position_base.x+_position_correction.x);
} }
// get accel based longitude // 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 _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 * 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 // get velocity in latitude & longitude directions
@ -297,3 +297,125 @@ float AP_InertialNav::get_longitude_velocity()
return -_velocity.y; return -_velocity.y;
} }
// 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;
}

135
libraries/AP_InertialNav/AP_InertialNav.h
View File

@ -6,12 +6,17 @@
#include <AP_AHRS.h> #include <AP_AHRS.h>
#include <AP_InertialSensor.h> // ArduPilot Mega IMU Library #include <AP_InertialSensor.h> // ArduPilot Mega IMU Library
#include <AP_Baro.h> // ArduPilot Mega Barometer Library #include <AP_Baro.h> // ArduPilot Mega Barometer Library
#include <ThirdOrderCompFilter.h> // Complementary filter for combining barometer altitude with accelerometers #include <AP_Buffer.h> // FIFO buffer library
#define AP_INTERTIALNAV_GRAVITY 9.80665 #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_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 #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 * 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), _ins(ins),
_baro(baro), _baro(baro),
_gps_ptr(gps_ptr), _gps_ptr(gps_ptr),
_baro_last_update(0),
_gps_last_update(0),
_xy_enabled(false), _xy_enabled(false),
_comp_filter(AP_INTERTIALNAV_TC_XY, AP_INTERTIALNAV_TC_Z) _gps_last_update(0),
_baro_last_update(0)
{} {}
// Initialisation // Initialisation
virtual void init() { virtual void init();
set_time_constant_xy(_time_constant_xy);
set_time_constant_z(_time_constant_z);
}
// save_params - save all parameters to eeprom // save_params - save all parameters to eeprom
virtual void save_params(); 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 // set time constant - set timeconstant used by complementary filter
virtual void set_time_constant_xy( float time_constant_in_seconds ); virtual void set_time_constant_xy( float time_constant_in_seconds );
// set time constant - set timeconstant used by complementary filter // altitude_ok, position_ok - true if inertial based altitude and position can be trusted
virtual void set_time_constant_z( float time_constant_in_seconds ); virtual bool position_ok();
// 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 // check_gps - check if new gps readings have arrived and use them to correct position estimates
virtual void check_gps(); virtual void check_gps();
@ -61,61 +60,91 @@ public:
// correct_with_gps - modifies accelerometer offsets using gps. dt is time since last gps update // 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); 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 // get latitude & longitude positions
virtual int32_t get_latitude(); virtual int32_t get_latitude();
virtual int32_t get_longitude(); 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_latitude_diff();
virtual float get_longitude_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_latitude_velocity();
virtual float get_longitude_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 // class level parameters
static const struct AP_Param::GroupInfo var_info[]; static const struct AP_Param::GroupInfo var_info[];
//protected: // public variables
AP_AHRS* _ahrs; AP_Vector3f accel_correction; // acceleration corrections
AP_InertialSensor* _ins; Vector3f accel_correction_ef; // earth frame accelerometer corrections. here for logging purposes only
AP_Baro* _baro;
GPS** _gps_ptr;
uint32_t _baro_last_update; protected:
virtual void update_gains(); // update_gains - update gains from time constant (given in seconds)
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
// 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_update; // system time of last gps update
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
uint8_t _historic_xy_counter; // counter used to slow saving of position estimates for later comparison to gps
bool _xy_enabled; 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
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_lat; // base latitude
int32_t _base_lon; // base longitude int32_t _base_lon; // base longitude
float _lon_to_m_scaling; // conversion of longitude to meters float _lon_to_m_scaling; // conversion of longitude to meters
ThirdOrderCompFilter _comp_filter; // 3rd order complementary filter for combining baro readings with accelerometers // 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__ #endif // __AP_INERTIALNAV_H__