Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-28 10:46:33 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

fw_att_control: publish vehicle_thrust_setpoint & vehicle_torque_setpoint

Browse Source
This commit is contained in:
Beat Küng
2021-11-29 13:52:23 +01:00
committed by Daniel Agar
parent 4becd6e4c7
commit 28e995ede2
2 changed files with 36 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_att_control/FixedwingAttitudeControl.cpp
+29
View File
@@ -660,6 +660,11 @@ void FixedwingAttitudeControl::Run()
_vcontrol_mode.flag_control_attitude_enabled || _vcontrol_mode.flag_control_attitude_enabled ||
_vcontrol_mode.flag_control_manual_enabled) { _vcontrol_mode.flag_control_manual_enabled) {
_actuators_0_pub.publish(_actuators); _actuators_0_pub.publish(_actuators);
if (!_vehicle_status.is_vtol) {
publishTorqueSetpoint(angular_velocity.timestamp_sample);
publishThrustSetpoint(angular_velocity.timestamp_sample);
}
} }
updateActuatorControlsStatus(dt); updateActuatorControlsStatus(dt);
@@ -668,6 +673,30 @@ void FixedwingAttitudeControl::Run()
perf_end(_loop_perf); perf_end(_loop_perf);
} }
void FixedwingAttitudeControl::publishTorqueSetpoint(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] = _actuators.control[actuator_controls_s::INDEX_ROLL];
v_torque_sp.xyz[1] = _actuators.control[actuator_controls_s::INDEX_PITCH];
v_torque_sp.xyz[2] = _actuators.control[actuator_controls_s::INDEX_YAW];
_vehicle_torque_setpoint_pub.publish(v_torque_sp);
}
void FixedwingAttitudeControl::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] = _actuators.control[actuator_controls_s::INDEX_THROTTLE];
v_thrust_sp.xyz[1] = 0.0f;
v_thrust_sp.xyz[2] = 0.0f;
_vehicle_thrust_setpoint_pub.publish(v_thrust_sp);
}
void FixedwingAttitudeControl::control_flaps(const float dt) void FixedwingAttitudeControl::control_flaps(const float dt)
{ {
/* default flaps to center */ /* default flaps to center */
src/modules/fw_att_control/FixedwingAttitudeControl.hpp
+7
View File
@@ -68,6 +68,8 @@
#include <uORB/topics/vehicle_land_detected.h> #include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_rates_setpoint.h> #include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_status.h> #include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_thrust_setpoint.h>
#include <uORB/topics/vehicle_torque_setpoint.h>
using matrix::Eulerf; using matrix::Eulerf;
using matrix::Quatf; using matrix::Quatf;
@@ -97,6 +99,9 @@ public:
private: private:
void Run() override; void Run() override;
void publishTorqueSetpoint(const hrt_abstime &timestamp_sample);
void publishThrustSetpoint(const hrt_abstime &timestamp_sample);
uORB::SubscriptionCallbackWorkItem _att_sub{this, ORB_ID(vehicle_attitude)}; /**< vehicle attitude */ uORB::SubscriptionCallbackWorkItem _att_sub{this, ORB_ID(vehicle_attitude)}; /**< vehicle attitude */
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
@@ -119,6 +124,8 @@ private:
uORB::Publication<vehicle_attitude_setpoint_s> _attitude_sp_pub; uORB::Publication<vehicle_attitude_setpoint_s> _attitude_sp_pub;
uORB::Publication<vehicle_rates_setpoint_s> _rate_sp_pub{ORB_ID(vehicle_rates_setpoint)}; uORB::Publication<vehicle_rates_setpoint_s> _rate_sp_pub{ORB_ID(vehicle_rates_setpoint)};
uORB::PublicationMulti<rate_ctrl_status_s> _rate_ctrl_status_pub{ORB_ID(rate_ctrl_status)}; uORB::PublicationMulti<rate_ctrl_status_s> _rate_ctrl_status_pub{ORB_ID(rate_ctrl_status)};
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)};
actuator_controls_s _actuators {}; /**< actuator control inputs */ actuator_controls_s _actuators {}; /**< actuator control inputs */
manual_control_setpoint_s _manual_control_setpoint {}; /**< r/c channel data */ manual_control_setpoint_s _manual_control_setpoint {}; /**< r/c channel data */