fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 21:43:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

VTOL: explicitly start all FW & MC controllers in VTOL mode

Browse Source
This commit is contained in:
Daniel Agar
2019-11-30 12:58:36 -05:00
committed by GitHub
parent e50a50876f
commit 0cc250194d
12 changed files with 149 additions and 240 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+33 -50
View File
@@ -33,13 +33,29 @@
#include "FixedwingPositionControl.hpp"
FixedwingPositionControl::FixedwingPositionControl() :
#include <vtol_att_control/vtol_type.h>
FixedwingPositionControl::FixedwingPositionControl(bool vtol) :
ModuleParams(nullptr),
WorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl),
_loop_perf(perf_alloc(PC_ELAPSED, "fw_pos_control_l1: cycle")),
_attitude_sp_pub(vtol ? ORB_ID(fw_virtual_attitude_setpoint) : ORB_ID(vehicle_attitude_setpoint)),
_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")),
_launchDetector(this),
_runway_takeoff(this)
{
if (vtol) {
_parameter_handles.airspeed_trans = param_find("VT_ARSP_TRANS");
// VTOL parameter VTOL_TYPE
int32_t vt_type = -1;
param_get(param_find("VT_TYPE"), &vt_type);
_vtol_tailsitter = (static_cast<vtol_type>(vt_type) == vtol_type::TAILSITTER);
} else {
_parameter_handles.airspeed_trans = PARAM_INVALID;
}
// limit to 50 Hz
_global_pos_sub.set_interval_ms(20);
@@ -98,13 +114,6 @@ FixedwingPositionControl::FixedwingPositionControl() :
_parameter_handles.speedrate_p = param_find("FW_T_SRATE_P");
_parameter_handles.loiter_radius = param_find("NAV_LOITER_RAD");
// if vehicle is vtol these handles will be set when we get the vehicle status
_parameter_handles.airspeed_trans = PARAM_INVALID;
_parameter_handles.vtol_type = PARAM_INVALID;
// initialize to invalid vtol type
_parameters.vtol_type = -1;
/* fetch initial parameter values */
parameters_update();
}
@@ -166,11 +175,6 @@ FixedwingPositionControl::parameters_update()
param_get(_parameter_handles.land_throtTC_scale, &(_parameters.land_throtTC_scale));
param_get(_parameter_handles.loiter_radius, &(_parameters.loiter_radius));
// VTOL parameter VTOL_TYPE
if (_parameter_handles.vtol_type != PARAM_INVALID) {
param_get(_parameter_handles.vtol_type, &_parameters.vtol_type);
}
// VTOL parameter VT_ARSP_TRANS
if (_parameter_handles.airspeed_trans != PARAM_INVALID) {
param_get(_parameter_handles.airspeed_trans, &_parameters.airspeed_trans);
@@ -333,27 +337,6 @@ FixedwingPositionControl::vehicle_command_poll()
}
}
void
FixedwingPositionControl::vehicle_status_poll()
{
if (_vehicle_status_sub.update(&_vehicle_status)) {
/* set correct uORB ID, depending on if vehicle is VTOL or not */
if (_attitude_setpoint_id == nullptr) {
if (_vehicle_status.is_vtol) {
_attitude_setpoint_id = ORB_ID(fw_virtual_attitude_setpoint);
_parameter_handles.airspeed_trans = param_find("VT_ARSP_TRANS");
_parameter_handles.vtol_type = param_find("VT_TYPE");
parameters_update();
} else {
_attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint);
}
}
}
}
void
FixedwingPositionControl::airspeed_poll()
{
@@ -399,12 +382,12 @@ FixedwingPositionControl::vehicle_attitude_poll()
// if the vehicle is a tailsitter we have to rotate the attitude by the pitch offset
// between multirotor and fixed wing flight
if (static_cast<vtol_type>(_parameters.vtol_type) == vtol_type::TAILSITTER && _vehicle_status.is_vtol) {
if (_vtol_tailsitter) {
Dcmf R_offset = Eulerf(0, M_PI_2_F, 0);
_R_nb = _R_nb * R_offset;
}
Eulerf euler_angles(_R_nb);
const Eulerf euler_angles(_R_nb);
_roll = euler_angles(0);
_pitch = euler_angles(1);
_yaw = euler_angles(2);
@@ -1708,7 +1691,7 @@ FixedwingPositionControl::Run()
vehicle_command_poll();
vehicle_control_mode_poll();
_vehicle_land_detected_sub.update(&_vehicle_land_detected);
vehicle_status_poll();
_vehicle_status_sub.update(&_vehicle_status);
_vehicle_acceleration_sub.update();
_vehicle_rates_sub.update();
@@ -1753,15 +1736,7 @@ FixedwingPositionControl::Run()
_control_mode.flag_control_acceleration_enabled ||
_control_mode.flag_control_altitude_enabled) {
/* lazily publish the setpoint only once available */
if (_attitude_sp_pub != nullptr) {
/* publish the attitude setpoint */
orb_publish(_attitude_setpoint_id, _attitude_sp_pub, &_att_sp);
} else if (_attitude_setpoint_id != nullptr) {
/* advertise and publish */
_attitude_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp);
}
_attitude_sp_pub.publish(_att_sp);
// only publish status in full FW mode
if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING
@@ -1898,7 +1873,7 @@ FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float airspee
// tailsitters use the multicopter frame as reference, in fixed wing
// we need to use the fixed wing frame
if (static_cast<vtol_type>(_parameters.vtol_type) == vtol_type::TAILSITTER && _vehicle_status.is_vtol) {
if (_vtol_tailsitter) {
float tmp = accel_body(0);
accel_body(0) = -accel_body(2);
accel_body(2) = tmp;
@@ -1944,7 +1919,15 @@ FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float airspee
int FixedwingPositionControl::task_spawn(int argc, char *argv[])
{
FixedwingPositionControl *instance = new FixedwingPositionControl();
bool vtol = false;
if (argc > 1) {
if (strcmp(argv[1], "vtol") == 0) {
vtol = true;
}
}
FixedwingPositionControl *instance = new FixedwingPositionControl(vtol);
if (instance) {
_object.store(instance);
@@ -1990,9 +1973,9 @@ fw_pos_control_l1 is the fixed wing position controller.
)DESCR_STR");
PRINT_MODULE_USAGE_NAME("fw_pos_control_l1", "controller");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_ARG("vtol", "VTOL mode", true);
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
return 0;