dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

vtol_att_control: publish vehicle_thrust_setpoint & vehicle_torque_setpoint

This commit is contained in:
Beat Küng 2021-11-29 13:50:47 +01:00 committed by Daniel Agar
parent d1abdd0f8d
commit 5da67e2e28
2 changed files with 48 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
src/modules/vtol_att_control/vtol_att_control_main.cpp
View File

@ -118,6 +118,10 @@ VtolAttitudeControl::VtolAttitudeControl() :
}
_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()
@ -499,6 +503,8 @@ VtolAttitudeControl::Run()
_vtol_type->fill_actuator_outputs();
_actuators_0_pub.publish(_actuators_out_0);
_actuators_1_pub.publish(_actuators_out_1);
publishTorqueSetpoints(0);
publishThrustSetpoints(0);
// Advertise/Publish vtol vehicle status
_vtol_vehicle_status.timestamp = hrt_absolute_time();
@ -508,6 +514,39 @@ VtolAttitudeControl::Run()
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
VtolAttitudeControl::task_spawn(int argc, char *argv[])
{

9
src/modules/vtol_att_control/vtol_att_control_main.h
View File

@ -62,6 +62,7 @@
#include <px4_platform_common/posix.h>
#include <px4_platform_common/px4_work_queue/WorkItem.hpp>
#include <uORB/Publication.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/action_request.h>
@ -80,6 +81,8 @@
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vtol_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 "tailsitter.h"
#include "tiltrotor.h"
@ -142,6 +145,8 @@ public:
struct Params *get_params() {return &_params;}
private:
void publishTorqueSetpoints(const hrt_abstime &timestamp_sample);
void publishThrustSetpoints(const hrt_abstime &timestamp_sample);
void Run() override;
@ -168,6 +173,10 @@ private:
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<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