fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

HACK: add new topic to log local position w/o reference

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2022-12-21 14:21:10 +01:00 committed by fury1895
parent ec1fbca575
commit 3db5a35005
4 changed files with 15 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
msg/vehicle_local_position.msg
View File

@ -72,5 +72,5 @@ float32 vz_max # maximum vertical speed - set to 0 when limiting not required
float32 hagl_min # minimum height above ground level - set to 0 when limiting not required (meters)
float32 hagl_max # maximum height above ground level - set to 0 when limiting not required (meters)
# TOPICS vehicle_local_position vehicle_local_position_groundtruth
# TOPICS vehicle_local_position vehicle_local_position_groundtruth vehicle_local_position_wo_ref
# TOPICS estimator_local_position

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

@ -56,6 +56,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
_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)),
_wind_pub(multi_mode ? ORB_ID(estimator_wind) : ORB_ID(wind)),
_local_position_wo_ref_pub(ORB_ID(vehicle_local_position_wo_ref)),
_params(_ekf.getParamHandle()),
_param_ekf2_predict_us(_params->filter_update_interval_us),
_param_ekf2_mag_delay(_params->mag_delay_ms),
@ -168,6 +169,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
// advertise expected minimal topic set immediately to ensure logging
_attitude_pub.advertise();
_local_position_pub.advertise();
_local_position_wo_ref_pub.advertise();
_estimator_event_flags_pub.advertise();
_estimator_innovation_test_ratios_pub.advertise();
@ -1041,6 +1043,14 @@ void EKF2::PublishLocalPosition(const hrt_abstime &timestamp)
// publish vehicle local position data
lpos.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
_local_position_pub.publish(lpos);
// publish vehicle_local_position_wo_ref (without lat/lon ref)
vehicle_local_position_s vehicle_local_position_wo_ref;
vehicle_local_position_wo_ref = lpos;
vehicle_local_position_wo_ref.ref_lat = static_cast<double>(NAN);
vehicle_local_position_wo_ref.ref_lon = static_cast<double>(NAN);
vehicle_local_position_wo_ref.ref_alt = NAN;
_local_position_wo_ref_pub.publish(vehicle_local_position_wo_ref);
}
void EKF2::PublishOdometry(const hrt_abstime &timestamp, const imuSample &imu)

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

@ -337,6 +337,8 @@ private:
uORB::PublicationMulti<vehicle_odometry_s> _odometry_pub;
uORB::PublicationMulti<wind_s> _wind_pub;
uORB::Publication<vehicle_local_position_s> _local_position_wo_ref_pub;
PreFlightChecker _preflt_checker;
Ekf _ekf;

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

@ -112,6 +112,8 @@ void LoggedTopics::add_default_topics()
add_optional_topic("vtol_vehicle_status", 200);
add_topic("wind", 1000);
add_topic("vehicle_local_position_wo_ref", 100);
if (!_do_not_log_position_data) {
add_optional_topic("camera_capture");
add_topic("follow_target_status", 100);