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

fw_att_control: publich torque&thrust sp for VTOL during fixed wing mode

This commit is contained in:
Julien Lecoeur 2019-11-19 16:01:40 +01:00
parent 0fede1ff50
commit 0bbfc36f11
2 changed files with 26 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
src/modules/fw_att_control/FixedwingAttitudeControl.cpp
View File

@ -655,6 +655,28 @@ void FixedwingAttitudeControl::Run()
}
_actuators_2_pub.publish(_actuators_airframe);
/* publish thrust and torque setpoint */
if (!_vehicle_status.is_vtol
|| (_vehicle_status.is_vtol && (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING))
) {
vehicle_torque_setpoint_s v_torque_sp = {};
v_torque_sp.timestamp = hrt_absolute_time();
v_torque_sp.timestamp_sample = _att.timestamp;
v_torque_sp.xyz[0] = (PX4_ISFINITE(_actuators.control[0])) ? _actuators.control[0] : 0.0f;
v_torque_sp.xyz[1] = (PX4_ISFINITE(_actuators.control[1])) ? _actuators.control[1] : 0.0f;
v_torque_sp.xyz[2] = (PX4_ISFINITE(_actuators.control[2])) ? _actuators.control[2] : 0.0f;
_vehicle_torque_setpoint_pub.publish(v_torque_sp);
vehicle_thrust_setpoint_s v_thrust_sp = {};
v_thrust_sp.timestamp = hrt_absolute_time();
v_thrust_sp.timestamp_sample = _att.timestamp;
v_thrust_sp.xyz[0] = (PX4_ISFINITE(_actuators.control[3])) ? _actuators.control[3] : 0.0f;
v_thrust_sp.xyz[1] = 0.0f;
v_thrust_sp.xyz[2] = 0.0f;
_vehicle_thrust_setpoint_pub.publish(v_thrust_sp);
}
}
}

4
src/modules/fw_att_control/FixedwingAttitudeControl.hpp
View File

@ -66,6 +66,8 @@
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_thrust_setpoint.h>
#include <uORB/topics/vehicle_torque_setpoint.h>
using matrix::Eulerf;
using matrix::Quatf;
@ -115,6 +117,8 @@ private:
uORB::Publication<actuator_controls_s> _actuators_2_pub{ORB_ID(actuator_controls_2)}; /**< actuator control group 1 setpoint (Airframe) */
uORB::Publication<vehicle_rates_setpoint_s> _rate_sp_pub{ORB_ID(vehicle_rates_setpoint)}; /**< rate setpoint publication */
uORB::PublicationMulti<rate_ctrl_status_s> _rate_ctrl_status_pub{ORB_ID(rate_ctrl_status)}; /**< rate controller status publication */
uORB::Publication<vehicle_thrust_setpoint_s> _vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)}; /**< vehicle_thrust_setpoint publication */
uORB::Publication<vehicle_torque_setpoint_s> _vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)}; /**< vehicle_torque_setpoint publication */
orb_id_t _attitude_setpoint_id{nullptr};
orb_advert_t _attitude_sp_pub{nullptr}; /**< attitude setpoint point */