mirror of https://github.com/ArduPilot/ardupilot
InertialNav: make parent virtual
This commit is contained in:
Randy Mackay
parent
4e7d92094c
commit
7221070533
|
|
@ -1,464 +0,0 @@
|
||||||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
||||||
#include <AP_HAL.h>
|
|
||||||
#include <AP_InertialNav.h>
|
|
||||||
|
|
||||||
extern const AP_HAL::HAL& hal;
|
|
||||||
|
|
||||||
// table of user settable parameters
|
|
||||||
const AP_Param::GroupInfo AP_InertialNav::var_info[] PROGMEM = {
|
|
||||||
// start numbering at 1 because 0 was previous used for body frame accel offsets
|
|
||||||
// @Param: TC_XY
|
|
||||||
// @DisplayName: Horizontal Time Constant
|
|
||||||
// @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 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),
|
|
||||||
|
|
||||||
AP_GROUPEND
|
|
||||||
};
|
|
||||||
|
|
||||||
// init - initialise library
|
|
||||||
void AP_InertialNav::init()
|
|
||||||
{
|
|
||||||
// recalculate the gains
|
|
||||||
update_gains();
|
|
||||||
}
|
|
||||||
|
|
||||||
// update - updates velocities and positions using latest info from ahrs and barometer if new data is available;
|
|
||||||
void AP_InertialNav::update(float dt)
|
|
||||||
{
|
|
||||||
// discard samples where dt is too large
|
|
||||||
if( dt > 0.1f ) {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
// decrement ignore error count if required
|
|
||||||
if (_flags.ignore_error > 0) {
|
|
||||||
_flags.ignore_error--;
|
|
||||||
}
|
|
||||||
|
|
||||||
// check if new baro readings have arrived and use them to correct vertical accelerometer offsets.
|
|
||||||
check_baro();
|
|
||||||
|
|
||||||
// check if home has moved and update
|
|
||||||
check_home();
|
|
||||||
|
|
||||||
// check if new gps readings have arrived and use them to correct position estimates
|
|
||||||
check_gps();
|
|
||||||
|
|
||||||
Vector3f accel_ef = _ahrs.get_accel_ef();
|
|
||||||
|
|
||||||
// remove influence of gravity
|
|
||||||
accel_ef.z += GRAVITY_MSS;
|
|
||||||
accel_ef *= 100.0f;
|
|
||||||
|
|
||||||
// remove xy if not enabled
|
|
||||||
if( !_xy_enabled ) {
|
|
||||||
accel_ef.x = 0.0f;
|
|
||||||
accel_ef.y = 0.0f;
|
|
||||||
}
|
|
||||||
|
|
||||||
//Convert North-East-Down to North-East-Up
|
|
||||||
accel_ef.z = -accel_ef.z;
|
|
||||||
|
|
||||||
// convert ef position error to horizontal body frame
|
|
||||||
Vector2f position_error_hbf;
|
|
||||||
position_error_hbf.x = _position_error.x * _ahrs.cos_yaw() + _position_error.y * _ahrs.sin_yaw();
|
|
||||||
position_error_hbf.y = -_position_error.x * _ahrs.sin_yaw() + _position_error.y * _ahrs.cos_yaw();
|
|
||||||
|
|
||||||
float tmp = _k3_xy * dt;
|
|
||||||
accel_correction_hbf.x += position_error_hbf.x * tmp;
|
|
||||||
accel_correction_hbf.y += position_error_hbf.y * tmp;
|
|
||||||
accel_correction_hbf.z += _position_error.z * _k3_z * dt;
|
|
||||||
|
|
||||||
tmp = _k2_xy * dt;
|
|
||||||
_velocity.x += _position_error.x * tmp;
|
|
||||||
_velocity.y += _position_error.y * tmp;
|
|
||||||
_velocity.z += _position_error.z * _k2_z * dt;
|
|
||||||
|
|
||||||
tmp = _k1_xy * dt;
|
|
||||||
_position_correction.x += _position_error.x * tmp;
|
|
||||||
_position_correction.y += _position_error.y * tmp;
|
|
||||||
_position_correction.z += _position_error.z * _k1_z * dt;
|
|
||||||
|
|
||||||
// convert horizontal body frame accel correction to earth frame
|
|
||||||
Vector2f accel_correction_ef;
|
|
||||||
accel_correction_ef.x = accel_correction_hbf.x * _ahrs.cos_yaw() - accel_correction_hbf.y * _ahrs.sin_yaw();
|
|
||||||
accel_correction_ef.y = accel_correction_hbf.x * _ahrs.sin_yaw() + accel_correction_hbf.y * _ahrs.cos_yaw();
|
|
||||||
|
|
||||||
// calculate velocity increase adding new acceleration from accelerometers
|
|
||||||
Vector3f velocity_increase;
|
|
||||||
velocity_increase.x = (accel_ef.x + accel_correction_ef.x) * dt;
|
|
||||||
velocity_increase.y = (accel_ef.y + accel_correction_ef.y) * dt;
|
|
||||||
velocity_increase.z = (accel_ef.z + accel_correction_hbf.z) * dt;
|
|
||||||
|
|
||||||
// calculate new estimate of position
|
|
||||||
_position_base += (_velocity + velocity_increase*0.5) * dt;
|
|
||||||
|
|
||||||
// update the corrected position estimate
|
|
||||||
_position = _position_base + _position_correction;
|
|
||||||
|
|
||||||
// calculate new velocity
|
|
||||||
_velocity += velocity_increase;
|
|
||||||
|
|
||||||
// store 3rd order estimate (i.e. estimated vertical position) for future use
|
|
||||||
_hist_position_estimate_z.push_back(_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.push_back(_position_base.x);
|
|
||||||
_hist_position_estimate_y.push_back(_position_base.y);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* get_filter_status : returns filter status as a series of flags
|
|
||||||
*/
|
|
||||||
nav_filter_status AP_InertialNav::get_filter_status() const
|
|
||||||
{
|
|
||||||
nav_filter_status ret;
|
|
||||||
ret.value = 0; // initialise to zero
|
|
||||||
ret.flags.attitude = true;
|
|
||||||
ret.flags.horiz_pos_abs = _xy_enabled;
|
|
||||||
ret.flags.horiz_pos_rel = false;
|
|
||||||
ret.flags.horiz_vel = _xy_enabled;
|
|
||||||
ret.flags.terrain_alt = false;
|
|
||||||
ret.flags.vert_pos = true;
|
|
||||||
ret.flags.vert_vel = true;
|
|
||||||
ret.flags.const_pos_mode = false;
|
|
||||||
ret.flags.pred_horiz_pos_rel = false;
|
|
||||||
ret.flags.pred_horiz_pos_abs = _xy_enabled;
|
|
||||||
return ret;
|
|
||||||
}
|
|
||||||
|
|
||||||
//
|
|
||||||
// XY Axis specific methods
|
|
||||||
//
|
|
||||||
|
|
||||||
// check_home - checks if the home position has moved and offsets everything so it still lines up
|
|
||||||
void AP_InertialNav::check_home() {
|
|
||||||
if (!_xy_enabled) {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
// get position move in lat, lon coordinates
|
|
||||||
int32_t lat_offset = _ahrs.get_home().lat - _last_home_lat;
|
|
||||||
int32_t lng_offset = _ahrs.get_home().lng - _last_home_lng;
|
|
||||||
|
|
||||||
if (lat_offset != 0) {
|
|
||||||
// calculate the position move in cm
|
|
||||||
float x_offset_cm = lat_offset * LATLON_TO_CM;
|
|
||||||
|
|
||||||
// move position
|
|
||||||
_position_base.x -= x_offset_cm;
|
|
||||||
_position.x -= x_offset_cm;
|
|
||||||
|
|
||||||
// update historic positions
|
|
||||||
for (uint8_t i = 0; i < _hist_position_estimate_x.size(); i++) {
|
|
||||||
float &x = _hist_position_estimate_x.peek_mutable(i);
|
|
||||||
x -= x_offset_cm;
|
|
||||||
}
|
|
||||||
|
|
||||||
// update lon scaling
|
|
||||||
_lon_to_cm_scaling = longitude_scale(_ahrs.get_home()) * LATLON_TO_CM;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (lng_offset != 0) {
|
|
||||||
// calculate the position move in cm
|
|
||||||
float y_offset_cm = lng_offset * _lon_to_cm_scaling;
|
|
||||||
|
|
||||||
// move position
|
|
||||||
_position_base.y -= y_offset_cm;
|
|
||||||
_position.y -= y_offset_cm;
|
|
||||||
|
|
||||||
// update historic positions
|
|
||||||
for (uint8_t i = 0; i < _hist_position_estimate_y.size(); i++) {
|
|
||||||
float &y = _hist_position_estimate_y.peek_mutable(i);
|
|
||||||
y -= y_offset_cm;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// store updated lat, lon position
|
|
||||||
_last_home_lat = _ahrs.get_home().lat;
|
|
||||||
_last_home_lng = _ahrs.get_home().lng;
|
|
||||||
}
|
|
||||||
|
|
||||||
// check_gps - check if new gps readings have arrived and use them to correct position estimates
|
|
||||||
void AP_InertialNav::check_gps()
|
|
||||||
{
|
|
||||||
const uint32_t now = hal.scheduler->millis();
|
|
||||||
|
|
||||||
// compare gps time to previous reading
|
|
||||||
const AP_GPS &gps = _ahrs.get_gps();
|
|
||||||
if(gps.last_fix_time_ms() != _gps_last_time ) {
|
|
||||||
|
|
||||||
// call position correction method
|
|
||||||
correct_with_gps(now, gps.location().lng, gps.location().lat);
|
|
||||||
|
|
||||||
// record gps time and system time of this update
|
|
||||||
_gps_last_time = gps.last_fix_time_ms();
|
|
||||||
}else{
|
|
||||||
// if GPS updates stop arriving degrade position error to 10% over 2 seconds (assumes 100hz update rate)
|
|
||||||
if (now - _gps_last_update > AP_INTERTIALNAV_GPS_TIMEOUT_MS) {
|
|
||||||
_position_error.x *= 0.9886f;
|
|
||||||
_position_error.y *= 0.9886f;
|
|
||||||
// increment error count
|
|
||||||
if (_flags.ignore_error == 0 && _error_count < 255 && _xy_enabled) {
|
|
||||||
_error_count++;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// correct_with_gps - modifies accelerometer offsets using gps
|
|
||||||
void AP_InertialNav::correct_with_gps(uint32_t now, int32_t lon, int32_t lat)
|
|
||||||
{
|
|
||||||
float dt,x,y;
|
|
||||||
float hist_position_base_x, hist_position_base_y;
|
|
||||||
|
|
||||||
// calculate time since last gps reading
|
|
||||||
dt = (float)(now - _gps_last_update) * 0.001f;
|
|
||||||
|
|
||||||
// update last gps update time
|
|
||||||
_gps_last_update = now;
|
|
||||||
|
|
||||||
// discard samples where dt is too large
|
|
||||||
if( dt > 1.0f || dt == 0.0f || !_xy_enabled) {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
// calculate distance from base location
|
|
||||||
x = (float)(lat - _ahrs.get_home().lat) * LATLON_TO_CM;
|
|
||||||
y = (float)(lon - _ahrs.get_home().lng) * _lon_to_cm_scaling;
|
|
||||||
|
|
||||||
// sanity check the gps position. Relies on the main code calling GPS_Glitch::check_position() immediatley after a GPS update
|
|
||||||
if (_glitch_detector.glitching()) {
|
|
||||||
// failed sanity check so degrate position_error to 10% over 2 seconds (assumes 5hz update rate)
|
|
||||||
_position_error.x *= 0.7943f;
|
|
||||||
_position_error.y *= 0.7943f;
|
|
||||||
}else{
|
|
||||||
// if our internal glitching flag (from previous iteration) is true we have just recovered from a glitch
|
|
||||||
// reset the inertial nav position and velocity to gps values
|
|
||||||
if (_flags.gps_glitching) {
|
|
||||||
set_position_xy(x,y);
|
|
||||||
_position_error.x = 0.0f;
|
|
||||||
_position_error.y = 0.0f;
|
|
||||||
}else{
|
|
||||||
// ublox gps positions are delayed by 400ms
|
|
||||||
// we store historical position at 10hz so 4 iterations ago
|
|
||||||
if( _hist_position_estimate_x.is_full()) {
|
|
||||||
hist_position_base_x = _hist_position_estimate_x.front();
|
|
||||||
hist_position_base_y = _hist_position_estimate_y.front();
|
|
||||||
}else{
|
|
||||||
hist_position_base_x = _position_base.x;
|
|
||||||
hist_position_base_y = _position_base.y;
|
|
||||||
}
|
|
||||||
|
|
||||||
// calculate error in position from gps with our historical estimate
|
|
||||||
_position_error.x = x - (hist_position_base_x + _position_correction.x);
|
|
||||||
_position_error.y = y - (hist_position_base_y + _position_correction.y);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// update our internal record of glitching flag so that we can notice a change
|
|
||||||
_flags.gps_glitching = _glitch_detector.glitching();
|
|
||||||
}
|
|
||||||
|
|
||||||
// get accel based latitude
|
|
||||||
int32_t AP_InertialNav::get_latitude() const
|
|
||||||
{
|
|
||||||
// make sure we've been initialised
|
|
||||||
if( !_xy_enabled ) {
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
return _ahrs.get_home().lat + (int32_t)(_position.x/LATLON_TO_CM);
|
|
||||||
}
|
|
||||||
|
|
||||||
// get accel based longitude
|
|
||||||
int32_t AP_InertialNav::get_longitude() const
|
|
||||||
{
|
|
||||||
// make sure we've been initialised
|
|
||||||
if( !_xy_enabled ) {
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
return _ahrs.get_home().lng + (int32_t)(_position.y / _lon_to_cm_scaling);
|
|
||||||
}
|
|
||||||
|
|
||||||
// setup_home_position - reset state for home position change
|
|
||||||
void AP_InertialNav::setup_home_position(void)
|
|
||||||
{
|
|
||||||
// set longitude to meters scaling to offset the shrinking longitude as we go towards the poles
|
|
||||||
_lon_to_cm_scaling = longitude_scale(_ahrs.get_home()) * LATLON_TO_CM;
|
|
||||||
|
|
||||||
// reset corrections to base position to zero
|
|
||||||
_position_base.x = 0.0f;
|
|
||||||
_position_base.y = 0.0f;
|
|
||||||
_position_correction.x = 0.0f;
|
|
||||||
_position_correction.y = 0.0f;
|
|
||||||
_position.x = 0.0f;
|
|
||||||
_position.y = 0.0f;
|
|
||||||
_last_home_lat = _ahrs.get_home().lat;
|
|
||||||
_last_home_lng = _ahrs.get_home().lng;
|
|
||||||
|
|
||||||
// clear historic estimates
|
|
||||||
_hist_position_estimate_x.clear();
|
|
||||||
_hist_position_estimate_y.clear();
|
|
||||||
|
|
||||||
// set xy as enabled
|
|
||||||
_xy_enabled = true;
|
|
||||||
}
|
|
||||||
|
|
||||||
// get accel based latitude
|
|
||||||
float AP_InertialNav::get_latitude_diff() const
|
|
||||||
{
|
|
||||||
// make sure we've been initialised
|
|
||||||
if( !_xy_enabled ) {
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
return (_position.x/LATLON_TO_CM);
|
|
||||||
}
|
|
||||||
|
|
||||||
// get accel based longitude
|
|
||||||
float AP_InertialNav::get_longitude_diff() const
|
|
||||||
{
|
|
||||||
// make sure we've been initialised
|
|
||||||
if( !_xy_enabled ) {
|
|
||||||
return 0.0f;
|
|
||||||
}
|
|
||||||
|
|
||||||
return (_position.y / _lon_to_cm_scaling);
|
|
||||||
}
|
|
||||||
|
|
||||||
// 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;
|
|
||||||
}
|
|
||||||
|
|
||||||
// set_velocity_xy - set velocity in latitude & longitude directions (in cm/s)
|
|
||||||
float AP_InertialNav::get_velocity_xy() const
|
|
||||||
{
|
|
||||||
return pythagorous2(_velocity.x, _velocity.y);
|
|
||||||
}
|
|
||||||
|
|
||||||
//
|
|
||||||
// Z Axis methods
|
|
||||||
//
|
|
||||||
|
|
||||||
// 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;
|
|
||||||
|
|
||||||
// calculate time since last baro reading (in ms)
|
|
||||||
baro_update_time = _baro.get_last_update();
|
|
||||||
if( baro_update_time != _baro_last_update ) {
|
|
||||||
const float dt = (float)(baro_update_time - _baro_last_update) * 0.001f; // in seconds
|
|
||||||
// call correction method
|
|
||||||
correct_with_baro(_baro.get_altitude()*100.0f, 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.5f ) {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
// discard first 10 reads but perform some initialisation
|
|
||||||
if( first_reads <= 10 ) {
|
|
||||||
set_altitude(baro_alt);
|
|
||||||
first_reads++;
|
|
||||||
}
|
|
||||||
|
|
||||||
// 3rd order samples (i.e. position from baro) are delayed by 150ms (15 iterations at 100hz)
|
|
||||||
// so we should calculate error using historical estimates
|
|
||||||
float hist_position_base_z;
|
|
||||||
if (_hist_position_estimate_z.is_full()) {
|
|
||||||
hist_position_base_z = _hist_position_estimate_z.front();
|
|
||||||
} else {
|
|
||||||
hist_position_base_z = _position_base.z;
|
|
||||||
}
|
|
||||||
|
|
||||||
// calculate error in position from baro with our estimate
|
|
||||||
_position_error.z = baro_alt - (hist_position_base_z + _position_correction.z);
|
|
||||||
}
|
|
||||||
|
|
||||||
// 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;
|
|
||||||
_position.z = new_altitude; // _position = _position_base + _position_correction
|
|
||||||
_hist_position_estimate_z.clear(); // reset z history to avoid fake z velocity at next baro calibration (next rearm)
|
|
||||||
}
|
|
||||||
|
|
||||||
//
|
|
||||||
// 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.0f) {
|
|
||||||
_k1_xy = _k2_xy = _k3_xy = 0.0f;
|
|
||||||
}else{
|
|
||||||
_k1_xy = 3.0f / _time_constant_xy;
|
|
||||||
_k2_xy = 3.0f / (_time_constant_xy*_time_constant_xy);
|
|
||||||
_k3_xy = 1.0f / (_time_constant_xy*_time_constant_xy*_time_constant_xy);
|
|
||||||
}
|
|
||||||
|
|
||||||
// Z axis time constant
|
|
||||||
if (_time_constant_z == 0.0f) {
|
|
||||||
_k1_z = _k2_z = _k3_z = 0.0f;
|
|
||||||
}else{
|
|
||||||
_k1_z = 3.0f / _time_constant_z;
|
|
||||||
_k2_z = 3.0f / (_time_constant_z*_time_constant_z);
|
|
||||||
_k3_z = 1.0f / (_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;
|
|
||||||
}
|
|
||||||
|
|
||||||
// set_position_xy - sets inertial navigation position to given xy coordinates from home
|
|
||||||
void AP_InertialNav::set_position_xy(float x, float y)
|
|
||||||
{
|
|
||||||
// reset position from home
|
|
||||||
_position_base.x = x;
|
|
||||||
_position_base.y = y;
|
|
||||||
_position_correction.x = 0.0f;
|
|
||||||
_position_correction.y = 0.0f;
|
|
||||||
|
|
||||||
// clear historic estimates
|
|
||||||
_hist_position_estimate_x.clear();
|
|
||||||
_hist_position_estimate_y.clear();
|
|
||||||
|
|
||||||
// add new position for future use
|
|
||||||
_historic_xy_counter = 0;
|
|
||||||
_hist_position_estimate_x.push_back(_position_base.x);
|
|
||||||
_hist_position_estimate_y.push_back(_position_base.y);
|
|
||||||
}
|
|
||||||
|
|
@ -10,15 +10,6 @@
|
||||||
#include <AP_GPS_Glitch.h> // GPS Glitch detection library
|
#include <AP_GPS_Glitch.h> // GPS Glitch detection library
|
||||||
#include "../AP_NavEKF/AP_Nav_Common.h" // definitions shared by inertial and ekf nav filters
|
#include "../AP_NavEKF/AP_Nav_Common.h" // definitions shared by inertial and ekf nav filters
|
||||||
|
|
||||||
#define AP_INTERTIALNAV_TC_XY 2.5f // default time constant for complementary filter's X & Y axis
|
|
||||||
#define AP_INTERTIALNAV_TC_Z 5.0f // 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
|
|
||||||
#define AP_INTERTIALNAV_GPS_TIMEOUT_MS 300 // timeout after which position error from GPS will fall to zero
|
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* AP_InertialNav blends accelerometer data with gps and barometer data to improve altitude and position hold.
|
* AP_InertialNav blends accelerometer data with gps and barometer data to improve altitude and position hold.
|
||||||
*
|
*
|
||||||
|
|
@ -38,31 +29,9 @@ class AP_InertialNav
|
||||||
public:
|
public:
|
||||||
|
|
||||||
// Constructor
|
// Constructor
|
||||||
AP_InertialNav(AP_AHRS &ahrs, AP_Baro &baro, GPS_Glitch& gps_glitch) :
|
AP_InertialNav(AP_AHRS &ahrs) :
|
||||||
_ahrs(ahrs),
|
_ahrs(ahrs)
|
||||||
_baro(baro),
|
{}
|
||||||
_xy_enabled(false),
|
|
||||||
_k1_xy(0.0f),
|
|
||||||
_k2_xy(0.0f),
|
|
||||||
_k3_xy(0.0f),
|
|
||||||
_gps_last_update(0),
|
|
||||||
_gps_last_time(0),
|
|
||||||
_historic_xy_counter(0),
|
|
||||||
_lon_to_cm_scaling(LATLON_TO_CM),
|
|
||||||
_k1_z(0.0f),
|
|
||||||
_k2_z(0.0f),
|
|
||||||
_k3_z(0.0f),
|
|
||||||
_baro_last_update(0),
|
|
||||||
_glitch_detector(gps_glitch),
|
|
||||||
_error_count(0)
|
|
||||||
{
|
|
||||||
AP_Param::setup_object_defaults(this, var_info);
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* initializes the object.
|
|
||||||
*/
|
|
||||||
virtual void init();
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* update - updates velocity and position estimates using latest info from accelerometers
|
* update - updates velocity and position estimates using latest info from accelerometers
|
||||||
|
|
@ -70,19 +39,19 @@ public:
|
||||||
*
|
*
|
||||||
* @param dt : time since last update in seconds
|
* @param dt : time since last update in seconds
|
||||||
*/
|
*/
|
||||||
virtual void update(float dt);
|
virtual void update(float dt) = 0;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* get_filter_status : returns filter status as a series of flags
|
* get_filter_status : returns filter status as a series of flags
|
||||||
*/
|
*/
|
||||||
virtual nav_filter_status get_filter_status() const;
|
virtual nav_filter_status get_filter_status() const = 0;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* get_origin - returns the inertial navigation origin in lat/lon/alt
|
* get_origin - returns the inertial navigation origin in lat/lon/alt
|
||||||
*
|
*
|
||||||
* @return origin Location
|
* @return origin Location
|
||||||
*/
|
*/
|
||||||
virtual struct Location get_origin() const { return _ahrs.get_home(); }
|
virtual struct Location get_origin() const = 0;
|
||||||
|
|
||||||
//
|
//
|
||||||
// XY Axis specific methods
|
// XY Axis specific methods
|
||||||
|
|
@ -93,33 +62,19 @@ public:
|
||||||
*
|
*
|
||||||
* @return
|
* @return
|
||||||
*/
|
*/
|
||||||
virtual const Vector3f& get_position() const { return _position; }
|
virtual const Vector3f& get_position() const = 0;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* get_latitude - returns the latitude of the current position estimation in 100 nano degrees (i.e. degree value multiplied by 10,000,000)
|
* get_latitude - returns the latitude of the current position estimation in 100 nano degrees (i.e. degree value multiplied by 10,000,000)
|
||||||
* @return
|
* @return
|
||||||
*/
|
*/
|
||||||
virtual int32_t get_latitude() const;
|
virtual int32_t get_latitude() const = 0;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* get_longitude - returns the longitude of the current position estimation in 100 nano degrees (i.e. degree value multiplied by 10,000,000)
|
* get_longitude - returns the longitude of the current position estimation in 100 nano degrees (i.e. degree value multiplied by 10,000,000)
|
||||||
* @return
|
* @return
|
||||||
*/
|
*/
|
||||||
virtual int32_t get_longitude() const;
|
virtual int32_t get_longitude() const = 0;
|
||||||
|
|
||||||
/**
|
|
||||||
* get_latitude_diff - returns the current latitude difference from the home location.
|
|
||||||
*
|
|
||||||
* @return difference in 100 nano degrees (i.e. degree value multiplied by 10,000,000)
|
|
||||||
*/
|
|
||||||
virtual float get_latitude_diff() const;
|
|
||||||
|
|
||||||
/**
|
|
||||||
* get_longitude_diff - returns the current longitude difference from the home location.
|
|
||||||
*
|
|
||||||
* @return difference in 100 nano degrees (i.e. degree value multiplied by 10,000,000)
|
|
||||||
*/
|
|
||||||
virtual float get_longitude_diff() const;
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* get_velocity - returns the current velocity in cm/s
|
* get_velocity - returns the current velocity in cm/s
|
||||||
|
|
@ -129,27 +84,14 @@ public:
|
||||||
* .y : longitude velocity in cm/s
|
* .y : longitude velocity in cm/s
|
||||||
* .z : vertical velocity in cm/s
|
* .z : vertical velocity in cm/s
|
||||||
*/
|
*/
|
||||||
virtual const Vector3f& get_velocity() const { return _velocity; }
|
virtual const Vector3f& get_velocity() const = 0;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* get_velocity_xy - returns the current horizontal velocity in cm/s
|
* get_velocity_xy - returns the current horizontal velocity in cm/s
|
||||||
*
|
*
|
||||||
* @returns the current horizontal velocity in cm/s
|
* @returns the current horizontal velocity in cm/s
|
||||||
*/
|
*/
|
||||||
virtual float get_velocity_xy() const;
|
virtual float get_velocity_xy() const = 0;
|
||||||
|
|
||||||
/**
|
|
||||||
* set_velocity_xy - overwrites the current horizontal velocity in cm/s
|
|
||||||
*
|
|
||||||
* @param x : latitude velocity in cm/s
|
|
||||||
* @param y : longitude velocity in cm/s
|
|
||||||
*/
|
|
||||||
void set_velocity_xy(float x, float y);
|
|
||||||
|
|
||||||
/**
|
|
||||||
setup_home_position - reset on home position change
|
|
||||||
*/
|
|
||||||
void setup_home_position(void);
|
|
||||||
|
|
||||||
//
|
//
|
||||||
// Z Axis methods
|
// Z Axis methods
|
||||||
|
|
@ -160,14 +102,7 @@ public:
|
||||||
* reference position
|
* reference position
|
||||||
* @return
|
* @return
|
||||||
*/
|
*/
|
||||||
virtual float get_altitude() const { return _position.z; }
|
virtual float get_altitude() const = 0;
|
||||||
|
|
||||||
/**
|
|
||||||
* set_altitude - overwrites the current altitude value.
|
|
||||||
*
|
|
||||||
* @param new_altitude : altitude in cm
|
|
||||||
*/
|
|
||||||
void set_altitude( float new_altitude);
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* get_velocity_z - returns the current climbrate.
|
* get_velocity_z - returns the current climbrate.
|
||||||
|
|
@ -176,132 +111,11 @@ public:
|
||||||
*
|
*
|
||||||
* @return climbrate in cm/s (positive up)
|
* @return climbrate in cm/s (positive up)
|
||||||
*/
|
*/
|
||||||
virtual float get_velocity_z() const { return _velocity.z; }
|
virtual float get_velocity_z() const = 0;
|
||||||
|
|
||||||
/**
|
|
||||||
* set_velocity_z - overwrites the current climbrate.
|
|
||||||
*
|
|
||||||
* @param new_velocity : climbrate in cm/s
|
|
||||||
*/
|
|
||||||
void set_velocity_z( float new_velocity );
|
|
||||||
|
|
||||||
/**
|
|
||||||
* error_count - returns number of missed updates from GPS
|
|
||||||
*/
|
|
||||||
uint8_t error_count() const { return _error_count; }
|
|
||||||
|
|
||||||
/**
|
|
||||||
* ignore_next_error - the next error (if it occurs immediately) will not be added to the error count
|
|
||||||
*/
|
|
||||||
void ignore_next_error() { _flags.ignore_error = 7; }
|
|
||||||
|
|
||||||
// class level parameters
|
|
||||||
static const struct AP_Param::GroupInfo var_info[];
|
|
||||||
|
|
||||||
// public variables
|
|
||||||
Vector3f accel_correction_hbf; // horizontal body frame accelerometer corrections. here for logging purposes only
|
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
|
|
||||||
/**
|
|
||||||
* correct_with_gps - calculates horizontal position error using gps
|
|
||||||
*
|
|
||||||
* @param now : current time since boot in milliseconds
|
|
||||||
* @param lon : longitude in 100 nano degrees (i.e. degree value multiplied by 10,000,000)
|
|
||||||
* @param lat : latitude in 100 nano degrees (i.e. degree value multiplied by 10,000,000)
|
|
||||||
*/
|
|
||||||
void correct_with_gps(uint32_t now, int32_t lon, int32_t lat);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* check_home - checks if the home position has moved and offsets everything so it still lines up
|
|
||||||
*/
|
|
||||||
void check_home();
|
|
||||||
|
|
||||||
/**
|
|
||||||
* check_gps - checks if new gps readings have arrived and calls correct_with_gps to
|
|
||||||
* calculate the horizontal position error
|
|
||||||
* @see correct_with_gps(int32_t lon, int32_t lat, float dt);
|
|
||||||
*/
|
|
||||||
void check_gps();
|
|
||||||
|
|
||||||
/**
|
|
||||||
* check_baro - checks if new baro readings have arrived and calls correct_with_baro to
|
|
||||||
* calculate the vertical position error
|
|
||||||
*
|
|
||||||
* @see correct_with_baro(float baro_alt, float dt);
|
|
||||||
*/
|
|
||||||
void check_baro();
|
|
||||||
|
|
||||||
/**
|
|
||||||
* correct_with_baro - calculates vertical position error using barometer.
|
|
||||||
*
|
|
||||||
* @param baro_alt : altitude in cm
|
|
||||||
* @param dt : time since last baro reading in s
|
|
||||||
*/
|
|
||||||
void correct_with_baro(float baro_alt, float dt);
|
|
||||||
|
|
||||||
|
|
||||||
/**
|
|
||||||
* update gains from time constant.
|
|
||||||
*/
|
|
||||||
void update_gains();
|
|
||||||
|
|
||||||
/**
|
|
||||||
* set_position_xy - overwrites the current position relative to the home location in cm
|
|
||||||
*
|
|
||||||
* the home location was set with AP_InertialNav::set_home_location(int32_t, int32_t)
|
|
||||||
*
|
|
||||||
* @param x : relative latitude position in cm
|
|
||||||
* @param y : relative longitude position in cm
|
|
||||||
*/
|
|
||||||
void set_position_xy(float x, float y);
|
|
||||||
|
|
||||||
// structure for holding flags
|
|
||||||
struct InertialNav_flags {
|
|
||||||
uint8_t gps_glitching : 1; // 1 if glitch detector was previously indicating a gps glitch
|
|
||||||
uint8_t ignore_error : 3; // the number of iterations for which we should ignore errors
|
|
||||||
} _flags;
|
|
||||||
|
|
||||||
AP_AHRS &_ahrs; // reference to ahrs object
|
AP_AHRS &_ahrs; // reference to ahrs object
|
||||||
AP_Baro &_baro; // reference to barometer
|
|
||||||
|
|
||||||
// parameters
|
|
||||||
AP_Float _time_constant_xy; // time constant for horizontal corrections in s
|
|
||||||
AP_Float _time_constant_z; // time constant for vertical corrections in s
|
|
||||||
|
|
||||||
// XY Axis specific variables
|
|
||||||
bool _xy_enabled; // xy position estimates enabled
|
|
||||||
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 in ms
|
|
||||||
uint32_t _gps_last_time; // time of last gps update according to the gps itself in ms
|
|
||||||
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 gpslag
|
|
||||||
AP_BufferFloat_Size5 _hist_position_estimate_y; // buffer of historic accel based position to account for gps lag
|
|
||||||
float _lon_to_cm_scaling; // conversion of longitude to centimeters
|
|
||||||
|
|
||||||
// Z Axis specific variables
|
|
||||||
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 in ms
|
|
||||||
AP_BufferFloat_Size15 _hist_position_estimate_z; // buffer of historic accel based altitudes to account for barometer lag
|
|
||||||
|
|
||||||
// general variables
|
|
||||||
Vector3f _position_base; // (uncorrected) position estimate in cm - relative to the home location (_base_lat, _base_lon, 0)
|
|
||||||
Vector3f _position_correction; // sum of corrections to _position_base from delayed 1st order samples in cm
|
|
||||||
Vector3f _velocity; // latest velocity estimate (integrated from accelerometer values) in cm/s
|
|
||||||
Vector3f _position_error; // current position error in cm - is set by the check_* methods and used by update method to calculate the correction terms
|
|
||||||
Vector3f _position; // sum(_position_base, _position_correction) - corrected position estimate in cm - relative to the home location (_base_lat, _base_lon, 0)
|
|
||||||
|
|
||||||
// error handling
|
|
||||||
GPS_Glitch& _glitch_detector; // GPS Glitch detector
|
|
||||||
uint8_t _error_count; // number of missed GPS updates
|
|
||||||
|
|
||||||
int32_t _last_home_lat;
|
|
||||||
int32_t _last_home_lng;
|
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
||||||
#if AP_AHRS_NAVEKF_AVAILABLE
|
#if AP_AHRS_NAVEKF_AVAILABLE
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue