diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index ea4c6610ba..539851874c 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -295,8 +295,8 @@ void EKF2::Run() imu_dt = imu.delta_angle_dt; - _estimator_status_pub.get().accel_device_id = imu.accel_device_id; - _estimator_status_pub.get().gyro_device_id = imu.gyro_device_id; + _device_id_accel = imu.accel_device_id; + _device_id_gyro = imu.gyro_device_id; } else { sensor_combined_s sensor_combined; @@ -370,8 +370,8 @@ void EKF2::Run() } } - _estimator_status_pub.get().accel_device_id = _sensor_selection.accel_device_id; - _estimator_status_pub.get().gyro_device_id = _sensor_selection.gyro_device_id; + _device_id_accel = _sensor_selection.accel_device_id; + _device_id_gyro = _sensor_selection.gyro_device_id; } } @@ -408,7 +408,7 @@ void EKF2::Run() } } - _estimator_status_pub.get().mag_device_id = magnetometer.device_id; + _device_id_mag = magnetometer.device_id; if ((_vehicle_status.arming_state != vehicle_status_s::ARMING_STATE_ARMED) && (_invalid_mag_id_count > 100)) { // the sensor ID used for the last saved mag bias is not confirmed to be the same as the current sensor ID @@ -452,7 +452,7 @@ void EKF2::Run() ekf2_timestamps.vehicle_air_data_timestamp_rel = (int16_t)((int64_t)airdata.timestamp / 100 - (int64_t)ekf2_timestamps.timestamp / 100); - _estimator_status_pub.get().baro_device_id = airdata.baro_device_id; + _device_id_baro = airdata.baro_device_id; } } @@ -490,37 +490,37 @@ void EKF2::Run() } } - if (_optical_flow_sub.updated()) { - _new_optical_flow_data_received = true; - optical_flow_s optical_flow; + bool new_optical_flow_data_received = false; + optical_flow_s optical_flow; - if (_optical_flow_sub.copy(&optical_flow)) { - flowSample flow {}; - // NOTE: the EKF uses the reverse sign convention to the flow sensor. EKF assumes positive LOS rate - // is produced by a RH rotation of the image about the sensor axis. - flow.flow_xy_rad(0) = -optical_flow.pixel_flow_x_integral; - flow.flow_xy_rad(1) = -optical_flow.pixel_flow_y_integral; - flow.gyro_xyz(0) = -optical_flow.gyro_x_rate_integral; - flow.gyro_xyz(1) = -optical_flow.gyro_y_rate_integral; - flow.gyro_xyz(2) = -optical_flow.gyro_z_rate_integral; - flow.quality = optical_flow.quality; - flow.dt = 1e-6f * (float)optical_flow.integration_timespan; - flow.time_us = optical_flow.timestamp; + if (_optical_flow_sub.update(&optical_flow)) { + flowSample flow {}; + // NOTE: the EKF uses the reverse sign convention to the flow sensor. EKF assumes positive LOS rate + // is produced by a RH rotation of the image about the sensor axis. + flow.flow_xy_rad(0) = -optical_flow.pixel_flow_x_integral; + flow.flow_xy_rad(1) = -optical_flow.pixel_flow_y_integral; + flow.gyro_xyz(0) = -optical_flow.gyro_x_rate_integral; + flow.gyro_xyz(1) = -optical_flow.gyro_y_rate_integral; + flow.gyro_xyz(2) = -optical_flow.gyro_z_rate_integral; + flow.quality = optical_flow.quality; + flow.dt = 1e-6f * (float)optical_flow.integration_timespan; + flow.time_us = optical_flow.timestamp; - if (PX4_ISFINITE(optical_flow.pixel_flow_y_integral) && - PX4_ISFINITE(optical_flow.pixel_flow_x_integral) && - flow.dt < 1) { + if (PX4_ISFINITE(optical_flow.pixel_flow_y_integral) && + PX4_ISFINITE(optical_flow.pixel_flow_x_integral) && + flow.dt < 1) { - _ekf.setOpticalFlowData(flow); - } + _ekf.setOpticalFlowData(flow); - // Save sensor limits reported by the optical flow sensor - _ekf.set_optical_flow_limits(optical_flow.max_flow_rate, optical_flow.min_ground_distance, - optical_flow.max_ground_distance); - - ekf2_timestamps.optical_flow_timestamp_rel = (int16_t)((int64_t)optical_flow.timestamp / 100 - - (int64_t)ekf2_timestamps.timestamp / 100); + new_optical_flow_data_received = true; } + + // Save sensor limits reported by the optical flow sensor + _ekf.set_optical_flow_limits(optical_flow.max_flow_rate, optical_flow.min_ground_distance, + optical_flow.max_ground_distance); + + ekf2_timestamps.optical_flow_timestamp_rel = (int16_t)((int64_t)optical_flow.timestamp / 100 - + (int64_t)ekf2_timestamps.timestamp / 100); } if (_range_finder_sub_index >= 0) { @@ -548,24 +548,21 @@ void EKF2::Run() } // get external vision data - // if error estimates are unavailable, use parameter defined defaults - new_ev_data_received = false; + bool new_ev_data_received = false; + vehicle_odometry_s ev_odom; - if (_ev_odom_sub.updated()) { - new_ev_data_received = true; + if (_ev_odom_sub.update(&ev_odom)) { + extVisionSample ev_data{}; - // copy both attitude & position, we need both to fill a single extVisionSample - _ev_odom_sub.copy(&_ev_odom); - - extVisionSample ev_data {}; + // if error estimates are unavailable, use parameter defined defaults // check for valid velocity data - if (PX4_ISFINITE(_ev_odom.vx) && PX4_ISFINITE(_ev_odom.vy) && PX4_ISFINITE(_ev_odom.vz)) { - ev_data.vel(0) = _ev_odom.vx; - ev_data.vel(1) = _ev_odom.vy; - ev_data.vel(2) = _ev_odom.vz; + if (PX4_ISFINITE(ev_odom.vx) && PX4_ISFINITE(ev_odom.vy) && PX4_ISFINITE(ev_odom.vz)) { + ev_data.vel(0) = ev_odom.vx; + ev_data.vel(1) = ev_odom.vy; + ev_data.vel(2) = ev_odom.vz; - if (_ev_odom.velocity_frame == vehicle_odometry_s::BODY_FRAME_FRD) { + if (ev_odom.velocity_frame == vehicle_odometry_s::BODY_FRAME_FRD) { ev_data.vel_frame = estimator::BODY_FRAME_FRD; } else { @@ -575,15 +572,15 @@ void EKF2::Run() // velocity measurement error from ev_data or parameters float param_evv_noise_var = sq(_param_ekf2_evv_noise.get()); - if (!_param_ekf2_ev_noise_md.get() && PX4_ISFINITE(_ev_odom.velocity_covariance[_ev_odom.COVARIANCE_MATRIX_VX_VARIANCE]) - && PX4_ISFINITE(_ev_odom.velocity_covariance[_ev_odom.COVARIANCE_MATRIX_VY_VARIANCE]) - && PX4_ISFINITE(_ev_odom.velocity_covariance[_ev_odom.COVARIANCE_MATRIX_VZ_VARIANCE])) { - ev_data.velCov(0, 0) = _ev_odom.velocity_covariance[_ev_odom.COVARIANCE_MATRIX_VX_VARIANCE]; - ev_data.velCov(0, 1) = ev_data.velCov(1, 0) = _ev_odom.velocity_covariance[1]; - ev_data.velCov(0, 2) = ev_data.velCov(2, 0) = _ev_odom.velocity_covariance[2]; - ev_data.velCov(1, 1) = _ev_odom.velocity_covariance[_ev_odom.COVARIANCE_MATRIX_VY_VARIANCE]; - ev_data.velCov(1, 2) = ev_data.velCov(2, 1) = _ev_odom.velocity_covariance[7]; - ev_data.velCov(2, 2) = _ev_odom.velocity_covariance[_ev_odom.COVARIANCE_MATRIX_VZ_VARIANCE]; + if (!_param_ekf2_ev_noise_md.get() && PX4_ISFINITE(ev_odom.velocity_covariance[ev_odom.COVARIANCE_MATRIX_VX_VARIANCE]) + && PX4_ISFINITE(ev_odom.velocity_covariance[ev_odom.COVARIANCE_MATRIX_VY_VARIANCE]) + && PX4_ISFINITE(ev_odom.velocity_covariance[ev_odom.COVARIANCE_MATRIX_VZ_VARIANCE])) { + ev_data.velCov(0, 0) = ev_odom.velocity_covariance[ev_odom.COVARIANCE_MATRIX_VX_VARIANCE]; + ev_data.velCov(0, 1) = ev_data.velCov(1, 0) = ev_odom.velocity_covariance[1]; + ev_data.velCov(0, 2) = ev_data.velCov(2, 0) = ev_odom.velocity_covariance[2]; + ev_data.velCov(1, 1) = ev_odom.velocity_covariance[ev_odom.COVARIANCE_MATRIX_VY_VARIANCE]; + ev_data.velCov(1, 2) = ev_data.velCov(2, 1) = ev_odom.velocity_covariance[7]; + ev_data.velCov(2, 2) = ev_odom.velocity_covariance[ev_odom.COVARIANCE_MATRIX_VZ_VARIANCE]; } else { ev_data.velCov = matrix::eye() * param_evv_noise_var; @@ -591,20 +588,20 @@ void EKF2::Run() } // check for valid position data - if (PX4_ISFINITE(_ev_odom.x) && PX4_ISFINITE(_ev_odom.y) && PX4_ISFINITE(_ev_odom.z)) { - ev_data.pos(0) = _ev_odom.x; - ev_data.pos(1) = _ev_odom.y; - ev_data.pos(2) = _ev_odom.z; + if (PX4_ISFINITE(ev_odom.x) && PX4_ISFINITE(ev_odom.y) && PX4_ISFINITE(ev_odom.z)) { + ev_data.pos(0) = ev_odom.x; + ev_data.pos(1) = ev_odom.y; + ev_data.pos(2) = ev_odom.z; float param_evp_noise_var = sq(_param_ekf2_evp_noise.get()); // position measurement error from ev_data or parameters - if (!_param_ekf2_ev_noise_md.get() && PX4_ISFINITE(_ev_odom.pose_covariance[_ev_odom.COVARIANCE_MATRIX_X_VARIANCE]) - && PX4_ISFINITE(_ev_odom.pose_covariance[_ev_odom.COVARIANCE_MATRIX_Y_VARIANCE]) - && PX4_ISFINITE(_ev_odom.pose_covariance[_ev_odom.COVARIANCE_MATRIX_Z_VARIANCE])) { - ev_data.posVar(0) = fmaxf(param_evp_noise_var, _ev_odom.pose_covariance[_ev_odom.COVARIANCE_MATRIX_X_VARIANCE]); - ev_data.posVar(1) = fmaxf(param_evp_noise_var, _ev_odom.pose_covariance[_ev_odom.COVARIANCE_MATRIX_Y_VARIANCE]); - ev_data.posVar(2) = fmaxf(param_evp_noise_var, _ev_odom.pose_covariance[_ev_odom.COVARIANCE_MATRIX_Z_VARIANCE]); + if (!_param_ekf2_ev_noise_md.get() && PX4_ISFINITE(ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_X_VARIANCE]) + && PX4_ISFINITE(ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_Y_VARIANCE]) + && PX4_ISFINITE(ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_Z_VARIANCE])) { + ev_data.posVar(0) = fmaxf(param_evp_noise_var, ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_X_VARIANCE]); + ev_data.posVar(1) = fmaxf(param_evp_noise_var, ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_Y_VARIANCE]); + ev_data.posVar(2) = fmaxf(param_evp_noise_var, ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_Z_VARIANCE]); } else { ev_data.posVar.setAll(param_evp_noise_var); @@ -612,14 +609,14 @@ void EKF2::Run() } // check for valid orientation data - if (PX4_ISFINITE(_ev_odom.q[0])) { - ev_data.quat = matrix::Quatf(_ev_odom.q); + if (PX4_ISFINITE(ev_odom.q[0])) { + ev_data.quat = matrix::Quatf(ev_odom.q); // orientation measurement error from ev_data or parameters float param_eva_noise_var = sq(_param_ekf2_eva_noise.get()); - if (!_param_ekf2_ev_noise_md.get() && PX4_ISFINITE(_ev_odom.pose_covariance[_ev_odom.COVARIANCE_MATRIX_YAW_VARIANCE])) { - ev_data.angVar = fmaxf(param_eva_noise_var, _ev_odom.pose_covariance[_ev_odom.COVARIANCE_MATRIX_YAW_VARIANCE]); + if (!_param_ekf2_ev_noise_md.get() && PX4_ISFINITE(ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_YAW_VARIANCE])) { + ev_data.angVar = fmaxf(param_eva_noise_var, ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_YAW_VARIANCE]); } else { ev_data.angVar = param_eva_noise_var; @@ -627,11 +624,13 @@ void EKF2::Run() } // use timestamp from external computer, clocks are synchronized when using MAVROS - ev_data.time_us = _ev_odom.timestamp_sample; + ev_data.time_us = ev_odom.timestamp_sample; _ekf.setExtVisionData(ev_data); - ekf2_timestamps.visual_odometry_timestamp_rel = (int16_t)((int64_t)_ev_odom.timestamp / 100 - + ekf2_timestamps.visual_odometry_timestamp_rel = (int16_t)((int64_t)ev_odom.timestamp / 100 - (int64_t)ekf2_timestamps.timestamp / 100); + + new_ev_data_received = true; } bool vehicle_land_detected_updated = _vehicle_land_detected_sub.updated(); @@ -676,19 +675,19 @@ void EKF2::Run() if (ekf_updated) { + vehicle_local_position_s lpos{}; + _ekf.get_ekf_lpos_accuracy(&lpos.eph, &lpos.epv); + 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 = _local_position_pub.get(); lpos.timestamp_sample = imu_sample_new.time_us; // Position of body origin in local NED frame const Vector3f position = _ekf.getPosition(); - const float lpos_x_prev = lpos.x; - const float lpos_y_prev = lpos.y; lpos.x = position(0); lpos.y = position(1); lpos.z = position(2); @@ -771,7 +770,6 @@ void EKF2::Run() _ekf.set_gnd_effect_flag(false); } - _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 @@ -802,87 +800,47 @@ void EKF2::Run() // publish vehicle local position data lpos.timestamp = _replay_mode ? now : hrt_absolute_time(); - _local_position_pub.update(); + _local_position_pub.publish(lpos); // publish vehicle_odometry publish_odometry(now, imu_sample_new, lpos); - // publish external visual odometry after fixed frame alignment if new odometry is received - if (new_ev_data_received) { - const Quatf quat_ev2ekf = _ekf.getVisionAlignmentQuaternion(); // rotates from EV to EKF navigation frame - const Dcmf ev_rot_mat(quat_ev2ekf); - - vehicle_odometry_s aligned_ev_odom = _ev_odom; - - // Rotate external position and velocity into EKF navigation frame - const Vector3f aligned_pos = ev_rot_mat * Vector3f(_ev_odom.x, _ev_odom.y, _ev_odom.z); - aligned_ev_odom.x = aligned_pos(0); - aligned_ev_odom.y = aligned_pos(1); - aligned_ev_odom.z = aligned_pos(2); - - switch (_ev_odom.velocity_frame) { - case vehicle_odometry_s::BODY_FRAME_FRD: { - const Vector3f aligned_vel = Dcmf(_ekf.getQuaternion()) * - Vector3f(_ev_odom.vx, _ev_odom.vy, _ev_odom.vz); - aligned_ev_odom.vx = aligned_vel(0); - aligned_ev_odom.vy = aligned_vel(1); - aligned_ev_odom.vz = aligned_vel(2); - break; - } - - case vehicle_odometry_s::LOCAL_FRAME_FRD: { - const Vector3f aligned_vel = ev_rot_mat * - Vector3f(_ev_odom.vx, _ev_odom.vy, _ev_odom.vz); - aligned_ev_odom.vx = aligned_vel(0); - aligned_ev_odom.vy = aligned_vel(1); - aligned_ev_odom.vz = aligned_vel(2); - break; - } - } - - aligned_ev_odom.velocity_frame = vehicle_odometry_s::LOCAL_FRAME_NED; - - // Compute orientation in EKF navigation frame - Quatf ev_quat_aligned = quat_ev2ekf * matrix::Quatf(_ev_odom.q) ; - ev_quat_aligned.normalize(); - - ev_quat_aligned.copyTo(aligned_ev_odom.q); - quat_ev2ekf.copyTo(aligned_ev_odom.q_offset); - - _estimator_visual_odometry_aligned_pub.publish(aligned_ev_odom); - } - + // publish vehicle_global_position if valid if (_ekf.global_position_is_valid() && !_preflt_checker.hasFailed()) { - // generate and publish global position data - vehicle_global_position_s &global_pos = _global_position_pub.get(); - global_pos.timestamp_sample = imu_sample_new.time_us; + // only publish if position has changed by at least 1 mm (map_projection_reproject is relatively expensive) + if ((_last_local_position_for_gpos - position).longerThan(0.001f)) { + + // generate and publish global position data + vehicle_global_position_s global_pos{}; + global_pos.timestamp_sample = imu_sample_new.time_us; - 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 = -lpos.z + lpos.ref_alt; // Altitude AMSL in meters + global_pos.alt_ellipsoid = filter_altitude_ellipsoid(global_pos.alt); + + // global altitude has opposite sign of local down position + global_pos.delta_alt = -lpos.delta_z; + + _ekf.get_ekf_gpos_accuracy(&global_pos.eph, &global_pos.epv); + + global_pos.terrain_alt_valid = lpos.dist_bottom_valid; + + if (global_pos.terrain_alt_valid) { + global_pos.terrain_alt = lpos.ref_alt - terrain_vpos; // Terrain altitude in m, WGS84 + + } else { + global_pos.terrain_alt = 0.0f; // Terrain altitude in m, WGS84 + } + + global_pos.dead_reckoning = _ekf.inertial_dead_reckoning(); // True if this position is estimated through dead-reckoning + global_pos.timestamp = _replay_mode ? now : hrt_absolute_time(); + _global_position_pub.publish(global_pos); + + _last_local_position_for_gpos = position; } - - 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_ellipsoid = filter_altitude_ellipsoid(global_pos.alt); - - // global altitude has opposite sign of local down position - global_pos.delta_alt = -lpos.delta_z; - - _ekf.get_ekf_gpos_accuracy(&global_pos.eph, &global_pos.epv); - - global_pos.terrain_alt_valid = lpos.dist_bottom_valid; - - if (global_pos.terrain_alt_valid) { - global_pos.terrain_alt = lpos.ref_alt - terrain_vpos; // Terrain altitude in m, WGS84 - - } else { - global_pos.terrain_alt = 0.0f; // Terrain altitude in m, WGS84 - } - - global_pos.dead_reckoning = _ekf.inertial_dead_reckoning(); // True if this position is estimated through dead-reckoning - global_pos.timestamp = _replay_mode ? now : hrt_absolute_time(); - _global_position_pub.update(); } } @@ -896,7 +854,7 @@ void EKF2::Run() _estimator_states_pub.publish(states); // publish estimator status - estimator_status_s &status = _estimator_status_pub.get(); + estimator_status_s status{}; status.timestamp_sample = imu_sample_new.time_us; _ekf.getOutputTrackingError().copyTo(status.output_tracking_error); _ekf.get_gps_check_status(&status.gps_check_fail_flags); @@ -910,8 +868,8 @@ void EKF2::Run() status.hgt_test_ratio, status.tas_test_ratio, status.hagl_test_ratio, status.beta_test_ratio); - status.pos_horiz_accuracy = _local_position_pub.get().eph; - status.pos_vert_accuracy = _local_position_pub.get().epv; + status.pos_horiz_accuracy = lpos.eph; + status.pos_vert_accuracy = lpos.epv; _ekf.get_ekf_soln_status(&status.solution_status_flags); _ekf.getImuVibrationMetrics().copyTo(status.vibe); status.time_slip = _last_time_slip_us * 1e-6f; @@ -920,8 +878,12 @@ void EKF2::Run() status.pre_flt_fail_innov_vel_vert = _preflt_checker.hasVertVelFailed(); status.pre_flt_fail_innov_height = _preflt_checker.hasHeightFailed(); status.pre_flt_fail_mag_field_disturbed = control_status.flags.mag_field_disturbed; + status.accel_device_id = _device_id_accel; + status.baro_device_id = _device_id_baro; + status.gyro_device_id = _device_id_gyro; + status.mag_device_id = _device_id_mag; status.timestamp = _replay_mode ? now : hrt_absolute_time(); - _estimator_status_pub.update(); + _estimator_status_pub.publish(status); // estimator_sensor_bias if (status.filter_fault_flags == 0) { @@ -930,9 +892,9 @@ void EKF2::Run() bias.timestamp_sample = imu_sample_new.time_us; // take device ids from sensor_selection_s if not using specific vehicle_imu_s - bias.gyro_device_id = _estimator_status_pub.get().gyro_device_id; - bias.accel_device_id = _estimator_status_pub.get().accel_device_id; - bias.mag_device_id = _estimator_status_pub.get().mag_device_id; + bias.gyro_device_id = _device_id_gyro; + bias.accel_device_id = _device_id_accel; + bias.mag_device_id = _device_id_mag; _ekf.getGyroBias().copyTo(bias.gyro_bias); _ekf.getAccelBias().copyTo(bias.accel_bias); @@ -1046,11 +1008,6 @@ void EKF2::Run() publish_yaw_estimator_status(now); - if (_new_optical_flow_data_received) { - publish_estimator_optical_flow_vel(now); - _new_optical_flow_data_received = false; - } - if (!_mag_decl_saved && (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY)) { _mag_decl_saved = update_mag_decl(_param_ekf2_mag_decl); } @@ -1142,6 +1099,53 @@ void EKF2::Run() } } + if (new_optical_flow_data_received) { + publish_estimator_optical_flow_vel(now); + } + + // publish external visual odometry after fixed frame alignment if new odometry is received + if (new_ev_data_received) { + const Quatf quat_ev2ekf = _ekf.getVisionAlignmentQuaternion(); // rotates from EV to EKF navigation frame + const Dcmf ev_rot_mat(quat_ev2ekf); + + vehicle_odometry_s aligned_ev_odom{ev_odom}; + + // Rotate external position and velocity into EKF navigation frame + const Vector3f aligned_pos = ev_rot_mat * Vector3f(ev_odom.x, ev_odom.y, ev_odom.z); + aligned_ev_odom.x = aligned_pos(0); + aligned_ev_odom.y = aligned_pos(1); + aligned_ev_odom.z = aligned_pos(2); + + switch (ev_odom.velocity_frame) { + case vehicle_odometry_s::BODY_FRAME_FRD: { + const Vector3f aligned_vel = Dcmf(_ekf.getQuaternion()) * Vector3f(ev_odom.vx, ev_odom.vy, ev_odom.vz); + aligned_ev_odom.vx = aligned_vel(0); + aligned_ev_odom.vy = aligned_vel(1); + aligned_ev_odom.vz = aligned_vel(2); + break; + } + + case vehicle_odometry_s::LOCAL_FRAME_FRD: { + const Vector3f aligned_vel = ev_rot_mat * Vector3f(ev_odom.vx, ev_odom.vy, ev_odom.vz); + aligned_ev_odom.vx = aligned_vel(0); + aligned_ev_odom.vy = aligned_vel(1); + aligned_ev_odom.vz = aligned_vel(2); + break; + } + } + + aligned_ev_odom.velocity_frame = vehicle_odometry_s::LOCAL_FRAME_NED; + + // Compute orientation in EKF navigation frame + Quatf ev_quat_aligned = quat_ev2ekf * matrix::Quatf(ev_odom.q) ; + ev_quat_aligned.normalize(); + + ev_quat_aligned.copyTo(aligned_ev_odom.q); + quat_ev2ekf.copyTo(aligned_ev_odom.q_offset); + + _estimator_visual_odometry_aligned_pub.publish(aligned_ev_odom); + } + // publish ekf2_timestamps _ekf2_timestamps_pub.publish(ekf2_timestamps); diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 0b1f8778a6..cb1dcb2348 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -184,11 +184,12 @@ private: bool _imu_bias_reset_request{false}; - // republished aligned external visual odometry - bool new_ev_data_received = false; - vehicle_odometry_s _ev_odom{}; + uint32_t _device_id_accel{0}; + uint32_t _device_id_baro{0}; + uint32_t _device_id_gyro{0}; + uint32_t _device_id_mag{0}; - bool _new_optical_flow_data_received{false}; + Vector3f _last_local_position_for_gpos{}; uORB::Subscription _airdata_sub{ORB_ID(vehicle_air_data)}; uORB::Subscription _airspeed_sub{ORB_ID(airspeed)}; @@ -216,24 +217,24 @@ private: vehicle_land_detected_s _vehicle_land_detected{}; vehicle_status_s _vehicle_status{}; - uORB::PublicationMulti _ekf2_timestamps_pub{ORB_ID(ekf2_timestamps)}; - uORB::PublicationMulti _ekf_gps_drift_pub{ORB_ID(ekf_gps_drift)}; - uORB::PublicationMulti _estimator_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)}; - uORB::PublicationMulti _estimator_innovation_variances_pub{ORB_ID(estimator_innovation_variances)}; - uORB::PublicationMulti _estimator_innovations_pub{ORB_ID(estimator_innovations)}; - uORB::PublicationMulti _estimator_optical_flow_vel_pub{ORB_ID(estimator_optical_flow_vel)}; - uORB::PublicationMulti _estimator_sensor_bias_pub{ORB_ID(estimator_sensor_bias)}; - uORB::PublicationMulti _estimator_states_pub{ORB_ID(estimator_states)}; - uORB::PublicationMultiData _estimator_status_pub{ORB_ID(estimator_status)}; - uORB::PublicationMultiData _estimator_visual_odometry_aligned_pub{ORB_ID(estimator_visual_odometry_aligned)}; - uORB::PublicationMulti _yaw_est_pub{ORB_ID(yaw_estimator_status)}; - uORB::PublicationMulti _wind_pub{ORB_ID(wind_estimate)}; + uORB::PublicationMulti _ekf2_timestamps_pub{ORB_ID(ekf2_timestamps)}; + uORB::PublicationMulti _ekf_gps_drift_pub{ORB_ID(ekf_gps_drift)}; + uORB::PublicationMulti _estimator_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)}; + uORB::PublicationMulti _estimator_innovation_variances_pub{ORB_ID(estimator_innovation_variances)}; + uORB::PublicationMulti _estimator_innovations_pub{ORB_ID(estimator_innovations)}; + uORB::PublicationMulti _estimator_optical_flow_vel_pub{ORB_ID(estimator_optical_flow_vel)}; + uORB::PublicationMulti _estimator_sensor_bias_pub{ORB_ID(estimator_sensor_bias)}; + uORB::PublicationMulti _estimator_states_pub{ORB_ID(estimator_states)}; + uORB::PublicationMulti _estimator_status_pub{ORB_ID(estimator_status)}; + uORB::PublicationMulti _estimator_visual_odometry_aligned_pub{ORB_ID(estimator_visual_odometry_aligned)}; + uORB::PublicationMulti _yaw_est_pub{ORB_ID(yaw_estimator_status)}; + uORB::PublicationMulti _wind_pub{ORB_ID(wind_estimate)}; // publications with topic dependent on multi-mode - uORB::PublicationMulti _attitude_pub; - uORB::PublicationMultiData _local_position_pub; - uORB::PublicationMultiData _global_position_pub; - uORB::PublicationMulti _odometry_pub; + uORB::PublicationMulti _attitude_pub; + uORB::PublicationMulti _local_position_pub; + uORB::PublicationMulti _global_position_pub; + uORB::PublicationMulti _odometry_pub; Ekf _ekf;