From 0cbfa65056bb4716ddaf98783a844a7bff2dd8ee Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Wed, 11 Feb 2015 17:57:33 +0100 Subject: AttPosEKF: Refactor and code cleanup --- ROMFS/px4fmu_common/init.d/rc.mc_apps | 6 +- src/drivers/gps/gps.cpp | 159 ++++- .../ekf_att_pos_estimator_main.cpp | 705 ++++++++++++--------- 3 files changed, 527 insertions(+), 343 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps index 2ecc104df..66eae6255 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -4,9 +4,9 @@ # att & pos estimator, att & pos control. # -attitude_estimator_ekf start -#ekf_att_pos_estimator start -position_estimator_inav start +#attitude_estimator_ekf start +ekf_att_pos_estimator start +#position_estimator_inav start if mc_att_control start then diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index f9595a734..2bbc6c916 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -81,6 +81,12 @@ #endif static const int ERROR = -1; +//DEBUG BEGIN + #include +static int sp_man_sub = -1; +static struct manual_control_setpoint_s sp_man; +//DEBUG END + /* class for dynamic allocation of satellite info data */ class GPS_Sat_Info { @@ -162,7 +168,7 @@ extern "C" __EXPORT int gps_main(int argc, char *argv[]); namespace { -GPS *g_dev; +GPS *g_dev = nullptr; } @@ -271,6 +277,27 @@ GPS::task_main_trampoline(void *arg) g_dev->task_main(); } +static bool orb_update(const struct orb_metadata *meta, int handle, void *buffer) +{ + bool newData = false; + + // check if there is new data to grab + if (orb_check(handle, &newData) != OK) { + return false; + } + + if (!newData) { + return false; + } + + if (orb_copy(meta, handle, buffer) != OK) { + return false; + } + + return true; +} + + void GPS::task_main() { @@ -288,31 +315,62 @@ GPS::task_main() uint64_t last_rate_measurement = hrt_absolute_time(); unsigned last_rate_count = 0; + //DEBUG BEGIN + sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + memset(&sp_man, 0, sizeof(sp_man)); + //DEBUG END + /* loop handling received serial bytes and also configuring in between */ while (!_task_should_exit) { if (_fake_gps) { - _report_gps_pos.timestamp_position = hrt_absolute_time(); - _report_gps_pos.lat = (int32_t)47.378301e7f; - _report_gps_pos.lon = (int32_t)8.538777e7f; - _report_gps_pos.alt = (int32_t)1200e3f; - _report_gps_pos.timestamp_variance = hrt_absolute_time(); - _report_gps_pos.s_variance_m_s = 10.0f; - _report_gps_pos.c_variance_rad = 0.1f; - _report_gps_pos.fix_type = 3; - _report_gps_pos.eph = 0.9f; - _report_gps_pos.epv = 1.8f; - _report_gps_pos.timestamp_velocity = hrt_absolute_time(); - _report_gps_pos.vel_n_m_s = 0.0f; - _report_gps_pos.vel_e_m_s = 0.0f; - _report_gps_pos.vel_d_m_s = 0.0f; - _report_gps_pos.vel_m_s = sqrtf(_report_gps_pos.vel_n_m_s * _report_gps_pos.vel_n_m_s + _report_gps_pos.vel_e_m_s * _report_gps_pos.vel_e_m_s + _report_gps_pos.vel_d_m_s * _report_gps_pos.vel_d_m_s); - _report_gps_pos.cog_rad = 0.0f; - _report_gps_pos.vel_ned_valid = true; + //DEBUG BEGIN: Disable GPS using aux1 + orb_update(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man); + if(isfinite(sp_man.aux1) && sp_man.aux1 >= 0.0f) { + _report_gps_pos.fix_type = 0; + _report_gps_pos.satellites_used = 0; + + //Don't modify Lat/Lon/AMSL + + _report_gps_pos.eph = (float)0xFFFF; + _report_gps_pos.epv = (float)0xFFFF; + _report_gps_pos.s_variance_m_s = (float)0xFFFF; + + _report_gps_pos.vel_m_s = 0.0f; + _report_gps_pos.vel_n_m_s = 0.0f; + _report_gps_pos.vel_e_m_s = 0.0f; + _report_gps_pos.vel_d_m_s = 0.0f; + _report_gps_pos.vel_ned_valid = false; + + _report_gps_pos.cog_rad = 0.0f; + _report_gps_pos.c_variance_rad = (float)0xFFFF; + } + //DEBUG END + + else { + _report_gps_pos.timestamp_position = hrt_absolute_time(); + _report_gps_pos.lat = (int32_t)47.378301e7f; + _report_gps_pos.lon = (int32_t)8.538777e7f; + _report_gps_pos.alt = (int32_t)1200e3f; + _report_gps_pos.timestamp_variance = hrt_absolute_time(); + _report_gps_pos.s_variance_m_s = 10.0f; + _report_gps_pos.c_variance_rad = 0.1f; + _report_gps_pos.fix_type = 3; + _report_gps_pos.eph = 0.9f; + _report_gps_pos.epv = 1.8f; + _report_gps_pos.timestamp_velocity = hrt_absolute_time(); + _report_gps_pos.vel_n_m_s = 0.0f; + _report_gps_pos.vel_e_m_s = 0.0f; + _report_gps_pos.vel_d_m_s = 0.0f; + _report_gps_pos.vel_m_s = sqrtf(_report_gps_pos.vel_n_m_s * _report_gps_pos.vel_n_m_s + _report_gps_pos.vel_e_m_s * _report_gps_pos.vel_e_m_s + _report_gps_pos.vel_d_m_s * _report_gps_pos.vel_d_m_s); + _report_gps_pos.cog_rad = 0.0f; + _report_gps_pos.vel_ned_valid = true; + } //no time and satellite information simulated + if (!(_pub_blocked)) { if (_report_gps_pos_pub > 0) { orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); @@ -364,6 +422,29 @@ GPS::task_main() if (!(_pub_blocked)) { if (helper_ret & 1) { + + //DEBUG BEGIN: Disable GPS using aux1 + orb_update(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man); + if(isfinite(sp_man.aux1) && sp_man.aux1 >= 0.0f) { + _report_gps_pos.fix_type = 0; + _report_gps_pos.satellites_used = 0; + + //Don't modify Lat/Lon/AMSL + + _report_gps_pos.eph = (float)0xFFFF; + _report_gps_pos.epv = (float)0xFFFF; + _report_gps_pos.s_variance_m_s = (float)0xFFFF; + + _report_gps_pos.vel_m_s = 0.0f; + _report_gps_pos.vel_n_m_s = 0.0f; + _report_gps_pos.vel_e_m_s = 0.0f; + _report_gps_pos.vel_d_m_s = 0.0f; + _report_gps_pos.vel_ned_valid = false; + + _report_gps_pos.cog_rad = 0.0f; + _report_gps_pos.c_variance_rad = (float)0xFFFF; + } + if (_report_gps_pos_pub > 0) { orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); @@ -478,25 +559,35 @@ GPS::cmd_reset() void GPS::print_info() { - switch (_mode) { - case GPS_DRIVER_MODE_UBX: - warnx("protocol: UBX"); - break; + //GPS Mode + if(_fake_gps) { + warnx("protocol: SIMULATED"); + } - case GPS_DRIVER_MODE_MTK: - warnx("protocol: MTK"); - break; + else { + switch (_mode) { + case GPS_DRIVER_MODE_UBX: + warnx("protocol: UBX"); + break; + + case GPS_DRIVER_MODE_MTK: + warnx("protocol: MTK"); + break; case GPS_DRIVER_MODE_ASHTECH: warnx("protocol: ASHTECH"); break; - default: - break; + default: + break; + } } warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK"); - warnx("sat info: %s", (_p_report_sat_info != nullptr) ? "enabled" : "disabled"); + warnx("sat info: %s, noise: %d, jamming detected: %s", + (_p_report_sat_info != nullptr) ? "enabled" : "disabled", + _report_gps_pos.noise_per_ms, + _report_gps_pos.jamming_indicator == 255 ? "YES" : "NO"); if (_report_gps_pos.timestamp_position != 0) { warnx("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report_gps_pos.fix_type, @@ -520,7 +611,7 @@ GPS::print_info() namespace gps { -GPS *g_dev; +GPS *g_dev = nullptr; void start(const char *uart_path, bool fake_gps, bool enable_sat_info); void stop(); @@ -664,6 +755,14 @@ gps_main(int argc, char *argv[]) gps::start(device_name, fake_gps, enable_sat_info); } + if (!strcmp(argv[1], "fake")) { + if(g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + } + gps::start(GPS_DEFAULT_UART_PORT, true, false); + } + if (!strcmp(argv[1], "stop")) gps::stop(); @@ -686,5 +785,5 @@ gps_main(int argc, char *argv[]) gps::info(); out: - errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status' [-d /dev/ttyS0-n][-f][-s]"); + errx(1, "unrecognized command, try 'start', 'stop', 'test', 'fake', 'reset' or 'status' [-d /dev/ttyS0-n][-f][-s]"); } 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 fbd695eac..2e1750b9e 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 @@ -103,6 +103,8 @@ static uint64_t IMUmsec = 0; static uint64_t IMUusec = 0; static const uint64_t FILTER_INIT_DELAY = 1 * 1000 * 1000; // units: microseconds +static constexpr float POS_RESET_THRESHOLD = 5.0f; ///< Seconds before we signal a total GPS failure + uint32_t millis() { return IMUmsec; @@ -169,7 +171,6 @@ public: int set_debuglevel(unsigned debug) { _debug = debug; return 0; } private: - bool _task_should_exit; /**< if true, sensor task should exit */ bool _task_running; /**< if true, task is running in its mainloop */ int _estimator_task; /**< task handle for sensor task */ @@ -185,8 +186,8 @@ private: int _baro_sub; /**< barometer subscription */ int _gps_sub; /**< GPS subscription */ int _vstatus_sub; /**< vehicle status subscription */ - int _params_sub; /**< notification of parameter updates */ - int _manual_control_sub; /**< notification of manual control updates */ + int _params_sub; /**< notification of parameter updates */ + int _manual_control_sub; /**< notification of manual control updates */ int _mission_sub; int _home_sub; /**< home position as defined by commander / user */ @@ -230,26 +231,28 @@ private: perf_counter_t _perf_mag; ///= 3 && _gps.eph < ephRequired) { + _gpsIsGood = true; + } + else { + _gpsIsGood = false; + } - _ekf->updateDtGpsFilt(math::constrain((_gps.timestamp_position - last_gps) / 1e6f, 0.01f, pos_reset_threshold)); + if (_gpsIsGood) { + _ekf->updateDtGpsFilt(math::constrain((_gps.timestamp_position - _lastGPSTimestamp) / 1e6f, 0.01f, POS_RESET_THRESHOLD)); /* fuse GPS updates */ @@ -1103,9 +1141,9 @@ void AttitudePositionEstimatorEKF::task_main() _ekf->posNE[1] = posNED[1]; // update LPF - _gps_alt_filt += (gps_elapsed / (rc + gps_elapsed)) * (_ekf->gpsHgt - _gps_alt_filt); + _gps_alt_filt += (dtGoodGPS / (rc + dtGoodGPS)) * (_ekf->gpsHgt - _gps_alt_filt); - //warnx("gps alt: %6.1f, interval: %6.3f", (double)_ekf->gpsHgt, (double)gps_elapsed); + //warnx("gps alt: %6.1f, interval: %6.3f", (double)_ekf->gpsHgt, (double)dtGoodGPS); // 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); @@ -1122,18 +1160,24 @@ void AttitudePositionEstimatorEKF::task_main() // warnx("vel: %8.4f pos: %8.4f", _gps.s_variance_m_s, _gps.p_variance_m); /* check if we had a GPS outage for a long time */ - /* timeout of 5 seconds */ - if (gps_elapsed > pos_reset_threshold) { + if (dtGoodGPS > POS_RESET_THRESHOLD) { _ekf->ResetPosition(); _ekf->ResetVelocity(); _ekf->ResetStoredStates(); } newDataGps = true; - last_gps = _gps.timestamp_position; - + _lastGPSTimestamp = _gps.timestamp_position; } - + else { + //Too poor GPS fix to use + newDataGps = false; + } + } + // If it has gone more than POS_RESET_THRESHOLD amount of seconds since we received a GPS update, + // then something is very wrong with the GPS (possibly a hardware failure or comlink error) + else if(dtGoodGPS > POS_RESET_THRESHOLD) { + _gpsIsGood = false; } bool baro_updated; @@ -1276,7 +1320,7 @@ void AttitudePositionEstimatorEKF::task_main() // } /* Initialize the filter first */ - if (!_gps_initialized && _gps.fix_type > 2 && _gps.eph < _parameters.pos_stddev_threshold && _gps.epv < _parameters.pos_stddev_threshold) { + if (!_gps_initialized && _gpsIsGood && _gps.epv < _parameters.pos_stddev_threshold) { // GPS is in scaled integers, convert double lat = _gps.lat / 1.0e7; @@ -1344,289 +1388,330 @@ void AttitudePositionEstimatorEKF::task_main() // 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 the strapdown INS equations every IMU update - _ekf->UpdateStrapdownEquationsNED(); - - // 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; - } + //Run EKF data fusion steps + updateSensorFusion(newDataGps, newDataMag, newRangeData, newHgtData, newAdsData); + + //Publish attitude estimations + publishAttitude(); - // 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; - } - - // 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 (!_gps_initialized) { - - // force static mode - _ekf->staticMode = true; - - // 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; + //Publish Local Position estimations + publishLocalPosition(); + + //Publish Global Position, but only if it's any good + if(_gps_initialized && _gpsIsGood) + { + publishGlobalPosition(); } - if (newHgtData) { - // 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; + //Publish wind estimates + if (hrt_elapsed_time(&_wind.timestamp) > 99000) { + publishWindEstimate(); } + } + } - // Fuse Magnetometer Measurements - if (newDataMag) { - _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(); + perf_end(_loop_perf); + } - } else { - _ekf->fuseMagData = false; - } + _task_running = false; - // Fuse Airspeed Measurements - if (newAdsData && _ekf->VtasMeas > 7.0f) { - _ekf->fuseVtasData = true; - _ekf->RecallStates(_ekf->statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data - _ekf->FuseAirspeed(); + warnx("exiting.\n"); - } else { - _ekf->fuseVtasData = false; - } + _estimator_task = -1; + _exit(0); +} - if (newRangeData) { +void AttitudePositionEstimatorEKF::publishAttitude() +{ + // 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(); - if (_ekf->Tnb.z.z > 0.9f) { - // _ekf->rngMea is set in sensor readout already - _ekf->fuseRngData = true; - _ekf->fuseOptFlowData = false; - _ekf->RecallStates(_ekf->statesAtRngTime, (IMUmsec - 100.0f)); - _ekf->OpticalFlowEKF(); - _ekf->fuseRngData = false; - } - } + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + PX4_R(_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); - // 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++) - PX4_R(_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); - } + } else { + /* advertise and publish */ + _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &_att); + } +} - //Publish position estimations - _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 && _gps.fix_type >= 3; - _local_pos.z_valid = true; - _local_pos.v_xy_valid = _gps_initialized && _gps.fix_type >= 3; - _local_pos.v_z_valid = true; - _local_pos.xy_global = _gps_initialized; - - _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); - } +void AttitudePositionEstimatorEKF::publishLocalPosition() +{ + _local_pos.timestamp = _last_sensor_timestamp; + _local_pos.x = _ekf->states[7]; + _local_pos.y = _ekf->states[8]; - //Publish Global Position, but only if it's any good - if(_gps_initialized && _gps.fix_type >= 3 && _gps.eph < _parameters.pos_stddev_threshold * 2.0f) - { - _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_utc_usec = _gps.time_utc_usec; - } - - 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; - } - - /* terrain altitude */ - _global_pos.terrain_alt = _ekf->hgtRef - _ekf->flowStates[1]; - _global_pos.terrain_alt_valid = (_distance_last_valid > 0) && - (hrt_elapsed_time(&_distance_last_valid) < 20 * 1000 * 1000); - - _global_pos.yaw = _local_pos.yaw; - - _global_pos.eph = _gps.eph; - _global_pos.epv = _gps.epv; - - /* 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); - } - } + // XXX need to announce change of Z reference somehow elegantly + _local_pos.z = _ekf->states[9] - _baro_ref_offset; - if (hrt_elapsed_time(&_wind.timestamp) > 99000) { - _wind.timestamp = _global_pos.timestamp; - _wind.windspeed_north = _ekf->states[14]; - _wind.windspeed_east = _ekf->states[15]; - // XXX we need to do something smart about the covariance here - // but we default to the estimate covariance for now - _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); - } - } - } + _local_pos.vx = _ekf->states[4]; + _local_pos.vy = _ekf->states[5]; + _local_pos.vz = _ekf->states[6]; - } + _local_pos.xy_valid = _gps_initialized && _gps.fix_type >= 3; + _local_pos.z_valid = true; + _local_pos.v_xy_valid = _gps_initialized && _gps.fix_type >= 3; + _local_pos.v_z_valid = true; + _local_pos.xy_global = _gps_initialized; + + _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); + } +} + +void AttitudePositionEstimatorEKF::publishGlobalPosition() +{ + _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_utc_usec = _gps.time_utc_usec; + } + 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; + } + + /* terrain altitude */ + _global_pos.terrain_alt = _ekf->hgtRef - _ekf->flowStates[1]; + _global_pos.terrain_alt_valid = (_distance_last_valid > 0) && + (hrt_elapsed_time(&_distance_last_valid) < 20 * 1000 * 1000); + + _global_pos.yaw = _local_pos.yaw; + _global_pos.eph = _gps.eph; + _global_pos.epv = _gps.epv; + + /* 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); + } +} + +void AttitudePositionEstimatorEKF::publishWindEstimate() +{ + _wind.timestamp = _global_pos.timestamp; + _wind.windspeed_north = _ekf->states[14]; + _wind.windspeed_east = _ekf->states[15]; + // XXX we need to do something smart about the covariance here + // but we default to the estimate covariance for now + _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); + } + +} + +void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const bool fuseMag, const bool fuseRangeSensor, + const bool fuseBaro, const bool fuseAirSpeed) +{ + // Run the strapdown INS equations every IMU update + _ekf->UpdateStrapdownEquationsNED(); + + // 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; + _covariancePredictionDt += _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 ((_covariancePredictionDt >= (_ekf->covTimeStepMax - _ekf->dtIMU)) + || (_ekf->summedDelAng.length() > _ekf->covDelAngMax)) { + _ekf->CovariancePrediction(_covariancePredictionDt); + _ekf->summedDelAng.zero(); + _ekf->summedDelVel.zero(); + _covariancePredictionDt = 0.0f; + } + + // Fuse GPS Measurements + if (fuseGPS && _gps_initialized) { + // Convert GPS measurements to Pos NE, hgt and Vel NED + + float gps_dt = (_gps.timestamp_position - _lastGPSTimestamp) / 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; } - perf_end(_loop_perf); + // 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 (!_gps_initialized) { + + // force static mode + _ekf->staticMode = true; + + // 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; } - _task_running = false; + if (fuseBaro) { + // 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; + } - warnx("exiting.\n"); + // Fuse Magnetometer Measurements + if (fuseMag) { + _ekf->fuseMagData = true; + _ekf->RecallStates(_ekf->statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data - _estimator_task = -1; - _exit(0); + _ekf->magstate.obsIndex = 0; + _ekf->FuseMagnetometer(); + _ekf->FuseMagnetometer(); + _ekf->FuseMagnetometer(); + + } + else { + _ekf->fuseMagData = false; + } + + // Fuse Airspeed Measurements + if (fuseAirSpeed && _ekf->VtasMeas > 7.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; + } + + // Fuse Rangefinder Measurements + if (fuseRangeSensor) { + if (_ekf->Tnb.z.z > 0.9f) { + // _ekf->rngMea is set in sensor readout already + _ekf->fuseRngData = true; + _ekf->fuseOptFlowData = false; + _ekf->RecallStates(_ekf->statesAtRngTime, (IMUmsec - 100.0f)); + _ekf->OpticalFlowEKF(); + _ekf->fuseRngData = false; + } + } } int AttitudePositionEstimatorEKF::start() -- cgit v1.2.3