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

Ensure states are actually copied in non-error mode

Browse Source
This commit is contained in:
Lorenz Meier
2014-06-04 10:31:23 +02:00
parent 19154f29d8
commit 5aef22310e
2 changed files with 21 additions and 18 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
+15 -18
View File
@@ -1042,33 +1042,30 @@ FixedwingEstimator::task_main()
if (_ekf_logging || check) {
rep.timestamp = hrt_absolute_time();
rep.nan_flags |= (((uint8_t)ekf_report.angNaN) << 0);
rep.nan_flags |= (((uint8_t)ekf_report.summedDelVelNaN) << 1);
rep.nan_flags |= (((uint8_t)ekf_report.KHNaN) << 2);
rep.nan_flags |= (((uint8_t)ekf_report.KHPNaN) << 3);
rep.nan_flags |= (((uint8_t)ekf_report.PNaN) << 4);
rep.nan_flags |= (((uint8_t)ekf_report.covarianceNaN) << 5);
rep.nan_flags |= (((uint8_t)ekf_report.kalmanGainsNaN) << 6);
rep.nan_flags |= (((uint8_t)ekf_report.statesNaN) << 7);
rep.nan_flags |= (((uint8_t)ekf_report.angNaN) << 0);
rep.nan_flags |= (((uint8_t)ekf_report.summedDelVelNaN) << 1);
rep.nan_flags |= (((uint8_t)ekf_report.KHNaN) << 2);
rep.nan_flags |= (((uint8_t)ekf_report.KHPNaN) << 3);
rep.nan_flags |= (((uint8_t)ekf_report.PNaN) << 4);
rep.nan_flags |= (((uint8_t)ekf_report.covarianceNaN) << 5);
rep.nan_flags |= (((uint8_t)ekf_report.kalmanGainsNaN) << 6);
rep.nan_flags |= (((uint8_t)ekf_report.statesNaN) << 7);
rep.health_flags |= (((uint8_t)ekf_report.velHealth) << 0);
rep.health_flags |= (((uint8_t)ekf_report.posHealth) << 1);
rep.health_flags |= (((uint8_t)ekf_report.hgtHealth) << 2);
rep.health_flags |= (((uint8_t)ekf_report.velHealth) << 0);
rep.health_flags |= (((uint8_t)ekf_report.posHealth) << 1);
rep.health_flags |= (((uint8_t)ekf_report.hgtHealth) << 2);
rep.timeout_flags |= (((uint8_t)ekf_report.velTimeout) << 0);
rep.timeout_flags |= (((uint8_t)ekf_report.posTimeout) << 1);
rep.timeout_flags |= (((uint8_t)ekf_report.hgtTimeout) << 2);
rep.timeout_flags |= (((uint8_t)ekf_report.velTimeout) << 0);
rep.timeout_flags |= (((uint8_t)ekf_report.posTimeout) << 1);
rep.timeout_flags |= (((uint8_t)ekf_report.hgtTimeout) << 2);
// Copy all states or at least all that we can fit
unsigned i = 0;
unsigned ekf_n_states = (sizeof(ekf_report.states) / sizeof(ekf_report.states[0]));
unsigned max_states = (sizeof(rep.states) / sizeof(rep.states[0]));
rep.n_states = (ekf_n_states < max_states) ? ekf_n_states : max_states;
while ((i < ekf_n_states) && (i < max_states)) {
for (unsigned i = 0; i < rep.n_states; i++) {
rep.states[i] = ekf_report.states[i];
i++;
}
if (_estimator_status_pub > 0) {
src/modules/ekf_att_pos_estimator/estimator.cpp
+6
View File
@@ -2610,6 +2610,12 @@ void AttPosEKF::ZeroVariables()
void AttPosEKF::GetFilterState(struct ekf_status_report *state)
{
// Copy states
for (unsigned i = 0; i < n_states; i++) {
current_ekf_state.states[i] = states[i];
}
memcpy(state, &current_ekf_state, sizeof(*state));
}