forked from Archive/PX4-Autopilot
VTOL: explicitly start all FW & MC controllers in VTOL mode
This commit is contained in:
Daniel Agar
GitHub
parent
e50a50876f
commit
0cc250194d
|
|
@ -15,16 +15,17 @@ ekf2 start
|
|||
# End Estimator group selection #
|
||||
###############################################################################
|
||||
|
||||
|
||||
vtol_att_control start
|
||||
mc_rate_control start
|
||||
mc_att_control start
|
||||
mc_pos_control start
|
||||
fw_att_control start
|
||||
fw_pos_control_l1 start
|
||||
airspeed_selector start
|
||||
|
||||
#
|
||||
vtol_att_control start
|
||||
|
||||
mc_rate_control start vtol
|
||||
mc_att_control start vtol
|
||||
mc_pos_control start vtol
|
||||
|
||||
fw_att_control start vtol
|
||||
fw_pos_control_l1 start vtol
|
||||
|
||||
# Start Land Detector
|
||||
# Multicopter for now until we have something for VTOL
|
||||
#
|
||||
|
|
|
|||
|
|
@ -239,15 +239,6 @@ class Graph(object):
|
|||
|
||||
special_cases_pub = [
|
||||
('replay', r'Replay\.cpp$', None, r'^sub\.orb_meta$'),
|
||||
('fw_pos_control_l1', r'FixedwingPositionControl\.cpp$', r'\b_attitude_setpoint_id=([^,)]+)', r'^_attitude_setpoint_id$'),
|
||||
|
||||
('mc_pos_control', r'mc_pos_control_main\.cpp$', r'\b_attitude_setpoint_id=([^,)]+)', r'^_attitude_setpoint_id$'),
|
||||
|
||||
('mc_rate_control', r'MulticopterRateControl\.cpp$', r'\b_actuators_id=([^,)]+)', r'^_actuators_id$'),
|
||||
('mc_att_control', r'mc_att_control_main\.cpp$', r'\_attitude_sp_id=([^,)]+)', r'^_attitude_sp_id$'),
|
||||
|
||||
('fw_att_control', r'FixedwingAttitudeControl\.cpp$', r'\b_actuators_id=([^,)]+)', r'^_actuators_id$'),
|
||||
('fw_att_control', r'FixedwingAttitudeControl\.cpp$', r'\b_attitude_setpoint_id=([^,)]+)', r'^_attitude_setpoint_id$'),
|
||||
|
||||
('uavcan', r'sensors/.*\.cpp$', r'\bUavcanCDevSensorBridgeBase\([^{]*DEVICE_PATH,([^,)]+)', r'^_orb_topic$'),
|
||||
]
|
||||
|
|
|
|||
|
|
@ -26,11 +26,11 @@ land_detector start vtol
|
|||
navigator start
|
||||
ekf2 start
|
||||
vtol_att_control start
|
||||
mc_pos_control start
|
||||
mc_att_control start
|
||||
mc_rate_control start
|
||||
fw_pos_control_l1 start
|
||||
fw_att_control start
|
||||
mc_pos_control start vtol
|
||||
mc_att_control start vtol
|
||||
mc_rate_control start vtol
|
||||
fw_pos_control_l1 start vtol
|
||||
fw_att_control start vtol
|
||||
airspeed_selector start
|
||||
|
||||
#mixer load /dev/pwm_output0 ROMFS/sitl/mixers/standard_vtol_sitl.main.mix
|
||||
|
|
|
|||
|
|
@ -40,13 +40,21 @@ using math::constrain;
|
|||
using math::gradual;
|
||||
using math::radians;
|
||||
|
||||
FixedwingAttitudeControl::FixedwingAttitudeControl() :
|
||||
FixedwingAttitudeControl::FixedwingAttitudeControl(bool vtol) :
|
||||
ModuleParams(nullptr),
|
||||
WorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl),
|
||||
_actuators_0_pub(vtol ? ORB_ID(actuator_controls_virtual_fw) : ORB_ID(actuator_controls_0)),
|
||||
_attitude_sp_pub(vtol ? ORB_ID(fw_virtual_attitude_setpoint) : ORB_ID(vehicle_attitude_setpoint)),
|
||||
_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
|
||||
{
|
||||
// check if VTOL first
|
||||
vehicle_status_poll();
|
||||
if (vtol) {
|
||||
int32_t vt_type = -1;
|
||||
|
||||
if (param_get(param_find("VT_TYPE"), &vt_type) == PX4_OK) {
|
||||
_is_tailsitter = (static_cast<vtol_type>(vt_type) == vtol_type::TAILSITTER);
|
||||
}
|
||||
}
|
||||
|
||||
/* fetch initial parameter values */
|
||||
parameters_update();
|
||||
|
|
@ -165,14 +173,7 @@ FixedwingAttitudeControl::vehicle_manual_poll()
|
|||
|
||||
_att_sp.timestamp = hrt_absolute_time();
|
||||
|
||||
if (_attitude_sp_pub != nullptr) {
|
||||
/* publish the attitude rates setpoint */
|
||||
orb_publish(_attitude_setpoint_id, _attitude_sp_pub, &_att_sp);
|
||||
|
||||
} else if (_attitude_setpoint_id) {
|
||||
/* advertise the attitude rates setpoint */
|
||||
_attitude_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp);
|
||||
}
|
||||
_attitude_sp_pub.publish(_att_sp);
|
||||
|
||||
} else if (_vcontrol_mode.flag_control_rates_enabled &&
|
||||
!_vcontrol_mode.flag_control_attitude_enabled) {
|
||||
|
|
@ -220,30 +221,6 @@ FixedwingAttitudeControl::vehicle_rates_setpoint_poll()
|
|||
}
|
||||
}
|
||||
|
||||
void
|
||||
FixedwingAttitudeControl::vehicle_status_poll()
|
||||
{
|
||||
if (_vehicle_status_sub.update(&_vehicle_status)) {
|
||||
/* set correct uORB ID, depending on if vehicle is VTOL or not */
|
||||
if (!_actuators_id) {
|
||||
if (_vehicle_status.is_vtol) {
|
||||
_actuators_id = ORB_ID(actuator_controls_virtual_fw);
|
||||
_attitude_setpoint_id = ORB_ID(fw_virtual_attitude_setpoint);
|
||||
|
||||
int32_t vt_type = -1;
|
||||
|
||||
if (param_get(param_find("VT_TYPE"), &vt_type) == PX4_OK) {
|
||||
_is_tailsitter = (static_cast<vtol_type>(vt_type) == vtol_type::TAILSITTER);
|
||||
}
|
||||
|
||||
} else {
|
||||
_actuators_id = ORB_ID(actuator_controls_0);
|
||||
_attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
FixedwingAttitudeControl::vehicle_land_detected_poll()
|
||||
{
|
||||
|
|
@ -378,7 +355,10 @@ void FixedwingAttitudeControl::Run()
|
|||
const matrix::Eulerf euler_angles(R);
|
||||
|
||||
vehicle_attitude_setpoint_poll();
|
||||
vehicle_status_poll(); // this poll has to be before the control_mode_poll, otherwise rate sp are not published during whole transition
|
||||
|
||||
// vehicle status update must be before the vehicle_control_mode_poll(), otherwise rate sp are not published during whole transition
|
||||
_vehicle_status_sub.update(&_vehicle_status);
|
||||
|
||||
vehicle_control_mode_poll();
|
||||
vehicle_manual_poll();
|
||||
_global_pos_sub.update(&_global_pos);
|
||||
|
|
@ -646,14 +626,8 @@ void FixedwingAttitudeControl::Run()
|
|||
if (_vcontrol_mode.flag_control_rates_enabled ||
|
||||
_vcontrol_mode.flag_control_attitude_enabled ||
|
||||
_vcontrol_mode.flag_control_manual_enabled) {
|
||||
/* publish the actuator controls */
|
||||
if (_actuators_0_pub != nullptr) {
|
||||
orb_publish(_actuators_id, _actuators_0_pub, &_actuators);
|
||||
|
||||
} else if (_actuators_id) {
|
||||
_actuators_0_pub = orb_advertise(_actuators_id, &_actuators);
|
||||
}
|
||||
|
||||
_actuators_0_pub.publish(_actuators);
|
||||
_actuators_2_pub.publish(_actuators_airframe);
|
||||
}
|
||||
}
|
||||
|
|
@ -728,7 +702,15 @@ void FixedwingAttitudeControl::control_flaps(const float dt)
|
|||
|
||||
int FixedwingAttitudeControl::task_spawn(int argc, char *argv[])
|
||||
{
|
||||
FixedwingAttitudeControl *instance = new FixedwingAttitudeControl();
|
||||
bool vtol = false;
|
||||
|
||||
if (argc > 1) {
|
||||
if (strcmp(argv[1], "vtol") == 0) {
|
||||
vtol = true;
|
||||
}
|
||||
}
|
||||
|
||||
FixedwingAttitudeControl *instance = new FixedwingAttitudeControl(vtol);
|
||||
|
||||
if (instance) {
|
||||
_object.store(instance);
|
||||
|
|
@ -774,8 +756,9 @@ fw_att_control is the fixed wing attitude controller.
|
|||
|
||||
)DESCR_STR");
|
||||
|
||||
PRINT_MODULE_USAGE_COMMAND("start");
|
||||
PRINT_MODULE_USAGE_NAME("fw_att_control", "controller");
|
||||
PRINT_MODULE_USAGE_COMMAND("start");
|
||||
PRINT_MODULE_USAGE_ARG("vtol", "VTOL mode", true);
|
||||
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
|
||||
|
||||
return 0;
|
||||
|
|
|
|||
|
|
@ -76,7 +76,7 @@ class FixedwingAttitudeControl final : public ModuleBase<FixedwingAttitudeContro
|
|||
public px4::WorkItem
|
||||
{
|
||||
public:
|
||||
FixedwingAttitudeControl();
|
||||
FixedwingAttitudeControl(bool vtol = false);
|
||||
~FixedwingAttitudeControl() override;
|
||||
|
||||
/** @see ModuleBase */
|
||||
|
|
@ -112,15 +112,11 @@ private:
|
|||
|
||||
uORB::SubscriptionData<airspeed_validated_s> _airspeed_validated_sub{ORB_ID(airspeed_validated)};
|
||||
|
||||
uORB::Publication<actuator_controls_s> _actuators_0_pub;
|
||||
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 */
|
||||
|
||||
orb_id_t _attitude_setpoint_id{nullptr};
|
||||
orb_advert_t _attitude_sp_pub{nullptr}; /**< attitude setpoint point */
|
||||
|
||||
orb_id_t _actuators_id{nullptr}; /**< pointer to correct actuator controls0 uORB metadata structure */
|
||||
orb_advert_t _actuators_0_pub{nullptr}; /**< actuator control group 0 setpoint */
|
||||
uORB::Publication<vehicle_attitude_setpoint_s> _attitude_sp_pub;
|
||||
uORB::Publication<vehicle_rates_setpoint_s> _rate_sp_pub{ORB_ID(vehicle_rates_setpoint)};
|
||||
uORB::PublicationMulti<rate_ctrl_status_s> _rate_ctrl_status_pub{ORB_ID(rate_ctrl_status)};
|
||||
|
||||
actuator_controls_s _actuators {}; /**< actuator control inputs */
|
||||
actuator_controls_s _actuators_airframe {}; /**< actuator control inputs */
|
||||
|
|
@ -233,7 +229,6 @@ private:
|
|||
void vehicle_manual_poll();
|
||||
void vehicle_attitude_setpoint_poll();
|
||||
void vehicle_rates_setpoint_poll();
|
||||
void vehicle_status_poll();
|
||||
void vehicle_land_detected_poll();
|
||||
|
||||
float get_airspeed_and_update_scaling();
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -129,7 +129,7 @@ class FixedwingPositionControl final : public ModuleBase<FixedwingPositionContro
|
|||
public px4::WorkItem
|
||||
{
|
||||
public:
|
||||
FixedwingPositionControl();
|
||||
FixedwingPositionControl(bool vtol = false);
|
||||
~FixedwingPositionControl() override;
|
||||
|
||||
/** @see ModuleBase */
|
||||
|
|
@ -165,9 +165,7 @@ private:
|
|||
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; ///< vehicle status subscription
|
||||
uORB::SubscriptionData<vehicle_angular_velocity_s> _vehicle_rates_sub{ORB_ID(vehicle_angular_velocity)};
|
||||
|
||||
orb_advert_t _attitude_sp_pub{nullptr}; ///< attitude setpoint
|
||||
orb_id_t _attitude_setpoint_id{nullptr};
|
||||
|
||||
uORB::Publication<vehicle_attitude_setpoint_s> _attitude_sp_pub;
|
||||
uORB::Publication<position_controller_status_s> _pos_ctrl_status_pub{ORB_ID(position_controller_status)}; ///< navigation capabilities publication
|
||||
uORB::Publication<position_controller_landing_status_s> _pos_ctrl_landing_status_pub{ORB_ID(position_controller_landing_status)}; ///< landing status publication
|
||||
uORB::Publication<tecs_status_s> _tecs_status_pub{ORB_ID(tecs_status)}; ///< TECS status publication
|
||||
|
|
@ -253,6 +251,8 @@ private:
|
|||
float _asp_after_transition{0.0f};
|
||||
bool _was_in_transition{false};
|
||||
|
||||
bool _vtol_tailsitter{false};
|
||||
|
||||
// estimator reset counters
|
||||
uint8_t _pos_reset_counter{0}; ///< captures the number of times the estimator has reset the horizontal position
|
||||
uint8_t _alt_reset_counter{0}; ///< captures the number of times the estimator has reset the altitude state
|
||||
|
|
@ -307,7 +307,6 @@ private:
|
|||
|
||||
// VTOL
|
||||
float airspeed_trans;
|
||||
int32_t vtol_type;
|
||||
} _parameters{}; ///< local copies of interesting parameters
|
||||
|
||||
struct {
|
||||
|
|
@ -370,8 +369,6 @@ private:
|
|||
param_t land_airspeed_scale;
|
||||
param_t land_throtTC_scale;
|
||||
param_t loiter_radius;
|
||||
|
||||
param_t vtol_type;
|
||||
} _parameter_handles {}; ///< handles for interesting parameters
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
|
|
|
|||
|
|
@ -66,9 +66,8 @@ class MulticopterAttitudeControl : public ModuleBase<MulticopterAttitudeControl>
|
|||
public px4::WorkItem
|
||||
{
|
||||
public:
|
||||
MulticopterAttitudeControl();
|
||||
|
||||
virtual ~MulticopterAttitudeControl();
|
||||
MulticopterAttitudeControl(bool vtol = false);
|
||||
~MulticopterAttitudeControl() override;
|
||||
|
||||
/** @see ModuleBase */
|
||||
static int task_spawn(int argc, char *argv[]);
|
||||
|
|
@ -93,8 +92,6 @@ private:
|
|||
*/
|
||||
void parameters_updated();
|
||||
|
||||
void vehicle_status_poll();
|
||||
|
||||
void publish_rates_setpoint();
|
||||
|
||||
float throttle_curve(float throttle_stick_input);
|
||||
|
|
@ -122,10 +119,7 @@ private:
|
|||
uORB::SubscriptionCallbackWorkItem _vehicle_attitude_sub{this, ORB_ID(vehicle_attitude)};
|
||||
|
||||
uORB::Publication<vehicle_rates_setpoint_s> _v_rates_sp_pub{ORB_ID(vehicle_rates_setpoint)}; /**< rate setpoint publication */
|
||||
|
||||
orb_advert_t _vehicle_attitude_setpoint_pub{nullptr};
|
||||
|
||||
orb_id_t _attitude_sp_id{nullptr}; /**< pointer to correct attitude setpoint uORB metadata structure */
|
||||
uORB::Publication<vehicle_attitude_setpoint_s> _vehicle_attitude_setpoint_pub;
|
||||
|
||||
struct vehicle_attitude_s _v_att {}; /**< vehicle attitude */
|
||||
struct vehicle_attitude_setpoint_s _v_att_sp {}; /**< vehicle attitude setpoint */
|
||||
|
|
|
|||
|
|
@ -51,11 +51,20 @@
|
|||
|
||||
using namespace matrix;
|
||||
|
||||
MulticopterAttitudeControl::MulticopterAttitudeControl() :
|
||||
MulticopterAttitudeControl::MulticopterAttitudeControl(bool vtol) :
|
||||
ModuleParams(nullptr),
|
||||
WorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl),
|
||||
_vehicle_attitude_setpoint_pub(vtol ? ORB_ID(mc_virtual_attitude_setpoint) : ORB_ID(vehicle_attitude_setpoint)),
|
||||
_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
|
||||
{
|
||||
if (vtol) {
|
||||
int32_t vt_type = -1;
|
||||
|
||||
if (param_get(param_find("VT_TYPE"), &vt_type) == PX4_OK) {
|
||||
_is_tailsitter = (static_cast<vtol_type>(vt_type) == vtol_type::TAILSITTER);
|
||||
}
|
||||
}
|
||||
|
||||
_vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
|
||||
|
||||
/* initialize quaternions in messages to be valid */
|
||||
|
|
@ -95,29 +104,6 @@ MulticopterAttitudeControl::parameters_updated()
|
|||
_man_tilt_max = math::radians(_param_mpc_man_tilt_max.get());
|
||||
}
|
||||
|
||||
void
|
||||
MulticopterAttitudeControl::vehicle_status_poll()
|
||||
{
|
||||
/* check if there is new status information */
|
||||
if (_vehicle_status_sub.update(&_vehicle_status)) {
|
||||
/* set correct uORB ID, depending on if vehicle is VTOL or not */
|
||||
if (_attitude_sp_id == nullptr) {
|
||||
if (_vehicle_status.is_vtol) {
|
||||
_attitude_sp_id = ORB_ID(mc_virtual_attitude_setpoint);
|
||||
|
||||
int32_t vt_type = -1;
|
||||
|
||||
if (param_get(param_find("VT_TYPE"), &vt_type) == PX4_OK) {
|
||||
_is_tailsitter = (static_cast<vtol_type>(vt_type) == vtol_type::TAILSITTER);
|
||||
}
|
||||
|
||||
} else {
|
||||
_attitude_sp_id = ORB_ID(vehicle_attitude_setpoint);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
float
|
||||
MulticopterAttitudeControl::throttle_curve(float throttle_stick_input)
|
||||
{
|
||||
|
|
@ -233,9 +219,7 @@ MulticopterAttitudeControl::generate_attitude_setpoint(float dt, bool reset_yaw_
|
|||
attitude_setpoint.thrust_body[2] = -throttle_curve(_manual_control_sp.z);
|
||||
attitude_setpoint.timestamp = hrt_absolute_time();
|
||||
|
||||
if (_attitude_sp_id != nullptr) {
|
||||
orb_publish_auto(_attitude_sp_id, &_vehicle_attitude_setpoint_pub, &attitude_setpoint, nullptr, ORB_PRIO_DEFAULT);
|
||||
}
|
||||
_vehicle_attitude_setpoint_pub.publish(attitude_setpoint);
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -308,7 +292,7 @@ MulticopterAttitudeControl::Run()
|
|||
_manual_control_sp_sub.update(&_manual_control_sp);
|
||||
_v_control_mode_sub.update(&_v_control_mode);
|
||||
_vehicle_land_detected_sub.update(&_vehicle_land_detected);
|
||||
vehicle_status_poll();
|
||||
_vehicle_status_sub.update(&_vehicle_status);
|
||||
|
||||
/* Check if we are in rattitude mode and the pilot is above the threshold on pitch
|
||||
* or roll (yaw can rotate 360 in normal att control). If both are true don't
|
||||
|
|
@ -365,7 +349,15 @@ MulticopterAttitudeControl::Run()
|
|||
|
||||
int MulticopterAttitudeControl::task_spawn(int argc, char *argv[])
|
||||
{
|
||||
MulticopterAttitudeControl *instance = new MulticopterAttitudeControl();
|
||||
bool vtol = false;
|
||||
|
||||
if (argc > 1) {
|
||||
if (strcmp(argv[1], "vtol") == 0) {
|
||||
vtol = true;
|
||||
}
|
||||
}
|
||||
|
||||
MulticopterAttitudeControl *instance = new MulticopterAttitudeControl(vtol);
|
||||
|
||||
if (instance) {
|
||||
_object.store(instance);
|
||||
|
|
@ -425,6 +417,7 @@ https://www.research-collection.ethz.ch/bitstream/handle/20.500.11850/154099/eth
|
|||
|
||||
PRINT_MODULE_USAGE_NAME("mc_att_control", "controller");
|
||||
PRINT_MODULE_USAGE_COMMAND("start");
|
||||
PRINT_MODULE_USAGE_ARG("vtol", "VTOL mode", true);
|
||||
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
|
||||
|
||||
return 0;
|
||||
|
|
|
|||
|
|
@ -83,8 +83,7 @@ class MulticopterPositionControl : public ModuleBase<MulticopterPositionControl>
|
|||
public ModuleParams, public px4::WorkItem
|
||||
{
|
||||
public:
|
||||
MulticopterPositionControl();
|
||||
|
||||
MulticopterPositionControl(bool vtol = false);
|
||||
~MulticopterPositionControl() override;
|
||||
|
||||
/** @see ModuleBase */
|
||||
|
|
@ -106,8 +105,7 @@ public:
|
|||
private:
|
||||
Takeoff _takeoff; /**< state machine and ramp to bring the vehicle off the ground without jumps */
|
||||
|
||||
orb_id_t _attitude_setpoint_id{nullptr}; ///< orb metadata to publish attitude setpoint dependent if VTOL or not
|
||||
orb_advert_t _vehicle_attitude_setpoint_pub{nullptr}; ///< attitude setpoint publication handle
|
||||
uORB::Publication<vehicle_attitude_setpoint_s> _vehicle_attitude_setpoint_pub;
|
||||
uORB::PublicationQueued<vehicle_command_s> _pub_vehicle_command{ORB_ID(vehicle_command)}; /**< vehicle command publication */
|
||||
orb_advert_t _mavlink_log_pub{nullptr};
|
||||
|
||||
|
|
@ -236,11 +234,6 @@ private:
|
|||
*/
|
||||
void warn_rate_limited(const char *str);
|
||||
|
||||
/**
|
||||
* Publish attitude.
|
||||
*/
|
||||
void publish_attitude();
|
||||
|
||||
/**
|
||||
* Adjust the setpoint during landing.
|
||||
* Thrust is adjusted to support the land-detector during detection.
|
||||
|
|
@ -280,15 +273,21 @@ private:
|
|||
void send_vehicle_cmd_do(uint8_t nav_state);
|
||||
};
|
||||
|
||||
MulticopterPositionControl::MulticopterPositionControl() :
|
||||
MulticopterPositionControl::MulticopterPositionControl(bool vtol) :
|
||||
SuperBlock(nullptr, "MPC"),
|
||||
ModuleParams(nullptr),
|
||||
WorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl),
|
||||
_vehicle_attitude_setpoint_pub(vtol ? ORB_ID(mc_virtual_attitude_setpoint) : ORB_ID(vehicle_attitude_setpoint)),
|
||||
_vel_x_deriv(this, "VELD"),
|
||||
_vel_y_deriv(this, "VELD"),
|
||||
_vel_z_deriv(this, "VELD"),
|
||||
_cycle_perf(perf_alloc_once(PC_ELAPSED, MODULE_NAME": cycle time"))
|
||||
{
|
||||
if (vtol) {
|
||||
// if vehicle is a VTOL we want to enable weathervane capabilities
|
||||
_wv_controller = new WeatherVane();
|
||||
}
|
||||
|
||||
// fetch initial parameter values
|
||||
parameters_update(true);
|
||||
|
||||
|
|
@ -394,24 +393,7 @@ MulticopterPositionControl::parameters_update(bool force)
|
|||
void
|
||||
MulticopterPositionControl::poll_subscriptions()
|
||||
{
|
||||
if (_vehicle_status_sub.update(&_vehicle_status)) {
|
||||
|
||||
// set correct uORB ID, depending on if vehicle is VTOL or not
|
||||
if (!_attitude_setpoint_id) {
|
||||
if (_vehicle_status.is_vtol) {
|
||||
_attitude_setpoint_id = ORB_ID(mc_virtual_attitude_setpoint);
|
||||
|
||||
} else {
|
||||
_attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint);
|
||||
}
|
||||
}
|
||||
|
||||
// if vehicle is a VTOL we want to enable weathervane capabilities
|
||||
if (_wv_controller == nullptr && _vehicle_status.is_vtol) {
|
||||
_wv_controller = new WeatherVane();
|
||||
}
|
||||
}
|
||||
|
||||
_vehicle_status_sub.update(&_vehicle_status);
|
||||
_vehicle_land_detected_sub.update(&_vehicle_land_detected);
|
||||
_control_mode_sub.update(&_control_mode);
|
||||
_home_pos_sub.update(&_home_pos);
|
||||
|
|
@ -702,9 +684,7 @@ MulticopterPositionControl::Run()
|
|||
// Not publishing when not running a flight task
|
||||
// in stabilized mode attitude setpoints get ignored
|
||||
// in offboard with attitude setpoints they come from MAVLink directly
|
||||
if (_attitude_setpoint_id != nullptr) {
|
||||
orb_publish_auto(_attitude_setpoint_id, &_vehicle_attitude_setpoint_pub, &attitude_setpoint, nullptr, ORB_PRIO_DEFAULT);
|
||||
}
|
||||
_vehicle_attitude_setpoint_pub.publish(attitude_setpoint);
|
||||
|
||||
_wv_dcm_z_sp_prev = Quatf(attitude_setpoint.q_d).dcm_z();
|
||||
|
||||
|
|
@ -1079,7 +1059,15 @@ void MulticopterPositionControl::send_vehicle_cmd_do(uint8_t nav_state)
|
|||
|
||||
int MulticopterPositionControl::task_spawn(int argc, char *argv[])
|
||||
{
|
||||
MulticopterPositionControl *instance = new MulticopterPositionControl();
|
||||
bool vtol = false;
|
||||
|
||||
if (argc > 1) {
|
||||
if (strcmp(argv[1], "vtol") == 0) {
|
||||
vtol = true;
|
||||
}
|
||||
}
|
||||
|
||||
MulticopterPositionControl *instance = new MulticopterPositionControl(vtol);
|
||||
|
||||
if (instance) {
|
||||
_object.store(instance);
|
||||
|
|
@ -1124,6 +1112,7 @@ logging.
|
|||
|
||||
PRINT_MODULE_USAGE_NAME("mc_pos_control", "controller");
|
||||
PRINT_MODULE_USAGE_COMMAND("start");
|
||||
PRINT_MODULE_USAGE_ARG("vtol", "VTOL mode", true);
|
||||
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
|
||||
|
||||
return 0;
|
||||
|
|
|
|||
|
|
@ -42,9 +42,10 @@ using namespace matrix;
|
|||
using namespace time_literals;
|
||||
using math::radians;
|
||||
|
||||
MulticopterRateControl::MulticopterRateControl() :
|
||||
MulticopterRateControl::MulticopterRateControl(bool vtol) :
|
||||
ModuleParams(nullptr),
|
||||
WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl),
|
||||
_actuators_0_pub(vtol ? ORB_ID(actuator_controls_virtual_mc) : ORB_ID(actuator_controls_0)),
|
||||
_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
|
||||
{
|
||||
_vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
|
||||
|
|
@ -97,23 +98,6 @@ MulticopterRateControl::parameters_updated()
|
|||
_actuators_0_circuit_breaker_enabled = circuit_breaker_enabled_by_val(_param_cbrk_rate_ctrl.get(), CBRK_RATE_CTRL_KEY);
|
||||
}
|
||||
|
||||
void
|
||||
MulticopterRateControl::vehicle_status_poll()
|
||||
{
|
||||
/* check if there is new status information */
|
||||
if (_vehicle_status_sub.update(&_vehicle_status)) {
|
||||
/* set correct uORB ID, depending on if vehicle is VTOL or not */
|
||||
if (_actuators_id == nullptr) {
|
||||
if (_vehicle_status.is_vtol) {
|
||||
_actuators_id = ORB_ID(actuator_controls_virtual_mc);
|
||||
|
||||
} else {
|
||||
_actuators_id = ORB_ID(actuator_controls_0);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
float
|
||||
MulticopterRateControl::get_landing_gear_state()
|
||||
{
|
||||
|
|
@ -183,7 +167,7 @@ MulticopterRateControl::Run()
|
|||
}
|
||||
}
|
||||
|
||||
vehicle_status_poll();
|
||||
_vehicle_status_sub.update(&_vehicle_status);
|
||||
|
||||
const bool manual_control_updated = _manual_control_sp_sub.update(&_manual_control_sp);
|
||||
|
||||
|
|
@ -326,14 +310,14 @@ MulticopterRateControl::Run()
|
|||
}
|
||||
|
||||
actuators.timestamp = hrt_absolute_time();
|
||||
orb_publish_auto(_actuators_id, &_actuators_0_pub, &actuators, nullptr, ORB_PRIO_DEFAULT);
|
||||
_actuators_0_pub.publish(actuators);
|
||||
|
||||
} else if (_v_control_mode.flag_control_termination_enabled) {
|
||||
if (!_vehicle_status.is_vtol) {
|
||||
// publish actuator controls
|
||||
actuator_controls_s actuators{};
|
||||
actuators.timestamp = hrt_absolute_time();
|
||||
orb_publish_auto(_actuators_id, &_actuators_0_pub, &actuators, nullptr, ORB_PRIO_DEFAULT);
|
||||
_actuators_0_pub.publish(actuators);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -343,7 +327,15 @@ MulticopterRateControl::Run()
|
|||
|
||||
int MulticopterRateControl::task_spawn(int argc, char *argv[])
|
||||
{
|
||||
MulticopterRateControl *instance = new MulticopterRateControl();
|
||||
bool vtol = false;
|
||||
|
||||
if (argc > 1) {
|
||||
if (strcmp(argv[1], "vtol") == 0) {
|
||||
vtol = true;
|
||||
}
|
||||
}
|
||||
|
||||
MulticopterRateControl *instance = new MulticopterRateControl(vtol);
|
||||
|
||||
if (instance) {
|
||||
_object.store(instance);
|
||||
|
|
@ -394,19 +386,15 @@ The controller has a PID loop for angular rate error.
|
|||
|
||||
)DESCR_STR");
|
||||
|
||||
PRINT_MODULE_USAGE_NAME(MODULE_NAME, "controller");
|
||||
PRINT_MODULE_USAGE_NAME("mc_rate_control", "controller");
|
||||
PRINT_MODULE_USAGE_COMMAND("start");
|
||||
PRINT_MODULE_USAGE_ARG("vtol", "VTOL mode", true);
|
||||
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Multicopter rate control app start / stop handling function
|
||||
*/
|
||||
extern "C" __EXPORT int mc_rate_control_main(int argc, char *argv[]);
|
||||
|
||||
int mc_rate_control_main(int argc, char *argv[])
|
||||
extern "C" __EXPORT int mc_rate_control_main(int argc, char *argv[])
|
||||
{
|
||||
return MulticopterRateControl::main(argc, argv);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -63,9 +63,8 @@
|
|||
class MulticopterRateControl : public ModuleBase<MulticopterRateControl>, public ModuleParams, public px4::WorkItem
|
||||
{
|
||||
public:
|
||||
MulticopterRateControl();
|
||||
|
||||
virtual ~MulticopterRateControl();
|
||||
MulticopterRateControl(bool vtol = false);
|
||||
~MulticopterRateControl() override;
|
||||
|
||||
/** @see ModuleBase */
|
||||
static int task_spawn(int argc, char *argv[]);
|
||||
|
|
@ -90,8 +89,6 @@ private:
|
|||
*/
|
||||
void parameters_updated();
|
||||
|
||||
void vehicle_status_poll();
|
||||
|
||||
/**
|
||||
* Get the landing gear state based on the manual control switch position
|
||||
* @return vehicle_attitude_setpoint_s::LANDING_GEAR_UP or vehicle_attitude_setpoint_s::LANDING_GEAR_DOWN
|
||||
|
|
@ -112,13 +109,11 @@ private:
|
|||
|
||||
uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)};
|
||||
|
||||
uORB::Publication<actuator_controls_s> _actuators_0_pub;
|
||||
uORB::PublicationMulti<rate_ctrl_status_s> _controller_status_pub{ORB_ID(rate_ctrl_status), ORB_PRIO_DEFAULT}; /**< controller status publication */
|
||||
uORB::Publication<landing_gear_s> _landing_gear_pub{ORB_ID(landing_gear)};
|
||||
uORB::Publication<vehicle_rates_setpoint_s> _v_rates_sp_pub{ORB_ID(vehicle_rates_setpoint)}; /**< rate setpoint publication */
|
||||
|
||||
orb_advert_t _actuators_0_pub{nullptr}; /**< attitude actuator controls publication */
|
||||
orb_id_t _actuators_id{nullptr}; /**< pointer to correct actuator controls0 uORB metadata structure */
|
||||
|
||||
landing_gear_s _landing_gear{};
|
||||
manual_control_setpoint_s _manual_control_sp{};
|
||||
vehicle_control_mode_s _v_control_mode{};
|
||||
|
|
|
|||
Loading…
Reference in New Issue