forked from Archive/PX4-Autopilot
Reworked the estimator initialization and recovery logic. Should be more resilient to mishaps now
This commit is contained in:
parent
b40fcb0aac
commit
94bed70e32
|
@ -577,6 +577,11 @@ FixedwingEstimator::task_main()
|
|||
bool newAdsData = false;
|
||||
bool newDataMag = false;
|
||||
|
||||
float posNED[3] = {0.0f, 0.0f, 0.0f}; // North, East Down position (m)
|
||||
_gps.vel_n_m_s = 0.0f;
|
||||
_gps.vel_e_m_s = 0.0f;
|
||||
_gps.vel_d_m_s = 0.0f;
|
||||
|
||||
while (!_task_should_exit) {
|
||||
|
||||
/* wait for up to 500ms for data */
|
||||
|
@ -926,8 +931,15 @@ FixedwingEstimator::task_main()
|
|||
newDataMag = false;
|
||||
}
|
||||
|
||||
/*
|
||||
* CHECK IF ITS THE RIGHT TIME TO RUN THINGS ALREADY
|
||||
*/
|
||||
if (hrt_elapsed_time(&_filter_start_time) < FILTER_INIT_DELAY) {
|
||||
continue;
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
/*
|
||||
* CHECK IF THE INPUT DATA IS SANE
|
||||
*/
|
||||
int check = _ekf->CheckAndBound();
|
||||
|
@ -959,6 +971,13 @@ FixedwingEstimator::task_main()
|
|||
mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str);
|
||||
break;
|
||||
}
|
||||
case 4:
|
||||
{
|
||||
const char* str = "excessive gyro offsets";
|
||||
warnx("%s", str);
|
||||
mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str);
|
||||
break;
|
||||
}
|
||||
|
||||
default:
|
||||
{
|
||||
|
@ -974,7 +993,7 @@ FixedwingEstimator::task_main()
|
|||
}
|
||||
|
||||
// If non-zero, we got a filter reset
|
||||
if (check) {
|
||||
if (check > 0 && check != 3) {
|
||||
|
||||
struct ekf_status_report ekf_report;
|
||||
|
||||
|
@ -1013,10 +1032,12 @@ FixedwingEstimator::task_main()
|
|||
|
||||
_baro_init = false;
|
||||
_gps_initialized = false;
|
||||
_initialized = false;
|
||||
last_sensor_timestamp = hrt_absolute_time();
|
||||
last_run = last_sensor_timestamp;
|
||||
|
||||
_ekf->ZeroVariables();
|
||||
_ekf->statesInitialised = false;
|
||||
_ekf->dtIMU = 0.01f;
|
||||
|
||||
// Let the system re-initialize itself
|
||||
|
@ -1027,23 +1048,26 @@ FixedwingEstimator::task_main()
|
|||
|
||||
/**
|
||||
* PART TWO: EXECUTE THE FILTER
|
||||
*
|
||||
* We run the filter only once all data has been fetched
|
||||
**/
|
||||
|
||||
if ((hrt_elapsed_time(&_filter_start_time) > FILTER_INIT_DELAY) && _baro_init && _gyro_valid && _accel_valid && _mag_valid) {
|
||||
if (_baro_init && _gyro_valid && _accel_valid && _mag_valid) {
|
||||
|
||||
float initVelNED[3];
|
||||
|
||||
/* Initialize the filter first */
|
||||
if (!_gps_initialized && _gps.fix_type > 2 && _gps.eph_m < _parameters.pos_stddev_threshold && _gps.epv_m < _parameters.pos_stddev_threshold) {
|
||||
|
||||
initVelNED[0] = _gps.vel_n_m_s;
|
||||
initVelNED[1] = _gps.vel_e_m_s;
|
||||
initVelNED[2] = _gps.vel_d_m_s;
|
||||
|
||||
// GPS is in scaled integers, convert
|
||||
double lat = _gps.lat / 1.0e7;
|
||||
double lon = _gps.lon / 1.0e7;
|
||||
float gps_alt = _gps.alt / 1e3f;
|
||||
|
||||
initVelNED[0] = _gps.vel_n_m_s;
|
||||
initVelNED[1] = _gps.vel_e_m_s;
|
||||
initVelNED[2] = _gps.vel_d_m_s;
|
||||
|
||||
// Set up height correctly
|
||||
orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
|
||||
_baro_gps_offset = _baro_ref - _baro.altitude;
|
||||
|
@ -1070,10 +1094,13 @@ FixedwingEstimator::task_main()
|
|||
|
||||
map_projection_init(&_pos_ref, lat, lon);
|
||||
mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt);
|
||||
|
||||
#if 0
|
||||
warnx("HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)gps_alt,
|
||||
(double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]);
|
||||
warnx("BARO: %8.4f m / ref: %8.4f m / gps offs: %8.4f m", (double)_ekf->baroHgt, (double)_baro_ref, (double)_baro_gps_offset);
|
||||
warnx("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph_m, (double)_gps.epv_m, (double)math::degrees(declination));
|
||||
#endif
|
||||
|
||||
_gps_initialized = true;
|
||||
|
||||
|
@ -1082,282 +1109,268 @@ FixedwingEstimator::task_main()
|
|||
initVelNED[0] = 0.0f;
|
||||
initVelNED[1] = 0.0f;
|
||||
initVelNED[2] = 0.0f;
|
||||
_ekf->posNED[0] = 0.0f;
|
||||
_ekf->posNED[1] = 0.0f;
|
||||
_ekf->posNED[2] = 0.0f;
|
||||
|
||||
_ekf->posNE[0] = _ekf->posNED[0];
|
||||
_ekf->posNE[1] = _ekf->posNED[1];
|
||||
_ekf->posNE[0] = posNED[0];
|
||||
_ekf->posNE[1] = posNED[1];
|
||||
|
||||
_local_pos.ref_alt = _baro_ref;
|
||||
_baro_gps_offset = 0.0f;
|
||||
|
||||
_ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f);
|
||||
}
|
||||
}
|
||||
} else if (_ekf->statesInitialised) {
|
||||
|
||||
// If valid IMU data and states initialised, predict states and covariances
|
||||
if (_ekf->statesInitialised) {
|
||||
// Run the strapdown INS equations every IMU update
|
||||
_ekf->UpdateStrapdownEquationsNED();
|
||||
#if 0
|
||||
// debug code - could be tunred into a filter mnitoring/watchdog function
|
||||
float tempQuat[4];
|
||||
|
||||
for (uint8_t j = 0; j <= 3; j++) tempQuat[j] = states[j];
|
||||
|
||||
quat2eul(eulerEst, tempQuat);
|
||||
|
||||
for (uint8_t j = 0; j <= 2; j++) eulerDif[j] = eulerEst[j] - ahrsEul[j];
|
||||
|
||||
if (eulerDif[2] > pi) eulerDif[2] -= 2 * pi;
|
||||
|
||||
if (eulerDif[2] < -pi) eulerDif[2] += 2 * pi;
|
||||
|
||||
#endif
|
||||
// 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;
|
||||
dt += _ekf->dtIMU;
|
||||
|
||||
// perform a covariance prediction if the total delta angle has exceeded the limit
|
||||
// or the time limit will be exceeded at the next IMU update
|
||||
if ((dt >= (_ekf->covTimeStepMax - _ekf->dtIMU)) || (_ekf->summedDelAng.length() > _ekf->covDelAngMax)) {
|
||||
_ekf->CovariancePrediction(dt);
|
||||
_ekf->summedDelAng.zero();
|
||||
_ekf->summedDelVel.zero();
|
||||
dt = 0.0f;
|
||||
}
|
||||
|
||||
_initialized = true;
|
||||
}
|
||||
|
||||
// Fuse GPS Measurements
|
||||
if (newDataGps && _gps_initialized) {
|
||||
// Convert GPS measurements to Pos NE, hgt and Vel NED
|
||||
_ekf->velNED[0] = _gps.vel_n_m_s;
|
||||
_ekf->velNED[1] = _gps.vel_e_m_s;
|
||||
_ekf->velNED[2] = _gps.vel_d_m_s;
|
||||
_ekf->calcposNED(_ekf->posNED, _ekf->gpsLat, _ekf->gpsLon, _ekf->gpsHgt, _ekf->latRef, _ekf->lonRef, _ekf->hgtRef);
|
||||
|
||||
_ekf->posNE[0] = _ekf->posNED[0];
|
||||
_ekf->posNE[1] = _ekf->posNED[1];
|
||||
// set fusion flags
|
||||
_ekf->fuseVelData = true;
|
||||
_ekf->fusePosData = true;
|
||||
// recall states stored at time of measurement after adjusting for delays
|
||||
_ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms));
|
||||
_ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms));
|
||||
// run the fusion step
|
||||
_ekf->FuseVelposNED();
|
||||
|
||||
} else if (_ekf->statesInitialised) {
|
||||
// Convert GPS measurements to Pos NE, hgt and Vel NED
|
||||
_ekf->velNED[0] = 0.0f;
|
||||
_ekf->velNED[1] = 0.0f;
|
||||
_ekf->velNED[2] = 0.0f;
|
||||
_ekf->posNED[0] = 0.0f;
|
||||
_ekf->posNED[1] = 0.0f;
|
||||
_ekf->posNED[2] = 0.0f;
|
||||
|
||||
_ekf->posNE[0] = _ekf->posNED[0];
|
||||
_ekf->posNE[1] = _ekf->posNED[1];
|
||||
// set fusion flags
|
||||
_ekf->fuseVelData = true;
|
||||
_ekf->fusePosData = true;
|
||||
// recall states stored at time of measurement after adjusting for delays
|
||||
_ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms));
|
||||
_ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms));
|
||||
// run the fusion step
|
||||
_ekf->FuseVelposNED();
|
||||
|
||||
} else {
|
||||
_ekf->fuseVelData = false;
|
||||
_ekf->fusePosData = false;
|
||||
}
|
||||
|
||||
if (newHgtData && _ekf->statesInitialised) {
|
||||
// Could use a blend of GPS and baro alt data if desired
|
||||
_ekf->hgtMea = 1.0f * (_ekf->baroHgt - _baro_ref);
|
||||
_ekf->fuseHgtData = true;
|
||||
// recall states stored at time of measurement after adjusting for delays
|
||||
_ekf->RecallStates(_ekf->statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms));
|
||||
// run the fusion step
|
||||
_ekf->FuseVelposNED();
|
||||
|
||||
} else {
|
||||
_ekf->fuseHgtData = false;
|
||||
}
|
||||
|
||||
// Fuse Magnetometer Measurements
|
||||
if (newDataMag && _ekf->statesInitialised) {
|
||||
_ekf->fuseMagData = true;
|
||||
_ekf->RecallStates(_ekf->statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data
|
||||
|
||||
} else {
|
||||
_ekf->fuseMagData = false;
|
||||
}
|
||||
|
||||
if (_ekf->statesInitialised) _ekf->FuseMagnetometer();
|
||||
|
||||
// Fuse Airspeed Measurements
|
||||
if (newAdsData && _ekf->statesInitialised && _ekf->VtasMeas > 8.0f) {
|
||||
_ekf->fuseVtasData = true;
|
||||
_ekf->RecallStates(_ekf->statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data
|
||||
_ekf->FuseAirspeed();
|
||||
|
||||
} else {
|
||||
_ekf->fuseVtasData = false;
|
||||
}
|
||||
|
||||
// Publish results
|
||||
if (_initialized && (check == OK)) {
|
||||
// We're apparently initialized in this case now
|
||||
|
||||
|
||||
// Run the strapdown INS equations every IMU update
|
||||
_ekf->UpdateStrapdownEquationsNED();
|
||||
#if 0
|
||||
// debug code - could be tunred into a filter mnitoring/watchdog function
|
||||
float tempQuat[4];
|
||||
|
||||
// State vector:
|
||||
// 0-3: quaternions (q0, q1, q2, q3)
|
||||
// 4-6: Velocity - m/sec (North, East, Down)
|
||||
// 7-9: Position - m (North, East, Down)
|
||||
// 10-12: Delta Angle bias - rad (X,Y,Z)
|
||||
// 13-14: Wind Vector - m/sec (North,East)
|
||||
// 15-17: Earth Magnetic Field Vector - milligauss (North, East, Down)
|
||||
// 18-20: Body Magnetic Field Vector - milligauss (X,Y,Z)
|
||||
for (uint8_t j = 0; j <= 3; j++) tempQuat[j] = states[j];
|
||||
|
||||
math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]);
|
||||
math::Matrix<3, 3> R = q.to_dcm();
|
||||
math::Vector<3> euler = R.to_euler();
|
||||
quat2eul(eulerEst, tempQuat);
|
||||
|
||||
for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
|
||||
_att.R[i][j] = R(i, j);
|
||||
for (uint8_t j = 0; j <= 2; j++) eulerDif[j] = eulerEst[j] - ahrsEul[j];
|
||||
|
||||
_att.timestamp = last_sensor_timestamp;
|
||||
_att.q[0] = _ekf->states[0];
|
||||
_att.q[1] = _ekf->states[1];
|
||||
_att.q[2] = _ekf->states[2];
|
||||
_att.q[3] = _ekf->states[3];
|
||||
_att.q_valid = true;
|
||||
_att.R_valid = true;
|
||||
if (eulerDif[2] > pi) eulerDif[2] -= 2 * pi;
|
||||
|
||||
_att.timestamp = last_sensor_timestamp;
|
||||
_att.roll = euler(0);
|
||||
_att.pitch = euler(1);
|
||||
_att.yaw = euler(2);
|
||||
if (eulerDif[2] < -pi) eulerDif[2] += 2 * pi;
|
||||
|
||||
_att.rollspeed = _ekf->angRate.x - _ekf->states[10];
|
||||
_att.pitchspeed = _ekf->angRate.y - _ekf->states[11];
|
||||
_att.yawspeed = _ekf->angRate.z - _ekf->states[12];
|
||||
// gyro offsets
|
||||
_att.rate_offsets[0] = _ekf->states[10];
|
||||
_att.rate_offsets[1] = _ekf->states[11];
|
||||
_att.rate_offsets[2] = _ekf->states[12];
|
||||
#endif
|
||||
// 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;
|
||||
dt += _ekf->dtIMU;
|
||||
|
||||
/* lazily publish the attitude only once available */
|
||||
if (_att_pub > 0) {
|
||||
/* publish the attitude setpoint */
|
||||
orb_publish(ORB_ID(vehicle_attitude), _att_pub, &_att);
|
||||
// perform a covariance prediction if the total delta angle has exceeded the limit
|
||||
// or the time limit will be exceeded at the next IMU update
|
||||
if ((dt >= (_ekf->covTimeStepMax - _ekf->dtIMU)) || (_ekf->summedDelAng.length() > _ekf->covDelAngMax)) {
|
||||
_ekf->CovariancePrediction(dt);
|
||||
_ekf->summedDelAng.zero();
|
||||
_ekf->summedDelVel.zero();
|
||||
dt = 0.0f;
|
||||
}
|
||||
|
||||
} else {
|
||||
/* advertise and publish */
|
||||
_att_pub = orb_advertise(ORB_ID(vehicle_attitude), &_att);
|
||||
}
|
||||
}
|
||||
_initialized = true;
|
||||
|
||||
if (_gps_initialized) {
|
||||
_local_pos.timestamp = last_sensor_timestamp;
|
||||
_local_pos.x = _ekf->states[7];
|
||||
_local_pos.y = _ekf->states[8];
|
||||
// XXX need to announce change of Z reference somehow elegantly
|
||||
_local_pos.z = _ekf->states[9] - _baro_gps_offset;
|
||||
// Fuse GPS Measurements
|
||||
if (newDataGps && _gps_initialized) {
|
||||
// Convert GPS measurements to Pos NE, hgt and Vel NED
|
||||
_ekf->velNED[0] = _gps.vel_n_m_s;
|
||||
_ekf->velNED[1] = _gps.vel_e_m_s;
|
||||
_ekf->velNED[2] = _gps.vel_d_m_s;
|
||||
_ekf->calcposNED(posNED, _ekf->gpsLat, _ekf->gpsLon, _ekf->gpsHgt, _ekf->latRef, _ekf->lonRef, _ekf->hgtRef);
|
||||
|
||||
_local_pos.vx = _ekf->states[4];
|
||||
_local_pos.vy = _ekf->states[5];
|
||||
_local_pos.vz = _ekf->states[6];
|
||||
_ekf->posNE[0] = posNED[0];
|
||||
_ekf->posNE[1] = posNED[1];
|
||||
// set fusion flags
|
||||
_ekf->fuseVelData = true;
|
||||
_ekf->fusePosData = true;
|
||||
// recall states stored at time of measurement after adjusting for delays
|
||||
_ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms));
|
||||
_ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms));
|
||||
// run the fusion step
|
||||
_ekf->FuseVelposNED();
|
||||
|
||||
_local_pos.xy_valid = _gps_initialized;
|
||||
_local_pos.z_valid = true;
|
||||
_local_pos.v_xy_valid = _gps_initialized;
|
||||
_local_pos.v_z_valid = true;
|
||||
_local_pos.xy_global = true;
|
||||
} else if (_ekf->statesInitialised) {
|
||||
// Convert GPS measurements to Pos NE, hgt and Vel NED
|
||||
_ekf->velNED[0] = 0.0f;
|
||||
_ekf->velNED[1] = 0.0f;
|
||||
_ekf->velNED[2] = 0.0f;
|
||||
|
||||
_local_pos.z_global = false;
|
||||
_local_pos.yaw = _att.yaw;
|
||||
_ekf->posNE[0] = 0.0f;
|
||||
_ekf->posNE[1] = 0.0f;
|
||||
// set fusion flags
|
||||
_ekf->fuseVelData = true;
|
||||
_ekf->fusePosData = true;
|
||||
// recall states stored at time of measurement after adjusting for delays
|
||||
_ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms));
|
||||
_ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms));
|
||||
// run the fusion step
|
||||
_ekf->FuseVelposNED();
|
||||
|
||||
/* lazily publish the local position only once available */
|
||||
if (_local_pos_pub > 0) {
|
||||
/* publish the attitude setpoint */
|
||||
orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &_local_pos);
|
||||
} else {
|
||||
_ekf->fuseVelData = false;
|
||||
_ekf->fusePosData = false;
|
||||
}
|
||||
|
||||
} else {
|
||||
/* advertise and publish */
|
||||
_local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &_local_pos);
|
||||
}
|
||||
if (newHgtData && _ekf->statesInitialised) {
|
||||
// Could use a blend of GPS and baro alt data if desired
|
||||
_ekf->hgtMea = 1.0f * (_ekf->baroHgt - _baro_ref);
|
||||
_ekf->fuseHgtData = true;
|
||||
// recall states stored at time of measurement after adjusting for delays
|
||||
_ekf->RecallStates(_ekf->statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms));
|
||||
// run the fusion step
|
||||
_ekf->FuseVelposNED();
|
||||
|
||||
_global_pos.timestamp = _local_pos.timestamp;
|
||||
} else {
|
||||
_ekf->fuseHgtData = false;
|
||||
}
|
||||
|
||||
if (_local_pos.xy_global) {
|
||||
double est_lat, est_lon;
|
||||
map_projection_reproject(&_pos_ref, _local_pos.x, _local_pos.y, &est_lat, &est_lon);
|
||||
_global_pos.lat = est_lat;
|
||||
_global_pos.lon = est_lon;
|
||||
_global_pos.time_gps_usec = _gps.time_gps_usec;
|
||||
_global_pos.eph = _gps.eph_m;
|
||||
_global_pos.epv = _gps.epv_m;
|
||||
}
|
||||
// Fuse Magnetometer Measurements
|
||||
if (newDataMag && _ekf->statesInitialised) {
|
||||
_ekf->fuseMagData = true;
|
||||
_ekf->RecallStates(_ekf->statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data
|
||||
|
||||
if (_local_pos.v_xy_valid) {
|
||||
_global_pos.vel_n = _local_pos.vx;
|
||||
_global_pos.vel_e = _local_pos.vy;
|
||||
} else {
|
||||
_global_pos.vel_n = 0.0f;
|
||||
_global_pos.vel_e = 0.0f;
|
||||
}
|
||||
} else {
|
||||
_ekf->fuseMagData = false;
|
||||
}
|
||||
|
||||
/* local pos alt is negative, change sign and add alt offsets */
|
||||
_global_pos.alt = _local_pos.ref_alt + _baro_gps_offset + (-_local_pos.z);
|
||||
if (_ekf->statesInitialised) _ekf->FuseMagnetometer();
|
||||
|
||||
if (_local_pos.v_z_valid) {
|
||||
_global_pos.vel_d = _local_pos.vz;
|
||||
}
|
||||
// Fuse Airspeed Measurements
|
||||
if (newAdsData && _ekf->statesInitialised && _ekf->VtasMeas > 8.0f) {
|
||||
_ekf->fuseVtasData = true;
|
||||
_ekf->RecallStates(_ekf->statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data
|
||||
_ekf->FuseAirspeed();
|
||||
|
||||
_global_pos.yaw = _local_pos.yaw;
|
||||
} else {
|
||||
_ekf->fuseVtasData = false;
|
||||
}
|
||||
|
||||
_global_pos.eph = _gps.eph_m;
|
||||
_global_pos.epv = _gps.epv_m;
|
||||
|
||||
_global_pos.timestamp = _local_pos.timestamp;
|
||||
// Output results
|
||||
math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]);
|
||||
math::Matrix<3, 3> R = q.to_dcm();
|
||||
math::Vector<3> euler = R.to_euler();
|
||||
|
||||
/* lazily publish the global position only once available */
|
||||
if (_global_pos_pub > 0) {
|
||||
/* publish the global position */
|
||||
orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &_global_pos);
|
||||
for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
|
||||
_att.R[i][j] = R(i, j);
|
||||
|
||||
} else {
|
||||
/* advertise and publish */
|
||||
_global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &_global_pos);
|
||||
}
|
||||
_att.timestamp = last_sensor_timestamp;
|
||||
_att.q[0] = _ekf->states[0];
|
||||
_att.q[1] = _ekf->states[1];
|
||||
_att.q[2] = _ekf->states[2];
|
||||
_att.q[3] = _ekf->states[3];
|
||||
_att.q_valid = true;
|
||||
_att.R_valid = true;
|
||||
|
||||
if (hrt_elapsed_time(&_wind.timestamp) > 99000) {
|
||||
_wind.timestamp = _global_pos.timestamp;
|
||||
_wind.windspeed_north = _ekf->states[14];
|
||||
_wind.windspeed_east = _ekf->states[15];
|
||||
_wind.covariance_north = 0.0f; // XXX get form filter
|
||||
_wind.covariance_east = 0.0f;
|
||||
_att.timestamp = last_sensor_timestamp;
|
||||
_att.roll = euler(0);
|
||||
_att.pitch = euler(1);
|
||||
_att.yaw = euler(2);
|
||||
|
||||
/* lazily publish the wind estimate only once available */
|
||||
if (_wind_pub > 0) {
|
||||
/* publish the wind estimate */
|
||||
orb_publish(ORB_ID(wind_estimate), _wind_pub, &_wind);
|
||||
_att.rollspeed = _ekf->angRate.x - _ekf->states[10];
|
||||
_att.pitchspeed = _ekf->angRate.y - _ekf->states[11];
|
||||
_att.yawspeed = _ekf->angRate.z - _ekf->states[12];
|
||||
// gyro offsets
|
||||
_att.rate_offsets[0] = _ekf->states[10];
|
||||
_att.rate_offsets[1] = _ekf->states[11];
|
||||
_att.rate_offsets[2] = _ekf->states[12];
|
||||
|
||||
/* lazily publish the attitude only once available */
|
||||
if (_att_pub > 0) {
|
||||
/* publish the attitude setpoint */
|
||||
orb_publish(ORB_ID(vehicle_attitude), _att_pub, &_att);
|
||||
|
||||
} else {
|
||||
/* advertise and publish */
|
||||
_wind_pub = orb_advertise(ORB_ID(wind_estimate), &_wind);
|
||||
_att_pub = orb_advertise(ORB_ID(vehicle_attitude), &_att);
|
||||
}
|
||||
|
||||
if (_gps_initialized) {
|
||||
_local_pos.timestamp = last_sensor_timestamp;
|
||||
_local_pos.x = _ekf->states[7];
|
||||
_local_pos.y = _ekf->states[8];
|
||||
// XXX need to announce change of Z reference somehow elegantly
|
||||
_local_pos.z = _ekf->states[9] - _baro_gps_offset;
|
||||
|
||||
_local_pos.vx = _ekf->states[4];
|
||||
_local_pos.vy = _ekf->states[5];
|
||||
_local_pos.vz = _ekf->states[6];
|
||||
|
||||
_local_pos.xy_valid = _gps_initialized;
|
||||
_local_pos.z_valid = true;
|
||||
_local_pos.v_xy_valid = _gps_initialized;
|
||||
_local_pos.v_z_valid = true;
|
||||
_local_pos.xy_global = true;
|
||||
|
||||
_local_pos.z_global = false;
|
||||
_local_pos.yaw = _att.yaw;
|
||||
|
||||
/* lazily publish the local position only once available */
|
||||
if (_local_pos_pub > 0) {
|
||||
/* publish the attitude setpoint */
|
||||
orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &_local_pos);
|
||||
|
||||
} else {
|
||||
/* advertise and publish */
|
||||
_local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &_local_pos);
|
||||
}
|
||||
|
||||
_global_pos.timestamp = _local_pos.timestamp;
|
||||
|
||||
if (_local_pos.xy_global) {
|
||||
double est_lat, est_lon;
|
||||
map_projection_reproject(&_pos_ref, _local_pos.x, _local_pos.y, &est_lat, &est_lon);
|
||||
_global_pos.lat = est_lat;
|
||||
_global_pos.lon = est_lon;
|
||||
_global_pos.time_gps_usec = _gps.time_gps_usec;
|
||||
_global_pos.eph = _gps.eph_m;
|
||||
_global_pos.epv = _gps.epv_m;
|
||||
}
|
||||
|
||||
if (_local_pos.v_xy_valid) {
|
||||
_global_pos.vel_n = _local_pos.vx;
|
||||
_global_pos.vel_e = _local_pos.vy;
|
||||
} else {
|
||||
_global_pos.vel_n = 0.0f;
|
||||
_global_pos.vel_e = 0.0f;
|
||||
}
|
||||
|
||||
/* local pos alt is negative, change sign and add alt offsets */
|
||||
_global_pos.alt = _local_pos.ref_alt + _baro_gps_offset + (-_local_pos.z);
|
||||
|
||||
if (_local_pos.v_z_valid) {
|
||||
_global_pos.vel_d = _local_pos.vz;
|
||||
}
|
||||
|
||||
|
||||
_global_pos.yaw = _local_pos.yaw;
|
||||
|
||||
_global_pos.eph = _gps.eph_m;
|
||||
_global_pos.epv = _gps.epv_m;
|
||||
|
||||
_global_pos.timestamp = _local_pos.timestamp;
|
||||
|
||||
/* lazily publish the global position only once available */
|
||||
if (_global_pos_pub > 0) {
|
||||
/* publish the global position */
|
||||
orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &_global_pos);
|
||||
|
||||
} else {
|
||||
/* advertise and publish */
|
||||
_global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &_global_pos);
|
||||
}
|
||||
|
||||
if (hrt_elapsed_time(&_wind.timestamp) > 99000) {
|
||||
_wind.timestamp = _global_pos.timestamp;
|
||||
_wind.windspeed_north = _ekf->states[14];
|
||||
_wind.windspeed_east = _ekf->states[15];
|
||||
_wind.covariance_north = 0.0f; // XXX get form filter
|
||||
_wind.covariance_east = 0.0f;
|
||||
|
||||
/* lazily publish the wind estimate only once available */
|
||||
if (_wind_pub > 0) {
|
||||
/* publish the wind estimate */
|
||||
orb_publish(ORB_ID(wind_estimate), _wind_pub, &_wind);
|
||||
|
||||
} else {
|
||||
/* advertise and publish */
|
||||
_wind_pub = orb_advertise(ORB_ID(wind_estimate), &_wind);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
@ -1407,9 +1420,10 @@ FixedwingEstimator::print_status()
|
|||
// 4-6: Velocity - m/sec (North, East, Down)
|
||||
// 7-9: Position - m (North, East, Down)
|
||||
// 10-12: Delta Angle bias - rad (X,Y,Z)
|
||||
// 13-14: Wind Vector - m/sec (North,East)
|
||||
// 15-17: Earth Magnetic Field Vector - gauss (North, East, Down)
|
||||
// 18-20: Body Magnetic Field Vector - gauss (X,Y,Z)
|
||||
// 13: Accelerometer offset
|
||||
// 14-15: Wind Vector - m/sec (North,East)
|
||||
// 16-18: Earth Magnetic Field Vector - gauss (North, East, Down)
|
||||
// 19-21: Body Magnetic Field Vector - gauss (X,Y,Z)
|
||||
|
||||
printf("dtIMU: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (int)IMUmsec);
|
||||
printf("ref alt: %8.6f\n", (double)_local_pos.ref_alt);
|
||||
|
|
|
@ -145,7 +145,7 @@ AttPosEKF::AttPosEKF()
|
|||
* instead to allow clean in-air re-initialization.
|
||||
*/
|
||||
{
|
||||
|
||||
memset(&last_ekf_error, 0, sizeof(last_ekf_error));
|
||||
ZeroVariables();
|
||||
InitialiseParameters();
|
||||
}
|
||||
|
@ -2382,7 +2382,7 @@ int AttPosEKF::CheckAndBound()
|
|||
|
||||
// Reset the filter if the IMU data is too old
|
||||
if (dtIMU > 0.3f) {
|
||||
|
||||
FillErrorReport(&last_ekf_error);
|
||||
ResetVelocity();
|
||||
ResetPosition();
|
||||
ResetHeight();
|
||||
|
@ -2397,6 +2397,7 @@ int AttPosEKF::CheckAndBound()
|
|||
|
||||
// Check if we switched between states
|
||||
if (currStaticMode != staticMode) {
|
||||
FillErrorReport(&last_ekf_error);
|
||||
ResetVelocity();
|
||||
ResetPosition();
|
||||
ResetHeight();
|
||||
|
@ -2405,6 +2406,15 @@ int AttPosEKF::CheckAndBound()
|
|||
return 3;
|
||||
}
|
||||
|
||||
// Reset the filter if gyro offsets are excessive
|
||||
if (fabs(states[10]) > 1.0f || fabsf(states[11]) > 1.0f || fabsf(states[12]) > 1.0f) {
|
||||
|
||||
InitializeDynamic(velNED, magDeclination);
|
||||
|
||||
// that's all we can do here, return
|
||||
return 4;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
@ -2531,8 +2541,6 @@ void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, do
|
|||
// the baro offset must be this difference now
|
||||
baroHgtOffset = baroHgt - referenceHgt;
|
||||
|
||||
memset(&last_ekf_error, 0, sizeof(last_ekf_error));
|
||||
|
||||
InitializeDynamic(initvelNED, declination);
|
||||
}
|
||||
|
||||
|
|
|
@ -200,7 +200,6 @@ public:
|
|||
float hgtMea; // measured height (m)
|
||||
float baroHgtOffset; ///< the baro (weather) offset from normalized altitude
|
||||
float rngMea; // Ground distance
|
||||
float posNED[3]; // North, East Down position (m)
|
||||
|
||||
float innovMag[3]; // innovation output
|
||||
float varInnovMag[3]; // innovation variance output
|
||||
|
|
Loading…
Reference in New Issue