fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

merge vehicle_angular_acceleration into vehicle_angular_velocity (#20531)

Browse Source 
- vehicle_angular_velocity and vehicle_angular_acceleration are produced together from the same input data, consumed together, and share the the same metadata (timestamps)
 - individually these topics each have 16 bytes of metadata (2 timestamps) for 12 bytes of data (x,y,z float32)
This commit is contained in:
Daniel Agar 2022-11-14 11:03:59 -05:00 committed by GitHub
parent 7d1f1d0f84
commit 9e7db0ed54
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
13 changed files with 13 additions and 50 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
.ci/Jenkinsfile-hardware
View File

@ -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'

1
msg/CMakeLists.txt
View File

@ -186,7 +186,6 @@ set(msg_files
UwbGrid.msg
VehicleAcceleration.msg
VehicleAirData.msg
VehicleAngularAcceleration.msg
VehicleAngularAccelerationSetpoint.msg
VehicleAngularVelocity.msg
VehicleAttitude.msg

5
msg/VehicleAngularAcceleration.msg
View File

@ -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

8
msg/VehicleAngularVelocity.msg
View File

@ -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

8
src/modules/angular_velocity_controller/AngularVelocityController.cpp
View File

@ -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;

2
src/modules/angular_velocity_controller/AngularVelocityController.hpp
View File

@ -50,7 +50,6 @@
#include <uORB/topics/hover_thrust_estimate.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/rate_ctrl_status.h>
#include <uORB/topics/vehicle_angular_acceleration.h>
#include <uORB/topics/vehicle_angular_acceleration_setpoint.h>
#include <uORB/topics/vehicle_angular_velocity.h>
#include <uORB/topics/vehicle_control_mode.h>
@ -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 */

5
src/modules/fw_att_control/FixedwingAttitudeControl.cpp
View File

@ -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

2
src/modules/fw_att_control/FixedwingAttitudeControl.hpp
View File

@ -67,7 +67,6 @@
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/rate_ctrl_status.h>
#include <uORB/topics/vehicle_angular_acceleration.h>
#include <uORB/topics/vehicle_angular_velocity.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
@ -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_s, 2> _control_allocator_status_subs{ORB_ID::control_allocator_status};

5
src/modules/logger/logged_topics.cpp
View File

@ -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");
}

6
src/modules/mc_rate_control/MulticopterRateControl.cpp
View File

@ -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);

2
src/modules/mc_rate_control/MulticopterRateControl.hpp
View File

@ -54,7 +54,6 @@
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/rate_ctrl_status.h>
#include <uORB/topics/vehicle_angular_acceleration.h>
#include <uORB/topics/vehicle_angular_velocity.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_land_detected.h>
@ -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)};

16
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
View File

@ -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 &timestamp_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 &timestamp_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);

2
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp
View File

@ -55,7 +55,6 @@
#include <uORB/topics/sensor_gyro_fft.h>
#include <uORB/topics/sensor_gyro_fifo.h>
#include <uORB/topics/sensor_selection.h>
#include <uORB/topics/vehicle_angular_acceleration.h>
#include <uORB/topics/vehicle_angular_velocity.h>
using namespace time_literals;
@ -99,7 +98,6 @@ private:
static constexpr int MAX_SENSOR_COUNT = 4;
uORB::Publication<vehicle_angular_acceleration_s> _vehicle_angular_acceleration_pub{ORB_ID(vehicle_angular_acceleration)};
uORB::Publication<vehicle_angular_velocity_s> _vehicle_angular_velocity_pub{ORB_ID(vehicle_angular_velocity)};
uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)};