diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index ae38338d05..e112e116a9 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -105,6 +105,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 0000000000..5151231c4b --- /dev/null +++ b/msg/sensor_gyro_control.msg @@ -0,0 +1,11 @@ + +# WARNING: Not recommend for custom development. +# This topic is part of an incremental refactoring of the sensor pipeline and will likely disappear post PX4 release v1.10 + +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[3] xyz # filtered angular velocity in the board axis in rad/s diff --git a/msg/tools/uorb_rtps_message_ids.yaml b/msg/tools/uorb_rtps_message_ids.yaml index 6a442c18da..8f8bcf9c66 100644 --- a/msg/tools/uorb_rtps_message_ids.yaml +++ b/msg/tools/uorb_rtps_message_ids.yaml @@ -258,6 +258,8 @@ rtps: id: 112 - msg: vehicle_acceleration id: 113 + - msg: sensor_gyro_control + id: 114 # multi topics - msg: actuator_controls_0 id: 120 diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp b/src/lib/drivers/gyroscope/PX4Gyroscope.cpp index 209eea03e5..b8b6ebf167 100644 --- a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp +++ b/src/lib/drivers/gyroscope/PX4Gyroscope.cpp @@ -40,12 +40,14 @@ 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(); @@ -94,6 +96,7 @@ 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 @@ -120,6 +123,13 @@ PX4Gyroscope::update(hrt_abstime timestamp, float x, float y, float 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; + val_filtered.copyTo(control.xyz); + control.timestamp = hrt_absolute_time(); + _sensor_gyro_control_pub.update(); // publish + // Integrated values matrix::Vector3f integrated_value; uint32_t integral_dt = 0; @@ -141,7 +151,7 @@ PX4Gyroscope::update(hrt_abstime timestamp, float x, float y, float z) report.z_integral = integrated_value(2); poll_notify(POLLIN); - _sensor_gyro_pub.update(); + _sensor_gyro_pub.update(); // publish } } @@ -158,4 +168,5 @@ PX4Gyroscope::print_status() (double)_calibration_offset(2)); print_message(_sensor_gyro_pub.get()); + print_message(_sensor_gyro_control_pub.get()); } diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp index 50e99e4655..97f9d78be3 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 { @@ -68,7 +69,8 @@ private: void configure_filter(float cutoff_freq) { _filter.set_cutoff_frequency(_sample_rate, cutoff_freq); } - uORB::PublicationMultiData _sensor_gyro_pub; + uORB::PublicationMultiData _sensor_gyro_pub; + uORB::PublicationMultiData _sensor_gyro_control_pub; math::LowPassFilter2pVector3f _filter{1000, 100}; Integrator _integrator{4000, true}; diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp index 41369e2d95..96c850d513 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp @@ -103,6 +103,17 @@ VehicleAngularVelocity::SensorBiasUpdate(bool force) bool VehicleAngularVelocity::SensorCorrectionsUpdate(bool force) { + if (_sensor_selection_sub.updated() || force) { + sensor_selection_s sensor_selection; + + if (_sensor_selection_sub.copy(&sensor_selection)) { + if (_selected_sensor_device_id != sensor_selection.gyro_device_id) { + _selected_sensor_device_id = sensor_selection.gyro_device_id; + force = true; + } + } + } + // check if the selected sensor has updated if (_sensor_correction_sub.updated() || force) { @@ -131,12 +142,40 @@ VehicleAngularVelocity::SensorCorrectionsUpdate(bool force) if ((_selected_sensor != corrections.selected_gyro_instance) || force) { if (corrections.selected_gyro_instance < MAX_SENSOR_COUNT) { // clear all registered callbacks + for (auto &sub : _sensor_control_sub) { + sub.unregister_callback(); + } + for (auto &sub : _sensor_sub) { sub.unregister_callback(); } const int sensor_new = corrections.selected_gyro_instance; + // subscribe to sensor_gyro_control if available + // currently not all drivers (eg df_*) provide sensor_gyro_control + for (int i = 0; i < MAX_SENSOR_COUNT; i++) { + sensor_gyro_control_s report{}; + _sensor_control_sub[i].copy(&report); + + if ((report.device_id != 0) && (report.device_id == _selected_sensor_device_id)) { + if (_sensor_control_sub[i].register_callback()) { + PX4_DEBUG("selected sensor (control) changed %d -> %d", _selected_sensor, i); + _selected_sensor_control = i; + + _sensor_control_available = true; + + // record selected sensor (sensor_gyro orb index) + _selected_sensor = sensor_new; + + return true; + } + } + } + + // otherwise fallback to using sensor_gyro (legacy that will be removed) + _sensor_control_available = false; + if (_sensor_sub[sensor_new].register_callback()) { PX4_DEBUG("selected sensor changed %d -> %d", _selected_sensor, sensor_new); _selected_sensor = sensor_new; @@ -183,32 +222,62 @@ VehicleAngularVelocity::Run() // update corrections first to set _selected_sensor SensorCorrectionsUpdate(); - sensor_gyro_s sensor_data; + if (_sensor_control_available) { + // using sensor_gyro_control is preferred, but currently not all drivers (eg df_*) provide sensor_gyro_control + sensor_gyro_control_s sensor_data; - if (_sensor_sub[_selected_sensor].update(&sensor_data)) { - perf_set_elapsed(_sensor_latency_perf, hrt_elapsed_time(&sensor_data.timestamp)); + if (_sensor_control_sub[_selected_sensor].update(&sensor_data)) { + perf_set_elapsed(_sensor_latency_perf, hrt_elapsed_time(&sensor_data.timestamp)); - ParametersUpdate(); - SensorBiasUpdate(); + ParametersUpdate(); + SensorBiasUpdate(); - // get the sensor data and correct for thermal errors - const Vector3f val{sensor_data.x, sensor_data.y, sensor_data.z}; + // get the sensor data and correct for thermal errors (apply offsets and scale) + Vector3f rates{(Vector3f{sensor_data.xyz} - _offset).emult(_scale)}; - // apply offsets and scale - Vector3f rates{(val - _offset).emult(_scale)}; + // rotate corrected measurements from sensor to body frame + rates = _board_rotation * rates; - // rotate corrected measurements from sensor to body frame - rates = _board_rotation * rates; + // correct for in-run bias errors + rates -= _bias; - // correct for in-run bias errors - rates -= _bias; + vehicle_angular_velocity_s angular_velocity; + angular_velocity.timestamp_sample = sensor_data.timestamp; + rates.copyTo(angular_velocity.xyz); + angular_velocity.timestamp = hrt_absolute_time(); - vehicle_angular_velocity_s angular_velocity; - angular_velocity.timestamp_sample = sensor_data.timestamp; - rates.copyTo(angular_velocity.xyz); - angular_velocity.timestamp = hrt_absolute_time(); + _vehicle_angular_velocity_pub.publish(angular_velocity); + } - _vehicle_angular_velocity_pub.publish(angular_velocity); + } else { + // otherwise fallback to using sensor_gyro (legacy that will be removed) + sensor_gyro_s sensor_data; + + if (_sensor_sub[_selected_sensor].update(&sensor_data)) { + perf_set_elapsed(_sensor_latency_perf, hrt_elapsed_time(&sensor_data.timestamp)); + + ParametersUpdate(); + SensorBiasUpdate(); + + // get the sensor data and correct for thermal errors + const Vector3f val{sensor_data.x, sensor_data.y, sensor_data.z}; + + // apply offsets and scale + Vector3f rates{(val - _offset).emult(_scale)}; + + // rotate corrected measurements from sensor to body frame + rates = _board_rotation * rates; + + // correct for in-run bias errors + rates -= _bias; + + vehicle_angular_velocity_s angular_velocity; + angular_velocity.timestamp_sample = sensor_data.timestamp; + rates.copyTo(angular_velocity.xyz); + angular_velocity.timestamp = hrt_absolute_time(); + + _vehicle_angular_velocity_pub.publish(angular_velocity); + } } perf_end(_cycle_perf); @@ -219,6 +288,13 @@ VehicleAngularVelocity::PrintStatus() { PX4_INFO("selected sensor: %d", _selected_sensor); + if (_selected_sensor_device_id != 0) { + PX4_INFO("using sensor_gyro_control: %d (%d)", _selected_sensor_device_id, _selected_sensor_control); + + } else { + PX4_WARN("sensor_gyro_control unavailable for selected sensor: %d (%d)", _selected_sensor_device_id, _selected_sensor); + } + perf_print_counter(_cycle_perf); perf_print_counter(_interval_perf); perf_print_counter(_sensor_latency_perf); diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp index fa2e15cb08..f9eaaf5cd7 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp @@ -50,6 +50,7 @@ #include #include +#include #include class VehicleAngularVelocity : public ModuleParams, public px4::WorkItem @@ -89,12 +90,19 @@ private: uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)}; /**< sensor thermal correction subscription */ uORB::SubscriptionCallbackWorkItem _sensor_selection_sub{this, ORB_ID(sensor_selection)}; /**< selected primary sensor subscription */ + uORB::SubscriptionCallbackWorkItem _sensor_sub[MAX_SENSOR_COUNT] { /**< sensor data subscription */ {this, ORB_ID(sensor_gyro), 0}, {this, ORB_ID(sensor_gyro), 1}, {this, ORB_ID(sensor_gyro), 2} }; + uORB::SubscriptionCallbackWorkItem _sensor_control_sub[MAX_SENSOR_COUNT] { /**< sensor control data subscription */ + {this, ORB_ID(sensor_gyro_control), 0}, + {this, ORB_ID(sensor_gyro_control), 1}, + {this, ORB_ID(sensor_gyro_control), 2} + }; + matrix::Dcmf _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ matrix::Vector3f _offset; @@ -105,6 +113,9 @@ private: perf_counter_t _interval_perf; perf_counter_t _sensor_latency_perf; + uint32_t _selected_sensor_device_id{0}; uint8_t _selected_sensor{0}; + uint8_t _selected_sensor_control{0}; + bool _sensor_control_available{false}; };