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

vtol_att_control: publish vehicle_thrust_setpoint & vehicle_torque_setpoint

Browse Source
This commit is contained in:
Beat Küng
2021-12-24 20:06:13 -05:00
committed by Daniel Agar
parent d1abdd0f8d
commit 5da67e2e28
2 changed files with 48 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/vtol_att_control/vtol_att_control_main.cpp
+39
View File
@@ -118,6 +118,10 @@ VtolAttitudeControl::VtolAttitudeControl() :
} }
_vtol_vehicle_status_pub.advertise(); _vtol_vehicle_status_pub.advertise();
_vehicle_thrust_setpoint0_pub.advertise();
_vehicle_torque_setpoint0_pub.advertise();
_vehicle_thrust_setpoint1_pub.advertise();
_vehicle_torque_setpoint1_pub.advertise();
} }
VtolAttitudeControl::~VtolAttitudeControl() VtolAttitudeControl::~VtolAttitudeControl()
@@ -499,6 +503,8 @@ VtolAttitudeControl::Run()
_vtol_type->fill_actuator_outputs(); _vtol_type->fill_actuator_outputs();
_actuators_0_pub.publish(_actuators_out_0); _actuators_0_pub.publish(_actuators_out_0);
_actuators_1_pub.publish(_actuators_out_1); _actuators_1_pub.publish(_actuators_out_1);
publishTorqueSetpoints(0);
publishThrustSetpoints(0);
// Advertise/Publish vtol vehicle status // Advertise/Publish vtol vehicle status
_vtol_vehicle_status.timestamp = hrt_absolute_time(); _vtol_vehicle_status.timestamp = hrt_absolute_time();
@@ -508,6 +514,39 @@ VtolAttitudeControl::Run()
perf_end(_loop_perf); perf_end(_loop_perf);
} }
void VtolAttitudeControl::publishTorqueSetpoints(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_out_0.control[actuator_controls_s::INDEX_ROLL];
v_torque_sp.xyz[1] = _actuators_out_0.control[actuator_controls_s::INDEX_PITCH];
v_torque_sp.xyz[2] = _actuators_out_0.control[actuator_controls_s::INDEX_YAW];
_vehicle_torque_setpoint0_pub.publish(v_torque_sp);
v_torque_sp.xyz[0] = _actuators_out_1.control[actuator_controls_s::INDEX_ROLL];
v_torque_sp.xyz[1] = _actuators_out_1.control[actuator_controls_s::INDEX_PITCH];
v_torque_sp.xyz[2] = _actuators_out_1.control[actuator_controls_s::INDEX_YAW];
_vehicle_torque_setpoint1_pub.publish(v_torque_sp);
}
void VtolAttitudeControl::publishThrustSetpoints(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;
// TODO: separate thrust
v_thrust_sp.xyz[0] = _actuators_out_0.control[actuator_controls_s::INDEX_THROTTLE];
v_thrust_sp.xyz[1] = 0.0f;
v_thrust_sp.xyz[2] = -_actuators_out_0.control[actuator_controls_s::INDEX_THROTTLE];
_vehicle_thrust_setpoint0_pub.publish(v_thrust_sp);
v_thrust_sp.xyz[0] = _actuators_out_1.control[actuator_controls_s::INDEX_THROTTLE];
v_thrust_sp.xyz[1] = 0.0f;
v_thrust_sp.xyz[2] = 0.0f;
_vehicle_thrust_setpoint1_pub.publish(v_thrust_sp);
}
int int
VtolAttitudeControl::task_spawn(int argc, char *argv[]) VtolAttitudeControl::task_spawn(int argc, char *argv[])
{ {
src/modules/vtol_att_control/vtol_att_control_main.h
+9
View File
@@ -62,6 +62,7 @@
#include <px4_platform_common/posix.h> #include <px4_platform_common/posix.h>
#include <px4_platform_common/px4_work_queue/WorkItem.hpp> #include <px4_platform_common/px4_work_queue/WorkItem.hpp>
#include <uORB/Publication.hpp> #include <uORB/Publication.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/Subscription.hpp> #include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp> #include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/action_request.h> #include <uORB/topics/action_request.h>
@@ -80,6 +81,8 @@
#include <uORB/topics/vehicle_local_position_setpoint.h> #include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vtol_vehicle_status.h> #include <uORB/topics/vtol_vehicle_status.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>
#include "standard.h" #include "standard.h"
#include "tailsitter.h" #include "tailsitter.h"
#include "tiltrotor.h" #include "tiltrotor.h"
@@ -142,6 +145,8 @@ public:
struct Params *get_params() {return &_params;} struct Params *get_params() {return &_params;}
private: private:
void publishTorqueSetpoints(const hrt_abstime &timestamp_sample);
void publishThrustSetpoints(const hrt_abstime &timestamp_sample);
void Run() override; void Run() override;
@@ -168,6 +173,10 @@ private:
uORB::Publication<actuator_controls_s> _actuators_1_pub{ORB_ID(actuator_controls_1)}; uORB::Publication<actuator_controls_s> _actuators_1_pub{ORB_ID(actuator_controls_1)};
uORB::Publication<vehicle_attitude_setpoint_s> _v_att_sp_pub{ORB_ID(vehicle_attitude_setpoint)}; uORB::Publication<vehicle_attitude_setpoint_s> _v_att_sp_pub{ORB_ID(vehicle_attitude_setpoint)};
uORB::Publication<vtol_vehicle_status_s> _vtol_vehicle_status_pub{ORB_ID(vtol_vehicle_status)}; uORB::Publication<vtol_vehicle_status_s> _vtol_vehicle_status_pub{ORB_ID(vtol_vehicle_status)};
uORB::PublicationMulti<vehicle_thrust_setpoint_s> _vehicle_thrust_setpoint0_pub{ORB_ID(vehicle_thrust_setpoint)};
uORB::PublicationMulti<vehicle_torque_setpoint_s> _vehicle_torque_setpoint0_pub{ORB_ID(vehicle_torque_setpoint)};
uORB::PublicationMulti<vehicle_thrust_setpoint_s> _vehicle_thrust_setpoint1_pub{ORB_ID(vehicle_thrust_setpoint)};
uORB::PublicationMulti<vehicle_torque_setpoint_s> _vehicle_torque_setpoint1_pub{ORB_ID(vehicle_torque_setpoint)};
orb_advert_t _mavlink_log_pub{nullptr}; // mavlink log uORB handle orb_advert_t _mavlink_log_pub{nullptr}; // mavlink log uORB handle