forked from Archive/PX4-Autopilot
vtol_att_control: publish vehicle_thrust_setpoint & vehicle_torque_setpoint
This commit is contained in:
parent
d1abdd0f8d
commit
5da67e2e28
|
@ -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 ×tamp_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 ×tamp_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[])
|
||||
{
|
||||
|
|
|
@ -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 ×tamp_sample);
|
||||
void publishThrustSetpoints(const hrt_abstime ×tamp_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
|
||||
|
||||
|
|
Loading…
Reference in New Issue