From d17488e4a784d83bdb918603499d74c142494db4 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 1 Aug 2019 11:34:27 -0400 Subject: [PATCH] sensors: create vehicle_acceleration module --- msg/CMakeLists.txt | 1 + msg/sensor_bias.msg | 16 +- msg/tools/uorb_rtps_message_ids.yaml | 2 + msg/vehicle_acceleration.msg | 6 + src/modules/commander/Commander.cpp | 6 +- src/modules/commander/Commander.hpp | 5 +- src/modules/ekf2/ekf2_main.cpp | 28 +-- .../FixedwingPositionControl.cpp | 12 +- .../FixedwingPositionControl.hpp | 6 +- .../land_detector/FixedwingLandDetector.cpp | 7 +- .../land_detector/FixedwingLandDetector.h | 6 +- .../land_detector/MulticopterLandDetector.cpp | 15 +- .../land_detector/MulticopterLandDetector.h | 6 +- .../LandingTargetEstimator.cpp | 9 +- .../LandingTargetEstimator.h | 14 +- src/modules/mavlink/mavlink_messages.cpp | 18 +- .../RoverPositionControl.cpp | 6 +- .../RoverPositionControl.hpp | 4 +- src/modules/sensors/CMakeLists.txt | 4 +- src/modules/sensors/sensors.cpp | 13 +- .../vehicle_acceleration/CMakeLists.txt | 37 +++ .../VehicleAcceleration.cpp | 225 ++++++++++++++++++ .../VehicleAcceleration.hpp | 110 +++++++++ .../VehicleAngularVelocity.cpp | 56 +++-- .../VehicleAngularVelocity.hpp | 20 +- 25 files changed, 493 insertions(+), 139 deletions(-) create mode 100644 msg/vehicle_acceleration.msg create mode 100644 src/modules/sensors/vehicle_acceleration/CMakeLists.txt create mode 100644 src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp create mode 100644 src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 24f94f457862..67ea1d8605ff 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -122,6 +122,7 @@ set(msg_files uavcan_parameter_value.msg ulog_stream.msg ulog_stream_ack.msg + vehicle_acceleration.msg vehicle_air_data.msg vehicle_angular_velocity.msg vehicle_attitude.msg diff --git a/msg/sensor_bias.msg b/msg/sensor_bias.msg index 29a14cb08390..ce81099c99e9 100644 --- a/msg/sensor_bias.msg +++ b/msg/sensor_bias.msg @@ -5,20 +5,10 @@ uint64 timestamp # time since system start (microseconds) -float32 accel_x # Bias corrected acceleration in body X axis (in m/s^2) -float32 accel_y # Bias corrected acceleration in body Y axis (in m/s^2) -float32 accel_z # Bias corrected acceleration in body Z axis (in m/s^2) - # In-run bias estimates (subtract from uncorrected data) -float32 gyro_x_bias # X gyroscope in-run bias in body frame (rad/s, x forward) -float32 gyro_y_bias # Y gyroscope in-run bias in body frame (rad/s, y right) -float32 gyro_z_bias # Z gyroscope in-run bias in body frame (rad/s, z down) +float32[3] gyro_bias # gyroscope in-run bias in body frame (rad/s) -float32 accel_x_bias # X accelerometer in-run bias in body frame (m/s^2, x forward) -float32 accel_y_bias # Y accelerometer in-run bias in body frame (m/s^2, y right) -float32 accel_z_bias # Z accelerometer in-run bias in body frame (m/s^2, z down) +float32[3] accel_bias # accelerometer in-run bias in body frame (m/s^2) -float32 mag_x_bias # X magnetometer in-run bias in body frame (Gauss, x forward) -float32 mag_y_bias # Y magnetometer in-run bias in body frame (Gauss, y right) -float32 mag_z_bias # Z magnetometer in-run bias in body frame (Gauss, z down) +float32[3] mag_bias # magnetometer in-run bias in body frame (Gauss) diff --git a/msg/tools/uorb_rtps_message_ids.yaml b/msg/tools/uorb_rtps_message_ids.yaml index 5af024de0617..d3208beb8472 100644 --- a/msg/tools/uorb_rtps_message_ids.yaml +++ b/msg/tools/uorb_rtps_message_ids.yaml @@ -256,6 +256,8 @@ rtps: id: 111 - msg: vehicle_angular_velocity id: 112 + - msg: vehicle_acceleration + id: 113 # multi topics - msg: actuator_controls_0 id: 120 diff --git a/msg/vehicle_acceleration.msg b/msg/vehicle_acceleration.msg new file mode 100644 index 000000000000..9f6e8664727d --- /dev/null +++ b/msg/vehicle_acceleration.msg @@ -0,0 +1,6 @@ + +uint64 timestamp # time since system start (microseconds) + +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + +float32[3] xyz # Bias corrected acceleration (including gravity) in body axis (in m/s^2) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 8e24ce1c49ac..8b67b5836a8b 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -3901,8 +3901,8 @@ void Commander::airspeed_use_check() _airspeed_sub.update(); const airspeed_s &airspeed = _airspeed_sub.get(); - _sensor_bias_sub.update(); - const sensor_bias_s &sensors = _sensor_bias_sub.get(); + vehicle_acceleration_s accel{}; + _vehicle_acceleration_sub.copy(&accel); bool is_fixed_wing = status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING; @@ -3988,7 +3988,7 @@ void Commander::airspeed_use_check() float max_lift_ratio = fmaxf(airspeed.indicated_airspeed_m_s, 0.7f) / fmaxf(_airspeed_stall.get(), 1.0f); max_lift_ratio *= max_lift_ratio; - _load_factor_ratio = 0.95f * _load_factor_ratio + 0.05f * (fabsf(sensors.accel_z) / CONSTANTS_ONE_G) / max_lift_ratio; + _load_factor_ratio = 0.95f * _load_factor_ratio + 0.05f * (fabsf(accel.xyz[2]) / CONSTANTS_ONE_G) / max_lift_ratio; _load_factor_ratio = math::constrain(_load_factor_ratio, 0.25f, 2.0f); load_factor_ratio_fail = (_load_factor_ratio > 1.1f); status.load_factor_ratio = _load_factor_ratio; diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 47c9328c5668..ffe7db573874 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -60,8 +60,8 @@ #include #include #include -#include #include +#include #include #include #include @@ -250,11 +250,12 @@ class Commander : public ModuleBase, public ModuleParams bool _print_avoidance_msg_once{false}; // Subscriptions + uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)}; + uORB::SubscriptionData _airspeed_sub{ORB_ID(airspeed)}; uORB::SubscriptionData _estimator_status_sub{ORB_ID(estimator_status)}; uORB::SubscriptionData _mission_result_sub{ORB_ID(mission_result)}; uORB::SubscriptionData _offboard_control_mode_sub{ORB_ID(offboard_control_mode)}; - uORB::SubscriptionData _sensor_bias_sub{ORB_ID(sensor_bias)}; uORB::SubscriptionData _global_position_sub{ORB_ID(vehicle_global_position)}; uORB::SubscriptionData _local_position_sub{ORB_ID(vehicle_local_position)}; diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index cca26abb6b47..60a7792a2b9d 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -1482,31 +1482,17 @@ void Ekf2::run() { // publish all corrected sensor readings and bias estimates after mag calibration is updated above - sensor_bias_s bias; + sensor_bias_s bias{}; bias.timestamp = now; // In-run bias estimates - float gyro_bias[3]; - _ekf.get_gyro_bias(gyro_bias); - bias.gyro_x_bias = gyro_bias[0]; - bias.gyro_y_bias = gyro_bias[1]; - bias.gyro_z_bias = gyro_bias[2]; - - float accel_bias[3]; - _ekf.get_accel_bias(accel_bias); - bias.accel_x_bias = accel_bias[0]; - bias.accel_y_bias = accel_bias[1]; - bias.accel_z_bias = accel_bias[2]; - - bias.mag_x_bias = _last_valid_mag_cal[0]; - bias.mag_y_bias = _last_valid_mag_cal[1]; - bias.mag_z_bias = _last_valid_mag_cal[2]; - - // TODO: remove from sensor_bias? - bias.accel_x = sensors.accelerometer_m_s2[0] - accel_bias[0]; - bias.accel_y = sensors.accelerometer_m_s2[1] - accel_bias[1]; - bias.accel_z = sensors.accelerometer_m_s2[2] - accel_bias[2]; + _ekf.get_gyro_bias(bias.gyro_bias); + _ekf.get_accel_bias(bias.accel_bias); + + bias.mag_bias[0] = _last_valid_mag_cal[0]; + bias.mag_bias[1] = _last_valid_mag_cal[1]; + bias.mag_bias[2] = _last_valid_mag_cal[2]; _sensor_bias_pub.publish(bias); } diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 22a567f7f5aa..e3f360d2b7ec 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -37,8 +37,6 @@ extern "C" __EXPORT int fw_pos_control_l1_main(int argc, char *argv[]); FixedwingPositionControl::FixedwingPositionControl() : ModuleParams(nullptr), - _sub_airspeed(ORB_ID(airspeed)), - _sub_sensors(ORB_ID(sensor_bias)), _loop_perf(perf_alloc(PC_ELAPSED, "fw_l1_control")), _launchDetector(this), _runway_takeoff(this) @@ -346,9 +344,9 @@ FixedwingPositionControl::airspeed_poll() { bool airspeed_valid = _airspeed_valid; - if (!_parameters.airspeed_disabled && _sub_airspeed.update()) { + if (!_parameters.airspeed_disabled && _airspeed_sub.update()) { - const airspeed_s &as = _sub_airspeed.get(); + const airspeed_s &as = _airspeed_sub.get(); if (PX4_ISFINITE(as.indicated_airspeed_m_s) && PX4_ISFINITE(as.true_airspeed_m_s) @@ -1292,7 +1290,7 @@ FixedwingPositionControl::control_takeoff(const Vector2f &curr_pos, const Vector } /* Detect launch using body X (forward) acceleration */ - _launchDetector.update(_sub_sensors.get().accel_x); + _launchDetector.update(_vehicle_acceleration_sub.get().xyz[0]); /* update our copy of the launch detection state */ _launch_detection_state = _launchDetector.getLaunchDetected(); @@ -1750,7 +1748,6 @@ FixedwingPositionControl::run() _alt_reset_counter = _global_pos.alt_reset_counter; _pos_reset_counter = _global_pos.lat_lon_reset_counter; - _sub_sensors.update(); airspeed_poll(); _manual_control_sub.update(&_manual); _pos_sp_triplet_sub.update(&_pos_sp_triplet); @@ -1759,6 +1756,7 @@ FixedwingPositionControl::run() vehicle_control_mode_poll(); _vehicle_land_detected_sub.update(&_vehicle_land_detected); vehicle_status_poll(); + _vehicle_acceleration_sub.update(); _vehicle_rates_sub.update(); Vector2f curr_pos((float)_global_pos.lat, (float)_global_pos.lon); @@ -1931,7 +1929,7 @@ FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float airspee float pitch_for_tecs = _pitch - _parameters.pitchsp_offset_rad; /* filter speed and altitude for controller */ - Vector3f accel_body(_sub_sensors.get().accel_x, _sub_sensors.get().accel_y, _sub_sensors.get().accel_z); + Vector3f accel_body(_vehicle_acceleration_sub.get().xyz); // tailsitters use the multicopter frame as reference, in fixed wing // we need to use the fixed wing frame diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index 5c2eaff6d142..f0dc053e42d6 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -74,8 +74,8 @@ #include #include #include -#include #include +#include #include #include #include @@ -184,8 +184,8 @@ class FixedwingPositionControl final : public ModuleBase _sub_airspeed; - SubscriptionData _sub_sensors; + SubscriptionData _airspeed_sub{ORB_ID(airspeed)}; + SubscriptionData _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)}; perf_counter_t _loop_perf; ///< loop performance counter */ diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index 6d0a865870e0..2a60b865e336 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -45,6 +45,7 @@ #include #include +#include namespace land_detector { @@ -59,7 +60,7 @@ FixedwingLandDetector::FixedwingLandDetector() void FixedwingLandDetector::_update_topics() { _airspeed_sub.update(&_airspeed); - _sensor_bias_sub.update(&_sensor_bias); + _vehicle_acceleration_sub.update(&_vehicle_acceleration); _vehicle_local_position_sub.update(&_vehicle_local_position); } @@ -110,8 +111,8 @@ bool FixedwingLandDetector::_get_landed_state() // A leaking lowpass prevents biases from building up, but // gives a mostly correct response for short impulses. - const float acc_hor = sqrtf(_sensor_bias.accel_x * _sensor_bias.accel_x + - _sensor_bias.accel_y * _sensor_bias.accel_y); + const matrix::Vector3f accel{_vehicle_acceleration.xyz}; + const float acc_hor = sqrtf(accel(0) * accel(0) + accel(1) * accel(1)); _accel_horz_lp = _accel_horz_lp * 0.8f + acc_hor * 0.18f; diff --git a/src/modules/land_detector/FixedwingLandDetector.h b/src/modules/land_detector/FixedwingLandDetector.h index cdfa44f28a1f..0c2d1a01b843 100644 --- a/src/modules/land_detector/FixedwingLandDetector.h +++ b/src/modules/land_detector/FixedwingLandDetector.h @@ -45,7 +45,7 @@ #include #include #include -#include +#include #include #include "LandDetector.h" @@ -75,11 +75,11 @@ class FixedwingLandDetector final : public LandDetector uORB::Subscription _airspeed_sub{ORB_ID(airspeed)}; uORB::Subscription _param_update_sub{ORB_ID(parameter_update)}; - uORB::Subscription _sensor_bias_sub{ORB_ID(sensor_bias)}; + uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)}; uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; airspeed_s _airspeed{}; - sensor_bias_s _sensor_bias{}; + vehicle_acceleration_s _vehicle_acceleration{}; vehicle_local_position_s _vehicle_local_position{}; float _accel_horz_lp{0.0f}; diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index eb06bc955d53..bc06ef057660 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -63,6 +63,7 @@ #include #include +#include #include "MulticopterLandDetector.h" @@ -94,7 +95,7 @@ void MulticopterLandDetector::_update_topics() { _actuator_controls_sub.update(&_actuator_controls); _battery_sub.update(&_battery_status); - _sensor_bias_sub.update(&_sensor_bias); + _vehicle_acceleration_sub.update(&_vehicle_acceleration); _vehicle_angular_velocity_sub.update(&_vehicle_angular_velocity); _vehicle_attitude_sub.update(&_vehicle_attitude); _vehicle_control_mode_sub.update(&_control_mode); @@ -126,17 +127,15 @@ bool MulticopterLandDetector::_get_freefall_state() return false; } - if (_sensor_bias.timestamp == 0) { + if (_vehicle_acceleration.timestamp == 0) { // _sensors is not valid yet, we have to assume we're not falling. return false; } - float acc_norm = _sensor_bias.accel_x * _sensor_bias.accel_x - + _sensor_bias.accel_y * _sensor_bias.accel_y - + _sensor_bias.accel_z * _sensor_bias.accel_z; - acc_norm = sqrtf(acc_norm); //norm of specific force. Should be close to 9.8 m/s^2 when landed. + // norm of specific force. Should be close to 9.8 m/s^2 when landed. + const matrix::Vector3f accel{_vehicle_acceleration.xyz}; - return (acc_norm < _params.freefall_acc_threshold); //true if we are currently falling + return (accel.norm() < _params.freefall_acc_threshold); // true if we are currently falling } bool MulticopterLandDetector::_get_ground_contact_state() @@ -216,7 +215,7 @@ bool MulticopterLandDetector::_get_maybe_landed_state() // Next look if all rotation angles are not moving. float maxRotationScaled = _params.maxRotation_rad_s * landThresholdFactor; - bool rotating = (fabsf(_vehicle_angular_velocity.xyz[0]) > maxRotationScaled) || + bool rotating = (fabsf(_vehicle_angular_velocity.xyz[0]) > maxRotationScaled) || (fabsf(_vehicle_angular_velocity.xyz[1]) > maxRotationScaled) || (fabsf(_vehicle_angular_velocity.xyz[2]) > maxRotationScaled); diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index 24b76d083f44..c434c136257d 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -52,7 +52,7 @@ #include #include #include -#include +#include #include #include #include @@ -126,7 +126,7 @@ class MulticopterLandDetector : public LandDetector uORB::Subscription _actuator_controls_sub{ORB_ID(actuator_controls_0)}; uORB::Subscription _battery_sub{ORB_ID(battery_status)}; - uORB::Subscription _sensor_bias_sub{ORB_ID(sensor_bias)}; + uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)}; uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; @@ -136,7 +136,7 @@ class MulticopterLandDetector : public LandDetector actuator_controls_s _actuator_controls {}; battery_status_s _battery_status {}; vehicle_control_mode_s _control_mode {}; - sensor_bias_s _sensor_bias {}; + vehicle_acceleration_s _vehicle_acceleration{}; vehicle_attitude_s _vehicle_attitude {}; vehicle_angular_velocity_s _vehicle_angular_velocity{}; vehicle_local_position_s _vehicle_local_position {}; diff --git a/src/modules/landing_target_estimator/LandingTargetEstimator.cpp b/src/modules/landing_target_estimator/LandingTargetEstimator.cpp index 61f48d427841..027a8627e00f 100644 --- a/src/modules/landing_target_estimator/LandingTargetEstimator.cpp +++ b/src/modules/landing_target_estimator/LandingTargetEstimator.cpp @@ -80,14 +80,11 @@ void LandingTargetEstimator::update() float dt = (hrt_absolute_time() - _last_predict) / SEC2USEC; // predict target position with the help of accel data - matrix::Vector3f a; + matrix::Vector3f a{_vehicle_acceleration.xyz}; - if (_vehicleAttitude_valid && _sensorBias_valid) { + if (_vehicleAttitude_valid && _vehicle_acceleration_valid) { matrix::Quaternion q_att(&_vehicleAttitude.q[0]); _R_att = matrix::Dcm(q_att); - a(0) = _sensorBias.accel_x; - a(1) = _sensorBias.accel_y; - a(2) = _sensorBias.accel_z; a = _R_att * a; } else { @@ -244,7 +241,7 @@ void LandingTargetEstimator::_update_topics() { _vehicleLocalPosition_valid = _vehicleLocalPositionSub.update(&_vehicleLocalPosition); _vehicleAttitude_valid = _attitudeSub.update(&_vehicleAttitude); - _sensorBias_valid = _sensorBiasSub.update(&_sensorBias); + _vehicle_acceleration_valid = _vehicle_acceleration_sub.update(&_vehicle_acceleration); _new_irlockReport = _irlockReportSub.update(&_irlockReport); } diff --git a/src/modules/landing_target_estimator/LandingTargetEstimator.h b/src/modules/landing_target_estimator/LandingTargetEstimator.h index 45062a24085d..ef4a12061756 100644 --- a/src/modules/landing_target_estimator/LandingTargetEstimator.h +++ b/src/modules/landing_target_estimator/LandingTargetEstimator.h @@ -47,9 +47,9 @@ #include #include #include -#include +#include #include -#include +#include #include #include #include @@ -130,18 +130,18 @@ class LandingTargetEstimator uORB::Subscription _vehicleLocalPositionSub{ORB_ID(vehicle_local_position)}; uORB::Subscription _attitudeSub{ORB_ID(vehicle_attitude)}; - uORB::Subscription _sensorBiasSub{ORB_ID(sensor_bias)}; + uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)}; uORB::Subscription _irlockReportSub{ORB_ID(irlock_report)}; vehicle_local_position_s _vehicleLocalPosition{}; - vehicle_attitude_s _vehicleAttitude{}; - sensor_bias_s _sensorBias{}; - irlock_report_s _irlockReport{}; + vehicle_attitude_s _vehicleAttitude{}; + vehicle_acceleration_s _vehicle_acceleration{}; + irlock_report_s _irlockReport{}; // keep track of which topics we have received bool _vehicleLocalPosition_valid{false}; bool _vehicleAttitude_valid{false}; - bool _sensorBias_valid{false}; + bool _vehicle_acceleration_valid{false}; bool _new_irlockReport{false}; bool _estimator_initialized{false}; // keep track of whether last measurement was rejected diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index e3b8df3bcf63..dc8f0e44adb9 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -848,15 +848,15 @@ class MavlinkStreamHighresIMU : public MavlinkStream mavlink_highres_imu_t msg = {}; msg.time_usec = sensor.timestamp; - msg.xacc = sensor.accelerometer_m_s2[0] - bias.accel_x_bias; - msg.yacc = sensor.accelerometer_m_s2[1] - bias.accel_y_bias; - msg.zacc = sensor.accelerometer_m_s2[2] - bias.accel_z_bias; - msg.xgyro = sensor.gyro_rad[0] - bias.gyro_x_bias; - msg.ygyro = sensor.gyro_rad[1] - bias.gyro_y_bias; - msg.zgyro = sensor.gyro_rad[2] - bias.gyro_z_bias; - msg.xmag = magnetometer.magnetometer_ga[0] - bias.mag_x_bias; - msg.ymag = magnetometer.magnetometer_ga[1] - bias.mag_y_bias; - msg.zmag = magnetometer.magnetometer_ga[2] - bias.mag_z_bias; + msg.xacc = sensor.accelerometer_m_s2[0] - bias.accel_bias[0]; + msg.yacc = sensor.accelerometer_m_s2[1] - bias.accel_bias[1]; + msg.zacc = sensor.accelerometer_m_s2[2] - bias.accel_bias[2]; + msg.xgyro = sensor.gyro_rad[0] - bias.gyro_bias[0]; + msg.ygyro = sensor.gyro_rad[1] - bias.gyro_bias[1]; + msg.zgyro = sensor.gyro_rad[2] - bias.gyro_bias[2]; + msg.xmag = magnetometer.magnetometer_ga[0] - bias.mag_bias[0]; + msg.ymag = magnetometer.magnetometer_ga[1] - bias.mag_bias[1]; + msg.zmag = magnetometer.magnetometer_ga[2] - bias.mag_bias[2]; msg.abs_pressure = air_data.baro_pressure_pa; msg.diff_pressure = differential_pressure.differential_pressure_raw_pa; msg.pressure_alt = air_data.baro_alt_meter; diff --git a/src/modules/rover_pos_control/RoverPositionControl.cpp b/src/modules/rover_pos_control/RoverPositionControl.cpp index f35a72906f01..e397695889f1 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.cpp +++ b/src/modules/rover_pos_control/RoverPositionControl.cpp @@ -61,10 +61,8 @@ extern "C" __EXPORT int rover_pos_control_main(int argc, char *argv[]); RoverPositionControl::RoverPositionControl() : ModuleParams(nullptr), /* performance counters */ - _sub_sensors(ORB_ID(sensor_bias)), _loop_perf(perf_alloc(PC_ELAPSED, "rover position control")) // TODO : do we even need these perf counters { - } RoverPositionControl::~RoverPositionControl() @@ -197,7 +195,7 @@ RoverPositionControl::control_position(const matrix::Vector2f ¤t_position, const Vector3f vel = R_to_body * Vector3f(ground_speed(0), ground_speed(1), ground_speed(2)); const float x_vel = vel(0); - const float x_acc = _sub_sensors.get().accel_x; + const float x_acc = _vehicle_acceleration_sub.get().xyz[0]; // Compute airspeed control out and just scale it as a constant mission_throttle = _param_throttle_speed_scaler.get() @@ -314,7 +312,7 @@ RoverPositionControl::run() vehicle_control_mode_poll(); //manual_control_setpoint_poll(); - _sub_sensors.update(); + _vehicle_acceleration_sub.update(); /* update parameters from storage */ parameters_update(_params_sub); diff --git a/src/modules/rover_pos_control/RoverPositionControl.hpp b/src/modules/rover_pos_control/RoverPositionControl.hpp index fc14dfafa27f..5d1231c2b96e 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.hpp +++ b/src/modules/rover_pos_control/RoverPositionControl.hpp @@ -58,8 +58,8 @@ #include #include #include -#include #include +#include #include #include #include @@ -116,7 +116,7 @@ class RoverPositionControl : public ModuleBase, public Mod vehicle_attitude_s _vehicle_att{}; sensor_combined_s _sensor_combined{}; - SubscriptionData _sub_sensors; + SubscriptionData _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)}; perf_counter_t _loop_perf; /**< loop performance counter */ diff --git a/src/modules/sensors/CMakeLists.txt b/src/modules/sensors/CMakeLists.txt index 611201aa4e4c..30115e313b3c 100644 --- a/src/modules/sensors/CMakeLists.txt +++ b/src/modules/sensors/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2015-2017 PX4 Development Team. All rights reserved. +# Copyright (c) 2015-2019 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 @@ -31,6 +31,7 @@ # ############################################################################ +add_subdirectory(vehicle_acceleration) add_subdirectory(vehicle_angular_velocity) px4_add_module( @@ -53,5 +54,6 @@ px4_add_module( git_ecl ecl_validation mathlib + sensors__vehicle_acceleration sensors__vehicle_angular_velocity ) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index b362f5b40f2a..5765df93e8d5 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -92,6 +92,7 @@ #include "rc_update.h" #include "voted_sensors_update.h" +#include "vehicle_acceleration/VehicleAcceleration.hpp" #include "vehicle_angular_velocity/VehicleAngularVelocity.hpp" using namespace DriverFramework; @@ -204,7 +205,8 @@ class Sensors : public ModuleBase, public ModuleParams VotedSensorsUpdate _voted_sensors_update; - VehicleAngularVelocity _angular_velocity; + VehicleAcceleration _vehicle_acceleration; + VehicleAngularVelocity _vehicle_angular_velocity; /** @@ -259,12 +261,14 @@ Sensors::Sensors(bool hil_enabled) : #endif /* BOARD_NUMBER_BRICKS > 0 */ - _angular_velocity.Start(); + _vehicle_acceleration.Start(); + _vehicle_angular_velocity.Start(); } Sensors::~Sensors() { - _angular_velocity.Stop(); + _vehicle_acceleration.Stop(); + _vehicle_angular_velocity.Stop(); } int @@ -736,7 +740,8 @@ int Sensors::print_status() PX4_INFO("Airspeed status:"); _airspeed_validator.print(); - _angular_velocity.PrintStatus(); + _vehicle_acceleration.PrintStatus(); + _vehicle_angular_velocity.PrintStatus(); return 0; } diff --git a/src/modules/sensors/vehicle_acceleration/CMakeLists.txt b/src/modules/sensors/vehicle_acceleration/CMakeLists.txt new file mode 100644 index 000000000000..337f659c46d1 --- /dev/null +++ b/src/modules/sensors/vehicle_acceleration/CMakeLists.txt @@ -0,0 +1,37 @@ +############################################################################ +# +# Copyright (c) 2019 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(sensors__vehicle_acceleration + VehicleAcceleration.cpp +) +target_link_libraries(sensors__vehicle_acceleration PRIVATE px4_work_queue) diff --git a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp new file mode 100644 index 000000000000..9e7f78f8281a --- /dev/null +++ b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp @@ -0,0 +1,225 @@ +/**************************************************************************** + * + * Copyright (c) 2019 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 "VehicleAcceleration.hpp" + +#include + +using namespace matrix; +using namespace time_literals; + +VehicleAcceleration::VehicleAcceleration() : + ModuleParams(nullptr), + WorkItem(px4::wq_configurations::att_pos_ctrl), + _cycle_perf(perf_alloc(PC_ELAPSED, "vehicle_acceleration: cycle time")), + _interval_perf(perf_alloc(PC_INTERVAL, "vehicle_acceleration: interval")), + _sensor_latency_perf(perf_alloc(PC_ELAPSED, "vehicle_acceleration: sensor latency")) +{ +} + +VehicleAcceleration::~VehicleAcceleration() +{ + Stop(); + + perf_free(_cycle_perf); + perf_free(_interval_perf); + perf_free(_sensor_latency_perf); +} + +bool +VehicleAcceleration::Start() +{ + // initialize thermal corrections as we might not immediately get a topic update (only non-zero values) + _scale = Vector3f{1.0f, 1.0f, 1.0f}; + _offset.zero(); + _bias.zero(); + + // force initial updates + ParametersUpdate(true); + SensorBiasUpdate(true); + + // needed to change the active sensor if the primary stops updating + _sensor_selection_sub.register_callback(); + + return SensorCorrectionsUpdate(true); +} + +void +VehicleAcceleration::Stop() +{ + Deinit(); + + // clear all registered callbacks + for (auto sub : _sensor_sub) { + sub.unregister_callback(); + } + + _sensor_selection_sub.unregister_callback(); +} + +void +VehicleAcceleration::SensorBiasUpdate(bool force) +{ + if (_sensor_bias_sub.updated() || force) { + sensor_bias_s bias; + + if (_sensor_bias_sub.copy(&bias)) { + // TODO: should be checking device ID + _bias = Vector3f{bias.accel_bias}; + } + } +} + +bool +VehicleAcceleration::SensorCorrectionsUpdate(bool force) +{ + // check if the selected sensor has updated + if (_sensor_correction_sub.updated() || force) { + + sensor_correction_s corrections{}; + _sensor_correction_sub.copy(&corrections); + + // TODO: should be checking device ID + if (_selected_sensor == 0) { + _offset = Vector3f{corrections.accel_offset_0}; + _scale = Vector3f{corrections.accel_scale_0}; + + } else if (_selected_sensor == 1) { + _offset = Vector3f{corrections.accel_offset_1}; + _scale = Vector3f{corrections.accel_scale_1}; + + } else if (_selected_sensor == 2) { + _offset = Vector3f{corrections.accel_offset_2}; + _scale = Vector3f{corrections.accel_scale_2}; + + } else { + _offset = Vector3f{0.0f, 0.0f, 0.0f}; + _scale = Vector3f{1.0f, 1.0f, 1.0f}; + } + + // update the latest sensor selection + if ((_selected_sensor != corrections.selected_accel_instance) || force) { + if (corrections.selected_accel_instance < MAX_SENSOR_COUNT) { + // clear all registered callbacks + for (auto sub : _sensor_sub) { + sub.unregister_callback(); + } + + const int sensor_new = corrections.selected_accel_instance; + + if (_sensor_sub[sensor_new].register_callback()) { + PX4_DEBUG("selected sensor changed %d -> %d", _selected_sensor, sensor_new); + _selected_sensor = sensor_new; + + return true; + } + } + } + } + + return false; +} + +void +VehicleAcceleration::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 matrix::Dcmf board_rotation = get_rot_matrix((enum Rotation)_param_sens_board_rot.get()); + + // fine tune the rotation + const Dcmf board_rotation_offset(Eulerf( + math::radians(_param_sens_board_x_off.get()), + math::radians(_param_sens_board_y_off.get()), + math::radians(_param_sens_board_z_off.get()))); + + _board_rotation = board_rotation_offset * board_rotation; + } +} + +void +VehicleAcceleration::Run() +{ + perf_begin(_cycle_perf); + perf_count(_interval_perf); + + // update corrections first to set _selected_sensor + SensorCorrectionsUpdate(); + + sensor_accel_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 accel{(val - _offset).emult(_scale)}; + + // rotate corrected measurements from sensor to body frame + accel = _board_rotation * accel; + + // correct for in-run bias errors + accel -= _bias; + + vehicle_acceleration_s out{}; + out.timestamp_sample = sensor_data.timestamp; + accel.copyTo(out.xyz); + out.timestamp = hrt_absolute_time(); + + _vehicle_acceleration_pub.publish(out); + } + + perf_end(_cycle_perf); +} + +void +VehicleAcceleration::PrintStatus() +{ + PX4_INFO("selected sensor: %d", _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_acceleration/VehicleAcceleration.hpp b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp new file mode 100644 index 000000000000..3274cc325f65 --- /dev/null +++ b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp @@ -0,0 +1,110 @@ +/**************************************************************************** + * + * Copyright (c) 2019 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 +#include + +class VehicleAcceleration : public ModuleParams, public px4::WorkItem +{ +public: + + VehicleAcceleration(); + virtual ~VehicleAcceleration(); + + void Run() override; + + bool Start(); + void Stop(); + + void PrintStatus(); + +private: + + void ParametersUpdate(bool force = false); + void SensorBiasUpdate(bool force = false); + bool 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::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::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_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::Vector3f _offset; + matrix::Vector3f _scale; + matrix::Vector3f _bias; + + perf_counter_t _cycle_perf; + perf_counter_t _interval_perf; + perf_counter_t _sensor_latency_perf; + + uint8_t _selected_sensor{0}; + +}; diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp index 01885e86b2e7..26fcb63cd7a7 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp @@ -35,15 +35,15 @@ #include -using namespace time_literals; using namespace matrix; +using namespace time_literals; VehicleAngularVelocity::VehicleAngularVelocity() : ModuleParams(nullptr), WorkItem(px4::wq_configurations::rate_ctrl), _cycle_perf(perf_alloc(PC_ELAPSED, "vehicle_angular_velocity: cycle time")), _interval_perf(perf_alloc(PC_INTERVAL, "vehicle_angular_velocity: interval")), - _sensor_gyro_latency_perf(perf_alloc(PC_ELAPSED, "vehicle_angular_velocity: sensor gyro latency")) + _sensor_latency_perf(perf_alloc(PC_ELAPSED, "vehicle_angular_velocity: sensor latency")) { } @@ -53,7 +53,7 @@ VehicleAngularVelocity::~VehicleAngularVelocity() perf_free(_cycle_perf); perf_free(_interval_perf); - perf_free(_sensor_gyro_latency_perf); + perf_free(_sensor_latency_perf); } bool @@ -80,7 +80,7 @@ VehicleAngularVelocity::Stop() Deinit(); // clear all registered callbacks - for (auto sub : _sensor_gyro_sub) { + for (auto sub : _sensor_sub) { sub.unregister_callback(); } @@ -95,9 +95,7 @@ VehicleAngularVelocity::SensorBiasUpdate(bool force) if (_sensor_bias_sub.copy(&bias)) { // TODO: should be checking device ID - _bias(0) = bias.gyro_x_bias; - _bias(1) = bias.gyro_y_bias; - _bias(2) = bias.gyro_z_bias; + _bias = Vector3f{bias.gyro_bias}; } } } @@ -105,22 +103,22 @@ VehicleAngularVelocity::SensorBiasUpdate(bool force) bool VehicleAngularVelocity::SensorCorrectionsUpdate(bool force) { - // check if the selected gyro has updated + // check if the selected sensor has updated if (_sensor_correction_sub.updated() || force) { sensor_correction_s corrections{}; _sensor_correction_sub.copy(&corrections); // TODO: should be checking device ID - if (_selected_gyro == 0) { + if (_selected_sensor == 0) { _offset = Vector3f{corrections.gyro_offset_0}; _scale = Vector3f{corrections.gyro_scale_0}; - } else if (_selected_gyro == 1) { + } else if (_selected_sensor == 1) { _offset = Vector3f{corrections.gyro_offset_1}; _scale = Vector3f{corrections.gyro_scale_1}; - } else if (_selected_gyro == 2) { + } else if (_selected_sensor == 2) { _offset = Vector3f{corrections.gyro_offset_2}; _scale = Vector3f{corrections.gyro_scale_2}; @@ -129,19 +127,19 @@ VehicleAngularVelocity::SensorCorrectionsUpdate(bool force) _scale = Vector3f{1.0f, 1.0f, 1.0f}; } - // update the latest gyro selection - if (_selected_gyro != corrections.selected_gyro_instance) { - if (corrections.selected_gyro_instance < MAX_GYRO_COUNT) { + // update the latest sensor selection + if ((_selected_sensor != corrections.selected_gyro_instance) || force) { + if (corrections.selected_gyro_instance < MAX_SENSOR_COUNT) { // clear all registered callbacks - for (auto sub : _sensor_gyro_sub) { + for (auto sub : _sensor_sub) { sub.unregister_callback(); } - const int gyro_new = corrections.selected_gyro_instance; + const int sensor_new = corrections.selected_gyro_instance; - if (_sensor_gyro_sub[gyro_new].register_callback()) { - PX4_DEBUG("selected gyro changed %d -> %d", _selected_gyro, gyro_new); - _selected_gyro = gyro_new; + if (_sensor_sub[sensor_new].register_callback()) { + PX4_DEBUG("selected sensor changed %d -> %d", _selected_sensor, sensor_new); + _selected_sensor = sensor_new; return true; } @@ -182,22 +180,22 @@ VehicleAngularVelocity::Run() perf_begin(_cycle_perf); perf_count(_interval_perf); - // update corrections first to set _selected_gyro + // update corrections first to set _selected_sensor SensorCorrectionsUpdate(); - sensor_gyro_s sensor_gyro; + sensor_gyro_s sensor_data; - if (_sensor_gyro_sub[_selected_gyro].update(&sensor_gyro)) { - perf_set_elapsed(_sensor_gyro_latency_perf, hrt_elapsed_time(&sensor_gyro.timestamp)); + if (_sensor_sub[_selected_sensor].update(&sensor_data)) { + perf_set_elapsed(_sensor_latency_perf, hrt_elapsed_time(&sensor_data.timestamp)); ParametersUpdate(); SensorBiasUpdate(); - // get the raw gyro data and correct for thermal errors - const Vector3f gyro{sensor_gyro.x, sensor_gyro.y, sensor_gyro.z}; + // 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{(gyro - _offset).emult(_scale)}; + Vector3f rates{(val - _offset).emult(_scale)}; // rotate corrected measurements from sensor to body frame rates = _board_rotation * rates; @@ -206,7 +204,7 @@ VehicleAngularVelocity::Run() rates -= _bias; vehicle_angular_velocity_s angular_velocity; - angular_velocity.timestamp_sample = sensor_gyro.timestamp; + angular_velocity.timestamp_sample = sensor_data.timestamp; rates.copyTo(angular_velocity.xyz); angular_velocity.timestamp = hrt_absolute_time(); @@ -219,9 +217,9 @@ VehicleAngularVelocity::Run() void VehicleAngularVelocity::PrintStatus() { - PX4_INFO("selected gyro: %d", _selected_gyro); + PX4_INFO("selected sensor: %d", _selected_sensor); perf_print_counter(_cycle_perf); perf_print_counter(_interval_perf); - perf_print_counter(_sensor_gyro_latency_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 cedcb733e014..fa2e15cb089c 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp @@ -33,15 +33,13 @@ #pragma once +#include +#include +#include #include #include -#include #include #include -#include -#include -#include -#include #include #include #include @@ -49,11 +47,10 @@ #include #include #include -#include #include -#include -#define MAX_GYRO_COUNT 3 +#include +#include class VehicleAngularVelocity : public ModuleParams, public px4::WorkItem { @@ -75,6 +72,7 @@ class VehicleAngularVelocity : public ModuleParams, public px4::WorkItem void SensorBiasUpdate(bool force = false); bool SensorCorrectionsUpdate(bool force = false); + static constexpr int MAX_SENSOR_COUNT = 3; DEFINE_PARAMETERS( (ParamInt) _param_sens_board_rot, @@ -91,7 +89,7 @@ class VehicleAngularVelocity : public ModuleParams, public px4::WorkItem 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_gyro_sub[MAX_GYRO_COUNT] { /**< gyro data 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} @@ -105,8 +103,8 @@ class VehicleAngularVelocity : public ModuleParams, public px4::WorkItem perf_counter_t _cycle_perf; perf_counter_t _interval_perf; - perf_counter_t _sensor_gyro_latency_perf; + perf_counter_t _sensor_latency_perf; - int _selected_gyro{-1}; + uint8_t _selected_sensor{0}; };