From 697dbfb9f8bebf69e7b07d1160d85384be7f6b1d Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 21 Jan 2020 16:47:38 -0500 Subject: [PATCH] sensors/vehicle_imu: incremental step towards multi-EKF --- msg/CMakeLists.txt | 1 + msg/tools/uorb_rtps_message_ids.yaml | 2 + msg/vehicle_imu.msg | 15 ++ src/modules/ekf2/ekf2_main.cpp | 161 +++++++++---- src/modules/ekf2/ekf2_params.c | 13 + src/modules/sensors/CMakeLists.txt | 2 + src/modules/sensors/sensor_params.c | 2 +- src/modules/sensors/sensors.cpp | 52 ++++ .../VehicleAcceleration.hpp | 36 +-- .../VehicleAngularVelocity.hpp | 36 +-- .../sensors/vehicle_imu/CMakeLists.txt | 38 +++ .../sensors/vehicle_imu/VehicleIMU.cpp | 228 ++++++++++++++++++ .../sensors/vehicle_imu/VehicleIMU.hpp | 104 ++++++++ 13 files changed, 611 insertions(+), 79 deletions(-) create mode 100644 msg/vehicle_imu.msg create mode 100644 src/modules/sensors/vehicle_imu/CMakeLists.txt create mode 100644 src/modules/sensors/vehicle_imu/VehicleIMU.cpp create mode 100644 src/modules/sensors/vehicle_imu/VehicleIMU.hpp diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 62ae55e38a..04017f2f49 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -144,6 +144,7 @@ set(msg_files vehicle_control_mode.msg vehicle_global_position.msg vehicle_gps_position.msg + vehicle_imu.msg vehicle_land_detected.msg vehicle_local_position.msg vehicle_local_position_setpoint.msg diff --git a/msg/tools/uorb_rtps_message_ids.yaml b/msg/tools/uorb_rtps_message_ids.yaml index fb046578f4..e05bc2187b 100644 --- a/msg/tools/uorb_rtps_message_ids.yaml +++ b/msg/tools/uorb_rtps_message_ids.yaml @@ -275,6 +275,8 @@ rtps: id: 122 - msg: sensor_gyro_integrated id: 123 + - msg: vehicle_imu + id: 124 ########## multi topics: begin ########## - msg: actuator_controls_0 id: 150 diff --git a/msg/vehicle_imu.msg b/msg/vehicle_imu.msg new file mode 100644 index 0000000000..0f036b4936 --- /dev/null +++ b/msg/vehicle_imu.msg @@ -0,0 +1,15 @@ +# IMU readings in SI-unit form. + +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample + +uint32 accel_device_id # Accelerometer unique device ID for the sensor that does not change between power cycles +uint32 gyro_device_id # Gyroscope unique device ID for the sensor that does not change between power cycles + +float32[3] delta_angle # delta angle in the NED board axis in rad/s over the integration time frame (dt) +float32[3] delta_velocity # delta velocity in the NED board axis in m/s over the integration time frame (dt) + +uint16 dt # integration period in us + +uint8 integrated_samples # number of samples integrated +uint8 clip_count # total clip count per integration period on any axis diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 4ea439792b..38b3a9ff7e 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -71,6 +71,7 @@ #include #include #include +#include #include #include #include @@ -92,7 +93,7 @@ using math::constrain; using namespace time_literals; -class Ekf2 final : public ModuleBase, public ModuleParams, public px4::WorkItem +class Ekf2 final : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { public: explicit Ekf2(bool replay_mode = false); @@ -129,7 +130,7 @@ private: template bool update_mag_decl(Param &mag_decl_param); - bool publish_attitude(const sensor_combined_s &sensors, const hrt_abstime &now); + bool publish_attitude(const hrt_abstime &now); bool publish_wind_estimate(const hrt_abstime ×tamp); /* @@ -241,7 +242,15 @@ private: uORB::Subscription _status_sub{ORB_ID(vehicle_status)}; uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; - uORB::SubscriptionCallbackWorkItem _sensors_sub{this, ORB_ID(sensor_combined)}; + uORB::SubscriptionCallbackWorkItem _sensor_combined_sub{this, ORB_ID(sensor_combined)}; + static constexpr int MAX_SENSOR_COUNT = 3; + uORB::SubscriptionCallbackWorkItem _vehicle_imu_subs[MAX_SENSOR_COUNT] { + {this, ORB_ID(vehicle_imu), 0}, + {this, ORB_ID(vehicle_imu), 1}, + {this, ORB_ID(vehicle_imu), 2} + }; + int _imu_sub_index{-1}; + bool _callback_registered{false}; // because we can have several distance sensor instances with different orientations uORB::Subscription _range_finder_subs[ORB_MULTI_MAX_INSTANCES] {{ORB_ID(distance_sensor), 0}, {ORB_ID(distance_sensor), 1}, {ORB_ID(distance_sensor), 2}, {ORB_ID(distance_sensor), 3}}; @@ -418,6 +427,8 @@ private: (ParamExtFloat) _param_ekf2_of_gate, ///< optical flow fusion innovation consistency gate size (STD) + (ParamInt) _param_ekf2_imu_id, + // sensor positions in body frame (ParamExtFloat) _param_ekf2_imu_pos_x, ///< X position of IMU in body frame (m) (ParamExtFloat) _param_ekf2_imu_pos_y, ///< Y position of IMU in body frame (m) @@ -521,7 +532,7 @@ private: Ekf2::Ekf2(bool replay_mode): ModuleParams(nullptr), - WorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl), _replay_mode(replay_mode), _ekf_update_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": update")), _params(_ekf.getParamHandle()), @@ -638,14 +649,39 @@ Ekf2::~Ekf2() perf_free(_ekf_update_perf); } -bool -Ekf2::init() +bool Ekf2::init() { - if (!_sensors_sub.registerCallback()) { - PX4_ERR("sensor combined callback registration failed!"); - return false; + const uint32_t device_id = _param_ekf2_imu_id.get(); + + // if EKF2_IMU_ID is non-zero we use the corresponding IMU, otherwise the voted primary (sensor_combined) + if (device_id != 0) { + for (int i = 0; i < MAX_SENSOR_COUNT; i++) { + vehicle_imu_s imu{}; + + if (_vehicle_imu_subs[i].copy(&imu)) { + if ((imu.accel_device_id > 0) && (imu.accel_device_id == device_id)) { + if (_vehicle_imu_subs[i].registerCallback()) { + PX4_INFO("subscribed to vehicle_imu:%d (%d)", i, device_id); + _imu_sub_index = i; + _callback_registered = true; + return true; + } + } + } + } + + } else { + _imu_sub_index = -1; + + if (_sensor_combined_sub.registerCallback()) { + _callback_registered = true; + return true; + } } + PX4_WARN("failed to register callback, retrying in 1 second"); + ScheduleDelayed(1_s); // retry in 1 second + return true; } @@ -698,14 +734,56 @@ bool Ekf2::update_mag_decl(Param &mag_decl_param) void Ekf2::Run() { if (should_exit()) { - _sensors_sub.unregisterCallback(); + _sensor_combined_sub.unregisterCallback(); + + for (auto &i : _vehicle_imu_subs) { + i.unregisterCallback(); + } + exit_and_cleanup(); return; } - sensor_combined_s sensors; + if (!_callback_registered) { + init(); + return; + } - if (_sensors_sub.update(&sensors)) { + bool updated = false; + imuSample imu_sample_new; + + hrt_abstime imu_dt = 0; // for tracking time slip later + sensor_bias_s bias{}; + + if (_imu_sub_index >= 0) { + vehicle_imu_s imu; + updated = _vehicle_imu_subs[_imu_sub_index].update(&imu); + + imu_sample_new.time_us = imu.timestamp_sample; + imu_sample_new.delta_ang_dt = imu.dt * 1.e-6f; + imu_sample_new.delta_ang = Vector3f{imu.delta_angle}; + imu_sample_new.delta_vel_dt = imu.dt * 1.e-6f; + imu_sample_new.delta_vel = Vector3f{imu.delta_velocity}; + + imu_dt = imu.dt; + + bias.accel_device_id = imu.accel_device_id; + bias.gyro_device_id = imu.gyro_device_id; + + } else { + sensor_combined_s sensor_combined; + updated = _sensor_combined_sub.update(&sensor_combined); + + imu_sample_new.time_us = sensor_combined.timestamp; + imu_sample_new.delta_ang_dt = sensor_combined.gyro_integral_dt * 1.e-6f; + imu_sample_new.delta_ang = Vector3f{sensor_combined.gyro_rad} * imu_sample_new.delta_ang_dt; + imu_sample_new.delta_vel_dt = sensor_combined.accelerometer_integral_dt * 1.e-6f; + imu_sample_new.delta_vel = Vector3f{sensor_combined.accelerometer_m_s2} * imu_sample_new.delta_vel_dt; + + imu_dt = sensor_combined.gyro_integral_dt; + } + + if (updated) { // check for parameter updates if (_parameter_update_sub.updated()) { @@ -717,9 +795,11 @@ void Ekf2::Run() updateParams(); } + const hrt_abstime now = imu_sample_new.time_us; + // ekf2_timestamps (using 0.1 ms relative timestamps) ekf2_timestamps_s ekf2_timestamps{}; - ekf2_timestamps.timestamp = sensors.timestamp; + ekf2_timestamps.timestamp = now; ekf2_timestamps.airspeed_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID; ekf2_timestamps.distance_sensor_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID; @@ -747,14 +827,17 @@ void Ekf2::Run() if (_sensor_selection_sub.copy(&_sensor_selection)) { if ((sensor_selection_prev.timestamp > 0) && (_sensor_selection.timestamp > sensor_selection_prev.timestamp)) { - if (_sensor_selection.accel_device_id != sensor_selection_prev.accel_device_id) { - PX4_WARN("accel id changed, resetting IMU bias"); - _imu_bias_reset_request = true; - } - if (_sensor_selection.gyro_device_id != sensor_selection_prev.gyro_device_id) { - PX4_WARN("gyro id changed, resetting IMU bias"); - _imu_bias_reset_request = true; + if (_imu_sub_index < 0) { + if (_sensor_selection.accel_device_id != sensor_selection_prev.accel_device_id) { + PX4_WARN("accel id changed, resetting IMU bias"); + _imu_bias_reset_request = true; + } + + if (_sensor_selection.gyro_device_id != sensor_selection_prev.gyro_device_id) { + PX4_WARN("gyro id changed, resetting IMU bias"); + _imu_bias_reset_request = true; + } } } } @@ -765,20 +848,11 @@ void Ekf2::Run() _imu_bias_reset_request = !_ekf.reset_imu_bias(); } - const hrt_abstime now = sensors.timestamp; - // push imu data into estimator - imuSample imu_sample_new; - imu_sample_new.time_us = now; - imu_sample_new.delta_ang_dt = sensors.gyro_integral_dt * 1.e-6f; - imu_sample_new.delta_ang = Vector3f{sensors.gyro_rad} * imu_sample_new.delta_ang_dt; - imu_sample_new.delta_vel_dt = sensors.accelerometer_integral_dt * 1.e-6f; - imu_sample_new.delta_vel = Vector3f{sensors.accelerometer_m_s2} * imu_sample_new.delta_vel_dt; - _ekf.setIMUData(imu_sample_new); // publish attitude immediately (uses quaternion from output predictor) - publish_attitude(sensors, now); + publish_attitude(now); // read mag data if (_magnetometer_sub.updated()) { @@ -1106,7 +1180,7 @@ void Ekf2::Run() // run the EKF update and output perf_begin(_ekf_update_perf); - const bool updated = _ekf.update(); + const bool ekf_updated = _ekf.update(); perf_end(_ekf_update_perf); // integrate time to monitor time slippage @@ -1115,11 +1189,11 @@ void Ekf2::Run() _last_time_slip_us = 0; } else if (_start_time_us > 0) { - _integrated_time_us += sensors.gyro_integral_dt; + _integrated_time_us += imu_dt; _last_time_slip_us = (now - _start_time_us) - _integrated_time_us; } - if (updated) { + if (ekf_updated) { filter_control_status_u control_status; _ekf.get_control_mode(&control_status.value); @@ -1205,9 +1279,10 @@ void Ekf2::Run() // Vehicle odometry angular rates float gyro_bias[3]; _ekf.get_gyro_bias(gyro_bias); - odom.rollspeed = sensors.gyro_rad[0] - gyro_bias[0]; - odom.pitchspeed = sensors.gyro_rad[1] - gyro_bias[1]; - odom.yawspeed = sensors.gyro_rad[2] - gyro_bias[2]; + const Vector3f rates{imu_sample_new.delta_ang * imu_sample_new.delta_ang_dt}; + odom.rollspeed = rates(0) - gyro_bias[0]; + odom.pitchspeed = rates(1) - gyro_bias[1]; + odom.yawspeed = rates(2) - gyro_bias[2]; lpos.dist_bottom_valid = _ekf.get_terrain_valid(); @@ -1382,12 +1457,14 @@ void Ekf2::Run() { // publish all corrected sensor readings and bias estimates after mag calibration is updated above - sensor_bias_s bias{}; - bias.timestamp = now; - bias.gyro_device_id = _sensor_selection.gyro_device_id; - bias.accel_device_id = _sensor_selection.accel_device_id; + // take device ids from sensor_selection_s if not using specific vehicle_imu_s + if (_imu_sub_index < 0) { + bias.gyro_device_id = _sensor_selection.gyro_device_id; + bias.accel_device_id = _sensor_selection.accel_device_id; + } + bias.mag_device_id = _sensor_selection.mag_device_id; // In-run bias estimates @@ -1593,7 +1670,7 @@ void Ekf2::Run() // calculate noise filtered velocity innovations which are used for pre-flight checking if (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) { - float dt_seconds = sensors.accelerometer_integral_dt * 1e-6f; + float dt_seconds = imu_sample_new.delta_ang_dt; runPreFlightChecks(dt_seconds, control_status, _vehicle_status, innovations); } else { @@ -1671,7 +1748,7 @@ int Ekf2::getRangeSubIndex() return -1; } -bool Ekf2::publish_attitude(const sensor_combined_s &sensors, const hrt_abstime &now) +bool Ekf2::publish_attitude(const hrt_abstime &now) { if (_ekf.attitude_valid()) { // generate vehicle attitude quaternion data diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c index 63a32c6a36..24782e5342 100644 --- a/src/modules/ekf2/ekf2_params.c +++ b/src/modules/ekf2/ekf2_params.c @@ -805,6 +805,19 @@ PARAM_DEFINE_FLOAT(EKF2_TERR_NOISE, 5.0f); */ PARAM_DEFINE_FLOAT(EKF2_TERR_GRAD, 0.5f); +/** + * Device id of IMU + * + * Set to 0 to use system selected (sensor_combined) IMU, + * otherwise set to the device id of the desired IMU (vehicle_imu). + * + * @group EKF2 + * @value 0 System Primary + * @category Developer + * + */ +PARAM_DEFINE_INT32(EKF2_IMU_ID, 0); + /** * X position of IMU in body frame * diff --git a/src/modules/sensors/CMakeLists.txt b/src/modules/sensors/CMakeLists.txt index cdc948bc38..a9b873ae4f 100644 --- a/src/modules/sensors/CMakeLists.txt +++ b/src/modules/sensors/CMakeLists.txt @@ -33,6 +33,7 @@ add_subdirectory(vehicle_acceleration) add_subdirectory(vehicle_angular_velocity) +add_subdirectory(vehicle_imu) px4_add_module( MODULE modules__sensors @@ -51,4 +52,5 @@ px4_add_module( mathlib vehicle_acceleration vehicle_angular_velocity + vehicle_imu ) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index cb28dc2503..a3d9034423 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -260,7 +260,7 @@ PARAM_DEFINE_FLOAT(IMU_GYRO_CUTOFF, 30.0f); /** * Gyro control data maximum publication rate * -* This is the maximum rate the gyro control data (sensor_gyro_control) will be allowed to publish at. +* This is the maximum rate the gyro control data (sensor_gyro) will be allowed to publish at. * Set to 0 to disable and publish at the native sensor sample rate. * * @min 0 diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 8d87810906..2001a7a775 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -72,6 +72,7 @@ #include "voted_sensors_update.h" #include "vehicle_acceleration/VehicleAcceleration.hpp" #include "vehicle_angular_velocity/VehicleAngularVelocity.hpp" +#include "vehicle_imu/VehicleIMU.hpp" using namespace sensors; using namespace time_literals; @@ -141,6 +142,9 @@ private: VehicleAcceleration _vehicle_acceleration; VehicleAngularVelocity _vehicle_angular_velocity; + static constexpr int MAX_SENSOR_COUNT = 3; + VehicleIMU *_vehicle_imu_list[MAX_SENSOR_COUNT] {}; + /** * Update our local parameter cache. @@ -173,6 +177,8 @@ private: */ void adc_poll(); + void InitializeVehicleIMU(); + }; Sensors::Sensors(bool hil_enabled) : @@ -188,12 +194,20 @@ Sensors::Sensors(bool hil_enabled) : _vehicle_acceleration.Start(); _vehicle_angular_velocity.Start(); + + InitializeVehicleIMU(); } Sensors::~Sensors() { _vehicle_acceleration.Stop(); _vehicle_angular_velocity.Stop(); + + for (auto &i : _vehicle_imu_list) { + if (i != nullptr) { + i->Stop(); + } + } } int @@ -376,6 +390,37 @@ Sensors::adc_poll() #endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */ } +void Sensors::InitializeVehicleIMU() +{ + // create a VehicleIMU instance for each accel/gyro pair + for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) { + if (_vehicle_imu_list[i] == nullptr) { + + uORB::Subscription accel_sub{ORB_ID(sensor_accel_integrated), i}; + sensor_accel_integrated_s accel{}; + accel_sub.copy(&accel); + + uORB::Subscription gyro_sub{ORB_ID(sensor_gyro_integrated), i}; + sensor_gyro_integrated_s gyro{}; + gyro_sub.copy(&gyro); + + if (accel.device_id > 0 && gyro.device_id > 0) { + VehicleIMU *imu = new VehicleIMU(i, i); + + if (imu != nullptr) { + // Start VehicleIMU instance and store + if (imu->Start()) { + _vehicle_imu_list[i] = imu; + + } else { + delete imu; + } + } + } + } + } +} + void Sensors::run() { @@ -482,6 +527,7 @@ Sensors::run() */ if (!_armed && hrt_elapsed_time(&last_config_update) > 500_ms) { _voted_sensors_update.initializeSensors(); + InitializeVehicleIMU(); last_config_update = hrt_absolute_time(); } else { @@ -524,6 +570,12 @@ int Sensors::print_status() _vehicle_acceleration.PrintStatus(); _vehicle_angular_velocity.PrintStatus(); + for (auto &i : _vehicle_imu_list) { + if (i != nullptr) { + i->PrintStatus(); + } + } + return 0; } diff --git a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp index f92eb58254..2ec0586ccc 100644 --- a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp +++ b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp @@ -58,14 +58,14 @@ public: VehicleAcceleration(); ~VehicleAcceleration() override; - void Run() override; + bool Start(); + void Stop(); - bool Start(); - void Stop(); - - void PrintStatus(); + void PrintStatus(); private: + void Run() override; + void ParametersUpdate(bool force = false); void SensorBiasUpdate(bool force = false); void SensorCorrectionsUpdate(bool force = false); @@ -81,26 +81,26 @@ private: (ParamFloat) _param_sens_board_z_off ) - uORB::Publication _vehicle_acceleration_pub{ORB_ID(vehicle_acceleration)}; + uORB::Publication _vehicle_acceleration_pub{ORB_ID(vehicle_acceleration)}; - uORB::Subscription _params_sub{ORB_ID(parameter_update)}; /**< parameter updates subscription */ - uORB::Subscription _sensor_bias_sub{ORB_ID(sensor_bias)}; /**< sensor in-run bias correction subscription */ - uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)}; /**< sensor thermal correction subscription */ + uORB::Subscription _params_sub{ORB_ID(parameter_update)}; + uORB::Subscription _sensor_bias_sub{ORB_ID(sensor_bias)}; + uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)}; - uORB::SubscriptionCallbackWorkItem _sensor_selection_sub{this, ORB_ID(sensor_selection)}; /**< selected primary sensor subscription */ - uORB::SubscriptionCallbackWorkItem _sensor_sub[MAX_SENSOR_COUNT] { /**< sensor data subscription */ + uORB::SubscriptionCallbackWorkItem _sensor_selection_sub{this, ORB_ID(sensor_selection)}; + uORB::SubscriptionCallbackWorkItem _sensor_sub[MAX_SENSOR_COUNT] { {this, ORB_ID(sensor_accel), 0}, {this, ORB_ID(sensor_accel), 1}, {this, ORB_ID(sensor_accel), 2} }; - matrix::Dcmf _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ + matrix::Dcmf _board_rotation; - matrix::Vector3f _bias{0.f, 0.f, 0.f}; - matrix::Vector3f _offset{0.f, 0.f, 0.f}; - matrix::Vector3f _scale{1.f, 1.f, 1.f}; + matrix::Vector3f _bias{0.f, 0.f, 0.f}; + matrix::Vector3f _offset{0.f, 0.f, 0.f}; + matrix::Vector3f _scale{1.f, 1.f, 1.f}; - uint32_t _selected_sensor_device_id{0}; - uint8_t _selected_sensor_sub_index{0}; - int8_t _corrections_selected_instance{-1}; + uint32_t _selected_sensor_device_id{0}; + uint8_t _selected_sensor_sub_index{0}; + int8_t _corrections_selected_instance{-1}; }; diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp index 0060c66241..317f260dc6 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp @@ -58,14 +58,14 @@ public: VehicleAngularVelocity(); ~VehicleAngularVelocity() override; - void Run() override; + bool Start(); + void Stop(); - bool Start(); - void Stop(); - - void PrintStatus(); + void PrintStatus(); private: + void Run() override; + void ParametersUpdate(bool force = false); void SensorBiasUpdate(bool force = false); void SensorCorrectionsUpdate(bool force = false); @@ -81,26 +81,26 @@ private: (ParamFloat) _param_sens_board_z_off ) - uORB::Publication _vehicle_angular_velocity_pub{ORB_ID(vehicle_angular_velocity)}; + uORB::Publication _vehicle_angular_velocity_pub{ORB_ID(vehicle_angular_velocity)}; - uORB::Subscription _params_sub{ORB_ID(parameter_update)}; /**< parameter updates subscription */ - uORB::Subscription _sensor_bias_sub{ORB_ID(sensor_bias)}; /**< sensor in-run bias correction subscription */ - uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)}; /**< sensor thermal correction subscription */ + uORB::Subscription _params_sub{ORB_ID(parameter_update)}; + uORB::Subscription _sensor_bias_sub{ORB_ID(sensor_bias)}; + uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)}; - uORB::SubscriptionCallbackWorkItem _sensor_selection_sub{this, ORB_ID(sensor_selection)}; /**< selected primary sensor subscription */ - uORB::SubscriptionCallbackWorkItem _sensor_sub[MAX_SENSOR_COUNT] { /**< sensor data subscription */ + uORB::SubscriptionCallbackWorkItem _sensor_selection_sub{this, ORB_ID(sensor_selection)}; + uORB::SubscriptionCallbackWorkItem _sensor_sub[MAX_SENSOR_COUNT] { {this, ORB_ID(sensor_gyro), 0}, {this, ORB_ID(sensor_gyro), 1}, {this, ORB_ID(sensor_gyro), 2} }; - matrix::Dcmf _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ + matrix::Dcmf _board_rotation; - matrix::Vector3f _bias{0.f, 0.f, 0.f}; - matrix::Vector3f _offset{0.f, 0.f, 0.f}; - matrix::Vector3f _scale{1.f, 1.f, 1.f}; + matrix::Vector3f _bias{0.f, 0.f, 0.f}; + matrix::Vector3f _offset{0.f, 0.f, 0.f}; + matrix::Vector3f _scale{1.f, 1.f, 1.f}; - uint32_t _selected_sensor_device_id{0}; - uint8_t _selected_sensor_sub_index{0}; - int8_t _corrections_selected_instance{-1}; + uint32_t _selected_sensor_device_id{0}; + uint8_t _selected_sensor_sub_index{0}; + int8_t _corrections_selected_instance{-1}; }; diff --git a/src/modules/sensors/vehicle_imu/CMakeLists.txt b/src/modules/sensors/vehicle_imu/CMakeLists.txt new file mode 100644 index 0000000000..36a6edaac6 --- /dev/null +++ b/src/modules/sensors/vehicle_imu/CMakeLists.txt @@ -0,0 +1,38 @@ +############################################################################ +# +# Copyright (c) 2020 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_library(vehicle_imu + VehicleIMU.cpp + VehicleIMU.hpp +) +target_link_libraries(vehicle_imu PRIVATE px4_work_queue) diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp new file mode 100644 index 0000000000..743ea310d0 --- /dev/null +++ b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp @@ -0,0 +1,228 @@ +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "VehicleIMU.hpp" + +#include + +using namespace matrix; +using namespace time_literals; +using math::radians; + +VehicleIMU::VehicleIMU(uint8_t accel_index, uint8_t gyro_index) : + ModuleParams(nullptr), + WorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl), + _sensor_accel_integrated_sub(this, ORB_ID(sensor_accel_integrated), accel_index), + _sensor_gyro_integrated_sub(this, ORB_ID(sensor_gyro_integrated), gyro_index) +{ +} + +VehicleIMU::~VehicleIMU() +{ + Stop(); + delete _name; +} + +bool VehicleIMU::Start() +{ + // force initial updates + ParametersUpdate(true); + + return _sensor_accel_integrated_sub.registerCallback() && _sensor_gyro_integrated_sub.registerCallback(); +} + +void VehicleIMU::Stop() +{ + Deinit(); + + // clear all registered callbacks + _sensor_accel_integrated_sub.unregisterCallback(); + _sensor_gyro_integrated_sub.unregisterCallback(); +} + +void VehicleIMU::SensorCorrectionsUpdate(bool force) +{ + // check if the selected sensor has updated + if (_sensor_correction_sub.updated() || force) { + sensor_correction_s corrections{}; + + if (_sensor_correction_sub.copy(&corrections)) { + + // accel + if (_accel_device_id > 0) { + if ((_corrections_selected_accel_instance < 0) || force) { + _corrections_selected_accel_instance = -1; + + // find sensor_corrections index + for (int i = 0; i < MAX_SENSOR_COUNT; i++) { + if (corrections.accel_device_ids[i] == _accel_device_id) { + _corrections_selected_accel_instance = i; + } + } + } + + switch (_corrections_selected_accel_instance) { + case 0: + _accel_offset = Vector3f{corrections.accel_offset_0}; + _accel_scale = Vector3f{corrections.accel_scale_0}; + break; + case 1: + _accel_offset = Vector3f{corrections.accel_offset_1}; + _accel_scale = Vector3f{corrections.accel_scale_1}; + break; + case 2: + _accel_offset = Vector3f{corrections.accel_offset_2}; + _accel_scale = Vector3f{corrections.accel_scale_2}; + break; + default: + _accel_offset = Vector3f{0.f, 0.f, 0.f}; + _accel_scale = Vector3f{1.f, 1.f, 1.f}; + } + } + + // gyro + if (_gyro_device_id > 0) { + if ((_corrections_selected_gyro_instance < 0) || force) { + _corrections_selected_gyro_instance = -1; + + // find sensor_corrections index + for (int i = 0; i < MAX_SENSOR_COUNT; i++) { + if (corrections.gyro_device_ids[i] == _gyro_device_id) { + _corrections_selected_gyro_instance = i; + } + } + } + + switch (_corrections_selected_gyro_instance) { + case 0: + _gyro_offset = Vector3f{corrections.gyro_offset_0}; + _gyro_scale = Vector3f{corrections.gyro_scale_0}; + break; + case 1: + _gyro_offset = Vector3f{corrections.gyro_offset_1}; + _gyro_scale = Vector3f{corrections.gyro_scale_1}; + break; + case 2: + _gyro_offset = Vector3f{corrections.gyro_offset_2}; + _gyro_scale = Vector3f{corrections.gyro_scale_2}; + break; + default: + _gyro_offset = Vector3f{0.f, 0.f, 0.f}; + _gyro_scale = Vector3f{1.f, 1.f, 1.f}; + } + } + } + } +} + +void VehicleIMU::ParametersUpdate(bool force) +{ + // Check if parameters have changed + if (_params_sub.updated() || force) { + // clear update + parameter_update_s param_update; + _params_sub.copy(¶m_update); + + updateParams(); + + // get transformation matrix from sensor/board to body frame + const Dcmf board_rotation = get_rot_matrix((enum Rotation)_param_sens_board_rot.get()); + + // fine tune the rotation + const Dcmf board_rotation_offset(Eulerf( + radians(_param_sens_board_x_off.get()), + radians(_param_sens_board_y_off.get()), + radians(_param_sens_board_z_off.get()))); + + _board_rotation = board_rotation_offset * board_rotation; + } +} + +void VehicleIMU::Run() +{ + if (_sensor_accel_integrated_sub.updated() && _sensor_gyro_integrated_sub.updated()) { + sensor_accel_integrated_s accel; + _sensor_accel_integrated_sub.copy(&accel); + _accel_device_id = accel.device_id; + + sensor_gyro_integrated_s gyro; + _sensor_gyro_integrated_sub.copy(&gyro); + _gyro_device_id = gyro.device_id; + + SensorCorrectionsUpdate(); + ParametersUpdate(); + + // delta angle: apply offsets, scale, and board rotation + Vector3f delta_angle{gyro.delta_angle}; + const float gyro_dt = 1.e-6f * gyro.dt; + // apply offsets + delta_angle = delta_angle - (_gyro_offset * gyro_dt); + // apply scale + delta_angle = delta_angle.emult(_gyro_scale); + // apply board rotation + delta_angle = _board_rotation * delta_angle; + + // delta velocity: apply offsets, scale, and board rotation + Vector3f delta_velocity{accel.delta_velocity}; + const float accel_dt = 1.e-6f * accel.dt; + // apply offsets + delta_velocity = delta_velocity - (_accel_offset * accel_dt); + // apply scale + delta_velocity = delta_velocity.emult(_accel_scale); + // apply board rotation + delta_velocity = _board_rotation * delta_velocity; + + + // publich vehicle_imu + vehicle_imu_s imu; + + imu.timestamp_sample = accel.timestamp_sample; + imu.accel_device_id = accel.device_id; + imu.gyro_device_id = gyro.device_id; + + delta_angle.copyTo(imu.delta_angle); + delta_velocity.copyTo(imu.delta_velocity); + + imu.dt = accel.dt; + imu.integrated_samples = accel.samples; + imu.clip_count = accel.clip_count; + imu.timestamp = hrt_absolute_time(); + + _vehicle_imu_pub.publish(imu); + } +} + +void VehicleIMU::PrintStatus() +{ + PX4_INFO("selected IMU: accel: %d gyro: %d", _accel_device_id, _gyro_device_id); +} diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.hpp b/src/modules/sensors/vehicle_imu/VehicleIMU.hpp new file mode 100644 index 0000000000..528fd4ac81 --- /dev/null +++ b/src/modules/sensors/vehicle_imu/VehicleIMU.hpp @@ -0,0 +1,104 @@ +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class VehicleIMU : public ModuleParams, public px4::WorkItem +{ +public: + VehicleIMU() = delete; + VehicleIMU(uint8_t accel_index = 0, uint8_t gyro_index = 0); + + ~VehicleIMU() override; + + bool Start(); + void Stop(); + + void PrintStatus(); + +private: + void Run() override; + + void ParametersUpdate(bool force = false); + void SensorCorrectionsUpdate(bool force = false); + + static constexpr int MAX_SENSOR_COUNT = 3; + + DEFINE_PARAMETERS( + (ParamInt) _param_sens_board_rot, + + (ParamFloat) _param_sens_board_x_off, + (ParamFloat) _param_sens_board_y_off, + (ParamFloat) _param_sens_board_z_off + ) + + uORB::PublicationMulti _vehicle_imu_pub{ORB_ID(vehicle_imu)}; + + uORB::Subscription _params_sub{ORB_ID(parameter_update)}; + uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)}; + + uORB::SubscriptionCallbackWorkItem _sensor_accel_integrated_sub; + uORB::SubscriptionCallbackWorkItem _sensor_gyro_integrated_sub; + + matrix::Dcmf _board_rotation; + + matrix::Vector3f _accel_offset{0.f, 0.f, 0.f}; + matrix::Vector3f _gyro_offset{0.f, 0.f, 0.f}; + matrix::Vector3f _accel_scale{1.f, 1.f, 1.f}; + matrix::Vector3f _gyro_scale{1.f, 1.f, 1.f}; + + char *_name{nullptr}; + + int8_t _corrections_selected_accel_instance{-1}; + int8_t _corrections_selected_gyro_instance{-1}; + + uint32_t _accel_device_id{0}; + uint32_t _gyro_device_id{0}; +};