diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index fd871cb0df88..7914dfa335bc 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -104,6 +104,7 @@ set(msg_files sensor_combined.msg sensor_correction.msg sensor_gyro.msg + sensor_gyro_control.msg sensor_mag.msg sensor_preflight.msg sensor_selection.msg diff --git a/msg/sensor_gyro_control.msg b/msg/sensor_gyro_control.msg new file mode 100644 index 000000000000..c06d472cf16b --- /dev/null +++ b/msg/sensor_gyro_control.msg @@ -0,0 +1,9 @@ +uint32 device_id # unique device ID for the sensor that does not change between power cycles + +uint64 timestamp # time since system start (microseconds) + +uint64 timestamp_sample # time the raw data was sampled (microseconds) + +float32 x # filtered angular velocity in the NED X board axis in rad/s +float32 y # filtered angular velocity in the NED Y board axis in rad/s +float32 z # filtered angular velocity in the NED Z board axis in rad/s diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp b/src/lib/drivers/gyroscope/PX4Gyroscope.cpp index facd9a933564..676004bd4301 100644 --- a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp +++ b/src/lib/drivers/gyroscope/PX4Gyroscope.cpp @@ -40,12 +40,13 @@ PX4Gyroscope::PX4Gyroscope(uint32_t device_id, uint8_t priority, enum Rotation r CDev(nullptr), ModuleParams(nullptr), _sensor_gyro_pub{ORB_ID(sensor_gyro), priority}, + _sensor_gyro_control_pub{ORB_ID(sensor_gyro_control), priority}, _rotation{rotation} { _class_device_instance = register_class_devname(GYRO_BASE_DEVICE_PATH); _sensor_gyro_pub.get().device_id = device_id; - _sensor_gyro_pub.get().scaling = 1.0f; + _sensor_gyro_control_pub.get().device_id = device_id; // set software low pass filter for controllers updateParams(); @@ -96,6 +97,7 @@ void PX4Gyroscope::set_device_type(uint8_t devtype) // copy back to report _sensor_gyro_pub.get().device_id = device_id.devid; + _sensor_gyro_control_pub.get().device_id = device_id.devid; } void PX4Gyroscope::set_sample_rate(unsigned rate) @@ -121,6 +123,15 @@ void PX4Gyroscope::update(hrt_abstime timestamp, int16_t x, int16_t y, int16_t z // Filtered values const matrix::Vector3f val_filtered{_filter.apply(val_calibrated)}; + // publish control data (filtered gyro) immediately + sensor_gyro_control_s &control = _sensor_gyro_control_pub.get(); + control.timestamp_sample = timestamp; + control.x = val_filtered(0); + control.y = val_filtered(1); + control.z = val_filtered(2); + control.timestamp = hrt_absolute_time(); + _sensor_gyro_control_pub.update(); // publish + // Integrated values matrix::Vector3f integrated_value; uint32_t integral_dt = 0; @@ -142,7 +153,7 @@ void PX4Gyroscope::update(hrt_abstime timestamp, int16_t x, int16_t y, int16_t z report.z_integral = integrated_value(2); poll_notify(POLLIN); - _sensor_gyro_pub.update(); + _sensor_gyro_pub.update(); // publish } } diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp index b731b1471d0b..646efab60da3 100644 --- a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp +++ b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp @@ -43,6 +43,7 @@ #include #include #include +#include class PX4Gyroscope : public cdev::CDev, public ModuleParams { @@ -69,6 +70,7 @@ class PX4Gyroscope : public cdev::CDev, public ModuleParams void configure_filter(float cutoff_freq) { _filter.set_cutoff_frequency(_sample_rate, cutoff_freq); } uORB::Publication _sensor_gyro_pub; + uORB::Publication _sensor_gyro_control_pub; math::LowPassFilter2pVector3f _filter{1000, 100}; Integrator _integrator{4000, true}; diff --git a/src/modules/mc_att_control/mc_att_control.hpp b/src/modules/mc_att_control/mc_att_control.hpp index f3da3b464290..251505e7e11a 100644 --- a/src/modules/mc_att_control/mc_att_control.hpp +++ b/src/modules/mc_att_control/mc_att_control.hpp @@ -51,7 +51,7 @@ #include #include #include -#include +#include #include #include #include @@ -155,7 +155,7 @@ class MulticopterAttitudeControl : public ModuleBase uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */ uORB::Subscription _landing_gear_sub{ORB_ID(landing_gear)}; - uORB::SubscriptionCallbackWorkItem _sensor_gyro_sub{this, ORB_ID(sensor_gyro)}; /**< gyro data subscription */ + uORB::SubscriptionCallbackWorkItem _sensor_gyro_sub{this, ORB_ID(sensor_gyro_control)}; /**< gyro data subscription */ unsigned _gyro_count{1}; int _selected_gyro{0}; @@ -179,7 +179,8 @@ class MulticopterAttitudeControl : public ModuleBase struct actuator_controls_s _actuators {}; /**< actuator controls */ struct vehicle_status_s _vehicle_status {}; /**< vehicle status */ struct battery_status_s _battery_status {}; /**< battery status */ - struct sensor_gyro_s _sensor_gyro {}; /**< gyro data before thermal correctons and ekf bias estimates are applied */ + struct sensor_gyro_control_s + _sensor_gyro {}; /**< gyro data before thermal correctons and ekf bias estimates are applied */ struct sensor_correction_s _sensor_correction {}; /**< sensor thermal corrections */ struct sensor_bias_s _sensor_bias {}; /**< sensor in-run bias corrections */ struct vehicle_land_detected_s _vehicle_land_detected {}; diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 8c238aa801ca..9b32322126b1 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -124,7 +124,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : parameters_updated(); - _gyro_count = math::constrain(orb_group_count(ORB_ID(sensor_gyro)), 1, MAX_GYRO_COUNT); + _gyro_count = math::constrain(orb_group_count(ORB_ID(sensor_gyro_control)), 1, MAX_GYRO_COUNT); } void @@ -558,7 +558,7 @@ MulticopterAttitudeControl::publish_actuator_controls() _actuators.control[3] = (PX4_ISFINITE(_thrust_sp)) ? _thrust_sp : 0.0f; _actuators.control[7] = (float)_landing_gear.landing_gear; _actuators.timestamp = hrt_absolute_time(); - _actuators.timestamp_sample = _sensor_gyro.timestamp; + _actuators.timestamp_sample = _sensor_gyro.timestamp_sample; /* scale effort by battery status */ if (_param_mc_bat_scale_en.get() && _battery_status.scale > 0.0f) { @@ -581,7 +581,7 @@ MulticopterAttitudeControl::Run() /* update the latest gyro selection */ if (_sensor_correction.selected_gyro_instance < _gyro_count) { if (_selected_gyro != _sensor_correction.selected_gyro_instance) { - if (_sensor_gyro_sub.change_topic(ORB_ID(sensor_gyro), _selected_gyro)) { + if (_sensor_gyro_sub.change_topic(ORB_ID(sensor_gyro_control), _selected_gyro)) { _selected_gyro = _sensor_correction.selected_gyro_instance; PX4_WARN("selected gyro changed %d -> %d", _selected_gyro, _sensor_correction.selected_gyro_instance); }