fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 14:58:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

uuv_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 590239dedb
commit 301100ce0e
2 changed files with 34 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/uuv_att_control/uuv_att_control.cpp
+27 -1
View File
@@ -294,12 +294,38 @@ void UUVAttitudeControl::Run()
_vcontrol_mode.flag_control_attitude_enabled) { _vcontrol_mode.flag_control_attitude_enabled) {
/* publish the actuator controls */ /* publish the actuator controls */
_actuator_controls_pub.publish(_actuators); _actuator_controls_pub.publish(_actuators);
publishTorqueSetpoint(0);
publishThrustSetpoint(0);
} }
perf_end(_loop_perf); perf_end(_loop_perf);
} }
void UUVAttitudeControl::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 UUVAttitudeControl::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);
}
int UUVAttitudeControl::task_spawn(int argc, char *argv[]) int UUVAttitudeControl::task_spawn(int argc, char *argv[])
{ {
UUVAttitudeControl *instance = new UUVAttitudeControl(); UUVAttitudeControl *instance = new UUVAttitudeControl();
src/modules/uuv_att_control/uuv_att_control.hpp
+7
View File
@@ -69,6 +69,8 @@
#include <uORB/topics/vehicle_rates_setpoint.h> #include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/actuator_controls.h> #include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_thrust_setpoint.h>
#include <uORB/topics/vehicle_torque_setpoint.h>
#include <uORB/uORB.h> #include <uORB/uORB.h>
using matrix::Eulerf; using matrix::Eulerf;
@@ -98,7 +100,12 @@ public:
bool init(); bool init();
private: private:
void publishTorqueSetpoint(const hrt_abstime &timestamp_sample);
void publishThrustSetpoint(const hrt_abstime &timestamp_sample);
uORB::Publication<actuator_controls_s> _actuator_controls_pub{ORB_ID(actuator_controls_0)}; uORB::Publication<actuator_controls_s> _actuator_controls_pub{ORB_ID(actuator_controls_0)};
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)};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};