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

AttPosEKF: Use multiplatform land detector (was custom FixedWing only)

This commit is contained in:
Johan Jansen 2015-02-13 10:50:55 +01:00
parent 6af2a38f36
commit 331352c75d
4 changed files with 46 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
View File

@ -49,6 +49,7 @@
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/parameter_update.h>
@ -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];

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

@ -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) {

34
src/modules/ekf_att_pos_estimator/estimator_22states.cpp
View File

@ -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;

12
src/modules/ekf_att_pos_estimator/estimator_22states.h
View File

@ -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();