fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-15 06:13:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

McRateControl: publish torque and thrust setpoint for control allocator

Browse Source
This commit is contained in:
bresch
2021-10-08 09:35:40 +02:00
committed by Beat Küng
parent 212df95193
commit 927c0c4296
2 changed files with 34 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_rate_control/MulticopterRateControl.cpp
+27
View File
@@ -236,6 +236,9 @@ MulticopterRateControl::Run()
actuators.control[actuator_controls_s::INDEX_LANDING_GEAR] = _landing_gear;
actuators.timestamp_sample = angular_velocity.timestamp_sample;
publishTorqueSetpoint(att_control, angular_velocity.timestamp_sample);
publishThrustSetpoint(angular_velocity.timestamp_sample);
// scale effort by battery status if enabled
if (_param_mc_bat_scale_en.get()) {
if (_battery_status_sub.updated()) {
@@ -271,6 +274,30 @@ MulticopterRateControl::Run()
perf_end(_loop_perf);
}
void MulticopterRateControl::publishTorqueSetpoint(const Vector3f &torque_sp, const hrt_abstime &timestamp_sample)
{
vehicle_torque_setpoint_s v_torque_sp = {};
v_torque_sp.timestamp = hrt_absolute_time();
v_torque_sp.timestamp_sample = timestamp_sample;
v_torque_sp.xyz[0] = (PX4_ISFINITE(torque_sp(0))) ? torque_sp(0) : 0.0f;
v_torque_sp.xyz[1] = (PX4_ISFINITE(torque_sp(1))) ? torque_sp(1) : 0.0f;
v_torque_sp.xyz[2] = (PX4_ISFINITE(torque_sp(2))) ? torque_sp(2) : 0.0f;
_vehicle_torque_setpoint_pub.publish(v_torque_sp);
}
void MulticopterRateControl::publishThrustSetpoint(const hrt_abstime &timestamp_sample)
{
vehicle_thrust_setpoint_s v_thrust_sp = {};
v_thrust_sp.timestamp = hrt_absolute_time();
v_thrust_sp.timestamp_sample = timestamp_sample;
v_thrust_sp.xyz[0] = 0.0f;
v_thrust_sp.xyz[1] = 0.0f;
v_thrust_sp.xyz[2] = PX4_ISFINITE(_thrust_sp) ? -_thrust_sp : 0.0f; // Z is Down
_vehicle_thrust_setpoint_pub.publish(v_thrust_sp);
}
void MulticopterRateControl::updateActuatorControlsStatus(const actuator_controls_s &actuators, float dt)
{
for (int i = 0; i < 4; i++) {
src/modules/mc_rate_control/MulticopterRateControl.hpp
+7
View File
@@ -60,6 +60,8 @@
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_thrust_setpoint.h>
#include <uORB/topics/vehicle_torque_setpoint.h>
using namespace time_literals;
@@ -90,6 +92,9 @@ private:
void updateActuatorControlsStatus(const actuator_controls_s &actuators, float dt);
void publishTorqueSetpoint(const matrix::Vector3f &torque_sp, const hrt_abstime &timestamp_sample);
void publishThrustSetpoint(const hrt_abstime &timestamp_sample);
RateControl _rate_control; ///< class for rate control calculations
uORB::Subscription _battery_status_sub{ORB_ID(battery_status)};
@@ -110,6 +115,8 @@ private:
uORB::Publication<actuator_controls_status_s> _actuator_controls_status_0_pub{ORB_ID(actuator_controls_status_0)};
uORB::PublicationMulti<rate_ctrl_status_s> _controller_status_pub{ORB_ID(rate_ctrl_status)};
uORB::Publication<vehicle_rates_setpoint_s> _v_rates_sp_pub{ORB_ID(vehicle_rates_setpoint)};
uORB::Publication<vehicle_thrust_setpoint_s> _vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)};
uORB::Publication<vehicle_torque_setpoint_s> _vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)};
vehicle_control_mode_s _v_control_mode{};
vehicle_status_s _vehicle_status{};