From a51d9983e413198d1123065acc704242dbda2e88 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 27 Jan 2020 16:18:20 -0500 Subject: [PATCH] mc_rate_control: publish acceleration setpoint --- msg/CMakeLists.txt | 1 + msg/tools/uorb_rtps_message_ids.yaml | 2 ++ msg/vehicle_angular_acceleration_setpoint.msg | 4 ++++ src/modules/logger/logged_topics.cpp | 4 ++++ src/modules/mc_rate_control/MulticopterRateControl.cpp | 6 ++++++ src/modules/mc_rate_control/MulticopterRateControl.hpp | 2 ++ .../mc_rate_control/RateControl/RateControl.cpp | 10 +++++++--- .../mc_rate_control/RateControl/RateControl.hpp | 9 +++++++++ 8 files changed, 35 insertions(+), 3 deletions(-) create mode 100644 msg/vehicle_angular_acceleration_setpoint.msg diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index e92eff278c7b..47d71d792134 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -136,6 +136,7 @@ set(msg_files vehicle_acceleration.msg vehicle_air_data.msg vehicle_angular_acceleration.msg + vehicle_angular_acceleration_setpoint.msg vehicle_angular_velocity.msg vehicle_attitude.msg vehicle_attitude_setpoint.msg diff --git a/msg/tools/uorb_rtps_message_ids.yaml b/msg/tools/uorb_rtps_message_ids.yaml index 69cea00748cc..71fd2d8fa0ca 100644 --- a/msg/tools/uorb_rtps_message_ids.yaml +++ b/msg/tools/uorb_rtps_message_ids.yaml @@ -279,6 +279,8 @@ rtps: id: 124 - msg: vehicle_angular_acceleration id: 125 + - msg: vehicle_angular_acceleration_setpoint + id: 126 ########## multi topics: begin ########## - msg: actuator_controls_0 id: 150 diff --git a/msg/vehicle_angular_acceleration_setpoint.msg b/msg/vehicle_angular_acceleration_setpoint.msg new file mode 100644 index 000000000000..335120ef35ae --- /dev/null +++ b/msg/vehicle_angular_acceleration_setpoint.msg @@ -0,0 +1,4 @@ + +uint64 timestamp # time since system start (microseconds) + +float32[3] xyz # angular acceleration about X, Y, Z body axis in rad/s^2 diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 9ff952532150..dab3bf906c03 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -119,6 +119,8 @@ 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_acceleration_setpoint", 10); add_topic("vehicle_angular_velocity", 10); add_topic("vehicle_attitude_groundtruth", 10); add_topic("vehicle_global_position_groundtruth", 100); @@ -134,6 +136,8 @@ void LoggedTopics::add_high_rate_topics() add_topic("manual_control_setpoint"); add_topic("rate_ctrl_status"); add_topic("sensor_combined"); + add_topic("vehicle_angular_acceleration"); + add_topic("vehicle_angular_acceleration_setpoint"); add_topic("vehicle_angular_velocity"); add_topic("vehicle_attitude"); add_topic("vehicle_attitude_setpoint"); diff --git a/src/modules/mc_rate_control/MulticopterRateControl.cpp b/src/modules/mc_rate_control/MulticopterRateControl.cpp index ce619a992acc..e1634d2c9f82 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.cpp +++ b/src/modules/mc_rate_control/MulticopterRateControl.cpp @@ -277,6 +277,12 @@ MulticopterRateControl::Run() // run rate controller const Vector3f att_control = _rate_control.update(rates, _rates_sp, dt, _maybe_landed || _landed); + // publish angular acceleration setpoint + vehicle_angular_acceleration_setpoint_s v_angular_accel_sp; + _rate_control.getAngularAccelerationSetpoint().copyTo(v_angular_accel_sp.xyz); + v_angular_accel_sp.timestamp = hrt_absolute_time(); + _vehicle_angular_acceleration_setpoint_pub.publish(v_angular_accel_sp); + // publish rate controller status rate_ctrl_status_s rate_ctrl_status{}; _rate_control.getRateControlStatus(rate_ctrl_status); diff --git a/src/modules/mc_rate_control/MulticopterRateControl.hpp b/src/modules/mc_rate_control/MulticopterRateControl.hpp index 5ab978ef9749..fe0af9175d55 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.hpp +++ b/src/modules/mc_rate_control/MulticopterRateControl.hpp @@ -54,6 +54,7 @@ #include #include #include +#include #include #include #include @@ -107,6 +108,7 @@ class MulticopterRateControl : public ModuleBase, public uORB::Publication _actuators_0_pub; uORB::PublicationMulti _controller_status_pub{ORB_ID(rate_ctrl_status), ORB_PRIO_DEFAULT}; /**< controller status publication */ + uORB::Publication _vehicle_angular_acceleration_setpoint_pub{ORB_ID(vehicle_angular_acceleration_setpoint)}; uORB::Publication _landing_gear_pub{ORB_ID(landing_gear)}; uORB::Publication _v_rates_sp_pub{ORB_ID(vehicle_rates_setpoint)}; /**< rate setpoint publication */ diff --git a/src/modules/mc_rate_control/RateControl/RateControl.cpp b/src/modules/mc_rate_control/RateControl/RateControl.cpp index 830a5bf43a6a..0d94dc8fb6ad 100644 --- a/src/modules/mc_rate_control/RateControl/RateControl.cpp +++ b/src/modules/mc_rate_control/RateControl/RateControl.cpp @@ -79,9 +79,13 @@ Vector3f RateControl::update(const Vector3f &rate, const Vector3f &rate_sp, cons rate_d = (rate_filtered - _rate_prev_filtered) / dt; } - // PID control with feed forward - const Vector3f torque = _gain_p.emult(rate_error) + _rate_int - _gain_d.emult(rate_d) + _gain_ff.emult(rate_sp); + // P + D control + _angular_accel_sp = _gain_p.emult(rate_error) - _gain_d.emult(rate_d); + // I + FF control + const Vector3f torque_feedforward = _rate_int + _gain_ff.emult(rate_sp); + + // save states _rate_prev = rate; _rate_prev_filtered = rate_filtered; @@ -90,7 +94,7 @@ Vector3f RateControl::update(const Vector3f &rate, const Vector3f &rate_sp, cons updateIntegral(rate_error, dt); } - return torque; + return _angular_accel_sp + torque_feedforward; } void RateControl::updateIntegral(Vector3f &rate_error, const float dt) diff --git a/src/modules/mc_rate_control/RateControl/RateControl.hpp b/src/modules/mc_rate_control/RateControl/RateControl.hpp index ddc4fb57f528..52a734a49e2f 100644 --- a/src/modules/mc_rate_control/RateControl/RateControl.hpp +++ b/src/modules/mc_rate_control/RateControl/RateControl.hpp @@ -96,6 +96,12 @@ class RateControl matrix::Vector3f update(const matrix::Vector3f &rate, const matrix::Vector3f &rate_sp, const float dt, const bool landed); + /** + * Get the desired angular acceleration + * @see _angular_accel_sp + */ + const matrix::Vector3f &getAngularAccelerationSetpoint() const { return _angular_accel_sp; }; + /** * Set the integral term to 0 to prevent windup * @see _rate_int @@ -125,4 +131,7 @@ class RateControl math::LowPassFilter2pVector3f _lp_filters_d{0.f, 0.f}; ///< low-pass filters for D-term (roll, pitch & yaw) bool _mixer_saturation_positive[3] {}; bool _mixer_saturation_negative[3] {}; + + // Output + matrix::Vector3f _angular_accel_sp{0.f, 0.f, 0.f}; //< Angular acceleration setpoint computed using P and D gains };