Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-18 16:19:23 +08:00
Code Issues Actions 25 Packages Projects Releases Wiki Activity

ekf2: avoid storing message copies

Browse Source
This commit is contained in:
Daniel Agar
2020-10-27 12:54:27 -04:00
parent 3f9f2c6fdf
commit db5235b074
2 changed files with 184 additions and 179 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF2.cpp
+163 -159
View File
@@ -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<float, 3>() * 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);
src/modules/ekf2/EKF2.hpp
+21 -20
View File
@@ -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_s> _ekf2_timestamps_pub{ORB_ID(ekf2_timestamps)};
uORB::PublicationMulti<ekf_gps_drift_s> _ekf_gps_drift_pub{ORB_ID(ekf_gps_drift)};
uORB::PublicationMulti<estimator_innovations_s> _estimator_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)};
uORB::PublicationMulti<estimator_innovations_s> _estimator_innovation_variances_pub{ORB_ID(estimator_innovation_variances)};
uORB::PublicationMulti<estimator_innovations_s> _estimator_innovations_pub{ORB_ID(estimator_innovations)};
uORB::PublicationMulti<estimator_optical_flow_vel_s> _estimator_optical_flow_vel_pub{ORB_ID(estimator_optical_flow_vel)};
uORB::PublicationMulti<estimator_sensor_bias_s> _estimator_sensor_bias_pub{ORB_ID(estimator_sensor_bias)};
uORB::PublicationMulti<estimator_states_s> _estimator_states_pub{ORB_ID(estimator_states)};
uORB::PublicationMultiData<estimator_status_s> _estimator_status_pub{ORB_ID(estimator_status)};
uORB::PublicationMultiData<vehicle_odometry_s> _estimator_visual_odometry_aligned_pub{ORB_ID(estimator_visual_odometry_aligned)};
uORB::PublicationMulti<yaw_estimator_status_s> _yaw_est_pub{ORB_ID(yaw_estimator_status)};
uORB::PublicationMulti<wind_estimate_s> _wind_pub{ORB_ID(wind_estimate)};
uORB::PublicationMulti<ekf2_timestamps_s> _ekf2_timestamps_pub{ORB_ID(ekf2_timestamps)};
uORB::PublicationMulti<ekf_gps_drift_s> _ekf_gps_drift_pub{ORB_ID(ekf_gps_drift)};
uORB::PublicationMulti<estimator_innovations_s> _estimator_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)};
uORB::PublicationMulti<estimator_innovations_s> _estimator_innovation_variances_pub{ORB_ID(estimator_innovation_variances)};
uORB::PublicationMulti<estimator_innovations_s> _estimator_innovations_pub{ORB_ID(estimator_innovations)};
uORB::PublicationMulti<estimator_optical_flow_vel_s> _estimator_optical_flow_vel_pub{ORB_ID(estimator_optical_flow_vel)};
uORB::PublicationMulti<estimator_sensor_bias_s> _estimator_sensor_bias_pub{ORB_ID(estimator_sensor_bias)};
uORB::PublicationMulti<estimator_states_s> _estimator_states_pub{ORB_ID(estimator_states)};
uORB::PublicationMulti<estimator_status_s> _estimator_status_pub{ORB_ID(estimator_status)};
uORB::PublicationMulti<vehicle_odometry_s> _estimator_visual_odometry_aligned_pub{ORB_ID(estimator_visual_odometry_aligned)};
uORB::PublicationMulti<yaw_estimator_status_s> _yaw_est_pub{ORB_ID(yaw_estimator_status)};
uORB::PublicationMulti<wind_estimate_s> _wind_pub{ORB_ID(wind_estimate)};
// publications with topic dependent on multi-mode
uORB::PublicationMulti<vehicle_attitude_s> _attitude_pub;
uORB::PublicationMultiData<vehicle_local_position_s> _local_position_pub;
uORB::PublicationMultiData<vehicle_global_position_s> _global_position_pub;
uORB::PublicationMulti<vehicle_odometry_s> _odometry_pub;
uORB::PublicationMulti<vehicle_attitude_s> _attitude_pub;
uORB::PublicationMulti<vehicle_local_position_s> _local_position_pub;
uORB::PublicationMulti<vehicle_global_position_s> _global_position_pub;
uORB::PublicationMulti<vehicle_odometry_s> _odometry_pub;
Ekf _ekf;