void AttitudePositionEstimatorEKF::task_main()
{
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
_ekf = new AttPosEKF();
_filter_start_time = hrt_absolute_time();
if (!_ekf) {
warnx("OUT OF MEM!");
return;
}
/*
* do subscriptions
*/
_distance_sub = orb_subscribe(ORB_ID(distance_sensor));
_baro_sub = orb_subscribe_multi(ORB_ID(sensor_baro), 0);
_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
_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));
_armedSub = orb_subscribe(ORB_ID(actuator_armed));
/* rate limit vehicle status updates to 5Hz */
orb_set_interval(_vstatus_sub, 200);
_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
/* XXX remove this!, BUT increase the data buffer size! */
orb_set_interval(_sensor_combined_sub, 9);
/* sets also parameters in the EKF object */
parameters_update();
/* wakeup source(s) */
px4_pollfd_struct_t fds[2];
/* Setup of loop */
fds[0].fd = _params_sub;
fds[0].events = POLLIN;
fds[1].fd = _sensor_combined_sub;
fds[1].events = POLLIN;
_gps.vel_n_m_s = 0.0f;
_gps.vel_e_m_s = 0.0f;
_gps.vel_d_m_s = 0.0f;
_task_running = true;
while (!_task_should_exit) {
/* wait for up to 100ms for data */
int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
/* timed out - periodic check for _task_should_exit, etc. */
if (pret == 0) {
continue;
}
/* this is undesirable but not much we can do - might want to flag unhappy status */
if (pret < 0) {
warn("POLL ERR %d, %d", pret, errno);
continue;
}
perf_begin(_loop_perf);
perf_count(_loop_intvl);
/* only update parameters if they changed */
if (fds[0].revents & POLLIN) {
/* read from param to clear updated flag */
struct parameter_update_s update;
orb_copy(ORB_ID(parameter_update), _params_sub, &update);
/* update parameters from storage */
parameters_update();
}
/* only run estimator if gyro updated */
if (fds[1].revents & POLLIN) {
/* check vehicle status for changes to publication state */
bool prev_hil = (_vstatus.hil_state == vehicle_status_s::HIL_STATE_ON);
vehicle_status_poll();
perf_count(_perf_gyro);
/* Reset baro reference if switching to HIL, reset sensor states */
if (!prev_hil && (_vstatus.hil_state == vehicle_status_s::HIL_STATE_ON)) {
/* system is in HIL now, wait for measurements to come in one last round */
usleep(60000);
/* now read all sensor publications to ensure all real sensor data is purged */
orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
/* set sensors to de-initialized state */
_gyro_valid = false;
_accel_valid = false;
_mag_valid = false;
_baro_init = false;
_gps_initialized = false;
_last_sensor_timestamp = hrt_absolute_time();
_last_run = _last_sensor_timestamp;
_ekf->ZeroVariables();
_ekf->dtIMU = 0.01f;
_filter_start_time = _last_sensor_timestamp;
/* now skip this loop and get data on the next one, which will also re-init the filter */
continue;
}
/**
* PART ONE: COLLECT ALL DATA
**/
pollData();
/*
* CHECK IF ITS THE RIGHT TIME TO RUN THINGS ALREADY
*/
if (hrt_elapsed_time(&_filter_start_time) < FILTER_INIT_DELAY) {
continue;
}
/**
* PART TWO: EXECUTE THE FILTER
*
* We run the filter only once all data has been fetched
**/
if (_baro_init && _gyro_valid && _accel_valid && _mag_valid) {
// maintain filtered baro and gps altitudes to calculate weather offset
// baro sample rate is ~70Hz and measurement bandwidth is high
// gps sample rate is 5Hz and altitude is assumed accurate when averaged over 30 seconds
// maintain heavily filtered values for both baro and gps altitude
// Assume the filtered output should be identical for both sensors
_baro_gps_offset = _baro_alt_filt - _gps_alt_filt;
// if (hrt_elapsed_time(&_last_debug_print) >= 5e6) {
// _last_debug_print = hrt_absolute_time();
// perf_print_counter(_perf_baro);
// perf_reset(_perf_baro);
// warnx("gpsoff: %5.1f, baro_alt_filt: %6.1f, gps_alt_filt: %6.1f, gpos.alt: %5.1f, lpos.z: %6.1f",
// (double)_baro_gps_offset,
// (double)_baro_alt_filt,
// (double)_gps_alt_filt,
// (double)_global_pos.alt,
// (double)_local_pos.z);
// }
/* Initialize the filter first */
if (!_ekf->statesInitialised) {
// North, East Down position (m)
float initVelNED[3] = {0.0f, 0.0f, 0.0f};
_ekf->posNE[0] = 0.0f;
_ekf->posNE[1] = 0.0f;
_local_pos.ref_alt = 0.0f;
_baro_ref_offset = 0.0f;
_baro_gps_offset = 0.0f;
_baro_alt_filt = _baro.altitude;
_ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f);
} else {
if (!_gps_initialized && _gpsIsGood) {
initializeGPS();
continue;
}
// 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();
if (check) {
// Let the system re-initialize itself
continue;
}
//Run EKF data fusion steps
updateSensorFusion(_gpsIsGood, _newDataMag, _newRangeData, _newHgtData, _newAdsData);
//Publish attitude estimations
publishAttitude();
//Publish Local Position estimations
publishLocalPosition();
//Publish Global Position, but only if it's any good
if (_gps_initialized && (_gpsIsGood || _global_pos.dead_reckoning)) {
publishGlobalPosition();
}
//Publish wind estimates
if (hrt_elapsed_time(&_wind.timestamp) > 99000) {
publishWindEstimate();
}
}
}
}
perf_end(_loop_perf);
}
_task_running = false;
_estimator_task = -1;
return;
}
void
FixedwingEstimator::task_main()
{
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
_ekf = new AttPosEKF();
float dt = 0.0f; // time lapsed since last covariance prediction
_filter_start_time = hrt_absolute_time();
if (!_ekf) {
errx(1, "OUT OF MEM!");
}
/*
* do subscriptions
*/
_baro_sub = orb_subscribe(ORB_ID(sensor_baro));
_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_home_sub = orb_subscribe(ORB_ID(home_position));
/* rate limit vehicle status updates to 5Hz */
orb_set_interval(_vstatus_sub, 200);
#ifndef SENSOR_COMBINED_SUB
_gyro_sub = orb_subscribe(ORB_ID(sensor_gyro));
_accel_sub = orb_subscribe(ORB_ID(sensor_accel));
_mag_sub = orb_subscribe(ORB_ID(sensor_mag));
/* rate limit gyro updates to 50 Hz */
/* XXX remove this!, BUT increase the data buffer size! */
orb_set_interval(_gyro_sub, 4);
#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, 9);
#endif
/* sets also parameters in the EKF object */
parameters_update();
Vector3f lastAngRate;
Vector3f lastAccel;
/* wakeup source(s) */
struct pollfd fds[2];
/* Setup of loop */
fds[0].fd = _params_sub;
fds[0].events = POLLIN;
#ifndef SENSOR_COMBINED_SUB
fds[1].fd = _gyro_sub;
fds[1].events = POLLIN;
#else
fds[1].fd = _sensor_combined_sub;
fds[1].events = POLLIN;
#endif
bool newDataGps = false;
bool newHgtData = false;
bool newAdsData = false;
bool newDataMag = false;
float posNED[3] = {0.0f, 0.0f, 0.0f}; // North, East Down position (m)
uint64_t last_gps = 0;
_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 */
int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
/* timed out - periodic check for _task_should_exit, etc. */
if (pret == 0)
continue;
/* this is undesirable but not much we can do - might want to flag unhappy status */
if (pret < 0) {
warn("POLL ERR %d, %d", pret, errno);
continue;
}
perf_begin(_loop_perf);
/* only update parameters if they changed */
if (fds[0].revents & POLLIN) {
/* read from param to clear updated flag */
struct parameter_update_s update;
orb_copy(ORB_ID(parameter_update), _params_sub, &update);
/* update parameters from storage */
parameters_update();
}
/* only run estimator if gyro updated */
if (fds[1].revents & POLLIN) {
/* check vehicle status for changes to publication state */
bool prev_hil = (_vstatus.hil_state == HIL_STATE_ON);
vehicle_status_poll();
bool accel_updated;
bool mag_updated;
perf_count(_perf_gyro);
/* Reset baro reference if switching to HIL, reset sensor states */
if (!prev_hil && (_vstatus.hil_state == HIL_STATE_ON)) {
/* system is in HIL now, wait for measurements to come in one last round */
usleep(60000);
#ifndef SENSOR_COMBINED_SUB
orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro);
orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel);
orb_copy(ORB_ID(sensor_mag), _mag_sub, &_mag);
#else
/* now read all sensor publications to ensure all real sensor data is purged */
orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
#endif
/* set sensors to de-initialized state */
_gyro_valid = false;
_accel_valid = false;
_mag_valid = false;
_baro_init = false;
_gps_initialized = false;
_last_sensor_timestamp = hrt_absolute_time();
_last_run = _last_sensor_timestamp;
_ekf->ZeroVariables();
_ekf->dtIMU = 0.01f;
_filter_start_time = _last_sensor_timestamp;
/* now skip this loop and get data on the next one, which will also re-init the filter */
continue;
}
/**
* PART ONE: COLLECT ALL DATA
**/
/* load local copies */
#ifndef SENSOR_COMBINED_SUB
orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro);
orb_check(_accel_sub, &accel_updated);
if (accel_updated) {
orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel);
}
_last_sensor_timestamp = _gyro.timestamp;
IMUmsec = _gyro.timestamp / 1e3f;
float deltaT = (_gyro.timestamp - _last_run) / 1e6f;
_last_run = _gyro.timestamp;
/* guard against too large deltaT's */
if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) {
deltaT = 0.01f;
}
// Always store data, independent of init status
/* fill in last data set */
_ekf->dtIMU = deltaT;
if (isfinite(_gyro.x) &&
isfinite(_gyro.y) &&
isfinite(_gyro.z)) {
_ekf->angRate.x = _gyro.x;
_ekf->angRate.y = _gyro.y;
_ekf->angRate.z = _gyro.z;
if (!_gyro_valid) {
lastAngRate = _ekf->angRate;
}
_gyro_valid = true;
}
if (accel_updated) {
_ekf->accel.x = _accel.x;
_ekf->accel.y = _accel.y;
_ekf->accel.z = _accel.z;
if (!_accel_valid) {
lastAccel = _ekf->accel;
}
_accel_valid = true;
}
_ekf->dAngIMU = 0.5f * (angRate + lastAngRate) * dtIMU;
_ekf->lastAngRate = angRate;
_ekf->dVelIMU = 0.5f * (accel + lastAccel) * dtIMU;
_ekf->lastAccel = accel;
#else
orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
static hrt_abstime last_accel = 0;
static hrt_abstime last_mag = 0;
if (last_accel != _sensor_combined.accelerometer_timestamp) {
accel_updated = true;
} else {
accel_updated = false;
}
last_accel = _sensor_combined.accelerometer_timestamp;
// Copy gyro and accel
_last_sensor_timestamp = _sensor_combined.timestamp;
IMUmsec = _sensor_combined.timestamp / 1e3f;
float deltaT = (_sensor_combined.timestamp - _last_run) / 1e6f;
/* guard against too large deltaT's */
if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) {
deltaT = 0.01f;
}
_last_run = _sensor_combined.timestamp;
// Always store data, independent of init status
/* fill in last data set */
_ekf->dtIMU = deltaT;
if (isfinite(_sensor_combined.gyro_rad_s[0]) &&
isfinite(_sensor_combined.gyro_rad_s[1]) &&
isfinite(_sensor_combined.gyro_rad_s[2])) {
_ekf->angRate.x = _sensor_combined.gyro_rad_s[0];
_ekf->angRate.y = _sensor_combined.gyro_rad_s[1];
_ekf->angRate.z = _sensor_combined.gyro_rad_s[2];
if (!_gyro_valid) {
lastAngRate = _ekf->angRate;
}
_gyro_valid = true;
perf_count(_perf_gyro);
}
if (accel_updated) {
_ekf->accel.x = _sensor_combined.accelerometer_m_s2[0];
_ekf->accel.y = _sensor_combined.accelerometer_m_s2[1];
_ekf->accel.z = _sensor_combined.accelerometer_m_s2[2];
if (!_accel_valid) {
lastAccel = _ekf->accel;
}
_accel_valid = true;
}
_ekf->dAngIMU = 0.5f * (_ekf->angRate + lastAngRate) * _ekf->dtIMU;
lastAngRate = _ekf->angRate;
_ekf->dVelIMU = 0.5f * (_ekf->accel + lastAccel) * _ekf->dtIMU;
lastAccel = _ekf->accel;
if (last_mag != _sensor_combined.magnetometer_timestamp) {
mag_updated = true;
newDataMag = true;
} else {
newDataMag = false;
}
last_mag = _sensor_combined.magnetometer_timestamp;
#endif
//warnx("dang: %8.4f %8.4f dvel: %8.4f %8.4f", _ekf->dAngIMU.x, _ekf->dAngIMU.z, _ekf->dVelIMU.x, _ekf->dVelIMU.z);
bool airspeed_updated;
orb_check(_airspeed_sub, &airspeed_updated);
if (airspeed_updated) {
orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
perf_count(_perf_airspeed);
_ekf->VtasMeas = _airspeed.true_airspeed_m_s;
newAdsData = true;
} else {
newAdsData = false;
}
bool gps_updated;
orb_check(_gps_sub, &gps_updated);
if (gps_updated) {
last_gps = _gps.timestamp_position;
orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps);
perf_count(_perf_gps);
if (_gps.fix_type < 3) {
newDataGps = false;
} else {
/* store time of valid GPS measurement */
_gps_start_time = hrt_absolute_time();
/* check if we had a GPS outage for a long time */
if (hrt_elapsed_time(&last_gps) > 5 * 1000 * 1000) {
_ekf->ResetPosition();
_ekf->ResetVelocity();
_ekf->ResetStoredStates();
}
/* fuse GPS updates */
//_gps.timestamp / 1e3;
_ekf->GPSstatus = _gps.fix_type;
_ekf->velNED[0] = _gps.vel_n_m_s;
_ekf->velNED[1] = _gps.vel_e_m_s;
_ekf->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]);
_ekf->gpsLat = math::radians(_gps.lat / (double)1e7);
_ekf->gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI;
_ekf->gpsHgt = _gps.alt / 1e3f;
// if (_gps.s_variance_m_s > 0.25f && _gps.s_variance_m_s < 100.0f * 100.0f) {
// _ekf->vneSigma = sqrtf(_gps.s_variance_m_s);
// } else {
// _ekf->vneSigma = _parameters.velne_noise;
// }
// if (_gps.p_variance_m > 0.25f && _gps.p_variance_m < 100.0f * 100.0f) {
// _ekf->posNeSigma = sqrtf(_gps.p_variance_m);
// } else {
// _ekf->posNeSigma = _parameters.posne_noise;
// }
// warnx("vel: %8.4f pos: %8.4f", _gps.s_variance_m_s, _gps.p_variance_m);
newDataGps = true;
}
}
bool baro_updated;
orb_check(_baro_sub, &baro_updated);
if (baro_updated) {
orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
_ekf->baroHgt = _baro.altitude;
if (!_baro_init) {
_baro_ref = _baro.altitude;
_baro_init = true;
warnx("ALT REF INIT");
}
perf_count(_perf_baro);
newHgtData = true;
} else {
newHgtData = false;
}
#ifndef SENSOR_COMBINED_SUB
orb_check(_mag_sub, &mag_updated);
#endif
if (mag_updated) {
_mag_valid = true;
perf_count(_perf_mag);
#ifndef SENSOR_COMBINED_SUB
orb_copy(ORB_ID(sensor_mag), _mag_sub, &_mag);
// XXX we compensate the offsets upfront - should be close to zero.
// 0.001f
_ekf->magData.x = _mag.x;
_ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset
_ekf->magData.y = _mag.y;
_ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset
_ekf->magData.z = _mag.z;
_ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset
#else
// XXX we compensate the offsets upfront - should be close to zero.
// 0.001f
_ekf->magData.x = _sensor_combined.magnetometer_ga[0];
_ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset
_ekf->magData.y = _sensor_combined.magnetometer_ga[1];
_ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset
_ekf->magData.z = _sensor_combined.magnetometer_ga[2];
_ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset
#endif
newDataMag = true;
} else {
newDataMag = false;
}
/*
* CHECK IF ITS THE RIGHT TIME TO RUN THINGS ALREADY
*/
if (hrt_elapsed_time(&_filter_start_time) < FILTER_INIT_DELAY) {
continue;
}
/**
* PART TWO: EXECUTE THE FILTER
*
* We run the filter only once all data has been fetched
**/
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 < _parameters.pos_stddev_threshold && _gps.epv < _parameters.pos_stddev_threshold) {
// 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_ref_offset = _ekf->states[9]; // this should become zero in the local frame
_baro_gps_offset = _baro.altitude - gps_alt;
_ekf->baroHgt = _baro.altitude;
_ekf->hgtMea = 1.0f * (_ekf->baroHgt - (_baro_ref));
// Set up position variables correctly
_ekf->GPSstatus = _gps.fix_type;
_ekf->gpsLat = math::radians(lat);
_ekf->gpsLon = math::radians(lon) - M_PI;
_ekf->gpsHgt = gps_alt;
// Look up mag declination based on current position
float declination = math::radians(get_mag_declination(lat, lon));
_ekf->InitialiseFilter(initVelNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt, declination);
// Initialize projection
_local_pos.ref_lat = lat;
_local_pos.ref_lon = lon;
_local_pos.ref_alt = gps_alt;
_local_pos.ref_timestamp = _gps.timestamp_position;
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_ref_offset);
warnx("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph, (double)_gps.epv, (double)math::degrees(declination));
#endif
_gps_initialized = true;
} else if (!_ekf->statesInitialised) {
initVelNED[0] = 0.0f;
initVelNED[1] = 0.0f;
initVelNED[2] = 0.0f;
_ekf->posNE[0] = posNED[0];
_ekf->posNE[1] = posNED[1];
_local_pos.ref_alt = _baro_ref;
_baro_ref_offset = 0.0f;
_baro_gps_offset = 0.0f;
_ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f);
} else if (_ekf->statesInitialised) {
// We're apparently initialized in this case now
int check = check_filter_state();
if (check) {
// Let the system re-initialize itself
continue;
}
// 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;
}
// Fuse GPS Measurements
if (newDataGps && _gps_initialized) {
// Convert GPS measurements to Pos NE, hgt and Vel NED
float gps_dt = (_gps.timestamp_position - last_gps) / 1e6f;
// Calculate acceleration predicted by GPS velocity change
if (((fabsf(_ekf->velNED[0] - _gps.vel_n_m_s) > FLT_EPSILON) ||
(fabsf(_ekf->velNED[1] - _gps.vel_e_m_s) > FLT_EPSILON) ||
(fabsf(_ekf->velNED[2] - _gps.vel_d_m_s) > FLT_EPSILON)) && (gps_dt > 0.00001f)) {
_ekf->accelGPSNED[0] = (_ekf->velNED[0] - _gps.vel_n_m_s) / gps_dt;
_ekf->accelGPSNED[1] = (_ekf->velNED[1] - _gps.vel_e_m_s) / gps_dt;
_ekf->accelGPSNED[2] = (_ekf->velNED[2] - _gps.vel_d_m_s) / gps_dt;
}
_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);
_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();
} 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->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();
} 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
_ekf->magstate.obsIndex = 0;
_ekf->FuseMagnetometer();
_ekf->FuseMagnetometer();
_ekf->FuseMagnetometer();
} else {
_ekf->fuseMagData = false;
}
// 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;
}
// 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();
for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
_att.R[i][j] = R(i, 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;
_att.timestamp = _last_sensor_timestamp;
_att.roll = euler(0);
_att.pitch = euler(1);
_att.yaw = euler(2);
_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 */
_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_ref_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;
_velocity_xy_filtered = 0.95f*_velocity_xy_filtered + 0.05f*sqrtf(_local_pos.vx*_local_pos.vx + _local_pos.vy*_local_pos.vy);
_velocity_z_filtered = 0.95f*_velocity_z_filtered + 0.05f*fabsf(_local_pos.vz);
_airspeed_filtered = 0.95f*_airspeed_filtered + + 0.05f*_airspeed.true_airspeed_m_s;
/* crude land detector for fixedwing only,
* TODO: adapt so that it works for both, maybe move to another location
*/
if (_velocity_xy_filtered < 5
&& _velocity_z_filtered < 10
&& _airspeed_filtered < 10) {
_local_pos.landed = true;
} else {
_local_pos.landed = false;
}
_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;
_global_pos.epv = _gps.epv;
}
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 = _baro_ref + (-_local_pos.z) - _baro_gps_offset;
if (_local_pos.v_z_valid) {
_global_pos.vel_d = _local_pos.vz;
}
_global_pos.yaw = _local_pos.yaw;
_global_pos.eph = _gps.eph;
_global_pos.epv = _gps.epv;
_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);
}
}
}
}
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 = _ekf->P[14][14];
_wind.covariance_east = _ekf->P[15][15];
/* 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);
}
}
}
}
perf_end(_loop_perf);
}
warnx("exiting.\n");
_estimator_task = -1;
_exit(0);
}
