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

ekf: Only return from start handler with all allocations done

This commit is contained in:
Lorenz Meier 2014-07-14 08:11:33 +02:00
parent e28ca7db1b
commit 5f176d14fe
1 changed files with 23 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -124,6 +124,13 @@ public:
*/ */
int start(); int start();
/**
* Task status
*
* @return true if the mainloop is running
*/
bool task_running() { return _task_running; }
/** /**
* Print the current status. * Print the current status.
*/ */
@ -151,7 +158,8 @@ public:
private: private:
bool _task_should_exit; /**< if true, sensor task should exit */ bool _task_should_exit; /**< if true, sensor task should exit */
int _estimator_task; /**< task handle for sensor task */ bool _task_running; /**< if true, task is running in its mainloop */
int _estimator_task; /**< task handle for sensor task */
#ifndef SENSOR_COMBINED_SUB #ifndef SENSOR_COMBINED_SUB
int _gyro_sub; /**< gyro sensor subscription */ int _gyro_sub; /**< gyro sensor subscription */
int _accel_sub; /**< accel sensor subscription */ int _accel_sub; /**< accel sensor subscription */
@ -313,12 +321,13 @@ namespace estimator
#endif #endif
static const int ERROR = -1; static const int ERROR = -1;
FixedwingEstimator *g_estimator; FixedwingEstimator *g_estimator = nullptr;
} }
FixedwingEstimator::FixedwingEstimator() : FixedwingEstimator::FixedwingEstimator() :
_task_should_exit(false), _task_should_exit(false),
_task_running(false),
_estimator_task(-1), _estimator_task(-1),
/* subscriptions */ /* subscriptions */
@ -772,6 +781,8 @@ FixedwingEstimator::task_main()
_gps.vel_e_m_s = 0.0f; _gps.vel_e_m_s = 0.0f;
_gps.vel_d_m_s = 0.0f; _gps.vel_d_m_s = 0.0f;
_task_running = true;
while (!_task_should_exit) { while (!_task_should_exit) {
/* wait for up to 500ms for data */ /* wait for up to 500ms for data */
@ -1518,6 +1529,8 @@ FixedwingEstimator::task_main()
perf_end(_loop_perf); perf_end(_loop_perf);
} }
_task_running = false;
warnx("exiting.\n"); warnx("exiting.\n");
_estimator_task = -1; _estimator_task = -1;
@ -1670,6 +1683,14 @@ int ekf_att_pos_estimator_main(int argc, char *argv[])
err(1, "start failed"); err(1, "start failed");
} }
/* avoid memory fragmentation by not exiting start handler until the task has fully started */
while (estimator::g_estimator == nullptr || !estimator::g_estimator->task_running()) {
usleep(50000);
printf(".");
fflush(stdout);
}
printf("\n");
exit(0); exit(0);
} }