From 331352c75d337e3190cedb5d53159cd63504223a Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Fri, 13 Feb 2015 10:50:55 +0100 Subject: AttPosEKF: Use multiplatform land detector (was custom FixedWing only) --- .../AttitudePositionEstimatorEKF.h | 3 ++ .../ekf_att_pos_estimator_main.cpp | 18 +++++++++--- .../ekf_att_pos_estimator/estimator_22states.cpp | 34 ++++++++++++---------- .../ekf_att_pos_estimator/estimator_22states.h | 12 ++++++-- 4 files changed, 46 insertions(+), 21 deletions(-) (limited to 'src') diff --git a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h index 07db924b2..228ffa853 100644 --- a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h +++ b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h @@ -49,6 +49,7 @@ #include #include #include +#include #include #include #include @@ -141,6 +142,7 @@ private: int _manual_control_sub; /**< notification of manual control updates */ int _mission_sub; int _home_sub; /**< home position as defined by commander / user */ + int _landDetectorSub; orb_advert_t _att_pub; /**< vehicle attitude */ orb_advert_t _global_pos_pub; /**< global position */ @@ -160,6 +162,7 @@ private: struct vehicle_gps_position_s _gps; /**< GPS position */ struct wind_estimate_s _wind; /**< wind estimate */ struct range_finder_report _distance; /**< distance estimate */ + struct vehicle_land_detected_s _landDetector; struct gyro_scale _gyro_offsets[3]; struct accel_scale _accel_offsets[3]; diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 511492b5a..5e5208e78 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -121,6 +121,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : _manual_control_sub(-1), _mission_sub(-1), _home_sub(-1), + _landDetectorSub(-1), /* publications */ _att_pub(-1), @@ -141,6 +142,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : _gps({}), _wind({}), _distance{}, + _landDetector{}, _gyro_offsets({}), _accel_offsets({}), @@ -496,6 +498,7 @@ void AttitudePositionEstimatorEKF::task_main() _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _home_sub = orb_subscribe(ORB_ID(home_position)); + _landDetectorSub = orb_subscribe(ORB_ID(vehicle_land_detected)); /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_vstatus_sub, 200); @@ -649,6 +652,9 @@ void AttitudePositionEstimatorEKF::task_main() } else if (_ekf->statesInitialised) { + // Check if on ground - status is used by covariance prediction + _ekf->setOnGround(_landDetector.landed); + // We're apparently initialized in this case now // check (and reset the filter as needed) int check = check_filter_state(); @@ -904,9 +910,6 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const // store the predicted states for subsequent use by measurement fusion _ekf->StoreStates(IMUmsec); - // Check if on ground - status is used by covariance prediction - _ekf->OnGroundCheck(); - // sum delta angles and time used by covariance prediction _ekf->summedDelAng = _ekf->summedDelAng + _ekf->correctedDelAng; _ekf->summedDelVel = _ekf->summedDelVel + _ekf->dVelIMU; @@ -1086,7 +1089,7 @@ void AttitudePositionEstimatorEKF::print_status() } printf("states: %s %s %s %s %s %s %s %s %s %s\n", (_ekf->statesInitialised) ? "INITIALIZED" : "NON_INIT", - (_ekf->onGround) ? "ON_GROUND" : "AIRBORNE", + (_landDetector.landed) ? "ON_GROUND" : "AIRBORNE", (_ekf->fuseVelData) ? "FUSE_VEL" : "INH_VEL", (_ekf->fusePosData) ? "FUSE_POS" : "INH_POS", (_ekf->fuseHgtData) ? "FUSE_HGT" : "INH_HGT", @@ -1218,6 +1221,13 @@ void AttitudePositionEstimatorEKF::pollData() //warnx("dang: %8.4f %8.4f dvel: %8.4f %8.4f", _ekf->dAngIMU.x, _ekf->dAngIMU.z, _ekf->dVelIMU.x, _ekf->dVelIMU.z); + //Update Land Detector + bool newLandData; + orb_check(_landDetectorSub, &newLandData); + if(newLandData) { + orb_copy(ORB_ID(vehicle_land_detected), _landDetectorSub, &_landDetector); + } + //Update AirSpeed orb_check(_airspeed_sub, &_newAdsData); if (_newAdsData) { diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp index f8c6510a2..700979eda 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp @@ -153,7 +153,6 @@ AttPosEKF::AttPosEKF() : inhibitGndState(true), inhibitScaleState(true), - onGround(true), staticMode(true), useGPS(false), useAirspeed(true), @@ -183,7 +182,9 @@ AttPosEKF::AttPosEKF() : auxFlowInnovGate(0.0f), rngInnovGate(0.0f), minFlowRng(0.0f), - moCompR_LOS(0.0f) + moCompR_LOS(0.0f), + + _onGround(true) { memset(&last_ekf_error, 0, sizeof(last_ekf_error)); @@ -442,7 +443,7 @@ void AttPosEKF::CovariancePrediction(float dt) daxCov = sq(dt*gyroProcessNoise); dayCov = sq(dt*gyroProcessNoise); dazCov = sq(dt*gyroProcessNoise); - if (onGround) dazCov = dazCov * sq(yawVarScale); + if (_onGround) dazCov = dazCov * sq(yawVarScale); accelProcessNoise = ConstrainFloat(accelProcessNoise, 5e-2, 1.0f); dvxCov = sq(dt*accelProcessNoise); dvyCov = sq(dt*accelProcessNoise); @@ -1132,6 +1133,7 @@ void AttPosEKF::FuseVelposNED() current_ekf_state.velHealth = false; } } + if (fusePosData) { // test horizontal position measurements @@ -1163,6 +1165,7 @@ void AttPosEKF::FuseVelposNED() current_ekf_state.posHealth = false; } } + // test height measurements if (fuseHgtData) { @@ -1608,7 +1611,7 @@ void AttPosEKF::FuseMagnetometer() KH[i][j] = Kfusion[i] * H_MAG[j]; } for (uint8_t j = 4; j <= 15; j++) KH[i][j] = 0.0f; - if (!onGround) + if (!_onGround) { for (uint8_t j = 16; j < EKF_STATE_ESTIMATES; j++) { @@ -1632,7 +1635,7 @@ void AttPosEKF::FuseMagnetometer() { KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; } - if (!onGround) + if (!_onGround) { for (uint8_t k = 16; k < EKF_STATE_ESTIMATES; k++) { @@ -2527,37 +2530,41 @@ void AttPosEKF::calcLLH(float posNED[3], double &lat, double &lon, float &hgt, d hgt = hgtRef - posNED[2]; } -void AttPosEKF::OnGroundCheck() +void AttPosEKF::setOnGround(const bool isLanded) { - onGround = (((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f) && (VtasMeas < 8.0f)); + _onGround = isLanded; if (staticMode) { staticMode = (!refSet || (GPSstatus < GPS_FIX_3D)); } // don't update wind states if there is no airspeed measurement - if (onGround || !useAirspeed) { + if (_onGround || !useAirspeed) { inhibitWindStates = true; } else { inhibitWindStates =false; } + // don't update magnetic field states if on ground or not using compass - if (onGround || !useCompass) { + if (_onGround || !useCompass) { inhibitMagStates = true; } else { inhibitMagStates = false; } + // don't update terrain offset state if there is no range finder and flying at low velocity or without GPS - if ((onGround || !useGPS) && !useRangeFinder) { + if ((_onGround || !useGPS) && !useRangeFinder) { inhibitGndState = true; } else { inhibitGndState = false; } + // don't update terrain offset state if there is no range finder and flying at low velocity, or without GPS, as it is poorly observable - if ((onGround || (globalTimeStamp_ms - lastFixTime_ms) > 1000) && !useRangeFinder) { + if ((_onGround || (globalTimeStamp_ms - lastFixTime_ms) > 1000) && !useRangeFinder) { inhibitGndState = true; } else { inhibitGndState = false; } + // Don't update focal length offset state if there is no range finder or optical flow sensor // we need both sensors to do this estimation if (!useRangeFinder || !useOpticalFlow) { @@ -2967,9 +2974,6 @@ int AttPosEKF::CheckAndBound(struct ekf_status_report *last_error) int ret = 0; - // Check if we're on ground - this also sets static mode. - OnGroundCheck(); - // Reset the filter if the states went NaN if (StatesNaN()) { ekf_debug("re-initializing dynamic"); @@ -3279,7 +3283,7 @@ void AttPosEKF::GetFilterState(struct ekf_status_report *err) current_ekf_state.states[i] = states[i]; } current_ekf_state.n_states = EKF_STATE_ESTIMATES; - current_ekf_state.onGround = onGround; + current_ekf_state.onGround = _onGround; current_ekf_state.staticMode = staticMode; current_ekf_state.useCompass = useCompass; current_ekf_state.useAirspeed = useAirspeed; diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.h b/src/modules/ekf_att_pos_estimator/estimator_22states.h index 19ef52145..8e820bfd9 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.h @@ -212,7 +212,6 @@ public: bool inhibitGndState; // true when the terrain ground height offset state and covariances are to remain constant bool inhibitScaleState; // true when the focal length scale factor state and covariances are to remain constant - bool onGround; ///< boolean true when the flight vehicle is on the ground (not flying) bool staticMode; ///< boolean true if no position feedback is fused bool useGPS; // boolean true if GPS data is being used bool useAirspeed; ///< boolean true if airspeed data is being used @@ -319,7 +318,11 @@ public: static inline float sq(float valIn) {return valIn * valIn;} - void OnGroundCheck(); + /** + * @brief + * Tell the EKF if the vehicle has landed + **/ + void setOnGround(const bool isLanded); void CovarianceInit(); @@ -396,6 +399,11 @@ protected: void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat); + void ResetStoredStates(); + +private: + bool _onGround; ///< boolean true when the flight vehicle is on the ground (not flying) + }; uint32_t millis(); -- cgit v1.2.3