Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-06 06:43:21 +08:00
Code Issues Actions 30 Packages Projects Releases Wiki Activity

att + pos EKF: Enable execution on Linux

Browse Source
This commit is contained in:
Lorenz Meier
2015-03-29 03:02:31 +02:00
committed by Mark Charlebois
parent b4d52327e8
commit 7bdaec9ff0
2 changed files with 50 additions and 64 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+38 -52
View File
@@ -44,6 +44,7 @@
#include "estimator_22states.h" #include "estimator_22states.h"
#include <px4_config.h> #include <px4_config.h>
#include <px4_tasks.h>
#include <stdio.h> #include <stdio.h>
#include <stdlib.h> #include <stdlib.h>
#include <string.h> #include <string.h>
@@ -281,7 +282,7 @@ AttitudePositionEstimatorEKF::~AttitudePositionEstimatorEKF()
/* if we have given up, kill it */ /* if we have given up, kill it */
if (++i > 50) { if (++i > 50) {
task_delete(_estimator_task); px4_task_delete(_estimator_task);
break; break;
} }
} while (_estimator_task != -1); } while (_estimator_task != -1);
@@ -490,7 +491,8 @@ void AttitudePositionEstimatorEKF::task_main()
_filter_start_time = hrt_absolute_time(); _filter_start_time = hrt_absolute_time();
if (!_ekf) { if (!_ekf) {
errx(1, "OUT OF MEM!"); warnx("OUT OF MEM!");
return;
} }
/* /*
@@ -696,10 +698,8 @@ void AttitudePositionEstimatorEKF::task_main()
_task_running = false; _task_running = false;
warnx("exiting.\n");
_estimator_task = -1; _estimator_task = -1;
_exit(0); return;
} }
void AttitudePositionEstimatorEKF::initializeGPS() void AttitudePositionEstimatorEKF::initializeGPS()
@@ -1039,7 +1039,7 @@ int AttitudePositionEstimatorEKF::start()
SCHED_DEFAULT, SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 40, SCHED_PRIORITY_MAX - 40,
7500, 7500,
(main_t)&AttitudePositionEstimatorEKF::task_main_trampoline, (px4_main_t)&AttitudePositionEstimatorEKF::task_main_trampoline,
nullptr); nullptr);
if (_estimator_task < 0) { if (_estimator_task < 0) {
@@ -1155,7 +1155,7 @@ void AttitudePositionEstimatorEKF::pollData()
float deltaT = (_sensor_combined.timestamp - _last_run) / 1e6f; float deltaT = (_sensor_combined.timestamp - _last_run) / 1e6f;
/* guard against too large deltaT's */ /* guard against too large deltaT's */
if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) { if (!std::isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) {
deltaT = 0.01f; deltaT = 0.01f;
} }
@@ -1167,9 +1167,9 @@ void AttitudePositionEstimatorEKF::pollData()
int last_gyro_main = _gyro_main; int last_gyro_main = _gyro_main;
if (isfinite(_sensor_combined.gyro_rad_s[0]) && if (std::isfinite(_sensor_combined.gyro_rad_s[0]) &&
isfinite(_sensor_combined.gyro_rad_s[1]) && std::isfinite(_sensor_combined.gyro_rad_s[1]) &&
isfinite(_sensor_combined.gyro_rad_s[2]) && std::isfinite(_sensor_combined.gyro_rad_s[2]) &&
(_sensor_combined.gyro_errcount <= _sensor_combined.gyro1_errcount)) { (_sensor_combined.gyro_errcount <= _sensor_combined.gyro1_errcount)) {
_ekf->angRate.x = _sensor_combined.gyro_rad_s[0]; _ekf->angRate.x = _sensor_combined.gyro_rad_s[0];
@@ -1178,9 +1178,9 @@ void AttitudePositionEstimatorEKF::pollData()
_gyro_main = 0; _gyro_main = 0;
_gyro_valid = true; _gyro_valid = true;
} else if (isfinite(_sensor_combined.gyro1_rad_s[0]) && } else if (std::isfinite(_sensor_combined.gyro1_rad_s[0]) &&
isfinite(_sensor_combined.gyro1_rad_s[1]) && std::isfinite(_sensor_combined.gyro1_rad_s[1]) &&
isfinite(_sensor_combined.gyro1_rad_s[2])) { std::isfinite(_sensor_combined.gyro1_rad_s[2])) {
_ekf->angRate.x = _sensor_combined.gyro1_rad_s[0]; _ekf->angRate.x = _sensor_combined.gyro1_rad_s[0];
_ekf->angRate.y = _sensor_combined.gyro1_rad_s[1]; _ekf->angRate.y = _sensor_combined.gyro1_rad_s[1];
@@ -1528,25 +1528,29 @@ int AttitudePositionEstimatorEKF::trip_nan()
int ekf_att_pos_estimator_main(int argc, char *argv[]) int ekf_att_pos_estimator_main(int argc, char *argv[])
{ {
if (argc < 2) { if (argc < 2) {
errx(1, "usage: ekf_att_pos_estimator {start|stop|status|logging}"); warnx("usage: ekf_att_pos_estimator {start|stop|status|logging}");
return 1;
} }
if (!strcmp(argv[1], "start")) { if (!strcmp(argv[1], "start")) {
if (estimator::g_estimator != nullptr) { if (estimator::g_estimator != nullptr) {
errx(1, "already running"); warnx("already running");
return 1;
} }
estimator::g_estimator = new AttitudePositionEstimatorEKF(); estimator::g_estimator = new AttitudePositionEstimatorEKF();
if (estimator::g_estimator == nullptr) { if (estimator::g_estimator == nullptr) {
errx(1, "alloc failed"); warnx("alloc failed");
return 1;
} }
if (OK != estimator::g_estimator->start()) { if (OK != estimator::g_estimator->start()) {
delete estimator::g_estimator; delete estimator::g_estimator;
estimator::g_estimator = nullptr; estimator::g_estimator = nullptr;
err(1, "start failed"); warnx("start failed");
return 1;
} }
/* avoid memory fragmentation by not exiting start handler until the task has fully started */ /* avoid memory fragmentation by not exiting start handler until the task has fully started */
@@ -1558,64 +1562,46 @@ int ekf_att_pos_estimator_main(int argc, char *argv[])
printf("\n"); printf("\n");
exit(0); return 0;
}
if (estimator::g_estimator == nullptr) {
warnx("not running");
return 1;
} }
if (!strcmp(argv[1], "stop")) { if (!strcmp(argv[1], "stop")) {
if (estimator::g_estimator == nullptr) {
errx(1, "not running");
}
delete estimator::g_estimator; delete estimator::g_estimator;
estimator::g_estimator = nullptr; estimator::g_estimator = nullptr;
exit(0); return 0;
} }
if (!strcmp(argv[1], "status")) { if (!strcmp(argv[1], "status")) {
if (estimator::g_estimator) { warnx("running");
warnx("running");
estimator::g_estimator->print_status(); estimator::g_estimator->print_status();
exit(0); return 0;
} else {
errx(1, "not running");
}
} }
if (!strcmp(argv[1], "trip")) { if (!strcmp(argv[1], "trip")) {
if (estimator::g_estimator) { int ret = estimator::g_estimator->trip_nan();
int ret = estimator::g_estimator->trip_nan();
exit(ret); return ret;
} else {
errx(1, "not running");
}
} }
if (!strcmp(argv[1], "logging")) { if (!strcmp(argv[1], "logging")) {
if (estimator::g_estimator) { int ret = estimator::g_estimator->enable_logging(true);
int ret = estimator::g_estimator->enable_logging(true);
exit(ret); return ret;
} else {
errx(1, "not running");
}
} }
if (!strcmp(argv[1], "debug")) { if (!strcmp(argv[1], "debug")) {
if (estimator::g_estimator) { int debug = strtoul(argv[2], NULL, 10);
int debug = strtoul(argv[2], NULL, 10); int ret = estimator::g_estimator->set_debuglevel(debug);
int ret = estimator::g_estimator->set_debuglevel(debug);
exit(ret); return ret;
} else {
errx(1, "not running");
}
} }
warnx("unrecognized command"); warnx("unrecognized command");
src/modules/ekf_att_pos_estimator/estimator_22states.cpp
+12 -12
View File
@@ -2392,9 +2392,9 @@ int AttPosEKF::RecallStates(float* statesForFusion, uint64_t msec)
if (bestTimeDelta < 200) // only output stored state if < 200 msec retrieval error if (bestTimeDelta < 200) // only output stored state if < 200 msec retrieval error
{ {
for (size_t i=0; i < EKF_STATE_ESTIMATES; i++) { for (size_t i=0; i < EKF_STATE_ESTIMATES; i++) {
if (isfinite(storedStates[i][bestStoreIndex])) { if (std::isfinite(storedStates[i][bestStoreIndex])) {
statesForFusion[i] = storedStates[i][bestStoreIndex]; statesForFusion[i] = storedStates[i][bestStoreIndex];
} else if (isfinite(states[i])) { } else if (std::isfinite(states[i])) {
statesForFusion[i] = states[i]; statesForFusion[i] = states[i];
} else { } else {
// There is not much we can do here, except reporting the error we just // There is not much we can do here, except reporting the error we just
@@ -2406,7 +2406,7 @@ int AttPosEKF::RecallStates(float* statesForFusion, uint64_t msec)
else // otherwise output current state else // otherwise output current state
{ {
for (size_t i = 0; i < EKF_STATE_ESTIMATES; i++) { for (size_t i = 0; i < EKF_STATE_ESTIMATES; i++) {
if (isfinite(states[i])) { if (std::isfinite(states[i])) {
statesForFusion[i] = states[i]; statesForFusion[i] = states[i];
} else { } else {
ret++; ret++;
@@ -2630,7 +2630,7 @@ float AttPosEKF::ConstrainFloat(float val, float min_val, float max_val)
ret = val; ret = val;
} }
if (!isfinite(val)) { if (!std::isfinite(val)) {
ekf_debug("constrain: non-finite!"); ekf_debug("constrain: non-finite!");
} }
@@ -2922,21 +2922,21 @@ bool AttPosEKF::StatesNaN() {
bool err = false; bool err = false;
// check all integrators // check all integrators
if (!isfinite(summedDelAng.x) || !isfinite(summedDelAng.y) || !isfinite(summedDelAng.z)) { if (!std::isfinite(summedDelAng.x) || !std::isfinite(summedDelAng.y) || !std::isfinite(summedDelAng.z)) {
current_ekf_state.angNaN = true; current_ekf_state.angNaN = true;
ekf_debug("summedDelAng NaN: x: %f y: %f z: %f", (double)summedDelAng.x, (double)summedDelAng.y, (double)summedDelAng.z); ekf_debug("summedDelAng NaN: x: %f y: %f z: %f", (double)summedDelAng.x, (double)summedDelAng.y, (double)summedDelAng.z);
err = true; err = true;
goto out; goto out;
} // delta angles } // delta angles
if (!isfinite(correctedDelAng.x) || !isfinite(correctedDelAng.y) || !isfinite(correctedDelAng.z)) { if (!std::isfinite(correctedDelAng.x) || !std::isfinite(correctedDelAng.y) || !std::isfinite(correctedDelAng.z)) {
current_ekf_state.angNaN = true; current_ekf_state.angNaN = true;
ekf_debug("correctedDelAng NaN: x: %f y: %f z: %f", (double)correctedDelAng.x, (double)correctedDelAng.y, (double)correctedDelAng.z); ekf_debug("correctedDelAng NaN: x: %f y: %f z: %f", (double)correctedDelAng.x, (double)correctedDelAng.y, (double)correctedDelAng.z);
err = true; err = true;
goto out; goto out;
} // delta angles } // delta angles
if (!isfinite(summedDelVel.x) || !isfinite(summedDelVel.y) || !isfinite(summedDelVel.z)) { if (!std::isfinite(summedDelVel.x) || !std::isfinite(summedDelVel.y) || !std::isfinite(summedDelVel.z)) {
current_ekf_state.summedDelVelNaN = true; current_ekf_state.summedDelVelNaN = true;
ekf_debug("summedDelVel NaN: x: %f y: %f z: %f", (double)summedDelVel.x, (double)summedDelVel.y, (double)summedDelVel.z); ekf_debug("summedDelVel NaN: x: %f y: %f z: %f", (double)summedDelVel.x, (double)summedDelVel.y, (double)summedDelVel.z);
err = true; err = true;
@@ -2946,7 +2946,7 @@ bool AttPosEKF::StatesNaN() {
// check all states and covariance matrices // check all states and covariance matrices
for (size_t i = 0; i < EKF_STATE_ESTIMATES; i++) { for (size_t i = 0; i < EKF_STATE_ESTIMATES; i++) {
for (size_t j = 0; j < EKF_STATE_ESTIMATES; j++) { for (size_t j = 0; j < EKF_STATE_ESTIMATES; j++) {
if (!isfinite(KH[i][j])) { if (!std::isfinite(KH[i][j])) {
current_ekf_state.KHNaN = true; current_ekf_state.KHNaN = true;
err = true; err = true;
@@ -2954,7 +2954,7 @@ bool AttPosEKF::StatesNaN() {
goto out; goto out;
} // intermediate result used for covariance updates } // intermediate result used for covariance updates
if (!isfinite(KHP[i][j])) { if (!std::isfinite(KHP[i][j])) {
current_ekf_state.KHPNaN = true; current_ekf_state.KHPNaN = true;
err = true; err = true;
@@ -2962,7 +2962,7 @@ bool AttPosEKF::StatesNaN() {
goto out; goto out;
} // intermediate result used for covariance updates } // intermediate result used for covariance updates
if (!isfinite(P[i][j])) { if (!std::isfinite(P[i][j])) {
current_ekf_state.covarianceNaN = true; current_ekf_state.covarianceNaN = true;
err = true; err = true;
@@ -2970,7 +2970,7 @@ bool AttPosEKF::StatesNaN() {
} // covariance matrix } // covariance matrix
} }
if (!isfinite(Kfusion[i])) { if (!std::isfinite(Kfusion[i])) {
current_ekf_state.kalmanGainsNaN = true; current_ekf_state.kalmanGainsNaN = true;
ekf_debug("Kfusion NaN"); ekf_debug("Kfusion NaN");
@@ -2978,7 +2978,7 @@ bool AttPosEKF::StatesNaN() {
goto out; goto out;
} // Kalman gains } // Kalman gains
if (!isfinite(states[i])) { if (!std::isfinite(states[i])) {
current_ekf_state.statesNaN = true; current_ekf_state.statesNaN = true;
ekf_debug("states NaN: i: %u val: %f", i, (double)states[i]); ekf_debug("states NaN: i: %u val: %f", i, (double)states[i]);