fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: Use driver-level provided and coning-corrected delta angles

Browse Source
This commit is contained in:
Lorenz Meier 2015-06-21 18:51:34 +02:00
parent c34a505576
commit f0ff10e40f
2 changed files with 53 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
View File

@ -70,6 +70,7 @@
#include <geo/geo.h>
#include <systemlib/perf_counter.h>
#include "estimator_22states.h"
//Forward declaration
class AttPosEKF;
@ -168,6 +169,11 @@ private:
struct vehicle_land_detected_s _landDetector;
struct actuator_armed_s _armed;
Vector3f lastAngRate;
Vector3f lastAccel;
hrt_abstime last_accel;
hrt_abstime last_mag;
struct gyro_scale _gyro_offsets[3];
struct accel_scale _accel_offsets[3];
struct mag_scale _mag_offsets[3];

62
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
View File

@ -155,6 +155,11 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
_landDetector {},
_armed {},
lastAngRate{},
lastAccel{},
last_accel(0),
last_mag(0),
_gyro_offsets({}),
_accel_offsets({}),
_mag_offsets({}),
@ -541,8 +546,6 @@ void AttitudePositionEstimatorEKF::task_main()
orb_set_interval(_vstatus_sub, 200);
_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
/* XXX remove this!, BUT increase the data buffer size! */
orb_set_interval(_sensor_combined_sub, 9);
/* sets also parameters in the EKF object */
parameters_update();
@ -836,9 +839,9 @@ void AttitudePositionEstimatorEKF::publishAttitude()
_att.pitch = euler(1);
_att.yaw = euler(2);
_att.rollspeed = _LP_att_P.apply(_ekf->angRate.x) - _ekf->states[10] / _ekf->dtIMUfilt;
_att.pitchspeed = _LP_att_Q.apply(_ekf->angRate.y) - _ekf->states[11] / _ekf->dtIMUfilt;
_att.yawspeed = _LP_att_R.apply(_ekf->angRate.z) - _ekf->states[12] / _ekf->dtIMUfilt;
_att.rollspeed = _LP_att_P.apply(_ekf->dAngIMU.x / _ekf->dtIMU) - _ekf->states[10] / _ekf->dtIMUfilt;
_att.pitchspeed = _LP_att_Q.apply(_ekf->dAngIMU.y / _ekf->dtIMU) - _ekf->states[11] / _ekf->dtIMUfilt;
_att.yawspeed = _LP_att_R.apply(_ekf->dAngIMU.z / _ekf->dtIMU) - _ekf->states[12] / _ekf->dtIMUfilt;
// gyro offsets
_att.rate_offsets[0] = _ekf->states[10] / _ekf->dtIMUfilt;
@ -1126,7 +1129,7 @@ int AttitudePositionEstimatorEKF::start()
/* start the task */
_estimator_task = px4_task_spawn_cmd("ekf_att_pos_estimator",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 40,
SCHED_PRIORITY_MAX - 20,
7500,
(px4_main_t)&AttitudePositionEstimatorEKF::task_main_trampoline,
nullptr);
@ -1218,15 +1221,10 @@ void AttitudePositionEstimatorEKF::pollData()
}
//Update Gyro and Accelerometer
static Vector3f lastAngRate;
static Vector3f lastAccel;
bool accel_updated = false;
orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
static hrt_abstime last_accel = 0;
static hrt_abstime last_mag = 0;
if (last_accel != _sensor_combined.accelerometer_timestamp) {
accel_updated = true;
@ -1244,6 +1242,38 @@ void AttitudePositionEstimatorEKF::pollData()
float deltaT = (_sensor_combined.timestamp - _last_run) / 1e6f;
// XXX this is for assessing the filter performance
// leave this in as long as larger improvements are still being made.
#if 0
float deltaTIntegral = (_sensor_combined.gyro_integral_dt) / 1e6f;
float deltaTIntAcc = (_sensor_combined.accelerometer_integral_dt) / 1e6f;
static unsigned dtoverflow5 = 0;
static unsigned dtoverflow10 = 0;
static hrt_abstime lastprint = 0;
if (hrt_elapsed_time(&lastprint) > 1000000) {
warnx("dt: %8.6f, dtg: %8.6f, dta: %8.6f, dt > 5 ms: %u, dt > 10 ms: %u",
(double)deltaT, (double)deltaTIntegral, (double)deltaTIntAcc,
dtoverflow5, dtoverflow10);
warnx("dang: %8.4f %8.4f dvel: %8.4f %8.4f",
(double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.z,
(double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.z);
lastprint = hrt_absolute_time();
}
if (deltaT > 0.005f) {
dtoverflow5++;
}
if (deltaT > 0.01f) {
dtoverflow10++;
}
#endif
/* guard against too large deltaT's */
if (!PX4_ISFINITE(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) {
deltaT = 0.01f;
@ -1323,10 +1353,12 @@ void AttitudePositionEstimatorEKF::pollData()
_accel_valid = true;
}
_ekf->dAngIMU = 0.5f * (_ekf->angRate + lastAngRate) * _ekf->dtIMU;
lastAngRate = _ekf->angRate;
_ekf->dVelIMU = 0.5f * (_ekf->accel + lastAccel) * _ekf->dtIMU;
lastAccel = _ekf->accel;
_ekf->dVelIMU.x = _sensor_combined.accelerometer_integral_m_s[0];
_ekf->dVelIMU.y = _sensor_combined.accelerometer_integral_m_s[1];
_ekf->dVelIMU.z = _sensor_combined.accelerometer_integral_m_s[2];
_ekf->dAngIMU.x = _sensor_combined.gyro_integral_rad[0];
_ekf->dAngIMU.y = _sensor_combined.gyro_integral_rad[1];
_ekf->dAngIMU.z = _sensor_combined.gyro_integral_rad[2];
if (last_mag != _sensor_combined.magnetometer_timestamp) {
_newDataMag = true;