From 05b3cd3a0c2ab465c5ccc2498bc5f44e4922257e Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 3 Jul 2018 17:06:26 -0400 Subject: [PATCH] EKF2 split output predictor and publish attitude immediately --- src/lib/ecl | 2 +- src/modules/ekf2/ekf2_main.cpp | 426 ++++++++++++++++----------------- 2 files changed, 214 insertions(+), 214 deletions(-) diff --git a/src/lib/ecl b/src/lib/ecl index b26c2d62b80d..aaff047e5852 160000 --- a/src/lib/ecl +++ b/src/lib/ecl @@ -1 +1 @@ -Subproject commit b26c2d62b80d6bee148fbfe01eb36809bfa721ea +Subproject commit aaff047e5852761f8d15bc695c7a1ca70beeeeb1 diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 49173bfd173b..ab665aa90eb9 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -73,6 +73,7 @@ #include using math::constrain; +using namespace time_literals; extern "C" __EXPORT int ekf2_main(int argc, char *argv[]); @@ -109,6 +110,7 @@ class Ekf2 final : public ModuleBase, public ModuleParams template void update_mag_bias(Param &mag_bias_param, int axis_index); + bool publish_attitude(const sensor_combined_s &sensors); bool publish_wind_estimate(const hrt_abstime ×tamp); const Vector3f get_vel_body_wind(); @@ -711,6 +713,9 @@ void Ekf2::run() _ekf.setIMUData(now, sensors.gyro_integral_dt, sensors.accelerometer_integral_dt, gyro_integral, accel_integral); + // publish attitude immediately (uses quaternion from output predictor) + publish_attitude(sensors); + // read mag data bool magnetometer_updated = false; orb_check(_magnetometer_sub, &magnetometer_updated); @@ -1032,205 +1037,181 @@ void Ekf2::run() const bool updated = _ekf.update(); perf_end(_perf_ekf_update); - if (updated) { - - // integrate time to monitor time slippage - if (_start_time_us == 0) { - _start_time_us = now; - _last_time_slip_us = 0; - - } else if (_start_time_us > 0) { - _integrated_time_us += sensors.gyro_integral_dt; - _last_time_slip_us = (now - _start_time_us) - _integrated_time_us; - } - - matrix::Quatf q; - _ekf.copy_quaternion(q.data()); - - // In-run bias estimates - float gyro_bias[3]; - _ekf.get_gyro_bias(gyro_bias); - - { - // generate vehicle attitude quaternion data - vehicle_attitude_s att; - att.timestamp = now; - - q.copyTo(att.q); - _ekf.get_quat_reset(&att.delta_q_reset[0], &att.quat_reset_counter); + // integrate time to monitor time slippage + if (_start_time_us == 0) { + _start_time_us = now; + _last_time_slip_us = 0; - att.rollspeed = sensors.gyro_rad[0] - gyro_bias[0]; - att.pitchspeed = sensors.gyro_rad[1] - gyro_bias[1]; - att.yawspeed = sensors.gyro_rad[2] - gyro_bias[2]; + } else if (_start_time_us > 0) { + _integrated_time_us += sensors.gyro_integral_dt; + _last_time_slip_us = (now - _start_time_us) - _integrated_time_us; + } - // publish vehicle attitude data - if (_att_pub == nullptr) { - _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att); + if (updated) { - } else { - orb_publish(ORB_ID(vehicle_attitude), _att_pub, &att); + filter_control_status_u control_status; + _ekf.get_control_mode(&control_status.value); + + // only publish position after successful alignment + if (control_status.flags.tilt_align) { + // generate vehicle local position data + vehicle_local_position_s &lpos = _vehicle_local_position_pub.get(); + + lpos.timestamp = now; + + // Position of body origin in local NED frame + float position[3]; + _ekf.get_position(position); + const float lpos_x_prev = lpos.x; + const float lpos_y_prev = lpos.y; + lpos.x = (_ekf.local_position_is_valid()) ? position[0] : 0.0f; + lpos.y = (_ekf.local_position_is_valid()) ? position[1] : 0.0f; + lpos.z = position[2]; + + // Velocity of body origin in local NED frame (m/s) + float velocity[3]; + _ekf.get_velocity(velocity); + lpos.vx = velocity[0]; + lpos.vy = velocity[1]; + lpos.vz = velocity[2]; + + // vertical position time derivative (m/s) + _ekf.get_pos_d_deriv(&lpos.z_deriv); + + // Acceleration of body origin in local NED frame + float vel_deriv[3]; + _ekf.get_vel_deriv_ned(vel_deriv); + lpos.ax = vel_deriv[0]; + lpos.ay = vel_deriv[1]; + lpos.az = vel_deriv[2]; + + // TODO: better status reporting + lpos.xy_valid = _ekf.local_position_is_valid() && !_preflt_horiz_fail; + lpos.z_valid = !_preflt_vert_fail; + lpos.v_xy_valid = _ekf.local_position_is_valid() && !_preflt_horiz_fail; + lpos.v_z_valid = !_preflt_vert_fail; + + // Position of local NED origin in GPS / WGS84 frame + map_projection_reference_s ekf_origin; + uint64_t origin_time; + + // true if position (x,y,z) has a valid WGS-84 global reference (ref_lat, ref_lon, alt) + 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 } - } - // generate vehicle local position data - vehicle_local_position_s &lpos = _vehicle_local_position_pub.get(); - - lpos.timestamp = now; - - // Position of body origin in local NED frame - float position[3]; - _ekf.get_position(position); - const float lpos_x_prev = lpos.x; - const float lpos_y_prev = lpos.y; - lpos.x = (_ekf.local_position_is_valid()) ? position[0] : 0.0f; - lpos.y = (_ekf.local_position_is_valid()) ? position[1] : 0.0f; - lpos.z = position[2]; - - // Velocity of body origin in local NED frame (m/s) - float velocity[3]; - _ekf.get_velocity(velocity); - lpos.vx = velocity[0]; - lpos.vy = velocity[1]; - lpos.vz = velocity[2]; - - // vertical position time derivative (m/s) - _ekf.get_pos_d_deriv(&lpos.z_deriv); - - // Acceleration of body origin in local NED frame - float vel_deriv[3]; - _ekf.get_vel_deriv_ned(vel_deriv); - lpos.ax = vel_deriv[0]; - lpos.ay = vel_deriv[1]; - lpos.az = vel_deriv[2]; - - // TODO: better status reporting - lpos.xy_valid = _ekf.local_position_is_valid() && !_preflt_horiz_fail; - lpos.z_valid = !_preflt_vert_fail; - lpos.v_xy_valid = _ekf.local_position_is_valid() && !_preflt_horiz_fail; - lpos.v_z_valid = !_preflt_vert_fail; - - // Position of local NED origin in GPS / WGS84 frame - map_projection_reference_s ekf_origin; - uint64_t origin_time; - - // true if position (x,y,z) has a valid WGS-84 global reference (ref_lat, ref_lon, alt) - 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::Quatf q; + _ekf.copy_quaternion(q.data()); + matrix::Eulerf euler(q); + lpos.yaw = euler.psi(); - // The rotation of the tangent plane vs. geographical north - matrix::Eulerf euler(q); - lpos.yaw = euler.psi(); + lpos.dist_bottom_valid = _ekf.get_terrain_valid(); - lpos.dist_bottom_valid = _ekf.get_terrain_valid(); + float terrain_vpos; + _ekf.get_terrain_vert_pos(&terrain_vpos); + lpos.dist_bottom = terrain_vpos - lpos.z; // Distance to bottom surface (ground) in meters - float terrain_vpos; - _ekf.get_terrain_vert_pos(&terrain_vpos); - lpos.dist_bottom = terrain_vpos - lpos.z; // Distance to bottom surface (ground) in meters + // constrain the distance to ground to _rng_gnd_clearance + if (lpos.dist_bottom < _rng_gnd_clearance.get()) { + lpos.dist_bottom = _rng_gnd_clearance.get(); + } - // constrain the distance to ground to _rng_gnd_clearance - if (lpos.dist_bottom < _rng_gnd_clearance.get()) { - lpos.dist_bottom = _rng_gnd_clearance.get(); - } + lpos.dist_bottom_rate = -lpos.vz; // Distance to bottom surface (ground) change rate - lpos.dist_bottom_rate = -lpos.vz; // Distance to bottom surface (ground) change rate + _ekf.get_ekf_lpos_accuracy(&lpos.eph, &lpos.epv); + _ekf.get_ekf_vel_accuracy(&lpos.evh, &lpos.evv); - _ekf.get_ekf_lpos_accuracy(&lpos.eph, &lpos.epv); - _ekf.get_ekf_vel_accuracy(&lpos.evh, &lpos.evv); + // get state reset information of position and velocity + _ekf.get_posD_reset(&lpos.delta_z, &lpos.z_reset_counter); + _ekf.get_velD_reset(&lpos.delta_vz, &lpos.vz_reset_counter); + _ekf.get_posNE_reset(&lpos.delta_xy[0], &lpos.xy_reset_counter); + _ekf.get_velNE_reset(&lpos.delta_vxy[0], &lpos.vxy_reset_counter); - // get state reset information of position and velocity - _ekf.get_posD_reset(&lpos.delta_z, &lpos.z_reset_counter); - _ekf.get_velD_reset(&lpos.delta_vz, &lpos.vz_reset_counter); - _ekf.get_posNE_reset(&lpos.delta_xy[0], &lpos.xy_reset_counter); - _ekf.get_velNE_reset(&lpos.delta_vxy[0], &lpos.vxy_reset_counter); + // get control limit information + _ekf.get_ekf_ctrl_limits(&lpos.vxy_max, &lpos.vz_max, &lpos.hagl_min, &lpos.hagl_max); - // get control limit information - _ekf.get_ekf_ctrl_limits(&lpos.vxy_max, &lpos.vz_max, &lpos.hagl_min, &lpos.hagl_max); + // convert NaN to INFINITY + if (!PX4_ISFINITE(lpos.vxy_max)) { + lpos.vxy_max = INFINITY; + } - // convert NaN to INFINITY - if (!PX4_ISFINITE(lpos.vxy_max)) { - lpos.vxy_max = INFINITY; - } + if (!PX4_ISFINITE(lpos.vz_max)) { + lpos.vz_max = INFINITY; + } - if (!PX4_ISFINITE(lpos.vz_max)) { - lpos.vz_max = INFINITY; - } + if (!PX4_ISFINITE(lpos.hagl_min)) { + lpos.hagl_min = INFINITY; + } - if (!PX4_ISFINITE(lpos.hagl_min)) { - lpos.hagl_min = INFINITY; - } + if (!PX4_ISFINITE(lpos.hagl_max)) { + lpos.hagl_max = INFINITY; + } - if (!PX4_ISFINITE(lpos.hagl_max)) { - lpos.hagl_max = INFINITY; - } + // publish vehicle local position data + _vehicle_local_position_pub.update(); - // publish vehicle local position data - _vehicle_local_position_pub.update(); + if (_ekf.global_position_is_valid() && !_preflt_fail) { + // generate and publish global position data + vehicle_global_position_s &global_pos = _vehicle_global_position_pub.get(); - if (_ekf.global_position_is_valid() && !_preflt_fail) { - // generate and publish global position data - vehicle_global_position_s &global_pos = _vehicle_global_position_pub.get(); + global_pos.timestamp = now; - global_pos.timestamp = now; + 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); + } - 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.lat_lon_reset_counter = lpos.xy_reset_counter; + global_pos.alt = -lpos.z + lpos.ref_alt; // Altitude AMSL in meters - global_pos.alt = -lpos.z + lpos.ref_alt; // Altitude AMSL in meters + // global altitude has opposite sign of local down position + global_pos.delta_alt = -lpos.delta_z; - // global altitude has opposite sign of local down position - global_pos.delta_alt = -lpos.delta_z; + global_pos.vel_n = lpos.vx; // Ground north velocity, m/s + global_pos.vel_e = lpos.vy; // Ground east velocity, m/s + global_pos.vel_d = lpos.vz; // Ground downside velocity, m/s - global_pos.vel_n = lpos.vx; // Ground north velocity, m/s - global_pos.vel_e = lpos.vy; // Ground east velocity, m/s - global_pos.vel_d = lpos.vz; // Ground downside velocity, m/s + global_pos.yaw = lpos.yaw; // Yaw in radians -PI..+PI. - global_pos.yaw = lpos.yaw; // Yaw in radians -PI..+PI. + _ekf.get_ekf_gpos_accuracy(&global_pos.eph, &global_pos.epv); - _ekf.get_ekf_gpos_accuracy(&global_pos.eph, &global_pos.epv); + global_pos.terrain_alt_valid = lpos.dist_bottom_valid; - global_pos.dead_reckoning = _ekf.inertial_dead_reckoning(); + if (global_pos.terrain_alt_valid) { + global_pos.terrain_alt = lpos.ref_alt - terrain_vpos; // Terrain altitude in m, WGS84 - global_pos.terrain_alt_valid = lpos.dist_bottom_valid; + } else { + global_pos.terrain_alt = 0.0f; // Terrain altitude in m, WGS84 + } - if (global_pos.terrain_alt_valid) { - global_pos.terrain_alt = lpos.ref_alt - terrain_vpos; // Terrain altitude in m, WGS84 + global_pos.dead_reckoning = _ekf.inertial_dead_reckoning(); // True if this position is estimated through dead-reckoning - } else { - global_pos.terrain_alt = 0.0f; // Terrain altitude in m, WGS84 + _vehicle_global_position_pub.update(); } - - global_pos.dead_reckoning = _ekf.inertial_dead_reckoning(); // True if this position is estimated through dead-reckoning - - _vehicle_global_position_pub.update(); } { // publish all corrected sensor readings and bias estimates after mag calibration is updated above - float accel_bias[3]; - _ekf.get_accel_bias(accel_bias); - sensor_bias_s bias; bias.timestamp = now; - bias.accel_x = sensors.accelerometer_m_s2[0] - accel_bias[0]; - bias.accel_y = sensors.accelerometer_m_s2[1] - accel_bias[1]; - bias.accel_z = sensors.accelerometer_m_s2[2] - accel_bias[2]; - + // In-run bias estimates + float gyro_bias[3]; + _ekf.get_gyro_bias(gyro_bias); bias.gyro_x_bias = gyro_bias[0]; bias.gyro_y_bias = gyro_bias[1]; bias.gyro_z_bias = gyro_bias[2]; + float accel_bias[3]; + _ekf.get_accel_bias(accel_bias); bias.accel_x_bias = accel_bias[0]; bias.accel_y_bias = accel_bias[1]; bias.accel_z_bias = accel_bias[2]; @@ -1239,6 +1220,11 @@ void Ekf2::run() bias.mag_y_bias = _last_valid_mag_cal[1]; bias.mag_z_bias = _last_valid_mag_cal[2]; + // TODO: remove from sensor_bias? + bias.accel_x = sensors.accelerometer_m_s2[0] - accel_bias[0]; + bias.accel_y = sensors.accelerometer_m_s2[1] - accel_bias[1]; + bias.accel_z = sensors.accelerometer_m_s2[2] - accel_bias[2]; + if (_sensor_bias_pub == nullptr) { _sensor_bias_pub = orb_advertise(ORB_ID(sensor_bias), &bias); @@ -1246,47 +1232,45 @@ void Ekf2::run() orb_publish(ORB_ID(sensor_bias), _sensor_bias_pub, &bias); } } - } - // publish estimator status - estimator_status_s status; - status.timestamp = now; - _ekf.get_state_delayed(status.states); - _ekf.get_covariances(status.covariances); - _ekf.get_gps_check_status(&status.gps_check_fail_flags); - _ekf.get_control_mode(&status.control_mode_flags); - _ekf.get_filter_fault_status(&status.filter_fault_flags); - _ekf.get_innovation_test_status(&status.innovation_check_flags, &status.mag_test_ratio, - &status.vel_test_ratio, &status.pos_test_ratio, - &status.hgt_test_ratio, &status.tas_test_ratio, - &status.hagl_test_ratio, &status.beta_test_ratio); - - status.pos_horiz_accuracy = _vehicle_local_position_pub.get().eph; - status.pos_vert_accuracy = _vehicle_local_position_pub.get().epv; - _ekf.get_ekf_soln_status(&status.solution_status_flags); - _ekf.get_imu_vibe_metrics(status.vibe); - status.time_slip = _last_time_slip_us / 1e6f; - status.nan_flags = 0.0f; // unused - status.health_flags = 0.0f; // unused - status.timeout_flags = 0.0f; // unused - status.pre_flt_fail = _preflt_fail; - - if (_estimator_status_pub == nullptr) { - _estimator_status_pub = orb_advertise(ORB_ID(estimator_status), &status); + // publish estimator status + estimator_status_s status; + status.timestamp = now; + _ekf.get_state_delayed(status.states); + _ekf.get_covariances(status.covariances); + _ekf.get_gps_check_status(&status.gps_check_fail_flags); + status.control_mode_flags = control_status.value; + _ekf.get_filter_fault_status(&status.filter_fault_flags); + _ekf.get_innovation_test_status(&status.innovation_check_flags, &status.mag_test_ratio, + &status.vel_test_ratio, &status.pos_test_ratio, + &status.hgt_test_ratio, &status.tas_test_ratio, + &status.hagl_test_ratio, &status.beta_test_ratio); + + status.pos_horiz_accuracy = _vehicle_local_position_pub.get().eph; + status.pos_vert_accuracy = _vehicle_local_position_pub.get().epv; + _ekf.get_ekf_soln_status(&status.solution_status_flags); + _ekf.get_imu_vibe_metrics(status.vibe); + status.time_slip = _last_time_slip_us / 1e6f; + status.nan_flags = 0.0f; // unused + status.health_flags = 0.0f; // unused + status.timeout_flags = 0.0f; // unused + status.pre_flt_fail = _preflt_fail; + + if (_estimator_status_pub == nullptr) { + _estimator_status_pub = orb_advertise(ORB_ID(estimator_status), &status); - } else { - orb_publish(ORB_ID(estimator_status), _estimator_status_pub, &status); - } + } else { + orb_publish(ORB_ID(estimator_status), _estimator_status_pub, &status); + } - if (updated) { { /* Check and save learned magnetometer bias estimates */ // Check if conditions are OK to for learning of magnetometer bias values if (!vehicle_land_detected.landed && // not on ground (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) && // vehicle is armed - (status.filter_fault_flags == 0) && // there are no filter faults - (status.control_mode_flags & (1 << 5))) { // the EKF is operating in the correct mode + !status.filter_fault_flags && // there are no filter faults + control_status.flags.mag_3D) { // the EKF is operating in the correct mode if (_last_magcal_us == 0) { _last_magcal_us = now; @@ -1307,7 +1291,7 @@ void Ekf2::run() } // Start checking mag bias estimates when we have accumulated sufficient calibration time - if (_total_cal_time_us > 120 * 1000 * 1000ULL) { + if (_total_cal_time_us > 120_s) { // we have sufficient accumulated valid flight time to form a reliable bias estimate // check that the state variance for each axis is within a range indicating filter convergence const float max_var_allowed = 100.0f * _mag_bias_saved_variance.get(); @@ -1390,9 +1374,7 @@ void Ekf2::run() // set the max allowed yaw innovaton depending on whether we are not aiding navigation using // observations in the NE reference frame. - filter_control_status_u _ekf_control_mask; - _ekf.get_control_mode(&_ekf_control_mask.value); - bool doing_ne_aiding = _ekf_control_mask.flags.gps || _ekf_control_mask.flags.ev_pos; + bool doing_ne_aiding = control_status.flags.gps || control_status.flags.ev_pos; float yaw_test_limit; @@ -1446,18 +1428,6 @@ void Ekf2::run() } } - } else if (_replay_mode) { - // in replay mode we have to tell the replay module not to wait for an update - // we do this by publishing an attitude with zero timestamp - vehicle_attitude_s att; - att.timestamp = now; - - if (_att_pub == nullptr) { - _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att); - - } else { - orb_publish(ORB_ID(vehicle_attitude), _att_pub, &att); - } } // publish ekf2_timestamps @@ -1491,10 +1461,44 @@ int Ekf2::getRangeSubIndex(const int *subs) return -1; } -bool Ekf2::publish_wind_estimate(const hrt_abstime ×tamp) +bool Ekf2::publish_attitude(const sensor_combined_s &sensors) { - bool published = false; + if (_ekf.attitude_valid()) { + // generate vehicle attitude quaternion data + vehicle_attitude_s att; + att.timestamp = hrt_absolute_time(); + + const Quatf q{_ekf.calculate_quaternion()}; + q.copyTo(att.q); + + _ekf.get_quat_reset(&att.delta_q_reset[0], &att.quat_reset_counter); + + // In-run bias estimates + float gyro_bias[3]; + _ekf.get_gyro_bias(gyro_bias); + att.rollspeed = sensors.gyro_rad[0] - gyro_bias[0]; + att.pitchspeed = sensors.gyro_rad[1] - gyro_bias[1]; + att.yawspeed = sensors.gyro_rad[2] - gyro_bias[2]; + int instance; + orb_publish_auto(ORB_ID(vehicle_attitude), &_att_pub, &att, &instance, ORB_PRIO_HIGH); + + return true; + + } else if (_replay_mode) { + // in replay mode we have to tell the replay module not to wait for an update + // we do this by publishing an attitude with zero timestamp + vehicle_attitude_s att = {}; + + int instance; + orb_publish_auto(ORB_ID(vehicle_attitude), &_att_pub, &att, &instance, ORB_PRIO_HIGH); + } + + return false; +} + +bool Ekf2::publish_wind_estimate(const hrt_abstime ×tamp) +{ if (_ekf.get_wind_status()) { float velNE_wind[2]; _ekf.get_wind_velocity(velNE_wind); @@ -1510,17 +1514,13 @@ bool Ekf2::publish_wind_estimate(const hrt_abstime ×tamp) wind_estimate.variance_north = wind_var[0]; wind_estimate.variance_east = wind_var[1]; - if (_wind_pub == nullptr) { - _wind_pub = orb_advertise(ORB_ID(wind_estimate), &wind_estimate); - - } else { - orb_publish(ORB_ID(wind_estimate), _wind_pub, &wind_estimate); - } + int instance; + orb_publish_auto(ORB_ID(wind_estimate), &_wind_pub, &wind_estimate, &instance, ORB_PRIO_DEFAULT); - published = true; + return true; } - return published; + return false; } const Vector3f Ekf2::get_vel_body_wind()