diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 04017f2f49..8bd94b73cc 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -61,6 +61,7 @@ set(msg_files esc_report.msg esc_status.msg estimator_innovations.msg + estimator_sensor_bias.msg estimator_status.msg follow_target.msg geofence_result.msg @@ -108,7 +109,6 @@ set(msg_files sensor_accel_integrated.msg sensor_accel_status.msg sensor_baro.msg - sensor_bias.msg sensor_combined.msg sensor_correction.msg sensor_gyro.msg diff --git a/msg/sensor_bias.msg b/msg/estimator_sensor_bias.msg similarity index 100% rename from msg/sensor_bias.msg rename to msg/estimator_sensor_bias.msg diff --git a/msg/tools/uorb_rtps_message_ids.yaml b/msg/tools/uorb_rtps_message_ids.yaml index e05bc2187b..e0e6d2deca 100644 --- a/msg/tools/uorb_rtps_message_ids.yaml +++ b/msg/tools/uorb_rtps_message_ids.yaml @@ -147,7 +147,7 @@ rtps: - msg: sensor_baro id: 63 send: true - - msg: sensor_bias + - msg: estimator_sensor_bias id: 64 - msg: sensor_combined id: 65 diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 427802285a..8f63c690f0 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -56,15 +56,15 @@ #include #include #include -#include #include -#include -#include #include +#include +#include +#include +#include #include #include #include -#include #include #include #include @@ -263,20 +263,20 @@ private: vehicle_land_detected_s _vehicle_land_detected{}; vehicle_status_s _vehicle_status{}; - uORB::Publication _estimator_innovations_pub{ORB_ID(estimator_innovations)}; - uORB::Publication _estimator_innovation_variances_pub{ORB_ID(estimator_innovation_variances)}; - uORB::Publication _estimator_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)}; uORB::Publication _ekf2_timestamps_pub{ORB_ID(ekf2_timestamps)}; uORB::Publication _ekf_gps_drift_pub{ORB_ID(ekf_gps_drift)}; uORB::Publication _blended_gps_pub{ORB_ID(ekf_gps_position)}; + uORB::Publication _estimator_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)}; + uORB::Publication _estimator_innovation_variances_pub{ORB_ID(estimator_innovation_variances)}; + uORB::Publication _estimator_innovations_pub{ORB_ID(estimator_innovations)}; + uORB::Publication _estimator_sensor_bias_pub{ORB_ID(estimator_sensor_bias)}; uORB::Publication _estimator_status_pub{ORB_ID(estimator_status)}; - uORB::Publication _sensor_bias_pub{ORB_ID(sensor_bias)}; uORB::Publication _att_pub{ORB_ID(vehicle_attitude)}; uORB::Publication _vehicle_odometry_pub{ORB_ID(vehicle_odometry)}; - uORB::PublicationMulti _wind_pub{ORB_ID(wind_estimate)}; uORB::PublicationData _vehicle_global_position_pub{ORB_ID(vehicle_global_position)}; uORB::PublicationData _vehicle_local_position_pub{ORB_ID(vehicle_local_position)}; uORB::PublicationData _vehicle_visual_odometry_aligned_pub{ORB_ID(vehicle_visual_odometry_aligned)}; + uORB::PublicationMulti _wind_pub{ORB_ID(wind_estimate)}; Ekf _ekf; @@ -753,7 +753,7 @@ void Ekf2::Run() imuSample imu_sample_new {}; hrt_abstime imu_dt = 0; // for tracking time slip later - sensor_bias_s bias{}; + estimator_sensor_bias_s bias{}; if (_imu_sub_index >= 0) { vehicle_imu_s imu; @@ -1490,7 +1490,7 @@ void Ekf2::Run() bias.mag_bias[1] = _last_valid_mag_cal[1]; bias.mag_bias[2] = _last_valid_mag_cal[2]; - _sensor_bias_pub.publish(bias); + _estimator_sensor_bias_pub.publish(bias); } // publish estimator status diff --git a/src/modules/land_detector/LandDetector.h b/src/modules/land_detector/LandDetector.h index da72901284..d9aa5b4e86 100644 --- a/src/modules/land_detector/LandDetector.h +++ b/src/modules/land_detector/LandDetector.h @@ -43,21 +43,21 @@ #pragma once #include -#include #include -#include -#include + +#include +#include #include #include -#include #include #include +#include +#include #include -#include #include +#include #include #include -#include #include #include #include diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 007fdb439a..9ff9525321 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -58,6 +58,7 @@ void LoggedTopics::add_default_topics() add_topic("estimator_innovation_test_ratios", 200); add_topic("estimator_innovation_variances", 200); add_topic("estimator_innovations", 200); + add_topic("estimator_sensor_bias", 1000); add_topic("estimator_status", 200); add_topic("home_position"); add_topic("input_rc", 200); @@ -70,7 +71,6 @@ void LoggedTopics::add_default_topics() add_topic("radio_status"); add_topic("rate_ctrl_status", 200); add_topic("safety", 1000); - add_topic("sensor_bias", 1000); add_topic("sensor_combined", 100); add_topic("sensor_correction", 1000); add_topic("sensor_preflight", 200); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 8b6884fb0c..7b607724dc 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -70,6 +70,7 @@ #include #include #include +#include #include #include #include @@ -84,7 +85,6 @@ #include #include #include -#include #include #include #include @@ -816,7 +816,7 @@ protected: explicit MavlinkStreamHighresIMU(Mavlink *mavlink) : MavlinkStream(mavlink), _sensor_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_combined))), _sensor_time(0), - _bias_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_bias))), + _bias_sub(_mavlink->add_orb_subscription(ORB_ID(estimator_sensor_bias))), _differential_pressure_sub(_mavlink->add_orb_subscription(ORB_ID(differential_pressure))), _magnetometer_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_magnetometer))), _air_data_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_air_data))), @@ -864,7 +864,7 @@ protected: _baro_timestamp = air_data.timestamp; } - sensor_bias_s bias = {}; + estimator_sensor_bias_s bias{}; _bias_sub->update(&bias); differential_pressure_s differential_pressure = {}; diff --git a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp index 33313f4c3b..56d78c3afa 100644 --- a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp +++ b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp @@ -79,10 +79,10 @@ void VehicleAcceleration::Stop() void VehicleAcceleration::SensorBiasUpdate(bool force) { - if (_sensor_bias_sub.updated() || force) { - sensor_bias_s bias; + if (_estimator_sensor_bias_sub.updated() || force) { + estimator_sensor_bias_s bias; - if (_sensor_bias_sub.copy(&bias)) { + if (_estimator_sensor_bias_sub.copy(&bias)) { if (bias.accel_device_id == _selected_sensor_device_id) { _bias = Vector3f{bias.accel_bias}; diff --git a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp index 2ec0586ccc..394bb4b1a7 100644 --- a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp +++ b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp @@ -36,19 +36,18 @@ #include #include #include -#include #include #include +#include #include #include #include #include +#include #include -#include +#include #include #include - -#include #include class VehicleAcceleration : public ModuleParams, public px4::WorkItem @@ -84,7 +83,7 @@ private: uORB::Publication _vehicle_acceleration_pub{ORB_ID(vehicle_acceleration)}; uORB::Subscription _params_sub{ORB_ID(parameter_update)}; - uORB::Subscription _sensor_bias_sub{ORB_ID(sensor_bias)}; + uORB::Subscription _estimator_sensor_bias_sub{ORB_ID(estimator_sensor_bias)}; uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)}; uORB::SubscriptionCallbackWorkItem _sensor_selection_sub{this, ORB_ID(sensor_selection)}; diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp index e3ed845898..9bcbb3f74f 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp @@ -79,10 +79,10 @@ void VehicleAngularVelocity::Stop() void VehicleAngularVelocity::SensorBiasUpdate(bool force) { - if (_sensor_bias_sub.updated() || force) { - sensor_bias_s bias; + if (_estimator_sensor_bias_sub.updated() || force) { + estimator_sensor_bias_s bias; - if (_sensor_bias_sub.copy(&bias)) { + if (_estimator_sensor_bias_sub.copy(&bias)) { if (bias.gyro_device_id == _selected_sensor_device_id) { _bias = Vector3f{bias.gyro_bias}; diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp index 317f260dc6..0d66296fa5 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp @@ -36,19 +36,18 @@ #include #include #include -#include #include #include +#include #include #include #include #include +#include #include -#include #include -#include - #include +#include #include class VehicleAngularVelocity : public ModuleParams, public px4::WorkItem @@ -84,7 +83,7 @@ private: uORB::Publication _vehicle_angular_velocity_pub{ORB_ID(vehicle_angular_velocity)}; uORB::Subscription _params_sub{ORB_ID(parameter_update)}; - uORB::Subscription _sensor_bias_sub{ORB_ID(sensor_bias)}; + uORB::Subscription _estimator_sensor_bias_sub{ORB_ID(estimator_sensor_bias)}; uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)}; uORB::SubscriptionCallbackWorkItem _sensor_selection_sub{this, ORB_ID(sensor_selection)};