diff --git a/msg/vehicle_global_position.msg b/msg/vehicle_global_position.msg index 8a2503d947..e1f4278cde 100644 --- a/msg/vehicle_global_position.msg +++ b/msg/vehicle_global_position.msg @@ -8,7 +8,6 @@ uint64 time_utc_usec # GPS UTC timestamp, (microseconds) float64 lat # Latitude, (degrees) float64 lon # Longitude, (degrees) float32 alt # Altitude AMSL, (meters) -float64[2] delta_lat_lon # Reset delta for horizontal position coordinates float32 delta_alt # Reset delta for altitude uint8 lat_lon_reset_counter # Counter for reset events on horizontal position coordinates uint8 alt_reset_counter # Counter for reset events on altitude diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 9c8f5f7f25..aefb74aa2a 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -41,29 +41,18 @@ #include #include #include -#include #include +#include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include + #include #include #include -#include #include #include +#include #include #include @@ -95,8 +84,7 @@ class Ekf2 : public control::SuperBlock, public ModuleBase { public: Ekf2(); - - ~Ekf2(); + ~Ekf2() override = default; /** @see ModuleBase */ static int task_spawn(int argc, char *argv[]); @@ -120,16 +108,12 @@ public: int print_status() override; private: - static constexpr float _dt_max = 0.02; ///< minimum allowed arrival time between non-IMU sensor readings (sec) - bool _replay_mode; ///< true when we use replay data from a log - int32_t _publish_replay_mode; ///< set to 1 if we should publish replay messages for logging - - float _default_ev_pos_noise = 0.05f; ///< external vision position noise used when an invalid value is supplied (m) - float _default_ev_ang_noise = 0.05f; ///< external vision angle noise used when an invalid value is supplied (rad) + bool _replay_mode = false; ///< true when we use replay data from a log // time slip monitoring - uint64_t integrated_time_us = 0; ///< integral of gyro delta time from start (uSec) - uint64_t start_time_us = 0; ///< system time at EKF start (uSec) + uint64_t _integrated_time_us = 0; ///< integral of gyro delta time from start (uSec) + uint64_t _start_time_us = 0; ///< system time at EKF start (uSec) + uint64_t _last_time_slip_us = 0; ///< Last time slip (uSec) // Initialise time stamps used to send sensor data to the EKF and for logging uint64_t _timestamp_mag_us = 0; ///< magnetomer data timestamp (uSec) @@ -155,31 +139,35 @@ private: // Used to check, save and use learned magnetometer biases hrt_abstime _last_magcal_us = 0; ///< last time the EKF was operating a mode that estimates magnetomer biases (uSec) hrt_abstime _total_cal_time_us = 0; ///< accumulated calibration time since the last save - hrt_abstime _last_time_slip_us = 0; ///< Last time slip (uSec) + float _last_valid_mag_cal[3] = {}; ///< last valid XYZ magnetometer bias estimates (mGauss) bool _valid_cal_available[3] = {}; ///< true when an unsaved valid calibration for the XYZ magnetometer bias is available float _last_valid_variance[3] = {}; ///< variances for the last valid magnetometer XYZ bias estimates (mGauss**2) // Used to filter velocity innovations during pre-flight checks + bool _vel_innov_preflt_fail = false; ///< true if the norm of the filtered innovation vector is too large before flight Vector3f _vel_innov_lpf_ned = {}; ///< Preflight low pass filtered velocity innovations (m/sec) float _hgt_innov_lpf = 0.0f; ///< Preflight low pass filtered height innovation (m) - const float _innov_lpf_tau_inv = 0.2f; ///< Preflight low pass filter time constant inverse (1/sec) - const float _vel_innov_test_lim = 0.5f; ///< Maximum permissible velocity innovation to pass pre-flight checks (m/sec) - const float _hgt_innov_test_lim = 1.5f; ///< Maximum permissible height innovation to pass pre-flight checks (m) + + static constexpr float _innov_lpf_tau_inv = 0.2f; ///< Preflight low pass filter time constant inverse (1/sec) + static constexpr float _vel_innov_test_lim = + 0.5f; ///< Maximum permissible velocity innovation to pass pre-flight checks (m/sec) + static constexpr float _hgt_innov_test_lim = + 1.5f; ///< Maximum permissible height innovation to pass pre-flight checks (m) const float _vel_innov_spike_lim = 2.0f * _vel_innov_test_lim; ///< preflight velocity innovation spike limit (m/sec) const float _hgt_innov_spike_lim = 2.0f * _hgt_innov_test_lim; ///< preflight position innovation spike limit (m) - bool _vel_innov_preflt_fail = false; ///< true if the norm of the filtered innovation vector is too large before flight orb_advert_t _att_pub; - orb_advert_t _lpos_pub; orb_advert_t _control_state_pub; - orb_advert_t _vehicle_global_position_pub; orb_advert_t _wind_pub; orb_advert_t _estimator_status_pub; orb_advert_t _estimator_innovations_pub; orb_advert_t _replay_pub; orb_advert_t _ekf2_timestamps_pub; + uORB::Publication _vehicle_local_position_pub; + uORB::Publication _vehicle_global_position_pub; + /* Low pass filter for attitude rates */ math::LowPassFilter2p _lp_roll_rate; ///< Low pass filter applied to roll rates published on the control_state message math::LowPassFilter2p _lp_pitch_rate; ///< Low pass filter applied to pitch rates published on the control_state message @@ -246,7 +234,8 @@ private: control::BlockParamExtFloat _requiredGDoP; ///< maximum acceptable geometric dilution of precision control::BlockParamExtFloat _requiredHdrift; ///< maximum acceptable horizontal drift speed (m/s) control::BlockParamExtFloat _requiredVdrift; ///< maximum acceptable vertical drift speed (m/s) - control::BlockParamExtInt _param_record_replay_msg; ///< turns on recording of ekf2 replay messages + + control::BlockParamInt _param_record_replay_msg; ///< turns on recording of ekf2 replay messages // measurement source control control::BlockParamExtInt @@ -267,8 +256,8 @@ private: _rng_aid_innov_gate; ///< gate size used for innovation consistency checks for range aid fusion (STD) // vision estimate fusion - control::BlockParamExtFloat _ev_pos_noise; ///< default position observation noise for exernal vision measurements (m) - control::BlockParamExtFloat _ev_ang_noise; ///< default angular observation noise for exernal vision measurements (rad) + control::BlockParamFloat _ev_pos_noise; ///< default position observation noise for exernal vision measurements (m) + control::BlockParamFloat _ev_ang_noise; ///< default angular observation noise for exernal vision measurements (rad) control::BlockParamExtFloat _ev_innov_gate; ///< external vision position innovation consistency gate size (STD) // optical flow fusion @@ -345,16 +334,15 @@ private: Ekf2::Ekf2(): SuperBlock(nullptr, "EKF"), _replay_mode(false), - _publish_replay_mode(0), _att_pub(nullptr), - _lpos_pub(nullptr), _control_state_pub(nullptr), - _vehicle_global_position_pub(nullptr), _wind_pub(nullptr), _estimator_status_pub(nullptr), _estimator_innovations_pub(nullptr), _replay_pub(nullptr), _ekf2_timestamps_pub(nullptr), + _vehicle_local_position_pub(ORB_ID(vehicle_local_position), -1, &getPublications()), + _vehicle_global_position_pub(ORB_ID(vehicle_global_position), -1, &getPublications()), _lp_roll_rate(250.0f, 30.0f), _lp_pitch_rate(250.0f, 30.0f), _lp_yaw_rate(250.0f, 20.0f), @@ -403,7 +391,7 @@ Ekf2::Ekf2(): _requiredGDoP(this, "EKF2_REQ_GDOP", false, _params->req_gdop), _requiredHdrift(this, "EKF2_REQ_HDRIFT", false, _params->req_hdrift), _requiredVdrift(this, "EKF2_REQ_VDRIFT", false, _params->req_vdrift), - _param_record_replay_msg(this, "EKF2_REC_RPL", false, _publish_replay_mode), + _param_record_replay_msg(this, "EKF2_REC_RPL", false), _fusion_mode(this, "EKF2_AID_MASK", false, _params->fusion_mode), _vdist_sensor_type(this, "EKF2_HGT_MODE", false, _params->vdist_sensor_type), _range_noise(this, "EKF2_RNG_NOISE", false, _params->range_noise), @@ -415,8 +403,8 @@ Ekf2::Ekf2(): _rng_aid_hor_vel_max(this, "EKF2_RNG_A_VMAX", false, _params->max_vel_for_range_aid), _rng_aid_height_max(this, "EKF2_RNG_A_HMAX", false, _params->max_hagl_for_range_aid), _rng_aid_innov_gate(this, "EKF2_RNG_A_IGATE", false, _params->range_aid_innov_gate), - _ev_pos_noise(this, "EKF2_EVP_NOISE", false, _default_ev_pos_noise), - _ev_ang_noise(this, "EKF2_EVA_NOISE", false, _default_ev_ang_noise), + _ev_pos_noise(this, "EKF2_EVP_NOISE", false), + _ev_ang_noise(this, "EKF2_EVA_NOISE", false), _ev_innov_gate(this, "EKF2_EV_GATE", false, _params->ev_innov_gate), _flow_noise(this, "EKF2_OF_N_MIN", false, _params->flow_noise), _flow_noise_qual_min(this, "EKF2_OF_N_MAX", false, _params->flow_noise_qual_min), @@ -461,12 +449,6 @@ Ekf2::Ekf2(): _K_pstatic_coef_y(this, "EKF2_PCOEF_Y", false), _K_pstatic_coef_z(this, "EKF2_PCOEF_Z", false) { - -} - -Ekf2::~Ekf2() -{ - } int Ekf2::print_status() @@ -546,6 +528,8 @@ void Ekf2::run() bool gps_updated = false; bool airspeed_updated = false; + bool baro_updated = false; + bool sensor_selection_updated = false; bool optical_flow_updated = false; bool range_finder_updated = false; bool vehicle_land_detected_updated = false; @@ -574,6 +558,18 @@ void Ekf2::run() orb_copy(ORB_ID(airspeed), airspeed_sub, &airspeed); } + orb_check(sensor_baro_sub, &baro_updated); + + if (baro_updated) { + orb_copy(ORB_ID(sensor_baro), sensor_baro_sub, &sensor_baro); + } + + orb_check(sensor_selection_sub, &sensor_selection_updated); + + if (sensor_selection_updated) { + orb_copy(ORB_ID(sensor_selection), sensor_selection_sub, &sensor_selection); + } + orb_check(optical_flow_sub, &optical_flow_updated); if (optical_flow_updated) { @@ -619,13 +615,14 @@ void Ekf2::run() gyro_integral[0] = sensors.gyro_rad[0] * gyro_dt; gyro_integral[1] = sensors.gyro_rad[1] * gyro_dt; gyro_integral[2] = sensors.gyro_rad[2] * gyro_dt; + float accel_integral[3]; float accel_dt = sensors.accelerometer_integral_dt / 1.e6f; accel_integral[0] = sensors.accelerometer_m_s2[0] * accel_dt; accel_integral[1] = sensors.accelerometer_m_s2[1] * accel_dt; accel_integral[2] = sensors.accelerometer_m_s2[2] * accel_dt; - _ekf.setIMUData(now, sensors.gyro_integral_dt, sensors.accelerometer_integral_dt, - gyro_integral, accel_integral); + + _ekf.setIMUData(now, sensors.gyro_integral_dt, sensors.accelerometer_integral_dt, gyro_integral, accel_integral); // read mag data if (sensors.magnetometer_timestamp_relative == sensor_combined_s::RELATIVE_TIMESTAMP_INVALID) { @@ -640,8 +637,6 @@ void Ekf2::run() // Do not reset parmameters when armed to prevent potential time slips casued by parameter set // and notification events // Check if there has been a persistant change in magnetometer ID - orb_copy(ORB_ID(sensor_selection), sensor_selection_sub, &sensor_selection); - if (sensor_selection.mag_device_id != 0 && sensor_selection.mag_device_id != _mag_bias_id.get()) { if (_invalid_mag_id_count < 200) { _invalid_mag_id_count++; @@ -718,7 +713,6 @@ void Ekf2::run() float balt_data_avg = _balt_data_sum / (float)_balt_sample_count; // estimate air density assuming typical 20degC ambient temperature - orb_copy(ORB_ID(sensor_baro), sensor_baro_sub, &sensor_baro); const float pressure_to_density = 100.0f / (CONSTANTS_AIR_GAS_CONST * (20.0f - CONSTANTS_ABSOLUTE_NULL_CELSIUS)); float rho = pressure_to_density * sensor_baro.pressure; _ekf.set_air_density(rho); @@ -741,7 +735,7 @@ void Ekf2::run() _K_pstatic_coef_z.get() * fminf(_vel_body_wind(2) * _vel_body_wind(2), max_airspeed_sq)); // correct baro measurement using pressure error estimate and assuming sea level gravity - balt_data_avg += pstatic_err / (rho * 9.80665f); + balt_data_avg += pstatic_err / (rho * CONSTANTS_ONE_G); // push to estimator _ekf.setBaroData(1000 * (uint64_t)balt_time_ms, balt_data_avg); @@ -756,7 +750,7 @@ void Ekf2::run() // read gps data if available if (gps_updated) { - struct gps_message gps_msg = {}; + struct gps_message gps_msg; gps_msg.time_usec = gps.timestamp; gps_msg.lat = gps.lat; gps_msg.lon = gps.lon; @@ -780,19 +774,22 @@ void Ekf2::run() // only set airspeed data if condition for airspeed fusion are met bool fuse_airspeed = airspeed_updated && !vehicle_status.is_rotary_wing - && _arspFusionThreshold.get() <= airspeed.true_airspeed_m_s && _arspFusionThreshold.get() >= 0.1f; + && (_arspFusionThreshold.get() > FLT_EPSILON) + && (airspeed.true_airspeed_m_s > _arspFusionThreshold.get()); if (fuse_airspeed) { float eas2tas = airspeed.true_airspeed_m_s / airspeed.indicated_airspeed_m_s; _ekf.setAirspeedData(airspeed.timestamp, airspeed.true_airspeed_m_s, eas2tas); } - // only fuse synthetic sideslip measurements if conditions are met - bool fuse_beta = !vehicle_status.is_rotary_wing && _fuseBeta.get(); - _ekf.set_fuse_beta_flag(fuse_beta); + if (vehicle_status_updated) { + // only fuse synthetic sideslip measurements if conditions are met + bool fuse_beta = !vehicle_status.is_rotary_wing && (_fuseBeta.get() == 1); + _ekf.set_fuse_beta_flag(fuse_beta); - // let the EKF know if the vehicle motion is that of a fixed wing (forward flight only relative to wind) - _ekf.set_is_fixed_wing(!vehicle_status.is_rotary_wing); + // let the EKF know if the vehicle motion is that of a fixed wing (forward flight only relative to wind) + _ekf.set_is_fixed_wing(!vehicle_status.is_rotary_wing); + } if (optical_flow_updated) { flow_message flow; @@ -825,8 +822,8 @@ void Ekf2::run() ev_data.quat = q; // position measurement error from parameters. TODO : use covariances from topic - ev_data.posErr = _default_ev_pos_noise; - ev_data.angErr = _default_ev_ang_noise; + ev_data.posErr = _ev_pos_noise.get(); + ev_data.angErr = _ev_ang_noise.get(); // use timestamp from external computer, clocks are synchronized when using MAVROS _ekf.setExtVisionData(vision_position_updated ? ev_pos.timestamp : ev_att.timestamp, &ev_data); @@ -843,11 +840,11 @@ void Ekf2::run() if (_ekf.update()) { // integrate time to monitor time slippage - if (start_time_us == 0) { - start_time_us = now; + if (_start_time_us == 0) { + _start_time_us = now; - } else if (start_time_us > 0) { - integrated_time_us += sensors.gyro_integral_dt; + } else if (_start_time_us > 0) { + _integrated_time_us += sensors.gyro_integral_dt; } matrix::Quaternion q; @@ -989,13 +986,16 @@ void Ekf2::run() } // generate vehicle local position data - struct vehicle_local_position_s lpos = {}; + vehicle_local_position_s &lpos = _vehicle_local_position_pub.get(); + float pos[3] = {}; lpos.timestamp = now; // Position of body origin in local NED frame _ekf.get_position(pos); + const float lpos_x_prev = lpos.x; + const float lpos_y_prev = lpos.y; lpos.x = (_ekf.local_position_is_valid()) ? pos[0] : 0.0f; lpos.y = (_ekf.local_position_is_valid()) ? pos[1] : 0.0f; lpos.z = pos[2]; @@ -1013,11 +1013,19 @@ void Ekf2::run() lpos.v_z_valid = !_vel_innov_preflt_fail; // Position of local NED origin in GPS / WGS84 frame - struct map_projection_reference_s ekf_origin = {}; + map_projection_reference_s ekf_origin = {}; + uint64_t origin_time = 0; + // true if position (x,y,z) has a valid WGS-84 global reference (ref_lat, ref_lon, alt) - lpos.xy_global = lpos.z_global = _ekf.get_ekf_origin(&lpos.ref_timestamp, &ekf_origin, &lpos.ref_alt); - lpos.ref_lat = ekf_origin.lat_rad * 180.0 / M_PI; // Reference point latitude in degrees - lpos.ref_lon = ekf_origin.lon_rad * 180.0 / M_PI; // Reference point longitude in degrees + const bool ekf_origin_valid = _ekf.get_ekf_origin(&origin_time, &ekf_origin, &lpos.ref_alt); + lpos.xy_global = ekf_origin_valid; + lpos.z_global = ekf_origin_valid; + + if (ekf_origin_valid && (origin_time > lpos.ref_timestamp)) { + lpos.ref_timestamp = origin_time; + lpos.ref_lat = ekf_origin.lat_rad * 180.0 / M_PI; // Reference point latitude in degrees + lpos.ref_lon = ekf_origin.lon_rad * 180.0 / M_PI; // Reference point longitude in degrees + } // The rotation of the tangent plane vs. geographical north matrix::Eulerf euler(q); @@ -1047,28 +1055,19 @@ void Ekf2::run() _ekf.get_velNE_reset(&lpos.delta_vxy[0], &lpos.vxy_reset_counter); // publish vehicle local position data - if (_lpos_pub == nullptr) { - _lpos_pub = orb_advertise(ORB_ID(vehicle_local_position), &lpos); - - } else { - orb_publish(ORB_ID(vehicle_local_position), _lpos_pub, &lpos); - } + _vehicle_local_position_pub.update(); if (_ekf.global_position_is_valid() && !_vel_innov_preflt_fail) { // generate and publish global position data - struct vehicle_global_position_s global_pos = {}; + vehicle_global_position_s &global_pos = _vehicle_global_position_pub.get(); global_pos.timestamp = now; global_pos.time_utc_usec = gps.time_utc_usec; // GPS UTC timestamp in microseconds - double est_lat, est_lon, lat_pre_reset, lon_pre_reset; - map_projection_reproject(&ekf_origin, lpos.x, lpos.y, &est_lat, &est_lon); - global_pos.lat = est_lat; // Latitude in degrees - global_pos.lon = est_lon; // Longitude in degrees - map_projection_reproject(&ekf_origin, lpos.x - lpos.delta_xy[0], lpos.y - lpos.delta_xy[1], &lat_pre_reset, - &lon_pre_reset); - global_pos.delta_lat_lon[0] = est_lat - lat_pre_reset; - global_pos.delta_lat_lon[1] = est_lon - lon_pre_reset; + if (fabsf(lpos_x_prev - lpos.x) > FLT_EPSILON || fabsf(lpos_y_prev - lpos.y) > FLT_EPSILON) { + map_projection_reproject(&ekf_origin, lpos.x, lpos.y, &global_pos.lat, &global_pos.lon); + } + global_pos.lat_lon_reset_counter = lpos.xy_reset_counter; global_pos.alt = -pos[2] + lpos.ref_alt; // Altitude AMSL in meters @@ -1100,17 +1099,12 @@ void Ekf2::run() global_pos.pressure_alt = sensors.baro_alt_meter; // Pressure altitude AMSL (m) - if (_vehicle_global_position_pub == nullptr) { - _vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos); - - } else { - orb_publish(ORB_ID(vehicle_global_position), _vehicle_global_position_pub, &global_pos); - } + _vehicle_global_position_pub.update(); } // publish estimator status { - struct estimator_status_s status = {}; + estimator_status_s status; status.timestamp = now; _ekf.get_state_delayed(status.states); _ekf.get_covariances(status.covariances); @@ -1128,9 +1122,9 @@ void Ekf2::run() _ekf.get_imu_vibe_metrics(status.vibe); // monitor time slippage - if (start_time_us != 0 && now > start_time_us) { - status.time_slip = (float)(1e-6 * ((double)(now - start_time_us) - (double) integrated_time_us)); - _last_time_slip_us = (now - start_time_us) - integrated_time_us; + if (_start_time_us != 0 && now > _start_time_us) { + status.time_slip = (float)(1e-6 * ((double)(now - _start_time_us) - (double) _integrated_time_us)); + _last_time_slip_us = (now - _start_time_us) - _integrated_time_us; } else { status.time_slip = 0.0f; @@ -1218,7 +1212,7 @@ void Ekf2::run() } // Publish wind estimate - struct wind_estimate_s wind_estimate = {}; + wind_estimate_s wind_estimate; wind_estimate.timestamp = now; wind_estimate.windspeed_north = status.states[22]; wind_estimate.windspeed_east = status.states[23]; @@ -1235,7 +1229,7 @@ void Ekf2::run() // publish estimator innovation data { - struct ekf2_innovations_s innovations = {}; + ekf2_innovations_s innovations; innovations.timestamp = now; _ekf.get_vel_pos_innov(&innovations.vel_pos_innov[0]); _ekf.get_mag_innov(&innovations.mag_innov[0]); @@ -1362,11 +1356,8 @@ void Ekf2::run() } } - // publish replay message if in replay mode - bool publish_replay_message = (bool)_param_record_replay_msg.get(); - - if (publish_replay_message) { + if (_param_record_replay_msg.get() == 1) { struct ekf2_replay_s replay = {}; replay.timestamp = now; replay.gyro_integral_dt = sensors.gyro_integral_dt; @@ -1439,9 +1430,9 @@ void Ekf2::run() replay.quat_ev[1] = ev_att.q[1]; replay.quat_ev[2] = ev_att.q[2]; replay.quat_ev[3] = ev_att.q[3]; - // TODO : switch to covariances from topic later - replay.pos_err = _default_ev_pos_noise; - replay.ang_err = _default_ev_ang_noise; + // TODO: switch to covariances from topic later + replay.pos_err = _ev_pos_noise.get(); + replay.ang_err = _ev_ang_noise.get(); } else { replay.ev_timestamp = 0; @@ -1463,9 +1454,11 @@ void Ekf2::run() orb_unsubscribe(optical_flow_sub); orb_unsubscribe(range_finder_sub); orb_unsubscribe(ev_pos_sub); + orb_unsubscribe(ev_att_sub); orb_unsubscribe(vehicle_land_detected_sub); orb_unsubscribe(status_sub); orb_unsubscribe(sensor_selection_sub); + orb_unsubscribe(sensor_baro_sub); } Ekf2 *Ekf2::instantiate(int argc, char *argv[]) @@ -1517,7 +1510,7 @@ int Ekf2::task_spawn(int argc, char *argv[]) _task_id = px4_task_spawn_cmd("ekf2", SCHED_DEFAULT, SCHED_PRIORITY_ESTIMATOR, - 5900, + 5700, (px4_main_t)&run_trampoline, (char *const *)argv);