dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

rename vehicle_visual_odometry_aligned -> estimator_visual_odometry_aligned 

- saves a small amount of work for the ekf2 selector in multi-EKF mode (visual_odometry_aligned now ignored)
 - helps to distinguish the origin/purpose from vehicle_odometry and vehicle_visual_odometry
This commit is contained in:
Daniel Agar 2020-10-27 12:33:39 -04:00 committed by GitHub
parent d5e68bc05a
commit df2f26ebdf
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
7 changed files with 6 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
msg/tools/uorb_rtps_message_ids.yaml
View File

@ -349,7 +349,7 @@ rtps:
- msg: vehicle_angular_velocity_groundtruth
id: 168
alias: vehicle_angular_velocity
- msg: vehicle_visual_odometry_aligned
- msg: estimator_visual_odometry_aligned
id: 169
alias: vehicle_odometry
- msg: estimator_innovation_variances

2
msg/vehicle_odometry.msg
View File

@ -62,5 +62,5 @@ float32 yawspeed # Angular velocity about Z body axis
# If angular velocity covariance invalid/unknown, 16th cell is NaN
float32[21] velocity_covariance
# TOPICS vehicle_odometry vehicle_mocap_odometry vehicle_visual_odometry vehicle_visual_odometry_aligned
# TOPICS vehicle_odometry vehicle_mocap_odometry vehicle_visual_odometry
# TOPICS estimator_odometry estimator_visual_odometry_aligned

6
src/modules/ekf2/EKF2.cpp
View File

@ -54,8 +54,6 @@ EKF2::EKF2(int instance, const px4::wq_config_t &config, int imu, int mag, bool
_local_position_pub(_multi_mode ? ORB_ID(estimator_local_position) : ORB_ID(vehicle_local_position)),
_global_position_pub(_multi_mode ? ORB_ID(estimator_global_position) : ORB_ID(vehicle_global_position)),
_odometry_pub(_multi_mode ? ORB_ID(estimator_odometry) : ORB_ID(vehicle_odometry)),
_visual_odometry_aligned_pub(_multi_mode ? ORB_ID(estimator_visual_odometry_aligned) :
ORB_ID(vehicle_visual_odometry_aligned)),
_params(_ekf.getParamHandle()),
_param_ekf2_min_obs_dt(_params->sensor_interval_min_ms),
_param_ekf2_mag_delay(_params->mag_delay_ms),
@ -171,7 +169,6 @@ EKF2::EKF2(int instance, const px4::wq_config_t &config, int imu, int mag, bool
_local_position_pub.advertise();
_global_position_pub.advertise();
_odometry_pub.advertise();
_visual_odometry_aligned_pub.advertise();
_ekf2_timestamps_pub.advertise();
_ekf_gps_drift_pub.advertise();
@ -182,6 +179,7 @@ EKF2::EKF2(int instance, const px4::wq_config_t &config, int imu, int mag, bool
_estimator_sensor_bias_pub.advertise();
_estimator_states_pub.advertise();
_estimator_status_pub.advertise();
_estimator_visual_odometry_aligned_pub.advertised();
_wind_pub.advertise();
_yaw_est_pub.advertise();
@ -851,7 +849,7 @@ void EKF2::Run()
ev_quat_aligned.copyTo(aligned_ev_odom.q);
quat_ev2ekf.copyTo(aligned_ev_odom.q_offset);
_visual_odometry_aligned_pub.publish(aligned_ev_odom);
_estimator_visual_odometry_aligned_pub.publish(aligned_ev_odom);
}
if (_ekf.global_position_is_valid() && !_preflt_checker.hasFailed()) {

2
src/modules/ekf2/EKF2.hpp
View File

@ -225,6 +225,7 @@ private:
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)};
@ -233,7 +234,6 @@ private:
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::PublicationMultiData<vehicle_odometry_s> _visual_odometry_aligned_pub;
Ekf _ekf;

12
src/modules/ekf2/EKF2Selector.cpp
View File

@ -586,18 +586,6 @@ void EKF2Selector::Run()
}
}
}
// selected estimator_visual_odometry_aligned -> vehicle_visual_odometry_aligned
if (_instance[_selected_instance].estimator_visual_odometry_aligned_sub.updated()) {
vehicle_odometry_s vehicle_visual_odometry_aligned;
if (_instance[_selected_instance].estimator_visual_odometry_aligned_sub.update(&vehicle_visual_odometry_aligned)) {
if (vehicle_visual_odometry_aligned.timestamp >= _instance[_selected_instance].estimator_status.timestamp_sample) {
vehicle_visual_odometry_aligned.timestamp = hrt_absolute_time();
_vehicle_visual_odometry_aligned_pub.publish(vehicle_visual_odometry_aligned);
}
}
}
}
if (_lockstep_component == -1) {

3
src/modules/ekf2/EKF2Selector.hpp
View File

@ -86,7 +86,6 @@ private:
estimator_local_position_sub{ORB_ID(estimator_local_position), i},
estimator_global_position_sub{ORB_ID(estimator_global_position), i},
estimator_odometry_sub{ORB_ID(estimator_odometry), i},
estimator_visual_odometry_aligned_sub{ORB_ID(estimator_visual_odometry_aligned), i},
instance(i)
{}
@ -96,7 +95,6 @@ private:
uORB::Subscription estimator_local_position_sub;
uORB::Subscription estimator_global_position_sub;
uORB::Subscription estimator_odometry_sub;
uORB::Subscription estimator_visual_odometry_aligned_sub;
estimator_status_s estimator_status{};
@ -187,7 +185,6 @@ private:
uORB::Publication<vehicle_global_position_s> _vehicle_global_position_pub{ORB_ID(vehicle_global_position)};
uORB::Publication<vehicle_local_position_s> _vehicle_local_position_pub{ORB_ID(vehicle_local_position)};
uORB::Publication<vehicle_odometry_s> _vehicle_odometry_pub{ORB_ID(vehicle_odometry)};
uORB::Publication<vehicle_odometry_s> _vehicle_visual_odometry_aligned_pub{ORB_ID(vehicle_visual_odometry_aligned)};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::EKF2_SEL_ERR_RED>) _param_ekf2_sel_err_red,

2
src/modules/logger/logged_topics.cpp
View File

@ -123,6 +123,7 @@ void LoggedTopics::add_default_topics()
add_topic_multi("estimator_sensor_bias", 1000, MAX_ESTIMATOR_INSTANCES);
add_topic_multi("estimator_states", 1000, MAX_ESTIMATOR_INSTANCES);
add_topic_multi("estimator_status", 500, MAX_ESTIMATOR_INSTANCES);
add_topic_multi("estimator_visual_odometry_aligned", 200, MAX_ESTIMATOR_INSTANCES);
add_topic_multi("yaw_estimator_status", 1000, MAX_ESTIMATOR_INSTANCES);
add_topic_multi("wind_estimate", 1000); // published by both ekf2 and airspeed_selector
@ -192,7 +193,6 @@ void LoggedTopics::add_estimator_replay_topics()
add_topic("vehicle_magnetometer");
add_topic("vehicle_status");
add_topic("vehicle_visual_odometry");
add_topic("vehicle_visual_odometry_aligned");
add_topic_multi("distance_sensor");
}