fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-23 11:07:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

airship_att_control: publish thrust + torque setpoint

Browse Source
This commit is contained in:
Beat Küng
2021-12-17 11:31:58 +01:00
committed by Daniel Agar
parent 478724c9fe
commit 09200b994d
2 changed files with 35 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/airship_att_control/airship_att_control.hpp
+7
View File
@@ -41,6 +41,8 @@
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_angular_velocity.h>
#include <uORB/topics/vehicle_thrust_setpoint.h>
#include <uORB/topics/vehicle_torque_setpoint.h>
using namespace time_literals;
@@ -82,6 +84,9 @@ private:
void publish_actuator_controls();
void publishTorqueSetpoint(const hrt_abstime &timestamp_sample);
void publishThrustSetpoint(const hrt_abstime &timestamp_sample);
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; /**< parameter updates subscription */
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */
uORB::Subscription _manual_control_sp_sub{ORB_ID(manual_control_setpoint)}; /**< manual control setpoint subscription */
@@ -89,6 +94,8 @@ private:
uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)};
uORB::Publication<actuator_controls_s> _actuators_0_pub;
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)};
struct manual_control_setpoint_s _manual_control_sp {}; /**< manual control setpoint */
struct vehicle_status_s _vehicle_status {}; /**< vehicle status */
src/modules/airship_att_control/airship_att_control_main.cpp
+28 -8
View File
@@ -102,6 +102,30 @@ AirshipAttitudeControl::publish_actuator_controls()
_actuators_0_pub.publish(_actuators);
}
void AirshipAttitudeControl::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[0];
v_torque_sp.xyz[1] = _actuators.control[1];
v_torque_sp.xyz[2] = _actuators.control[2];
_vehicle_torque_setpoint_pub.publish(v_torque_sp);
}
void AirshipAttitudeControl::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[3];
v_thrust_sp.xyz[1] = 0.0f;
v_thrust_sp.xyz[2] = 0.0f;
_vehicle_thrust_setpoint_pub.publish(v_thrust_sp);
}
void
AirshipAttitudeControl::Run()
{
@@ -118,22 +142,18 @@ AirshipAttitudeControl::Run()
if (_vehicle_angular_velocity_sub.update(&angular_velocity)) {
const Vector3f rates{angular_velocity.xyz};
_actuators.timestamp_sample = angular_velocity.timestamp_sample;
/* run the rate controller immediately after a gyro update */
publish_actuator_controls();
publishTorqueSetpoint(angular_velocity.timestamp_sample);
publishThrustSetpoint(angular_velocity.timestamp_sample);
/* check for updates in manual control topic */
if (_manual_control_sp_sub.updated()) {
_manual_control_sp_sub.update(&_manual_control_sp);
}
_manual_control_sp_sub.update(&_manual_control_sp);
/* check for updates in vehicle status topic */
if (_vehicle_status_sub.updated()) {
_vehicle_status_sub.update(&_vehicle_status);
}
_vehicle_status_sub.update(&_vehicle_status);
parameter_update_poll();
}