diff --git a/.ci/Jenkinsfile-hardware b/.ci/Jenkinsfile-hardware index f3e3f3ca46..f8bd7ad00c 100644 --- a/.ci/Jenkinsfile-hardware +++ b/.ci/Jenkinsfile-hardware @@ -920,7 +920,6 @@ void printTopics() { sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener tune_control" || true' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_acceleration" || true' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_air_data" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_angular_acceleration" || true' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_angular_velocity" || true' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_attitude" || true' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_attitude_setpoint" || true' diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 756f2f454c..9d5ea63722 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -186,7 +186,6 @@ set(msg_files UwbGrid.msg VehicleAcceleration.msg VehicleAirData.msg - VehicleAngularAcceleration.msg VehicleAngularAccelerationSetpoint.msg VehicleAngularVelocity.msg VehicleAttitude.msg diff --git a/msg/VehicleAngularAcceleration.msg b/msg/VehicleAngularAcceleration.msg deleted file mode 100644 index a072035616..0000000000 --- a/msg/VehicleAngularAcceleration.msg +++ /dev/null @@ -1,5 +0,0 @@ - -uint64 timestamp # time since system start (microseconds) -uint64 timestamp_sample # timestamp of the data sample on which this message is based (microseconds) - -float32[3] xyz # angular acceleration about the FRD body frame XYZ-axis in rad/s^2 diff --git a/msg/VehicleAngularVelocity.msg b/msg/VehicleAngularVelocity.msg index a8748772b7..db3767c0aa 100644 --- a/msg/VehicleAngularVelocity.msg +++ b/msg/VehicleAngularVelocity.msg @@ -1,7 +1,9 @@ -uint64 timestamp # time since system start (microseconds) -uint64 timestamp_sample # timestamp of the data sample on which this message is based (microseconds) +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # timestamp of the data sample on which this message is based (microseconds) -float32[3] xyz # Bias corrected angular velocity about the FRD body frame XYZ-axis in rad/s +float32[3] xyz # Bias corrected angular velocity about the FRD body frame XYZ-axis in rad/s + +float32[3] xyz_derivative # angular acceleration about the FRD body frame XYZ-axis in rad/s^2 # TOPICS vehicle_angular_velocity vehicle_angular_velocity_groundtruth diff --git a/src/modules/angular_velocity_controller/AngularVelocityController.cpp b/src/modules/angular_velocity_controller/AngularVelocityController.cpp index b54ad9031a..52f374f07b 100644 --- a/src/modules/angular_velocity_controller/AngularVelocityController.cpp +++ b/src/modules/angular_velocity_controller/AngularVelocityController.cpp @@ -136,6 +136,7 @@ AngularVelocityController::Run() _last_run = now; const Vector3f angular_velocity{vehicle_angular_velocity.xyz}; + _angular_acceleration = Vector3f(vehicle_angular_velocity.xyz_derivative); /* check for updates in other topics */ _vehicle_status_sub.update(&_vehicle_status); @@ -164,13 +165,6 @@ AngularVelocityController::Run() } } - // check angular acceleration topic - vehicle_angular_acceleration_s vehicle_angular_acceleration; - - if (_vehicle_angular_acceleration_sub.update(&vehicle_angular_acceleration)) { - _angular_acceleration = Vector3f(vehicle_angular_acceleration.xyz); - } - // check rates setpoint topic vehicle_rates_setpoint_s vehicle_rates_setpoint; diff --git a/src/modules/angular_velocity_controller/AngularVelocityController.hpp b/src/modules/angular_velocity_controller/AngularVelocityController.hpp index c80529753e..c28e36a78d 100644 --- a/src/modules/angular_velocity_controller/AngularVelocityController.hpp +++ b/src/modules/angular_velocity_controller/AngularVelocityController.hpp @@ -50,7 +50,6 @@ #include #include #include -#include #include #include #include @@ -105,7 +104,6 @@ private: uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; uORB::Subscription _control_allocator_status_sub{ORB_ID(control_allocator_status)}; /**< motor limits subscription */ - uORB::Subscription _vehicle_angular_acceleration_sub{ORB_ID(vehicle_angular_acceleration)}; /**< vehicle angular acceleration subscription */ uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle control mode subscription */ uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */ uORB::Subscription _vehicle_rates_setpoint_sub{ORB_ID(vehicle_rates_setpoint)}; /**< vehicle rates setpoint subscription */ diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index 80c7457596..f7651c759a 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -305,10 +305,7 @@ void FixedwingAttitudeControl::Run() float pitchspeed = angular_velocity.xyz[1]; float yawspeed = angular_velocity.xyz[2]; const Vector3f rates(rollspeed, pitchspeed, yawspeed); - - vehicle_angular_acceleration_s angular_acceleration{}; - _vehicle_angular_acceleration_sub.copy(&angular_acceleration); - const Vector3f angular_accel{angular_acceleration.xyz}; + const Vector3f angular_accel{angular_velocity.xyz_derivative}; if (_vehicle_status.is_vtol_tailsitter) { /* vehicle is a tailsitter, we need to modify the estimated attitude for fw mode diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp index 4e65aad515..25dabed2f0 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp @@ -67,7 +67,6 @@ #include #include #include -#include #include #include #include @@ -127,7 +126,6 @@ private: uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */ uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */ uORB::Subscription _vehicle_rates_sub{ORB_ID(vehicle_angular_velocity)}; - uORB::Subscription _vehicle_angular_acceleration_sub{ORB_ID(vehicle_angular_acceleration)}; uORB::SubscriptionMultiArray _control_allocator_status_subs{ORB_ID::control_allocator_status}; diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 8878673c5a..40adfede97 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -216,7 +216,6 @@ void LoggedTopics::add_default_topics() // additional control allocation logging add_topic("actuator_motors", 100); add_topic("actuator_servos", 100); - add_topic("vehicle_angular_acceleration", 20); add_topic_multi("vehicle_thrust_setpoint", 20, 1); add_topic_multi("vehicle_torque_setpoint", 20, 2); @@ -237,7 +236,6 @@ void LoggedTopics::add_default_topics() add_topic("fw_virtual_attitude_setpoint"); add_topic("mc_virtual_attitude_setpoint"); add_topic("time_offset"); - add_topic("vehicle_angular_acceleration", 10); add_topic("vehicle_angular_velocity", 10); add_topic("vehicle_angular_velocity_groundtruth", 10); add_topic("vehicle_attitude_groundtruth", 10); @@ -294,7 +292,6 @@ void LoggedTopics::add_high_rate_topics() add_topic("manual_control_setpoint"); add_topic("rate_ctrl_status", 20); add_topic("sensor_combined"); - add_topic("vehicle_angular_acceleration"); add_topic("vehicle_angular_velocity"); add_topic("vehicle_attitude"); add_topic("vehicle_attitude_setpoint"); @@ -377,7 +374,7 @@ void LoggedTopics::add_system_identification_topics() add_topic("actuator_controls_0"); add_topic("actuator_controls_1"); add_topic("sensor_combined"); - add_topic("vehicle_angular_acceleration"); + add_topic("vehicle_angular_velocity"); add_topic("vehicle_torque_setpoint"); } diff --git a/src/modules/mc_rate_control/MulticopterRateControl.cpp b/src/modules/mc_rate_control/MulticopterRateControl.cpp index 6eccec7809..716ac105ed 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.cpp +++ b/src/modules/mc_rate_control/MulticopterRateControl.cpp @@ -122,18 +122,14 @@ MulticopterRateControl::Run() if (_vehicle_angular_velocity_sub.update(&angular_velocity)) { - // grab corresponding vehicle_angular_acceleration immediately after vehicle_angular_velocity copy - vehicle_angular_acceleration_s v_angular_acceleration{}; - _vehicle_angular_acceleration_sub.copy(&v_angular_acceleration); - const hrt_abstime now = angular_velocity.timestamp_sample; // Guard against too small (< 0.125ms) and too large (> 20ms) dt's. const float dt = math::constrain(((now - _last_run) * 1e-6f), 0.000125f, 0.02f); _last_run = now; - const Vector3f angular_accel{v_angular_acceleration.xyz}; const Vector3f rates{angular_velocity.xyz}; + const Vector3f angular_accel{angular_velocity.xyz_derivative}; /* check for updates in other topics */ _vehicle_control_mode_sub.update(&_vehicle_control_mode); diff --git a/src/modules/mc_rate_control/MulticopterRateControl.hpp b/src/modules/mc_rate_control/MulticopterRateControl.hpp index 64aaf90ce1..a9ac2eeb1d 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.hpp +++ b/src/modules/mc_rate_control/MulticopterRateControl.hpp @@ -54,7 +54,6 @@ #include #include #include -#include #include #include #include @@ -101,7 +100,6 @@ private: uORB::Subscription _control_allocator_status_sub{ORB_ID(control_allocator_status)}; uORB::Subscription _landing_gear_sub{ORB_ID(landing_gear)}; uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; - uORB::Subscription _vehicle_angular_acceleration_sub{ORB_ID(vehicle_angular_acceleration)}; uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; uORB::Subscription _vehicle_rates_setpoint_sub{ORB_ID(vehicle_rates_setpoint)}; diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp index 00dc3d906a..e688a788e9 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp @@ -46,7 +46,6 @@ VehicleAngularVelocity::VehicleAngularVelocity() : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl) { - _vehicle_angular_acceleration_pub.advertise(); _vehicle_angular_velocity_pub.advertise(); } @@ -918,17 +917,6 @@ bool VehicleAngularVelocity::CalibrateAndPublish(const hrt_abstime ×tamp_sa { if (timestamp_sample >= _last_publish + _publish_interval_min_us) { - // Publish vehicle_angular_acceleration - vehicle_angular_acceleration_s angular_acceleration; - angular_acceleration.timestamp_sample = timestamp_sample; - - // Angular acceleration: rotate sensor frame to board, scale raw data to SI, apply any additional configured rotation - _angular_acceleration = _calibration.rotation() * angular_acceleration_uncalibrated; - _angular_acceleration.copyTo(angular_acceleration.xyz); - - angular_acceleration.timestamp = hrt_absolute_time(); - _vehicle_angular_acceleration_pub.publish(angular_acceleration); - // Publish vehicle_angular_velocity vehicle_angular_velocity_s angular_velocity; angular_velocity.timestamp_sample = timestamp_sample; @@ -937,6 +925,10 @@ bool VehicleAngularVelocity::CalibrateAndPublish(const hrt_abstime ×tamp_sa _angular_velocity = _calibration.Correct(angular_velocity_uncalibrated) - _bias; _angular_velocity.copyTo(angular_velocity.xyz); + // Angular acceleration: rotate sensor frame to board, scale raw data to SI, apply any additional configured rotation + _angular_acceleration = _calibration.rotation() * angular_acceleration_uncalibrated; + _angular_acceleration.copyTo(angular_velocity.xyz_derivative); + angular_velocity.timestamp = hrt_absolute_time(); _vehicle_angular_velocity_pub.publish(angular_velocity); diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp index 27eae95c84..8aae8a3c03 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp @@ -55,7 +55,6 @@ #include #include #include -#include #include using namespace time_literals; @@ -99,7 +98,6 @@ private: static constexpr int MAX_SENSOR_COUNT = 4; - uORB::Publication _vehicle_angular_acceleration_pub{ORB_ID(vehicle_angular_acceleration)}; uORB::Publication _vehicle_angular_velocity_pub{ORB_ID(vehicle_angular_velocity)}; uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)};