diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-02-01 12:09:54 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-02-01 12:09:54 +0100 |
commit | 70ffa27acd6b5cd276e45d8c029ea743c32b2bc7 (patch) | |
tree | cd03fb26ee2c8a1abd9a62bfbb94044a8a9d176b /src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp | |
parent | dba229ffa5fbdd6e7863f528412be429a84837ac (diff) | |
download | px4-firmware-70ffa27acd6b5cd276e45d8c029ea743c32b2bc7.tar.gz px4-firmware-70ffa27acd6b5cd276e45d8c029ea743c32b2bc7.tar.bz2 px4-firmware-70ffa27acd6b5cd276e45d8c029ea743c32b2bc7.zip |
Rewrote the filter mainloop to match the order in the offboard simulator, added a number of scaling fixes, initializing all structs correctly
Diffstat (limited to 'src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp')
-rw-r--r-- | src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp | 229 |
1 files changed, 123 insertions, 106 deletions
diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 444597e81..8efd87557 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -92,7 +92,7 @@ extern "C" __EXPORT int fw_att_pos_estimator_main(int argc, char *argv[]); __EXPORT uint32_t millis(); static uint64_t last_run = 0; -static uint32_t IMUmsec = 0; +static uint64_t IMUmsec = 0; uint32_t millis() { @@ -383,11 +383,11 @@ FixedwingEstimator::task_main() /* rate limit gyro updates to 50 Hz */ /* XXX remove this!, BUT increase the data buffer size! */ - orb_set_interval(_gyro_sub, 17); + orb_set_interval(_gyro_sub, 6); #else _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); /* XXX remove this!, BUT increase the data buffer size! */ - orb_set_interval(_sensor_combined_sub, 17); + orb_set_interval(_sensor_combined_sub, 6); #endif parameters_update(); @@ -459,6 +459,10 @@ FixedwingEstimator::task_main() perf_count(_perf_gyro); + /** + * PART ONE: COLLECT ALL DATA + **/ + hrt_abstime last_sensor_timestamp; /* load local copies */ @@ -498,7 +502,10 @@ FixedwingEstimator::task_main() dAngIMU = 0.5f * (angRate + lastAngRate) * dtIMU; lastAngRate = angRate; - dVelIMU = 0.5f * (accel + lastAccel) * dtIMU; + // dVelIMU = 0.5f * (accel + lastAccel) * dtIMU; + dVelIMU.x = 0.0f; + dVelIMU.y = 0.0f; + dVelIMU.z = 0.0f; lastAccel = accel; @@ -522,7 +529,7 @@ FixedwingEstimator::task_main() last_run = _sensor_combined.timestamp; /* guard against too large deltaT's */ - if (deltaT > 1.0f) + if (deltaT > 1.0f || deltaT < 0.000001f) deltaT = 0.01f; // Always store data, independent of init status @@ -549,41 +556,13 @@ FixedwingEstimator::task_main() #endif - if (_initialized) { - - /* predict states and covariances */ - - /* run the strapdown INS every sensor update */ - UpdateStrapdownEquationsNED(); - - /* store the predictions */ - StoreStates(); - - /* evaluate if on ground */ - OnGroundCheck(); - - /* prepare the delta angles and time used by the covariance prediction */ - summedDelAng = summedDelAng + correctedDelAng; - summedDelVel = summedDelVel + correctedDelVel; - dt += dtIMU; - - /* predict the covairance if the total delta angle has exceeded the threshold - * or the time limit will be exceeded on the next measurement update - */ - if ((dt >= (covTimeStepMax - dtIMU)) || (summedDelAng.length() > covDelAngMax)) { - CovariancePrediction(); - summedDelAng = summedDelAng.zero(); - summedDelVel = summedDelVel.zero(); - dt = 0.0f; - } - } - - bool baro_updated; - orb_check(_baro_sub, &baro_updated); - if (baro_updated) { - orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); + bool airspeed_updated; + orb_check(_airspeed_sub, &airspeed_updated); + if (airspeed_updated) { + orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); + perf_count(_perf_airspeed); - baroHgt = _baro.altitude; + VtasMeas = _airspeed.true_airspeed_m_s; } bool gps_updated; @@ -592,65 +571,36 @@ FixedwingEstimator::task_main() orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps); perf_count(_perf_gps); - if (_gps.fix_type > 2) { + if (_gps.fix_type < 3) { + gps_updated = false; + } else { /* fuse GPS updates */ //_gps.timestamp / 1e3; GPSstatus = _gps.fix_type; - gpsCourse = _gps.cog_rad; - gpsGndSpd = sqrtf(_gps.vel_n_m_s * _gps.vel_n_m_s + _gps.vel_e_m_s * _gps.vel_e_m_s); - gpsVelD = _gps.vel_d_m_s; + velNED[0] = _gps.vel_n_m_s; + velNED[1] = _gps.vel_e_m_s; + velNED[2] = _gps.vel_d_m_s; + + // warnx("GPS updated: status: %d, vel: %8.4f %8.4f %8.4f", (int)GPSstatus, velNED[0], velNED[1], velNED[2]); + gpsLat = math::radians(_gps.lat / (double)1e7); gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI; gpsHgt = _gps.alt / 1e3f; - if (hrt_elapsed_time(&start_time) > 500000 && !_initialized) { - InitialiseFilter(); - _initialized = true; - - warnx("init done."); - continue; - } - - if (_initialized) { - - /* convert GPS measurements to horizontal NE, altitude and 3D velocity NED */ - velNED[0] = _gps.vel_n_m_s; - velNED[1] = _gps.vel_e_m_s; - velNED[2] = _gps.vel_d_m_s; - calcposNED(posNED, gpsLat, gpsLon, gpsHgt, latRef, lonRef, hgtRef); - - posNE[0] = posNED[0]; - posNE[1] = posNED[1]; - hgtMea = -posNED[2]; - - // set flags for further processing - fuseVelData = true; - fusePosData = true; - fuseHgtData = true; - - /* recall states after adjusting for delays */ - RecallStates(statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms)); - RecallStates(statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms)); - RecallStates(statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms)); + } - /* run the actual fusion */ - FuseVelposNED(); - } + } - } else { + bool baro_updated; + orb_check(_baro_sub, &baro_updated); + if (baro_updated) { + orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); - /* do not fuse anything, we got no position / vel update */ - fuseVelData = false; - fusePosData = false; - fuseHgtData = false; - } + baroHgt = _baro.altitude; - } else { - /* do not fuse anything, we got no position / vel update */ - fuseVelData = false; - fusePosData = false; - fuseHgtData = false; + // Could use a blend of GPS and baro alt data if desired + hgtMea = 1.0f*baroHgt + 0.0f*gpsHgt; } #ifndef SENSOR_COMBINED_SUB @@ -689,41 +639,107 @@ FixedwingEstimator::task_main() #endif - if (_initialized) { + } + + + /** + * PART TWO: EXECUTE THE FILTER + **/ + + if (hrt_elapsed_time(&start_time) > 500000 && !_initialized && (GPSstatus == 3)) { + InitialiseFilter(velNED); + _initialized = true; + + warnx("init done."); + } + + if (_initialized) { + + /* predict states and covariances */ + + /* run the strapdown INS every sensor update */ + UpdateStrapdownEquationsNED(); + + /* store the predictions */ + StoreStates(IMUmsec); + + /* evaluate if on ground */ + OnGroundCheck(); - fuseMagData = true; - RecallStates(statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); - FuseMagnetometer(); + /* prepare the delta angles and time used by the covariance prediction */ + summedDelAng = summedDelAng + correctedDelAng; + summedDelVel = summedDelVel + correctedDelVel; + dt += dtIMU; + + /* predict the covairance if the total delta angle has exceeded the threshold + * or the time limit will be exceeded on the next measurement update + */ + if ((dt >= (covTimeStepMax - dtIMU)) || (summedDelAng.length() > covDelAngMax)) { + CovariancePrediction(); + summedDelAng = summedDelAng.zero(); + summedDelVel = summedDelVel.zero(); + dt = 0.0f; } + } + + + if (gps_updated && _initialized) { + + /* convert GPS measurements to horizontal NE, altitude and 3D velocity NED */ + calcposNED(posNED, gpsLat, gpsLon, gpsHgt, latRef, lonRef, hgtRef); + posNE[0] = posNED[0]; + posNE[1] = posNED[1]; + + // set flags for further processing + fuseVelData = true; + fusePosData = true; + + /* recall states after adjusting for delays */ + RecallStates(statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms)); + RecallStates(statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms)); + + /* run the actual fusion */ + FuseVelposNED(); } else { - fuseMagData = false; + fuseVelData = false; + fusePosData = false; } - bool airspeed_updated; - orb_check(_airspeed_sub, &airspeed_updated); - if (airspeed_updated && _initialized) { - orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); - perf_count(_perf_airspeed); + if (baro_updated && _initialized) { + + fuseHgtData = true; + // recall states stored at time of measurement after adjusting for delays + RecallStates(statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms)); + // run the fusion step + FuseVelposNED(); + } else { + fuseHgtData = false; + } - if (_airspeed.true_airspeed_m_s > 8.0f /* XXX magic number */) { + if (mag_updated && _initialized) { + fuseMagData = true; + RecallStates(statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); - VtasMeas = _airspeed.true_airspeed_m_s; + } else { + fuseMagData = false; + } - if (_initialized) { + if (_initialized) { + FuseMagnetometer(); + } - fuseVtasData = true; - RecallStates(statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data - FuseAirspeed(); - } - } else { - fuseVtasData = false; - } + if (airspeed_updated && _initialized + && _airspeed.true_airspeed_m_s > 8.0f /* XXX magic number */) { + fuseVtasData = true; + RecallStates(statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data + FuseAirspeed(); } else { fuseVtasData = false; } + // Publish results if (_initialized) { // State vector: @@ -894,6 +910,7 @@ int fw_att_pos_estimator_main(int argc, char *argv[]) // 15-17: Earth Magnetic Field Vector - milligauss (North, East, Down) // 18-20: Body Magnetic Field Vector - milligauss (X,Y,Z) + printf("dt: %8.6f\n", dtIMU); printf("states (quat) [1-4]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)states[0], (double)states[1], (double)states[2], (double)states[3]); printf("states (vel m/s) [5-7]: %8.4f, %8.4f, %8.4f\n", (double)states[4], (double)states[5], (double)states[6]); printf("states (pos m) [8-10]: %8.4f, %8.4f, %8.4f\n", (double)states[7], (double)states[8], (double)states[9]); |