mirror of https://github.com/ArduPilot/ardupilot
AP_InertialNav: reanme AP_InertialNav and ThirdOrderCompFilter classes to resolve desktop build compiler errors
This commit is contained in:
rmackay9
parent
2df7a407da
commit
b13264c884
|
|
@ -105,8 +105,8 @@
|
|||
#include <AP_Camera.h> // Photo or video camera
|
||||
#include <AP_Mount.h> // Camera/Antenna mount
|
||||
#include <AP_Airspeed.h> // needed for AHRS build
|
||||
#include <AP_InertialNav3D.h> // ArduPilot Mega inertial navigation library
|
||||
#include <ThirdOrderCompFilter3D.h> // Complementary filter for combining barometer altitude with accelerometers
|
||||
#include <AP_InertialNav.h> // ArduPilot Mega inertial navigation library
|
||||
#include <ThirdOrderCompFilter.h> // Complementary filter for combining barometer altitude with accelerometers
|
||||
#include <memcheck.h>
|
||||
|
||||
// Configuration
|
||||
|
|
@ -857,7 +857,7 @@ static float G_Dt = 0.02;
|
|||
// Inertial Navigation
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
#if INERTIAL_NAV == ENABLED
|
||||
AP_InertialNav3D inertial_nav(&ahrs, &ins, &barometer, &g_gps);
|
||||
AP_InertialNav inertial_nav(&ahrs, &ins, &barometer, &g_gps);
|
||||
#endif
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
|
|
|
|||
|
|
@ -790,10 +790,10 @@ static void Log_Write_INAV(float delta_t)
|
|||
DataFlash.WriteInt((int16_t)inertial_nav._position.z); // 2 accel + baro filtered altitude
|
||||
DataFlash.WriteInt((int16_t)climb_rate_actual); // 3 barometer based climb rate
|
||||
DataFlash.WriteInt((int16_t)inertial_nav._velocity.z); // 4 accel + baro based climb rate
|
||||
DataFlash.WriteLong(get_int(inertial_nav._comp_filter3D._comp_k1o.x)); // 5 accel correction x-axis
|
||||
DataFlash.WriteLong(get_int(inertial_nav._comp_filter3D._comp_k1o.y)); // 6 accel correction y-axis
|
||||
DataFlash.WriteLong(get_int(inertial_nav._comp_filter3D._comp_k1o.z)); // 7 accel correction z-axis
|
||||
DataFlash.WriteLong(get_int(inertial_nav._comp_filter3D.comp_k1o_ef.z));// 8 accel correction earth frame
|
||||
DataFlash.WriteLong(get_int(inertial_nav._comp_filter._comp_k1o.x)); // 5 accel correction x-axis
|
||||
DataFlash.WriteLong(get_int(inertial_nav._comp_filter._comp_k1o.y)); // 6 accel correction y-axis
|
||||
DataFlash.WriteLong(get_int(inertial_nav._comp_filter._comp_k1o.z)); // 7 accel correction z-axis
|
||||
DataFlash.WriteLong(get_int(inertial_nav._comp_filter.comp_k1o_ef.z));// 8 accel correction earth frame
|
||||
DataFlash.WriteLong(get_int(inertial_nav._accel_ef.x)); // 9 accel earth frame x-axis
|
||||
DataFlash.WriteLong(get_int(inertial_nav._accel_ef.y)); // 10 accel earth frame y-axis
|
||||
DataFlash.WriteLong(get_int(inertial_nav._accel_ef.z)); // 11 accel earth frame z-axis
|
||||
|
|
|
|||
|
|
@ -371,7 +371,7 @@ const AP_Param::Info var_info[] PROGMEM = {
|
|||
#if INERTIAL_NAV == ENABLED
|
||||
// @Group: INAV_
|
||||
// @Path: ../libraries/AP_InertialNav/AP_InertialNav.cpp
|
||||
GOBJECT(inertial_nav, "INAV_", AP_InertialNav3D),
|
||||
GOBJECT(inertial_nav, "INAV_", AP_InertialNav),
|
||||
#endif
|
||||
|
||||
GOBJECT(gcs0, "SR0_", GCS_MAVLINK),
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
#include <FastSerial.h>
|
||||
#include <AP_InertialNav3D.h>
|
||||
#include <AP_InertialNav.h>
|
||||
|
||||
#if defined(ARDUINO) && ARDUINO >= 100
|
||||
#include "Arduino.h"
|
||||
|
|
@ -9,36 +9,36 @@
|
|||
#endif
|
||||
|
||||
// table of user settable parameters
|
||||
const AP_Param::GroupInfo AP_InertialNav3D::var_info[] PROGMEM = {
|
||||
const AP_Param::GroupInfo AP_InertialNav::var_info[] PROGMEM = {
|
||||
// index 0 and 1 are for old parameters that are no longer used
|
||||
|
||||
// @Param: ACORR
|
||||
// @DisplayName: Inertial Nav calculated accelerometer corrections
|
||||
// @Description: calculated accelerometer corrections
|
||||
// @Increment: 1
|
||||
AP_GROUPINFO("ACORR", 0, AP_InertialNav3D, _accel_correction, 0),
|
||||
AP_GROUPINFO("ACORR", 0, AP_InertialNav, _accel_correction, 0),
|
||||
|
||||
// @Param: TC_XY
|
||||
// @DisplayName: Horizontal Time Constant
|
||||
// @Description: Time constnat for GPS and accel mixing. Higher TC increases GPS impact on position
|
||||
// @Range: 0 10
|
||||
// @Increment: 0.1
|
||||
AP_GROUPINFO("TC_XY", 1, AP_InertialNav3D, _time_constant_xy, AP_INTERTIALNAV_TC_XY),
|
||||
AP_GROUPINFO("TC_XY", 1, AP_InertialNav, _time_constant_xy, AP_INTERTIALNAV_TC_XY),
|
||||
|
||||
// @Param: TC_Z
|
||||
// @DisplayName: Vertical Time Constant
|
||||
// @Description: Time constnat for baro and accel mixing. Higher TC increases barometers impact on altitude
|
||||
// @Range: 0 10
|
||||
// @Increment: 0.1
|
||||
AP_GROUPINFO("TC_Z", 2, AP_InertialNav3D, _time_constant_z, AP_INTERTIALNAV_TC_Z),
|
||||
AP_GROUPINFO("TC_Z", 2, AP_InertialNav, _time_constant_z, AP_INTERTIALNAV_TC_Z),
|
||||
|
||||
AP_GROUPEND
|
||||
};
|
||||
|
||||
// save_params - save all parameters to eeprom
|
||||
void AP_InertialNav3D::save_params()
|
||||
void AP_InertialNav::save_params()
|
||||
{
|
||||
Vector3f accel_corr = _comp_filter3D.get_1st_order_correction();
|
||||
Vector3f accel_corr = _comp_filter.get_1st_order_correction();
|
||||
accel_corr.x = constrain(accel_corr.x,-200,200);
|
||||
accel_corr.y = constrain(accel_corr.y,-200,200);
|
||||
accel_corr.z = constrain(accel_corr.z,-200,200);
|
||||
|
|
@ -46,27 +46,27 @@ void AP_InertialNav3D::save_params()
|
|||
}
|
||||
|
||||
// set time constant - set timeconstant used by complementary filter
|
||||
void AP_InertialNav3D::set_time_constant_xy( float time_constant_in_seconds )
|
||||
void AP_InertialNav::set_time_constant_xy( float time_constant_in_seconds )
|
||||
{
|
||||
// ensure it's a reasonable value
|
||||
if( time_constant_in_seconds > 0 && time_constant_in_seconds < 30 ) {
|
||||
_time_constant_xy = time_constant_in_seconds;
|
||||
_comp_filter3D.update_gains(_time_constant_xy, _time_constant_z);
|
||||
_comp_filter.update_gains(_time_constant_xy, _time_constant_z);
|
||||
}
|
||||
}
|
||||
|
||||
// set time constant - set timeconstant used by complementary filter
|
||||
void AP_InertialNav3D::set_time_constant_z( float time_constant_in_seconds )
|
||||
void AP_InertialNav::set_time_constant_z( float time_constant_in_seconds )
|
||||
{
|
||||
// ensure it's a reasonable value
|
||||
if( time_constant_in_seconds > 0 && time_constant_in_seconds < 30 ) {
|
||||
_time_constant_z = time_constant_in_seconds;
|
||||
_comp_filter3D.update_gains(_time_constant_xy, _time_constant_z);
|
||||
_comp_filter.update_gains(_time_constant_xy, _time_constant_z);
|
||||
}
|
||||
}
|
||||
|
||||
// check_baro - check if new baro readings have arrived and use them to correct vertical accelerometer offsets
|
||||
void AP_InertialNav3D::check_baro()
|
||||
void AP_InertialNav::check_baro()
|
||||
{
|
||||
uint32_t baro_update_time;
|
||||
|
||||
|
|
@ -85,7 +85,7 @@ void AP_InertialNav3D::check_baro()
|
|||
|
||||
|
||||
// correct_with_baro - modifies accelerometer offsets using barometer. dt is time since last baro reading
|
||||
void AP_InertialNav3D::correct_with_baro(float baro_alt, float dt)
|
||||
void AP_InertialNav::correct_with_baro(float baro_alt, float dt)
|
||||
{
|
||||
static uint8_t first_reads = 0;
|
||||
|
||||
|
|
@ -96,8 +96,8 @@ void AP_InertialNav3D::correct_with_baro(float baro_alt, float dt)
|
|||
|
||||
// discard first 10 reads but perform some initialisation
|
||||
if( first_reads <= 10 ) {
|
||||
_comp_filter3D.set_3rd_order_z(baro_alt);
|
||||
//_comp_filter3D.set_2nd_order_z(climb_rate);
|
||||
_comp_filter.set_3rd_order_z(baro_alt);
|
||||
//_comp_filter.set_2nd_order_z(climb_rate);
|
||||
first_reads++;
|
||||
}
|
||||
|
||||
|
|
@ -105,11 +105,11 @@ void AP_InertialNav3D::correct_with_baro(float baro_alt, float dt)
|
|||
Matrix3f dcm = _ahrs->get_dcm_matrix();
|
||||
|
||||
// provide baro alt to filter
|
||||
_comp_filter3D.correct_3rd_order_z(baro_alt, dcm, dt);
|
||||
_comp_filter.correct_3rd_order_z(baro_alt, dcm, dt);
|
||||
}
|
||||
|
||||
// set_current_position - all internal calculations are recorded as the distances from this point
|
||||
void AP_InertialNav3D::set_current_position(int32_t lon, int32_t lat)
|
||||
void AP_InertialNav::set_current_position(int32_t lon, int32_t lat)
|
||||
{
|
||||
// set base location
|
||||
_base_lon = lon;
|
||||
|
|
@ -120,14 +120,14 @@ void AP_InertialNav3D::set_current_position(int32_t lon, int32_t lat)
|
|||
_lon_to_m_scaling = cos((fabs((float)lat)/10000000.0) * 0.0174532925);
|
||||
|
||||
// set estimated position to this position
|
||||
_comp_filter3D.set_3rd_order_xy(0,0);
|
||||
_comp_filter.set_3rd_order_xy(0,0);
|
||||
|
||||
// set xy as enabled
|
||||
_xy_enabled = true;
|
||||
}
|
||||
|
||||
// check_gps - check if new gps readings have arrived and use them to correct position estimates
|
||||
void AP_InertialNav3D::check_gps()
|
||||
void AP_InertialNav::check_gps()
|
||||
{
|
||||
uint32_t gps_time;
|
||||
uint32_t now;
|
||||
|
|
@ -155,7 +155,7 @@ void AP_InertialNav3D::check_gps()
|
|||
}
|
||||
|
||||
// correct_with_gps - modifies accelerometer offsets using gps. dt is time since last gps update
|
||||
void AP_InertialNav3D::correct_with_gps(int32_t lon, int32_t lat, float dt)
|
||||
void AP_InertialNav::correct_with_gps(int32_t lon, int32_t lat, float dt)
|
||||
{
|
||||
float x,y;
|
||||
|
||||
|
|
@ -174,12 +174,12 @@ void AP_InertialNav3D::correct_with_gps(int32_t lon, int32_t lat, float dt)
|
|||
Matrix3f dcm = _ahrs->get_dcm_matrix();
|
||||
|
||||
// call comp filter's correct xy
|
||||
_comp_filter3D.correct_3rd_order_xy(-x, -y, dcm, dt);
|
||||
_comp_filter.correct_3rd_order_xy(-x, -y, dcm, dt);
|
||||
//Notes: with +x above, accel lat comes out reversed
|
||||
}
|
||||
|
||||
// update - updates velocities and positions using latest info from ahrs, ins and barometer if new data is available;
|
||||
void AP_InertialNav3D::update(float dt)
|
||||
void AP_InertialNav::update(float dt)
|
||||
{
|
||||
// discard samples where dt is too large
|
||||
if( dt > 0.1 ) {
|
||||
|
|
@ -210,24 +210,24 @@ void AP_InertialNav3D::update(float dt)
|
|||
}
|
||||
|
||||
// provide accelerometer values to filter
|
||||
_comp_filter3D.add_1st_order_sample(_accel_ef);
|
||||
_comp_filter.add_1st_order_sample(_accel_ef);
|
||||
|
||||
// recalculate estimates
|
||||
_comp_filter3D.calculate(dt, dcm);
|
||||
_comp_filter.calculate(dt, dcm);
|
||||
|
||||
// get position and velocity estimates
|
||||
_position = _comp_filter3D.get_3rd_order_estimate();
|
||||
_velocity = _comp_filter3D.get_2nd_order_estimate();
|
||||
_position = _comp_filter.get_3rd_order_estimate();
|
||||
_velocity = _comp_filter.get_2nd_order_estimate();
|
||||
}
|
||||
|
||||
// position_ok - return true if position has been initialised and have received gps data within 3 seconds
|
||||
bool AP_InertialNav3D::position_ok()
|
||||
bool AP_InertialNav::position_ok()
|
||||
{
|
||||
return _xy_enabled && (millis() - _gps_last_update < 3000);
|
||||
}
|
||||
|
||||
// get accel based latitude
|
||||
int32_t AP_InertialNav3D::get_latitude()
|
||||
int32_t AP_InertialNav::get_latitude()
|
||||
{
|
||||
// make sure we've been initialised
|
||||
if( !_xy_enabled ) {
|
||||
|
|
@ -239,7 +239,7 @@ int32_t AP_InertialNav3D::get_latitude()
|
|||
}
|
||||
|
||||
// get accel based longitude
|
||||
int32_t AP_InertialNav3D::get_longitude()
|
||||
int32_t AP_InertialNav::get_longitude()
|
||||
{
|
||||
// make sure we've been initialised
|
||||
if( !_xy_enabled ) {
|
||||
|
|
@ -251,7 +251,7 @@ int32_t AP_InertialNav3D::get_longitude()
|
|||
}
|
||||
|
||||
// get accel based latitude
|
||||
float AP_InertialNav3D::get_latitude_diff()
|
||||
float AP_InertialNav::get_latitude_diff()
|
||||
{
|
||||
// make sure we've been initialised
|
||||
if( !_xy_enabled ) {
|
||||
|
|
@ -264,7 +264,7 @@ float AP_InertialNav3D::get_latitude_diff()
|
|||
}
|
||||
|
||||
// get accel based longitude
|
||||
float AP_InertialNav3D::get_longitude_diff()
|
||||
float AP_InertialNav::get_longitude_diff()
|
||||
{
|
||||
// make sure we've been initialised
|
||||
if( !_xy_enabled ) {
|
||||
|
|
@ -277,7 +277,7 @@ float AP_InertialNav3D::get_longitude_diff()
|
|||
}
|
||||
|
||||
// get velocity in latitude & longitude directions
|
||||
float AP_InertialNav3D::get_latitude_velocity()
|
||||
float AP_InertialNav::get_latitude_velocity()
|
||||
{
|
||||
// make sure we've been initialised
|
||||
if( !_xy_enabled ) {
|
||||
|
|
@ -288,7 +288,7 @@ float AP_InertialNav3D::get_latitude_velocity()
|
|||
// Note: is +_velocity.x the output velocity in logs is in reverse direction from accel lat
|
||||
}
|
||||
|
||||
float AP_InertialNav3D::get_longitude_velocity()
|
||||
float AP_InertialNav::get_longitude_velocity()
|
||||
{
|
||||
// make sure we've been initialised
|
||||
if( !_xy_enabled ) {
|
||||
|
|
@ -1,26 +1,26 @@
|
|||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
#ifndef __AP_INERTIALNAV3D_H__
|
||||
#define __AP_INERTIALNAV3D_H__
|
||||
#ifndef __AP_INERTIALNAV_H__
|
||||
#define __AP_INERTIALNAV_H__
|
||||
|
||||
#include <AP_AHRS.h>
|
||||
#include <AP_InertialSensor.h> // ArduPilot Mega IMU Library
|
||||
#include <AP_Baro.h> // ArduPilot Mega Barometer Library
|
||||
#include <ThirdOrderCompFilter3D.h> // Complementary filter for combining barometer altitude with accelerometers
|
||||
#include <ThirdOrderCompFilter.h> // Complementary filter for combining barometer altitude with accelerometers
|
||||
|
||||
#define AP_INTERTIALNAV_GRAVITY 9.80665
|
||||
#define AP_INTERTIALNAV_TC_XY 3.0 // default time constant for complementary filter's X & Y axis
|
||||
#define AP_INTERTIALNAV_TC_Z 1.5 // default time constant for complementary filter's Z axis
|
||||
|
||||
/*
|
||||
* AP_InertialNav3D is an attempt to use accelerometers to augment other sensors to improve altitud e position hold
|
||||
* AP_InertialNav is an attempt to use accelerometers to augment other sensors to improve altitud e position hold
|
||||
*/
|
||||
class AP_InertialNav3D
|
||||
class AP_InertialNav
|
||||
{
|
||||
public:
|
||||
|
||||
// Constructor
|
||||
AP_InertialNav3D( AP_AHRS* ahrs, AP_InertialSensor* ins, AP_Baro* baro, GPS** gps_ptr ) :
|
||||
AP_InertialNav( AP_AHRS* ahrs, AP_InertialSensor* ins, AP_Baro* baro, GPS** gps_ptr ) :
|
||||
_ahrs(ahrs),
|
||||
_ins(ins),
|
||||
_baro(baro),
|
||||
|
|
@ -28,7 +28,7 @@ public:
|
|||
_baro_last_update(0),
|
||||
_gps_last_update(0),
|
||||
_xy_enabled(false),
|
||||
_comp_filter3D(AP_INTERTIALNAV_TC_XY, AP_INTERTIALNAV_TC_Z)
|
||||
_comp_filter(AP_INTERTIALNAV_TC_XY, AP_INTERTIALNAV_TC_Z)
|
||||
{}
|
||||
|
||||
// Initialisation
|
||||
|
|
@ -70,11 +70,11 @@ public:
|
|||
|
||||
// get_altitude - get latest altitude estimate in cm
|
||||
virtual float get_altitude() { return _position.z; }
|
||||
virtual void set_altitude( int32_t new_altitude) { _comp_filter3D.set_3rd_order_z(new_altitude); }
|
||||
virtual void set_altitude( int32_t new_altitude) { _comp_filter.set_3rd_order_z(new_altitude); }
|
||||
|
||||
// get_velocity_z - get latest climb rate (in cm/s)
|
||||
virtual float get_velocity_z() { return _velocity.z; }
|
||||
virtual void set_velocity_z( int32_t new_velocity ) { _comp_filter3D.set_2nd_order_z(new_velocity); }
|
||||
virtual void set_velocity_z( int32_t new_velocity ) { _comp_filter.set_2nd_order_z(new_velocity); }
|
||||
|
||||
// get latitude & longitude positions
|
||||
virtual int32_t get_latitude();
|
||||
|
|
@ -114,8 +114,8 @@ public:
|
|||
int32_t _base_lon; // base longitude
|
||||
float _lon_to_m_scaling; // conversion of longitude to meters
|
||||
|
||||
ThirdOrderCompFilter3D _comp_filter3D; // 3rd order complementary filter for combining baro readings with accelerometers
|
||||
ThirdOrderCompFilter _comp_filter; // 3rd order complementary filter for combining baro readings with accelerometers
|
||||
|
||||
};
|
||||
|
||||
#endif // __AP_INERTIALNAV3D_H__
|
||||
#endif // __AP_INERTIALNAV_H__
|
||||
|
|
@ -24,8 +24,8 @@
|
|||
#include <AP_Airspeed.h>
|
||||
#include <AC_PID.h> // PID library
|
||||
#include <APM_PI.h> // PID library
|
||||
#include <AP_InertialNav3D.h>
|
||||
#include <ThirdOrderCompFilter3D.h> // Complementary filter for combining barometer altitude with accelerometers
|
||||
#include <AP_InertialNav.h>
|
||||
#include <ThirdOrderCompFilter.h> // Complementary filter for combining barometer altitude with accelerometers
|
||||
#include <AP_Buffer.h> // ArduPilot general purpose FIFO buffer
|
||||
|
||||
FastSerialPort(Serial, 0);
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
#include <FastSerial.h>
|
||||
#include <ThirdOrderCompFilter3D.h>
|
||||
#include <ThirdOrderCompFilter.h>
|
||||
|
||||
#if defined(ARDUINO) && ARDUINO >= 100
|
||||
#include "Arduino.h"
|
||||
|
|
@ -11,7 +11,7 @@
|
|||
// Public Methods //////////////////////////////////////////////////////////////
|
||||
|
||||
// update_gains - update gains from time constant (given in seconds)
|
||||
void ThirdOrderCompFilter3D::update_gains(float time_constant_seconds_xy, float time_constant_seconds_z)
|
||||
void ThirdOrderCompFilter::update_gains(float time_constant_seconds_xy, float time_constant_seconds_z)
|
||||
{
|
||||
static float last_time_constant_xy = 0;
|
||||
static float last_time_constant_z = 0;
|
||||
|
|
@ -42,7 +42,7 @@ void ThirdOrderCompFilter3D::update_gains(float time_constant_seconds_xy, float
|
|||
}
|
||||
|
||||
// set_3rd_order - resets the first order value (i.e. position)
|
||||
void ThirdOrderCompFilter3D::set_3rd_order_xy(float x, float y)
|
||||
void ThirdOrderCompFilter::set_3rd_order_xy(float x, float y)
|
||||
{
|
||||
_comp_h.x = x;
|
||||
_comp_h.y = y;
|
||||
|
|
@ -55,27 +55,27 @@ void ThirdOrderCompFilter3D::set_3rd_order_xy(float x, float y)
|
|||
}
|
||||
|
||||
// set_3rd_order - resets the first order value (i.e. position)
|
||||
void ThirdOrderCompFilter3D::set_3rd_order_z(float z )
|
||||
void ThirdOrderCompFilter::set_3rd_order_z(float z )
|
||||
{
|
||||
_comp_h.z = z;
|
||||
_comp_h_correction.z = 0;
|
||||
}
|
||||
|
||||
// set_2nd_order - resets the second order value (i.e. velocity)
|
||||
void ThirdOrderCompFilter3D::set_2nd_order_xy(float x, float y)
|
||||
void ThirdOrderCompFilter::set_2nd_order_xy(float x, float y)
|
||||
{
|
||||
_comp_v.x = x;
|
||||
_comp_v.y = y;
|
||||
}
|
||||
|
||||
// set_2nd_order - resets the second order value (i.e. velocity)
|
||||
void ThirdOrderCompFilter3D::set_2nd_order_z(float z )
|
||||
void ThirdOrderCompFilter::set_2nd_order_z(float z )
|
||||
{
|
||||
_comp_v.z = z;
|
||||
}
|
||||
|
||||
// correct_3rd_order_z - correct accelerometer offsets using barometer or gps
|
||||
void ThirdOrderCompFilter3D::correct_3rd_order_xy(float x, float y, Matrix3f& dcm_matrix, float deltat)
|
||||
void ThirdOrderCompFilter::correct_3rd_order_xy(float x, float y, Matrix3f& dcm_matrix, float deltat)
|
||||
{
|
||||
float hist_comp_h_x, hist_comp_h_y;
|
||||
|
||||
|
|
@ -106,7 +106,7 @@ void ThirdOrderCompFilter3D::correct_3rd_order_xy(float x, float y, Matrix3f& dc
|
|||
}
|
||||
|
||||
// correct_3rd_order_z - correct accelerometer offsets using barometer or gps
|
||||
void ThirdOrderCompFilter3D::correct_3rd_order_z(float third_order_sample, Matrix3f& dcm_matrix, float deltat)
|
||||
void ThirdOrderCompFilter::correct_3rd_order_z(float third_order_sample, Matrix3f& dcm_matrix, float deltat)
|
||||
{
|
||||
float hist_comp_h_z;
|
||||
|
||||
|
|
@ -133,7 +133,7 @@ void ThirdOrderCompFilter3D::correct_3rd_order_z(float third_order_sample, Matri
|
|||
}
|
||||
|
||||
// recalculate the 2nd and 3rd order estimates
|
||||
void ThirdOrderCompFilter3D::calculate(float deltat, Matrix3f& dcm_matrix)
|
||||
void ThirdOrderCompFilter::calculate(float deltat, Matrix3f& dcm_matrix)
|
||||
{
|
||||
// get earth frame accelerometer correction
|
||||
comp_k1o_ef = dcm_matrix * _comp_k1o;
|
||||
|
|
@ -6,12 +6,12 @@
|
|||
// your option) any later version.
|
||||
//
|
||||
|
||||
/// @file ThirdOrderCompFilter3D.h
|
||||
/// @file ThirdOrderCompFilter.h
|
||||
/// @brief A class to implement third order complementary filter (for combining barometer and GPS with accelerometer data)
|
||||
/// math provided by Jonathan Challenger
|
||||
|
||||
#ifndef __THIRDORDERCOMPFILTER3D_H__
|
||||
#define __THIRDORDERCOMPFILTER3D_H__
|
||||
#ifndef __THIRD_ORDER_COMP_FILTER_H__
|
||||
#define __THIRD_ORDER_COMP_FILTER_H__
|
||||
|
||||
#include <inttypes.h>
|
||||
#include <AP_Math.h> // Math library for matrix and vector math
|
||||
|
|
@ -24,11 +24,11 @@
|
|||
|
||||
#define THIRD_ORDER_COMP_FILTER_HISTORIC_XY_SAVE_COUNTER_DEFAULT THIRD_ORDER_SAVE_POS_10HZ
|
||||
|
||||
class ThirdOrderCompFilter3D
|
||||
class ThirdOrderCompFilter
|
||||
{
|
||||
public:
|
||||
// constructor
|
||||
ThirdOrderCompFilter3D(float time_constant_seconds_xy, float time_constant_seconds_z)
|
||||
ThirdOrderCompFilter(float time_constant_seconds_xy, float time_constant_seconds_z)
|
||||
{
|
||||
update_gains(time_constant_seconds_xy, time_constant_seconds_z);
|
||||
};
|
||||
|
|
@ -86,4 +86,4 @@ public:
|
|||
Vector3f comp_k1o_ef; // accelerometer correction in earth frame (only z element is used). here for debug purposes
|
||||
};
|
||||
|
||||
#endif // __THIRDORDERCOMPFILTER3D_H__
|
||||
#endif // __THIRD_ORDER_COMP_FILTER_H__
|
||||
Loading…
Reference in New Issue