dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

att + pos EKF: Enable execution on Linux

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -44,6 +44,7 @@
#include "estimator_22states.h"
#include <px4_config.h>
#include <px4_tasks.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
@ -281,7 +282,7 @@ AttitudePositionEstimatorEKF::~AttitudePositionEstimatorEKF()
/* if we have given up, kill it */
if (++i > 50) {
task_delete(_estimator_task);
px4_task_delete(_estimator_task);
break;
}
} while (_estimator_task != -1);
@ -490,7 +491,8 @@ void AttitudePositionEstimatorEKF::task_main()
_filter_start_time = hrt_absolute_time();
if (!_ekf) {
errx(1, "OUT OF MEM!");
warnx("OUT OF MEM!");
return;
}
/*
@ -696,10 +698,8 @@ void AttitudePositionEstimatorEKF::task_main()
_task_running = false;
warnx("exiting.\n");
_estimator_task = -1;
_exit(0);
return;
}
void AttitudePositionEstimatorEKF::initializeGPS()
@ -1039,7 +1039,7 @@ int AttitudePositionEstimatorEKF::start()
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 40,
7500,
(main_t)&AttitudePositionEstimatorEKF::task_main_trampoline,
(px4_main_t)&AttitudePositionEstimatorEKF::task_main_trampoline,
nullptr);
if (_estimator_task < 0) {
@ -1155,7 +1155,7 @@ void AttitudePositionEstimatorEKF::pollData()
float deltaT = (_sensor_combined.timestamp - _last_run) / 1e6f;
/* 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;
}
@ -1167,9 +1167,9 @@ void AttitudePositionEstimatorEKF::pollData()
int last_gyro_main = _gyro_main;
if (isfinite(_sensor_combined.gyro_rad_s[0]) &&
isfinite(_sensor_combined.gyro_rad_s[1]) &&
isfinite(_sensor_combined.gyro_rad_s[2]) &&
if (std::isfinite(_sensor_combined.gyro_rad_s[0]) &&
std::isfinite(_sensor_combined.gyro_rad_s[1]) &&
std::isfinite(_sensor_combined.gyro_rad_s[2]) &&
(_sensor_combined.gyro_errcount <= _sensor_combined.gyro1_errcount)) {
_ekf->angRate.x = _sensor_combined.gyro_rad_s[0];
@ -1178,9 +1178,9 @@ void AttitudePositionEstimatorEKF::pollData()
_gyro_main = 0;
_gyro_valid = true;
} else if (isfinite(_sensor_combined.gyro1_rad_s[0]) &&
isfinite(_sensor_combined.gyro1_rad_s[1]) &&
isfinite(_sensor_combined.gyro1_rad_s[2])) {
} else if (std::isfinite(_sensor_combined.gyro1_rad_s[0]) &&
std::isfinite(_sensor_combined.gyro1_rad_s[1]) &&
std::isfinite(_sensor_combined.gyro1_rad_s[2])) {
_ekf->angRate.x = _sensor_combined.gyro1_rad_s[0];
_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[])
{
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 (estimator::g_estimator != nullptr) {
errx(1, "already running");
warnx("already running");
return 1;
}
estimator::g_estimator = new AttitudePositionEstimatorEKF();
if (estimator::g_estimator == nullptr) {
errx(1, "alloc failed");
warnx("alloc failed");
return 1;
}
if (OK != estimator::g_estimator->start()) {
delete estimator::g_estimator;
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 */
@ -1558,64 +1562,46 @@ int ekf_att_pos_estimator_main(int argc, char *argv[])
printf("\n");
exit(0);
return 0;
}
if (estimator::g_estimator == nullptr) {
warnx("not running");
return 1;
}
if (!strcmp(argv[1], "stop")) {
if (estimator::g_estimator == nullptr) {
errx(1, "not running");
}
delete estimator::g_estimator;
estimator::g_estimator = nullptr;
exit(0);
return 0;
}
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);
} else {
errx(1, "not running");
}
return 0;
}
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);
} else {
errx(1, "not running");
}
return ret;
}
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);
} else {
errx(1, "not running");
}
return ret;
}
if (!strcmp(argv[1], "debug")) {
if (estimator::g_estimator) {
int debug = strtoul(argv[2], NULL, 10);
int ret = estimator::g_estimator->set_debuglevel(debug);
int debug = strtoul(argv[2], NULL, 10);
int ret = estimator::g_estimator->set_debuglevel(debug);
exit(ret);
} else {
errx(1, "not running");
}
return ret;
}
warnx("unrecognized command");

24
src/modules/ekf_att_pos_estimator/estimator_22states.cpp
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
{
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];
} else if (isfinite(states[i])) {
} else if (std::isfinite(states[i])) {
statesForFusion[i] = states[i];
} else {
// 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
{
for (size_t i = 0; i < EKF_STATE_ESTIMATES; i++) {
if (isfinite(states[i])) {
if (std::isfinite(states[i])) {
statesForFusion[i] = states[i];
} else {
ret++;
@ -2630,7 +2630,7 @@ float AttPosEKF::ConstrainFloat(float val, float min_val, float max_val)
ret = val;
}
if (!isfinite(val)) {
if (!std::isfinite(val)) {
ekf_debug("constrain: non-finite!");
}
@ -2922,21 +2922,21 @@ bool AttPosEKF::StatesNaN() {
bool err = false;
// 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;
ekf_debug("summedDelAng NaN: x: %f y: %f z: %f", (double)summedDelAng.x, (double)summedDelAng.y, (double)summedDelAng.z);
err = true;
goto out;
} // 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;
ekf_debug("correctedDelAng NaN: x: %f y: %f z: %f", (double)correctedDelAng.x, (double)correctedDelAng.y, (double)correctedDelAng.z);
err = true;
goto out;
} // 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;
ekf_debug("summedDelVel NaN: x: %f y: %f z: %f", (double)summedDelVel.x, (double)summedDelVel.y, (double)summedDelVel.z);
err = true;
@ -2946,7 +2946,7 @@ bool AttPosEKF::StatesNaN() {
// check all states and covariance matrices
for (size_t i = 0; i < EKF_STATE_ESTIMATES; i++) {
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;
err = true;
@ -2954,7 +2954,7 @@ bool AttPosEKF::StatesNaN() {
goto out;
} // intermediate result used for covariance updates
if (!isfinite(KHP[i][j])) {
if (!std::isfinite(KHP[i][j])) {
current_ekf_state.KHPNaN = true;
err = true;
@ -2962,7 +2962,7 @@ bool AttPosEKF::StatesNaN() {
goto out;
} // intermediate result used for covariance updates
if (!isfinite(P[i][j])) {
if (!std::isfinite(P[i][j])) {
current_ekf_state.covarianceNaN = true;
err = true;
@ -2970,7 +2970,7 @@ bool AttPosEKF::StatesNaN() {
} // covariance matrix
}
if (!isfinite(Kfusion[i])) {
if (!std::isfinite(Kfusion[i])) {
current_ekf_state.kalmanGainsNaN = true;
ekf_debug("Kfusion NaN");
@ -2978,7 +2978,7 @@ bool AttPosEKF::StatesNaN() {
goto out;
} // Kalman gains
if (!isfinite(states[i])) {
if (!std::isfinite(states[i])) {
current_ekf_state.statesNaN = true;
ekf_debug("states NaN: i: %u val: %f", i, (double)states[i]);