fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-28 21:33:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' of github.com:PX4/Firmware into control_state

Browse Source
This commit is contained in:
Youssef Demitri
2015-10-14 11:02:23 +02:00
parent 1ec96d87d7 c599cf1256
commit c22c678b7d
498 changed files with 15244 additions and 11123 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
+3
View File
@@ -134,6 +134,8 @@ public:
*/
int set_debuglevel(unsigned debug) { _debug = debug; return 0; }
static constexpr unsigned MAX_PREDICITION_STEPS = 3; /**< maximum number of prediction steps between updates */
private:
bool _task_should_exit; /**< if true, sensor task should exit */
bool _task_running; /**< if true, task is running in its mainloop */
@@ -177,6 +179,7 @@ private:
hrt_abstime _last_accel;
hrt_abstime _last_mag;
unsigned _prediction_steps;
struct sensor_combined_s _sensor_combined;
src/modules/ekf_att_pos_estimator/module.mk → src/modules/ekf_att_pos_estimator/CMakeLists.txt
+17 -13
View File
@@ -1,6 +1,6 @@
############################################################################
#
# Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
# Copyright (c) 2015 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
@@ -31,16 +31,20 @@
#
############################################################################
#
# Main EKF Attitude and Position Estimator
#
MODULE_COMMAND = ekf_att_pos_estimator
SRCS = ekf_att_pos_estimator_main.cpp \
ekf_att_pos_estimator_params.c \
estimator_22states.cpp \
estimator_utilities.cpp
EXTRACXXFLAGS = -Weffc++ -Wframe-larger-than=3400
set(MODULE_CFLAGS )
if(NOT ${OS} STREQUAL "qurt")
list(APPEND MODULE_CFLAGS -Wframe-larger-than=3400)
endif()
px4_add_module(
MODULE modules__ekf_att_pos_estimator
MAIN ekf_att_pos_estimator
COMPILE_FLAGS ${MODULE_CFLAGS}
SRCS
ekf_att_pos_estimator_main.cpp
estimator_22states.cpp
estimator_utilities.cpp
DEPENDS
platforms__common
)
# vim: set noet ft=cmake fenc=utf-8 ff=unix :
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+31 -19
View File
@@ -109,12 +109,6 @@ uint64_t getMicros()
namespace estimator
{
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
AttitudePositionEstimatorEKF *g_estimator = nullptr;
}
@@ -164,6 +158,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
_last_accel(0),
_last_mag(0),
_prediction_steps(0),
_sensor_combined{},
@@ -1073,6 +1068,14 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const
_ekf->summedDelVel = _ekf->summedDelVel + _ekf->dVelIMU;
_covariancePredictionDt += _ekf->dtIMU;
// only fuse every few steps
if (_prediction_steps < MAX_PREDICITION_STEPS) {
_prediction_steps++;
return;
} else {
_prediction_steps = 0;
}
// perform a covariance prediction if the total delta angle has exceeded the limit
// or the time limit will be exceeded at the next IMU update
if ((_covariancePredictionDt >= (_ekf->covTimeStepMax - _ekf->dtIMU))
@@ -1189,7 +1192,7 @@ int AttitudePositionEstimatorEKF::start()
_estimator_task = px4_task_spawn_cmd("ekf_att_pos_estimator",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 20,
4800,
4200,
(px4_main_t)&AttitudePositionEstimatorEKF::task_main_trampoline,
nullptr);
@@ -1249,11 +1252,12 @@ void AttitudePositionEstimatorEKF::print_status()
PX4_INFO("states (terrain) [22]: %8.4f", (double)_ekf->states[22]);
} else {
PX4_INFO("states (wind) [13-14]: %8.4f, %8.4f", (double)_ekf->states[13], (double)_ekf->states[14]);
PX4_INFO("states (earth mag) [15-17]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[15], (double)_ekf->states[16],
(double)_ekf->states[17]);
PX4_INFO("states (body mag) [18-20]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[18], (double)_ekf->states[19],
(double)_ekf->states[20]);
PX4_INFO("states (accel offs) [13]: %8.4f", (double)_ekf->states[13]);
PX4_INFO("states (wind) [14-15]: %8.4f, %8.4f", (double)_ekf->states[14], (double)_ekf->states[15]);
PX4_INFO("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[16], (double)_ekf->states[17],
(double)_ekf->states[18]);
PX4_INFO("states (mag bias) [19-21]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[19], (double)_ekf->states[20],
(double)_ekf->states[21]);
}
PX4_INFO("states: %s %s %s %s %s %s %s %s %s %s",
@@ -1332,7 +1336,7 @@ void AttitudePositionEstimatorEKF::pollData()
} else {
_ekf->dVelIMU.x = 0.5f * (_ekf->accel.x + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 0]);
_ekf->dVelIMU.y = 0.5f * (_ekf->accel.y + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 1]);
_ekf->dVelIMU.z = 0.5f * (_ekf->accel.z + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 2]);
_ekf->dVelIMU.z = 0.5f * (_ekf->accel.z + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 2]) + 9.80665f;
}
_ekf->accel.x = _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 0];
@@ -1400,8 +1404,8 @@ void AttitudePositionEstimatorEKF::pollData()
// 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;
float deltaTIntegral = (_sensor_combined.gyro_integral_dt[0]) / 1e6f;
float deltaTIntAcc = (_sensor_combined.accelerometer_integral_dt[0]) / 1e6f;
static unsigned dtoverflow5 = 0;
static unsigned dtoverflow10 = 0;
@@ -1412,13 +1416,21 @@ void AttitudePositionEstimatorEKF::pollData()
(double)deltaT, (double)deltaTIntegral, (double)deltaTIntAcc,
dtoverflow5, dtoverflow10);
warnx("DRV: dang: %8.4f %8.4f dvel: %8.4f %8.4f",
warnx("EKF: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f",
(double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.z,
(double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.z);
(double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z);
warnx("EKF: dang: %8.4f %8.4f dvel: %8.4f %8.4f",
warnx("INT: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f",
(double)(_sensor_combined.gyro_integral_rad[0]), (double)(_sensor_combined.gyro_integral_rad[2]),
(double)(_sensor_combined.accelerometer_integral_m_s[0]),
(double)(_sensor_combined.accelerometer_integral_m_s[1]),
(double)(_sensor_combined.accelerometer_integral_m_s[2]));
warnx("DRV: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f",
(double)(_sensor_combined.gyro_rad_s[0] * deltaT), (double)(_sensor_combined.gyro_rad_s[2] * deltaT),
(double)(_sensor_combined.accelerometer_m_s2[0] * deltaT), (double)(_sensor_combined.accelerometer_m_s2[2] * deltaT));
(double)(_sensor_combined.accelerometer_m_s2[0] * deltaT),
(double)(_sensor_combined.accelerometer_m_s2[1] * deltaT),
(double)((_sensor_combined.accelerometer_m_s2[2] + 9.8665f) * deltaT));
lastprint = hrt_absolute_time();
}
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c
+1 -5
View File
@@ -36,13 +36,9 @@
*
* Parameters defined by the attitude and position estimator task
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Lorenz Meier <lorenz@px4.io>
*/
#include <px4_config.h>
#include <systemlib/param/param.h>
/*
* Estimator parameters, accessible via MAVLink
*