diff --git a/Tools/uorb_graph/create.py b/Tools/uorb_graph/create.py
index d5fb89b52a8f26ad291600bc6d6952fb675618f0..7f4dd2c67abd2f82c28b3f90e751b0ac830b62f5 100755
--- a/Tools/uorb_graph/create.py
+++ b/Tools/uorb_graph/create.py
@@ -246,10 +246,8 @@ class Graph:
('mc_pos_control', r'mc_pos_control_main\.cpp$', r'\b_attitude_setpoint_id=([^,)]+)', r'^_attitude_setpoint_id$'),
- ('mc_att_control', r'mc_att_control_main\.cpp$', r'\b_rates_sp_id=([^,)]+)', r'^_rates_sp_id$'),
('mc_att_control', r'mc_att_control_main\.cpp$', r'\b_actuators_id=([^,)]+)', r'^_actuators_id$'),
- ('fw_att_control', r'FixedwingAttitudeControl\.cpp$', r'\b_rates_sp_id=([^,)]+)', r'^_rates_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$'),
diff --git a/msg/vehicle_attitude_setpoint.msg b/msg/vehicle_attitude_setpoint.msg
index da0b01b97f24a41aa4471849f7585cf7cd68eef9..9b5c5db015946a82c9d5abe1cf8859e2d1439d43 100644
--- a/msg/vehicle_attitude_setpoint.msg
+++ b/msg/vehicle_attitude_setpoint.msg
@@ -12,7 +12,9 @@ float32 yaw_sp_move_rate # rad/s (commanded by user)
float32[4] q_d # Desired quaternion for quaternion control
bool q_d_valid # Set to true if quaternion vector is valid
-float32 thrust # Thrust in Newton the power system should generate
+# For clarification: For multicopters thrust_body[0] and thrust[1] are usually 0 and thrust[2] is the negative throttle demand.
+# For fixed wings thrust_x is the throttle demand and thrust_y, thrust_z will usually be zero.
+float32[3] thrust_body # Normalized thrust command in body NED frame [-1,1]
bool roll_reset_integral # Reset roll integral part (navigation logic change)
bool pitch_reset_integral # Reset pitch integral part (navigation logic change)
diff --git a/msg/vehicle_rates_setpoint.msg b/msg/vehicle_rates_setpoint.msg
index 14753f4be8c856c21efe3316e7c172b3b4bde97e..ae7a3c78223d2aa99d395b0cd9e09240273c660f 100644
--- a/msg/vehicle_rates_setpoint.msg
+++ b/msg/vehicle_rates_setpoint.msg
@@ -3,6 +3,7 @@ uint64 timestamp # time since system start (microseconds)
float32 roll # body angular rates in NED frame
float32 pitch # body angular rates in NED frame
float32 yaw # body angular rates in NED frame
-float32 thrust # thrust normalized to 0..1
-# TOPICS vehicle_rates_setpoint mc_virtual_rates_setpoint fw_virtual_rates_setpoint
+# For clarification: For multicopters thrust_body[0] and thrust[1] are usually 0 and thrust[2] is the negative throttle demand.
+# For fixed wings thrust_x is the throttle demand and thrust_y, thrust_z will usually be zero.
+float32[3] thrust_body # Normalized thrust command in body NED frame [-1,1]
diff --git a/src/examples/rover_steering_control/main.cpp b/src/examples/rover_steering_control/main.cpp
index d72f1cd9cb2df9a86016ada7e17b13744f965a94..723a371b0514aaf20695192963173e644e6a6a15 100644
--- a/src/examples/rover_steering_control/main.cpp
+++ b/src/examples/rover_steering_control/main.cpp
@@ -182,7 +182,7 @@ void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const st
actuators->control[2] = yaw_err * pp.yaw_p;
/* copy throttle */
- actuators->control[3] = att_sp->thrust;
+ actuators->control[3] = att_sp->thrust_body[0];
actuators->timestamp = hrt_absolute_time();
}
diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp
index 2511eef767c620a188e525c908058379e5cdb009..acc3e5bb9f15d269f9e0bf29e770516672995596 100644
--- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp
+++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp
@@ -48,6 +48,9 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_nonfinite_input_perf(perf_alloc(PC_COUNT, "fwa_nani")),
_nonfinite_output_perf(perf_alloc(PC_COUNT, "fwa_nano"))
{
+ // check if VTOL first
+ vehicle_status_poll();
+
_parameter_handles.p_tc = param_find("FW_P_TC");
_parameter_handles.p_p = param_find("FW_PR_P");
_parameter_handles.p_i = param_find("FW_PR_I");
@@ -116,13 +119,43 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
// initialize to invalid VTOL type
_parameters.vtol_type = -1;
+ _parameters.vtol_airspeed_rule = 0;
/* fetch initial parameter values */
parameters_update();
+
+ // set initial maximum body rate setpoints
+ _roll_ctrl.set_max_rate(_parameters.acro_max_x_rate_rad);
+ _pitch_ctrl.set_max_rate_pos(_parameters.acro_max_y_rate_rad);
+ _pitch_ctrl.set_max_rate_neg(_parameters.acro_max_y_rate_rad);
+ _yaw_ctrl.set_max_rate(_parameters.acro_max_z_rate_rad);
+
+ // subscriptions
+ _att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
+ _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
+ _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
+ _params_sub = orb_subscribe(ORB_ID(parameter_update));
+ _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
+ _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
+ _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
+ _vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
+ _battery_status_sub = orb_subscribe(ORB_ID(battery_status));
+ _rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
}
FixedwingAttitudeControl::~FixedwingAttitudeControl()
{
+ orb_unsubscribe(_att_sub);
+ orb_unsubscribe(_att_sp_sub);
+ orb_unsubscribe(_vcontrol_mode_sub);
+ orb_unsubscribe(_params_sub);
+ orb_unsubscribe(_manual_sub);
+ orb_unsubscribe(_global_pos_sub);
+ orb_unsubscribe(_vehicle_status_sub);
+ orb_unsubscribe(_vehicle_land_detected_sub);
+ orb_unsubscribe(_battery_status_sub);
+ orb_unsubscribe(_rates_sp_sub);
+
perf_free(_loop_perf);
perf_free(_nonfinite_input_perf);
perf_free(_nonfinite_output_perf);
@@ -131,7 +164,6 @@ FixedwingAttitudeControl::~FixedwingAttitudeControl()
int
FixedwingAttitudeControl::parameters_update()
{
-
int32_t tmp = 0;
param_get(_parameter_handles.p_tc, &(_parameters.p_tc));
param_get(_parameter_handles.p_p, &(_parameters.p_p));
@@ -209,6 +241,7 @@ FixedwingAttitudeControl::parameters_update()
if (_vehicle_status.is_vtol) {
param_get(_parameter_handles.vtol_type, &_parameters.vtol_type);
+ param_get(_parameter_handles.vtol_airspeed_rule, &_parameters.vtol_airspeed_rule);
}
param_get(_parameter_handles.bat_scale_en, &_parameters.bat_scale_en);
@@ -249,13 +282,12 @@ FixedwingAttitudeControl::parameters_update()
void
FixedwingAttitudeControl::vehicle_control_mode_poll()
{
- bool vcontrol_mode_updated;
+ bool updated = false;
/* Check if vehicle control mode has changed */
- orb_check(_vcontrol_mode_sub, &vcontrol_mode_updated);
-
- if (vcontrol_mode_updated) {
+ orb_check(_vcontrol_mode_sub, &updated);
+ if (updated) {
orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &_vcontrol_mode);
}
}
@@ -263,8 +295,8 @@ FixedwingAttitudeControl::vehicle_control_mode_poll()
void
FixedwingAttitudeControl::vehicle_manual_poll()
{
- // only update manual if in a manual mode
- if (_vcontrol_mode.flag_control_manual_enabled) {
+
+ if (_vcontrol_mode.flag_control_manual_enabled && !_vehicle_status.is_rotary_wing) {
// Always copy the new manual setpoint, even if it wasn't updated, to fill the _actuators with valid values
if (orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &_manual) == PX4_OK) {
@@ -287,7 +319,7 @@ FixedwingAttitudeControl::vehicle_manual_poll()
_att_sp.pitch_body = -_manual.x * _parameters.man_pitch_max + _parameters.pitchsp_offset_rad;
_att_sp.pitch_body = math::constrain(_att_sp.pitch_body, -_parameters.man_pitch_max, _parameters.man_pitch_max);
_att_sp.yaw_body = 0.0f;
- _att_sp.thrust = _manual.z;
+ _att_sp.thrust_body[0] = _manual.z;
Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body));
q.copyTo(_att_sp.q_d);
@@ -310,15 +342,15 @@ FixedwingAttitudeControl::vehicle_manual_poll()
_rates_sp.roll = _manual.y * _parameters.acro_max_x_rate_rad;
_rates_sp.pitch = -_manual.x * _parameters.acro_max_y_rate_rad;
_rates_sp.yaw = _manual.r * _parameters.acro_max_z_rate_rad;
- _rates_sp.thrust = _manual.z;
+ _rates_sp.thrust_body[0] = _manual.z;
if (_rate_sp_pub != nullptr) {
/* publish the attitude rates setpoint */
- orb_publish(_rates_sp_id, _rate_sp_pub, &_rates_sp);
+ orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &_rates_sp);
- } else if (_rates_sp_id) {
+ } else {
/* advertise the attitude rates setpoint */
- _rate_sp_pub = orb_advertise(_rates_sp_id, &_rates_sp);
+ _rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_rates_sp);
}
} else {
@@ -335,14 +367,36 @@ FixedwingAttitudeControl::vehicle_manual_poll()
}
void
-FixedwingAttitudeControl::vehicle_setpoint_poll()
+FixedwingAttitudeControl::vehicle_attitude_setpoint_poll()
{
/* check if there is a new setpoint */
- bool att_sp_updated;
- orb_check(_att_sp_sub, &att_sp_updated);
+ bool updated = false;
+ orb_check(_att_sp_sub, &updated);
+
+ if (updated) {
+ if (orb_copy(ORB_ID(vehicle_attitude_setpoint), _att_sp_sub, &_att_sp) == PX4_OK) {
+ _rates_sp.thrust_body[0] = _att_sp.thrust_body[0];
+ _rates_sp.thrust_body[1] = _att_sp.thrust_body[1];
+ _rates_sp.thrust_body[2] = _att_sp.thrust_body[2];
+ }
+ }
+}
- if (att_sp_updated) {
- orb_copy(ORB_ID(vehicle_attitude_setpoint), _att_sp_sub, &_att_sp);
+void
+FixedwingAttitudeControl::vehicle_rates_setpoint_poll()
+{
+ /* check if there is a new setpoint */
+ bool updated = false;
+ orb_check(_rates_sp_sub, &updated);
+
+ if (updated) {
+ orb_copy(ORB_ID(vehicle_rates_setpoint), _rates_sp_sub, &_rates_sp);
+
+ if (_parameters.vtol_type == vtol_type::TAILSITTER) {
+ float tmp = _rates_sp.roll;
+ _rates_sp.roll = -_rates_sp.yaw;
+ _rates_sp.yaw = tmp;
+ }
}
}
@@ -368,19 +422,28 @@ FixedwingAttitudeControl::vehicle_status_poll()
if (vehicle_status_updated) {
orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
+ // if VTOL and not in fixed wing mode we should only control body-rates which are published
+ // by the multicoper attitude controller. Therefore, modify the control mode to achieve rate
+ // control only
+ if (_vehicle_status.is_vtol) {
+ if (_vehicle_status.is_rotary_wing) {
+ _vcontrol_mode.flag_control_attitude_enabled = false;
+ _vcontrol_mode.flag_control_manual_enabled = false;
+ }
+ }
+
/* set correct uORB ID, depending on if vehicle is VTOL or not */
- if (!_rates_sp_id) {
+ if (!_actuators_id) {
if (_vehicle_status.is_vtol) {
- _rates_sp_id = ORB_ID(fw_virtual_rates_setpoint);
_actuators_id = ORB_ID(actuator_controls_virtual_fw);
_attitude_setpoint_id = ORB_ID(fw_virtual_attitude_setpoint);
_parameter_handles.vtol_type = param_find("VT_TYPE");
+ _parameter_handles.vtol_airspeed_rule = param_find("VT_AIRSPD_RULE");
parameters_update();
} else {
- _rates_sp_id = ORB_ID(vehicle_rates_setpoint);
_actuators_id = ORB_ID(actuator_controls_0);
_attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint);
}
@@ -404,30 +467,44 @@ FixedwingAttitudeControl::vehicle_land_detected_poll()
}
}
-void FixedwingAttitudeControl::run()
+void FixedwingAttitudeControl::get_airspeed_and_scaling(float &airspeed, float &airspeed_scaling)
{
- /*
- * do subscriptions
- */
- _att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
- _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
- _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
- _params_sub = orb_subscribe(ORB_ID(parameter_update));
- _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
- _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
- _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
- _vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
- _battery_status_sub = orb_subscribe(ORB_ID(battery_status));
+ const bool airspeed_valid = PX4_ISFINITE(_airspeed_sub.get().indicated_airspeed_m_s)
+ && (_airspeed_sub.get().indicated_airspeed_m_s > 0.0f)
+ && (hrt_elapsed_time(&_airspeed_sub.get().timestamp) < 1e6);
- parameters_update();
+ if (!_parameters.airspeed_disabled && airspeed_valid) {
+ /* prevent numerical drama by requiring 0.5 m/s minimal speed */
+ airspeed = math::max(0.5f, _airspeed_sub.get().indicated_airspeed_m_s);
- /* get an initial update for all sensor and status data */
- vehicle_setpoint_poll();
- vehicle_control_mode_poll();
- vehicle_manual_poll();
- vehicle_status_poll();
- vehicle_land_detected_poll();
+ } else {
+ // if no airspeed measurement is available out best guess is to use the trim airspeed
+ airspeed = _parameters.airspeed_trim;
+
+ // VTOL: if we have no airspeed available and we are in hover mode then assume the lowest airspeed possible
+ // this assumption is good as long as the vehicle is not hovering in a headwind which is much larger
+ // than the minimum airspeed
+ if (_vehicle_status.is_vtol && _vehicle_status.is_rotary_wing && !_vehicle_status.in_transition_mode) {
+ airspeed = _parameters.airspeed_min;
+ }
+ if (!airspeed_valid) {
+ perf_count(_nonfinite_input_perf);
+ }
+ }
+
+ /*
+ * For scaling our actuators using anything less than the min (close to stall)
+ * speed doesn't make any sense - its the strongest reasonable deflection we
+ * want to do in flight and its the baseline a human pilot would choose.
+ *
+ * Forcing the scaling to this value allows reasonable handheld tests.
+ */
+ airspeed_scaling = _parameters.airspeed_trim / math::max(airspeed, _parameters.airspeed_min);
+}
+
+void FixedwingAttitudeControl::run()
+{
/* wakeup source */
px4_pollfd_struct_t fds[1];
@@ -529,7 +606,7 @@ void FixedwingAttitudeControl::run()
matrix::Eulerf euler_angles(R);
_airspeed_sub.update();
- vehicle_setpoint_poll();
+ vehicle_attitude_setpoint_poll();
vehicle_control_mode_poll();
vehicle_manual_poll();
global_pos_poll();
@@ -560,35 +637,10 @@ void FixedwingAttitudeControl::run()
/* decide if in stabilized or full manual control */
if (_vcontrol_mode.flag_control_rates_enabled) {
- /* scale around tuning airspeed */
- float airspeed;
-
- /* if airspeed is non-finite or not valid or if we are asked not to control it, we assume the normal average speed */
- const bool airspeed_valid = PX4_ISFINITE(_airspeed_sub.get().indicated_airspeed_m_s)
- && (_airspeed_sub.get().indicated_airspeed_m_s > 0.0f)
- && (hrt_elapsed_time(&_airspeed_sub.get().timestamp) < 1e6);
-
- if (!_parameters.airspeed_disabled && airspeed_valid) {
- /* prevent numerical drama by requiring 0.5 m/s minimal speed */
- airspeed = math::max(0.5f, _airspeed_sub.get().indicated_airspeed_m_s);
-
- } else {
- airspeed = _parameters.airspeed_trim;
-
- if (!airspeed_valid) {
- perf_count(_nonfinite_input_perf);
- }
- }
- /*
- * For scaling our actuators using anything less than the min (close to stall)
- * speed doesn't make any sense - its the strongest reasonable deflection we
- * want to do in flight and its the baseline a human pilot would choose.
- *
- * Forcing the scaling to this value allows reasonable handheld tests.
- */
- float airspeed_scaling = _parameters.airspeed_trim / ((airspeed < _parameters.airspeed_min) ? _parameters.airspeed_min :
- airspeed);
+ float airspeed;
+ float airspeed_scaling;
+ get_airspeed_and_scaling(airspeed, airspeed_scaling);
/* Use min airspeed to calculate ground speed scaling region.
* Don't scale below gspd_scaling_trim
@@ -624,10 +676,6 @@ void FixedwingAttitudeControl::run()
_wheel_ctrl.reset_integrator();
}
- float roll_sp = _att_sp.roll_body;
- float pitch_sp = _att_sp.pitch_body;
- float yaw_sp = _att_sp.yaw_body;
-
/* Prepare data for attitude controllers */
struct ECL_ControlData control_input = {};
control_input.roll = euler_angles.phi();
@@ -636,9 +684,9 @@ void FixedwingAttitudeControl::run()
control_input.body_x_rate = _att.rollspeed;
control_input.body_y_rate = _att.pitchspeed;
control_input.body_z_rate = _att.yawspeed;
- control_input.roll_setpoint = roll_sp;
- control_input.pitch_setpoint = pitch_sp;
- control_input.yaw_setpoint = yaw_sp;
+ control_input.roll_setpoint = _att_sp.roll_body;
+ control_input.pitch_setpoint = _att_sp.pitch_body;
+ control_input.yaw_setpoint = _att_sp.yaw_body;
control_input.airspeed_min = _parameters.airspeed_min;
control_input.airspeed_max = _parameters.airspeed_max;
control_input.airspeed = airspeed;
@@ -649,7 +697,7 @@ void FixedwingAttitudeControl::run()
/* reset body angular rate limits on mode change */
if ((_vcontrol_mode.flag_control_attitude_enabled != _flag_control_attitude_enabled_last) || params_updated) {
- if (_vcontrol_mode.flag_control_attitude_enabled) {
+ if (_vcontrol_mode.flag_control_attitude_enabled || _vehicle_status.is_rotary_wing) {
_roll_ctrl.set_max_rate(math::radians(_parameters.r_rmax));
_pitch_ctrl.set_max_rate_pos(math::radians(_parameters.p_rmax_pos));
_pitch_ctrl.set_max_rate_neg(math::radians(_parameters.p_rmax_neg));
@@ -693,7 +741,7 @@ void FixedwingAttitudeControl::run()
/* Run attitude controllers */
if (_vcontrol_mode.flag_control_attitude_enabled) {
- if (PX4_ISFINITE(roll_sp) && PX4_ISFINITE(pitch_sp)) {
+ if (PX4_ISFINITE(_att_sp.roll_body) && PX4_ISFINITE(_att_sp.pitch_body)) {
_roll_ctrl.control_attitude(control_input);
_pitch_ctrl.control_attitude(control_input);
_yaw_ctrl.control_attitude(control_input); //runs last, because is depending on output of roll and pitch attitude
@@ -744,8 +792,8 @@ void FixedwingAttitudeControl::run()
}
/* throttle passed through if it is finite and if no engine failure was detected */
- _actuators.control[actuator_controls_s::INDEX_THROTTLE] = (PX4_ISFINITE(_att_sp.thrust)
- && !_vehicle_status.engine_failure) ? _att_sp.thrust : 0.0f;
+ _actuators.control[actuator_controls_s::INDEX_THROTTLE] = (PX4_ISFINITE(_att_sp.thrust_body[0])
+ && !_vehicle_status.engine_failure) ? _att_sp.thrust_body[0] : 0.0f;
/* scale effort by battery status */
if (_parameters.bat_scale_en &&
@@ -783,18 +831,19 @@ void FixedwingAttitudeControl::run()
if (_rate_sp_pub != nullptr) {
/* publish the attitude rates setpoint */
- orb_publish(_rates_sp_id, _rate_sp_pub, &_rates_sp);
+ orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &_rates_sp);
- } else if (_rates_sp_id) {
+ } else {
/* advertise the attitude rates setpoint */
- _rate_sp_pub = orb_advertise(_rates_sp_id, &_rates_sp);
+ _rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_rates_sp);
}
} else {
- // pure rate control
+ vehicle_rates_setpoint_poll();
+
_roll_ctrl.set_bodyrate_setpoint(_rates_sp.roll);
- _pitch_ctrl.set_bodyrate_setpoint(_rates_sp.pitch);
_yaw_ctrl.set_bodyrate_setpoint(_rates_sp.yaw);
+ _pitch_ctrl.set_bodyrate_setpoint(_rates_sp.pitch);
float roll_u = _roll_ctrl.control_bodyrate(control_input);
_actuators.control[actuator_controls_s::INDEX_ROLL] = (PX4_ISFINITE(roll_u)) ? roll_u + trim_roll : trim_roll;
@@ -805,7 +854,8 @@ void FixedwingAttitudeControl::run()
float yaw_u = _yaw_ctrl.control_bodyrate(control_input);
_actuators.control[actuator_controls_s::INDEX_YAW] = (PX4_ISFINITE(yaw_u)) ? yaw_u + trim_yaw : trim_yaw;
- _actuators.control[actuator_controls_s::INDEX_THROTTLE] = PX4_ISFINITE(_rates_sp.thrust) ? _rates_sp.thrust : 0.0f;
+ _actuators.control[actuator_controls_s::INDEX_THROTTLE] = PX4_ISFINITE(_rates_sp.thrust_body[0]) ?
+ _rates_sp.thrust_body[0] : 0.0f;
}
rate_ctrl_status_s rate_ctrl_status;
diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp
index b4d7e14e28bc8757f334db6dddbf3414449ffa92..d9c414f78db5bc45085d7ee95a6ffd30bac62614 100644
--- a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp
+++ b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp
@@ -95,6 +95,7 @@ private:
int _att_sub{-1}; /**< vehicle attitude */
int _att_sp_sub{-1}; /**< vehicle attitude setpoint */
+ int _rates_sp_sub{-1}; /**< vehicle rates setpoint */
int _battery_status_sub{-1}; /**< battery status subscription */
int _global_pos_sub{-1}; /**< global position subscription */
int _manual_sub{-1}; /**< notification of manual control updates */
@@ -109,7 +110,6 @@ private:
orb_advert_t _actuators_2_pub{nullptr}; /**< actuator control group 1 setpoint (Airframe) */
orb_advert_t _rate_ctrl_status_pub{nullptr}; /**< rate controller status publication */
- orb_id_t _rates_sp_id{nullptr}; // pointer to correct rates setpoint uORB metadata structure
orb_id_t _actuators_id{nullptr}; // pointer to correct actuator controls0 uORB metadata structure
orb_id_t _attitude_setpoint_id{nullptr};
@@ -185,7 +185,7 @@ private:
float pitchsp_offset_deg; /**< Pitch Setpoint Offset in deg */
float rollsp_offset_rad; /**< Roll Setpoint Offset in rad */
float pitchsp_offset_rad; /**< Pitch Setpoint Offset in rad */
- float man_roll_max; /**< Max Roll in rad */
+ float man_roll_max; /**< Max Roll in rad */
float man_pitch_max; /**< Max Pitch in rad */
float man_roll_scale; /**< scale factor applied to roll actuator control in pure manual mode */
float man_pitch_scale; /**< scale factor applied to pitch actuator control in pure manual mode */
@@ -195,13 +195,14 @@ private:
float acro_max_y_rate_rad;
float acro_max_z_rate_rad;
- float flaps_scale; /**< Scale factor for flaps */
- float flaps_takeoff_scale; /**< Scale factor for flaps on take-off */
+ float flaps_scale; /**< Scale factor for flaps */
+ float flaps_takeoff_scale; /**< Scale factor for flaps on take-off */
float flaperon_scale; /**< Scale factor for flaperons */
float rattitude_thres;
- int32_t vtol_type; /**< VTOL type: 0 = tailsitter, 1 = tiltrotor */
+ int32_t vtol_type; /**< VTOL type: 0 = tailsitter, 1 = tiltrotor */
+ int32_t vtol_airspeed_rule;
int32_t bat_scale_en; /**< Battery scaling enabled */
bool airspeed_disabled;
@@ -271,6 +272,7 @@ private:
param_t rattitude_thres;
param_t vtol_type;
+ param_t vtol_airspeed_rule;
param_t bat_scale_en;
param_t airspeed_mode;
@@ -291,9 +293,10 @@ private:
void vehicle_control_mode_poll();
void vehicle_manual_poll();
- void vehicle_setpoint_poll();
+ void vehicle_attitude_setpoint_poll();
+ void vehicle_rates_setpoint_poll();
void global_pos_poll();
void vehicle_status_poll();
void vehicle_land_detected_poll();
-
+ void get_airspeed_and_scaling(float &airspeed, float &airspeed_scaling);
};
diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
index 45314e9397b0af7b50015d5a71f5a5272a0facc1..23a0ee172fcd8e18150728ca564fcb964c0ed520 100644
--- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
@@ -916,7 +916,7 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto
}
if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
- _att_sp.thrust = 0.0f;
+ _att_sp.thrust_body[0] = 0.0f;
_att_sp.roll_body = 0.0f;
_att_sp.pitch_body = 0.0f;
@@ -1185,30 +1185,30 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto
/* making sure again that the correct thrust is used,
* without depending on library calls for safety reasons.
the pre-takeoff throttle and the idle throttle normally map to the same parameter. */
- _att_sp.thrust = _parameters.throttle_idle;
+ _att_sp.thrust_body[0] = _parameters.throttle_idle;
} else if (_control_mode_current == FW_POSCTRL_MODE_AUTO &&
pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF &&
_runway_takeoff.runwayTakeoffEnabled()) {
- _att_sp.thrust = _runway_takeoff.getThrottle(min(get_tecs_thrust(), throttle_max));
+ _att_sp.thrust_body[0] = _runway_takeoff.getThrottle(min(get_tecs_thrust(), throttle_max));
} else if (_control_mode_current == FW_POSCTRL_MODE_AUTO &&
pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
- _att_sp.thrust = 0.0f;
+ _att_sp.thrust_body[0] = 0.0f;
} else if (_control_mode_current == FW_POSCTRL_MODE_OTHER) {
- _att_sp.thrust = min(_att_sp.thrust, _parameters.throttle_max);
+ _att_sp.thrust_body[0] = min(_att_sp.thrust_body[0], _parameters.throttle_max);
} else {
/* Copy thrust and pitch values from tecs */
if (_vehicle_land_detected.landed) {
// when we are landed state we want the motor to spin at idle speed
- _att_sp.thrust = min(_parameters.throttle_idle, throttle_max);
+ _att_sp.thrust_body[0] = min(_parameters.throttle_idle, throttle_max);
} else {
- _att_sp.thrust = min(get_tecs_thrust(), throttle_max);
+ _att_sp.thrust_body[0] = min(get_tecs_thrust(), throttle_max);
}
}
diff --git a/src/modules/gnd_att_control/GroundRoverAttitudeControl.cpp b/src/modules/gnd_att_control/GroundRoverAttitudeControl.cpp
index 57870524277ceabd2ef797a8775783c8837f5a53..fe2f12b0e70b96dc16ce524633055111e58c2763 100644
--- a/src/modules/gnd_att_control/GroundRoverAttitudeControl.cpp
+++ b/src/modules/gnd_att_control/GroundRoverAttitudeControl.cpp
@@ -303,7 +303,7 @@ GroundRoverAttitudeControl::task_main()
}
/* throttle passed through if it is finite and if no engine failure was detected */
- _actuators.control[actuator_controls_s::INDEX_THROTTLE] = _att_sp.thrust;
+ _actuators.control[actuator_controls_s::INDEX_THROTTLE] = _att_sp.thrust_body[0];
/* scale effort by battery status */
if (_parameters.bat_scale_en && _battery_status.scale > 0.0f &&
diff --git a/src/modules/gnd_pos_control/GroundRoverPositionControl.cpp b/src/modules/gnd_pos_control/GroundRoverPositionControl.cpp
index 8593b807a02005c947e8a49173b2522204b4fc5c..4960b788e95ddbdbf75d4d0f7fd7e754a1c30a69 100644
--- a/src/modules/gnd_pos_control/GroundRoverPositionControl.cpp
+++ b/src/modules/gnd_pos_control/GroundRoverPositionControl.cpp
@@ -261,7 +261,7 @@ GroundRoverPositionControl::control_position(const matrix::Vector2f ¤t_pos
_att_sp.roll_body = 0.0f;
_att_sp.pitch_body = 0.0f;
_att_sp.yaw_body = 0.0f;
- _att_sp.thrust = 0.0f;
+ _att_sp.thrust_body[0] = 0.0f;
} else if ((pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION)
|| (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF)) {
@@ -272,7 +272,7 @@ GroundRoverPositionControl::control_position(const matrix::Vector2f ¤t_pos
_att_sp.pitch_body = 0.0f;
_att_sp.yaw_body = _gnd_control.nav_bearing();
_att_sp.fw_control_yaw = true;
- _att_sp.thrust = mission_throttle;
+ _att_sp.thrust_body[0] = mission_throttle;
} else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
@@ -284,7 +284,7 @@ GroundRoverPositionControl::control_position(const matrix::Vector2f ¤t_pos
_att_sp.pitch_body = 0.0f;
_att_sp.yaw_body = _gnd_control.nav_bearing();
_att_sp.fw_control_yaw = true;
- _att_sp.thrust = 0.0f;
+ _att_sp.thrust_body[0] = 0.0f;
}
if (was_circle_mode && !_gnd_control.circle_mode()) {
@@ -299,7 +299,7 @@ GroundRoverPositionControl::control_position(const matrix::Vector2f ¤t_pos
_att_sp.pitch_body = 0.0f;
_att_sp.yaw_body = 0.0f;
_att_sp.fw_control_yaw = true;
- _att_sp.thrust = 0.0f;
+ _att_sp.thrust_body[0] = 0.0f;
/* do not publish the setpoint */
setpoint = false;
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 7817e884c80bd48486c81871dd4f66181daeb4c6..5ae3b873c10d7b06d1b96b2865bb1c4a58adefd8 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -3138,7 +3138,7 @@ protected:
msg.body_pitch_rate = att_rates_sp.pitch;
msg.body_yaw_rate = att_rates_sp.yaw;
- msg.thrust = att_sp.thrust;
+ msg.thrust = att_sp.thrust_body[0];
mavlink_msg_attitude_target_send_struct(_mavlink->get_channel(), &msg);
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index b062aa9f08a9c5b5ecbb484f625663d6f1cb2926..c7875f37c19474b1938efeff7fbb71370569adae 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -1447,8 +1447,11 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
att_sp.yaw_sp_move_rate = 0.0f;
}
+ // TODO: We assume offboard is only used for multicopters which produce thrust along the
+ // body z axis. If we want to support fixed wing as well we need to handle it differently here, e.g.
+ // in that case we should assign att_sp.thrust_body[0]
if (!_offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid
- att_sp.thrust = set_attitude_target.thrust;
+ att_sp.thrust_body[2] = -set_attitude_target.thrust;
}
if (_att_sp_pub == nullptr) {
@@ -1472,7 +1475,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
}
if (!_offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid
- rates_sp.thrust = set_attitude_target.thrust;
+ rates_sp.thrust_body[2] = -set_attitude_target.thrust;
}
if (_rates_sp_pub == nullptr) {
diff --git a/src/modules/mc_att_control/mc_att_control.hpp b/src/modules/mc_att_control/mc_att_control.hpp
index eb95da8d183f96ebff5f681f474a1448ad40c04a..c3dad3c7ab562c3f542b08d46b13ed6dcfb0905d 100644
--- a/src/modules/mc_att_control/mc_att_control.hpp
+++ b/src/modules/mc_att_control/mc_att_control.hpp
@@ -166,7 +166,6 @@ private:
orb_advert_t _controller_status_pub{nullptr}; /**< controller status publication */
orb_advert_t _vehicle_attitude_setpoint_pub{nullptr};
- orb_id_t _rates_sp_id{nullptr}; /**< pointer to correct rates setpoint uORB metadata structure */
orb_id_t _actuators_id{nullptr}; /**< pointer to correct actuator controls0 uORB metadata structure */
bool _actuators_0_circuit_breaker_enabled{false}; /**< circuit breaker to suppress output */
@@ -196,8 +195,9 @@ private:
matrix::Vector3f _rates_prev_filtered; /**< angular rates on previous step (low-pass filtered) */
matrix::Vector3f _rates_sp; /**< angular rates setpoint */
matrix::Vector3f _rates_int; /**< angular rates integral error */
- float _thrust_sp; /**< thrust setpoint */
+
matrix::Vector3f _att_control; /**< attitude control vector */
+ float _thrust_sp{0.0f}; /**< thrust setpoint */
matrix::Dcmf _board_rotation; /**< rotation matrix for the orientation that the board is mounted */
diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
index c4858a5db364cc56871d8343ddb2563438b34c04..020096ac63c1e3321f355cf638e9f93f8c00945f 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -273,13 +273,11 @@ MulticopterAttitudeControl::vehicle_status_poll()
orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
/* set correct uORB ID, depending on if vehicle is VTOL or not */
- if (_rates_sp_id == nullptr) {
+ if (_actuators_id == nullptr) {
if (_vehicle_status.is_vtol) {
- _rates_sp_id = ORB_ID(mc_virtual_rates_setpoint);
_actuators_id = ORB_ID(actuator_controls_virtual_mc);
} else {
- _rates_sp_id = ORB_ID(vehicle_rates_setpoint);
_actuators_id = ORB_ID(actuator_controls_0);
}
}
@@ -508,7 +506,7 @@ MulticopterAttitudeControl::generate_attitude_setpoint(float dt, bool reset_yaw_
q_sp.copyTo(attitude_setpoint.q_d);
attitude_setpoint.q_d_valid = true;
- attitude_setpoint.thrust = throttle_curve(_manual_control_sp.z);
+ attitude_setpoint.thrust_body[2] = -throttle_curve(_manual_control_sp.z);
attitude_setpoint.landing_gear = get_landing_gear_state();
attitude_setpoint.timestamp = hrt_absolute_time();
@@ -524,7 +522,9 @@ void
MulticopterAttitudeControl::control_attitude()
{
vehicle_attitude_setpoint_poll();
- _thrust_sp = _v_att_sp.thrust;
+
+ // physical thrust axis is the negative of body z axis
+ _thrust_sp = -_v_att_sp.thrust_body[2];
/* prepare yaw weight from the ratio between roll/pitch and yaw gains */
Vector3f attitude_gain = _attitude_p;
@@ -607,7 +607,7 @@ Vector3f
MulticopterAttitudeControl::pid_attenuations(float tpa_breakpoint, float tpa_rate)
{
/* throttle pid attenuation factor */
- float tpa = 1.0f - tpa_rate * (fabsf(_v_rates_sp.thrust) - tpa_breakpoint) / (1.0f - tpa_breakpoint);
+ float tpa = 1.0f - tpa_rate * (fabsf(_thrust_sp) - tpa_breakpoint) / (1.0f - tpa_breakpoint);
tpa = fmaxf(TPA_RATE_LOWER_LIMIT, fminf(1.0f, tpa));
Vector3f pidAttenuationPerAxis;
@@ -731,9 +731,11 @@ MulticopterAttitudeControl::publish_rates_setpoint()
_v_rates_sp.roll = _rates_sp(0);
_v_rates_sp.pitch = _rates_sp(1);
_v_rates_sp.yaw = _rates_sp(2);
- _v_rates_sp.thrust = _thrust_sp;
+ _v_rates_sp.thrust_body[0] = 0.0f;
+ _v_rates_sp.thrust_body[1] = 0.0f;
+ _v_rates_sp.thrust_body[2] = -_thrust_sp;
_v_rates_sp.timestamp = hrt_absolute_time();
- orb_publish_auto(_rates_sp_id, &_v_rates_sp_pub, &_v_rates_sp, nullptr, ORB_PRIO_DEFAULT);
+ orb_publish_auto(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp_pub, &_v_rates_sp, nullptr, ORB_PRIO_DEFAULT);
}
void
@@ -886,7 +888,7 @@ MulticopterAttitudeControl::run()
bool attitude_setpoint_generated = false;
- if (_v_control_mode.flag_control_attitude_enabled) {
+ if (_v_control_mode.flag_control_attitude_enabled && _vehicle_status.is_rotary_wing) {
if (attitude_updated) {
// Generate the attitude setpoint from stick inputs if we are in Manual/Stabilized mode
if (_v_control_mode.flag_control_manual_enabled &&
@@ -903,7 +905,7 @@ MulticopterAttitudeControl::run()
} else {
/* attitude controller disabled, poll rates setpoint topic */
- if (_v_control_mode.flag_control_manual_enabled) {
+ if (_v_control_mode.flag_control_manual_enabled && _vehicle_status.is_rotary_wing) {
if (manual_control_updated) {
/* manual rates control - ACRO mode */
Vector3f man_rate_sp(
@@ -921,7 +923,7 @@ MulticopterAttitudeControl::run()
_rates_sp(0) = _v_rates_sp.roll;
_rates_sp(1) = _v_rates_sp.pitch;
_rates_sp(2) = _v_rates_sp.yaw;
- _thrust_sp = _v_rates_sp.thrust;
+ _thrust_sp = -_v_rates_sp.thrust_body[2];
}
}
}
diff --git a/src/modules/mc_pos_control/Utility/ControlMath.cpp b/src/modules/mc_pos_control/Utility/ControlMath.cpp
index f5d28d9dbe289b591fbf9a2472e1bad522269c41..b3cfa8afa1d2a0d4559fc2b1747bcbed29a08cbf 100644
--- a/src/modules/mc_pos_control/Utility/ControlMath.cpp
+++ b/src/modules/mc_pos_control/Utility/ControlMath.cpp
@@ -103,7 +103,7 @@ vehicle_attitude_setpoint_s thrustToAttitude(const Vector3f &thr_sp, const float
Eulerf euler = R_sp;
att_sp.roll_body = euler(0);
att_sp.pitch_body = euler(1);
- att_sp.thrust = thr_sp.length();
+ att_sp.thrust_body[2] = -thr_sp.length();
return att_sp;
}
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index 0b50c5d7b4c2695da56d8d58b0cd56761e4bf719..94e1380cc3e69a8faf3d5d797a0fd9ce54c0be7d 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -827,7 +827,7 @@ MulticopterPositionControl::run()
matrix::Quatf q_sp = matrix::Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body);
q_sp.copyTo(_att_sp.q_d);
_att_sp.q_d_valid = true;
- _att_sp.thrust = 0.0f;
+ _att_sp.thrust_body[2] = 0.0f;
}
}
diff --git a/src/modules/navigator/gpsfailure.cpp b/src/modules/navigator/gpsfailure.cpp
index d51e5433848bebce5ec25a6624cfaea67b0746ad..69b37c793fbfde811cbe08cf6e896cc7cd4c6c4c 100644
--- a/src/modules/navigator/gpsfailure.cpp
+++ b/src/modules/navigator/gpsfailure.cpp
@@ -86,7 +86,7 @@ GpsFailure::on_active()
att_sp.timestamp = hrt_absolute_time();
att_sp.roll_body = math::radians(_param_openlooploiter_roll.get());
att_sp.pitch_body = math::radians(_param_openlooploiter_pitch.get());
- att_sp.thrust = _param_openlooploiter_thrust.get();
+ att_sp.thrust_body[0] = _param_openlooploiter_thrust.get();
Quatf q(Eulerf(att_sp.roll_body, att_sp.pitch_body, 0.0f));
q.copyTo(att_sp.q_d);
diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp
index 506f810f402ba908d23a02791d2ef4d37f4b483e..7f37bc2f69357d2117c1e8e18ca115edfd718235 100644
--- a/src/modules/vtol_att_control/standard.cpp
+++ b/src/modules/vtol_att_control/standard.cpp
@@ -46,13 +46,10 @@
#include <float.h>
-using matrix::wrap_pi;
+using namespace matrix;
Standard::Standard(VtolAttitudeControl *attc) :
- VtolType(attc),
- _pusher_throttle(0.0f),
- _reverse_output(0.0f),
- _airspeed_trans_blend_margin(0.0f)
+ VtolType(attc)
{
_vtol_schedule.flight_mode = MC_MODE;
_vtol_schedule.transition_start = 0;
@@ -156,8 +153,8 @@ void Standard::update_vtol_state()
} else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) {
// transition to MC mode if transition time has passed or forward velocity drops below MPC cruise speed
- const matrix::Dcmf R_to_body(matrix::Quatf(_v_att->q).inversed());
- const matrix::Vector3f vel = R_to_body * matrix::Vector3f(_local_pos->vx, _local_pos->vy, _local_pos->vz);
+ const Dcmf R_to_body(Quatf(_v_att->q).inversed());
+ const Vector3f vel = R_to_body * Vector3f(_local_pos->vx, _local_pos->vy, _local_pos->vz);
float x_vel = vel(0);
@@ -261,7 +258,7 @@ void Standard::update_transition_state()
// ramp up FW_PSP_OFF
_v_att_sp->pitch_body = _params_standard.pitch_setpoint_offset * (1.0f - mc_weight);
- matrix::Quatf q_sp(matrix::Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body));
+ const Quatf q_sp(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body));
q_sp.copyTo(_v_att_sp->q_d);
_v_att_sp->q_d_valid = true;
@@ -277,7 +274,7 @@ void Standard::update_transition_state()
// maintain FW_PSP_OFF
_v_att_sp->pitch_body = _params_standard.pitch_setpoint_offset;
- matrix::Quatf q_sp(matrix::Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body));
+ const Quatf q_sp(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body));
q_sp.copyTo(_v_att_sp->q_d);
_v_att_sp->q_d_valid = true;
@@ -333,18 +330,18 @@ void Standard::update_mc_state()
return;
}
- matrix::Dcmf R(matrix::Quatf(_v_att->q));
- matrix::Dcmf R_sp(matrix::Quatf(_v_att_sp->q_d));
- matrix::Eulerf euler(R);
- matrix::Eulerf euler_sp(R_sp);
+ const Dcmf R(Quatf(_v_att->q));
+ const Dcmf R_sp(Quatf(_v_att_sp->q_d));
+ const Eulerf euler(R);
+ const Eulerf euler_sp(R_sp);
_pusher_throttle = 0.0f;
// direction of desired body z axis represented in earth frame
- matrix::Vector3f body_z_sp(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2));
+ Vector3f body_z_sp(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2));
// rotate desired body z axis into new frame which is rotated in z by the current
// heading of the vehicle. we refer to this as the heading frame.
- matrix::Dcmf R_yaw = matrix::Eulerf(0.0f, 0.0f, -euler(2));
+ Dcmf R_yaw = Eulerf(0.0f, 0.0f, -euler(2));
body_z_sp = R_yaw * body_z_sp;
body_z_sp.normalize();
@@ -365,25 +362,25 @@ void Standard::update_mc_state()
float pitch_new = 0.0f;
// create corrected desired body z axis in heading frame
- matrix::Dcmf R_tmp = matrix::Eulerf(roll_new, pitch_new, 0.0f);
- matrix::Vector3f tilt_new(R_tmp(0, 2), R_tmp(1, 2), R_tmp(2, 2));
+ const Dcmf R_tmp = Eulerf(roll_new, pitch_new, 0.0f);
+ Vector3f tilt_new(R_tmp(0, 2), R_tmp(1, 2), R_tmp(2, 2));
// rotate the vector into a new frame which is rotated in z by the desired heading
// with respect to the earh frame.
const float yaw_error = wrap_pi(euler_sp(2) - euler(2));
- matrix::Dcmf R_yaw_correction = matrix::Eulerf(0.0f, 0.0f, -yaw_error);
+ const Dcmf R_yaw_correction = Eulerf(0.0f, 0.0f, -yaw_error);
tilt_new = R_yaw_correction * tilt_new;
// now extract roll and pitch setpoints
_v_att_sp->pitch_body = atan2f(tilt_new(0), tilt_new(2));
_v_att_sp->roll_body = -asinf(tilt_new(1));
- R_sp = matrix::Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, euler_sp(2));
- matrix::Quatf q_sp(R_sp);
+
+ const Quatf q_sp(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, euler_sp(2)));
q_sp.copyTo(_v_att_sp->q_d);
+ _v_att_sp->q_d_valid = true;
}
_pusher_throttle = _pusher_throttle < 0.0f ? 0.0f : _pusher_throttle;
-
}
void Standard::update_fw_state()
@@ -475,5 +472,5 @@ void
Standard::waiting_on_tecs()
{
// keep thrust from transition
- _v_att_sp->thrust = _pusher_throttle;
+ _v_att_sp->thrust_body[0] = _pusher_throttle;
};
diff --git a/src/modules/vtol_att_control/standard.h b/src/modules/vtol_att_control/standard.h
index 884753fa6f3f8aa7463b7e350ceb5fbfc11525a5..27ada9c8e6f316dad904441232d8d3f7eed80eb3 100644
--- a/src/modules/vtol_att_control/standard.h
+++ b/src/modules/vtol_att_control/standard.h
@@ -55,14 +55,14 @@ class Standard : public VtolType
public:
Standard(VtolAttitudeControl *_att_controller);
- ~Standard() = default;
+ ~Standard() override = default;
- virtual void update_vtol_state();
- virtual void update_transition_state();
- virtual void update_fw_state();
- virtual void update_mc_state();
- virtual void fill_actuator_outputs();
- virtual void waiting_on_tecs();
+ void update_vtol_state() override;
+ void update_transition_state() override;
+ void update_fw_state() override;
+ void update_mc_state() override;
+ void fill_actuator_outputs() override;
+ void waiting_on_tecs() override;
private:
@@ -98,10 +98,10 @@ private:
hrt_abstime transition_start; // at what time did we start a transition (front- or backtransition)
} _vtol_schedule;
- float _pusher_throttle;
- float _reverse_output;
- float _airspeed_trans_blend_margin;
+ float _pusher_throttle{0.0f};
+ float _reverse_output{0.0f};
+ float _airspeed_trans_blend_margin{0.0f};
- virtual void parameters_update();
+ void parameters_update() override;
};
#endif
diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp
index be21121bc6fc013689bec122180e003985228faf..974f1c2826258b9bf0c05ce75600e1fed2007151 100644
--- a/src/modules/vtol_att_control/tailsitter.cpp
+++ b/src/modules/vtol_att_control/tailsitter.cpp
@@ -47,11 +47,10 @@
#define PITCH_TRANSITION_FRONT_P1 -1.1f // pitch angle to switch to TRANSITION_P2
#define PITCH_TRANSITION_BACK -0.25f // pitch angle to switch to MC
+using namespace matrix;
+
Tailsitter::Tailsitter(VtolAttitudeControl *attc) :
- VtolType(attc),
- _thrust_transition_start(0.0f),
- _yaw_transition(0.0f),
- _pitch_transition_start(0.0f)
+ VtolType(attc)
{
_vtol_schedule.flight_mode = MC_MODE;
_vtol_schedule.transition_start = 0;
@@ -63,6 +62,7 @@ Tailsitter::Tailsitter(VtolAttitudeControl *attc) :
_flag_was_in_trans_mode = false;
_params_handles_tailsitter.front_trans_dur_p2 = param_find("VT_TRANS_P2_DUR");
+ _params_handles_tailsitter.fw_pitch_sp_offset = param_find("FW_PSP_OFF");
}
void
@@ -73,18 +73,20 @@ Tailsitter::parameters_update()
/* vtol front transition phase 2 duration */
param_get(_params_handles_tailsitter.front_trans_dur_p2, &v);
_params_tailsitter.front_trans_dur_p2 = v;
+
+ param_get(_params_handles_tailsitter.fw_pitch_sp_offset, &v);
+ _params_tailsitter.fw_pitch_sp_offset = math::radians(v);
}
void Tailsitter::update_vtol_state()
{
-
/* simple logic using a two way switch to perform transitions.
* after flipping the switch the vehicle will start tilting in MC control mode, picking up
* forward speed. After the vehicle has picked up enough and sufficient pitch angle the uav will go into FW mode.
* For the backtransition the pitch is controlled in MC mode again and switches to full MC control reaching the sufficient pitch angle.
*/
- matrix::Eulerf euler = matrix::Quatf(_v_att->q);
+ Eulerf euler = Quatf(_v_att->q);
float pitch = euler.theta();
if (!_attc->is_fixed_wing_requested()) {
@@ -104,9 +106,10 @@ void Tailsitter::update_vtol_state()
break;
case TRANSITION_BACK:
+ float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f;
// check if we have reached pitch angle to switch to MC mode
- if (pitch >= PITCH_TRANSITION_BACK) {
+ if (pitch >= PITCH_TRANSITION_BACK || time_since_trans_start > _params->back_trans_duration) {
_vtol_schedule.flight_mode = MC_MODE;
}
@@ -176,89 +179,94 @@ void Tailsitter::update_transition_state()
float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f;
if (!_flag_was_in_trans_mode) {
- // save desired heading for transition and last thrust value
- _yaw_transition = _v_att_sp->yaw_body;
- //transition should start from current attitude instead of current setpoint
- matrix::Eulerf euler = matrix::Quatf(_v_att->q);
- _pitch_transition_start = euler.theta();
- _thrust_transition_start = _v_att_sp->thrust;
_flag_was_in_trans_mode = true;
- }
- if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1) {
+ if (_vtol_schedule.flight_mode == TRANSITION_BACK) {
+ // calculate rotation axis for transition.
+ _q_trans_start = Quatf(_v_att->q);
+ Vector3f z = -_q_trans_start.dcm_z();
+ _trans_rot_axis = z.cross(Vector3f(0, 0, -1));
+
+ // as heading setpoint we choose the heading given by the direction the vehicle points
+ float yaw_sp = atan2f(z(1), z(0));
- // create time dependant pitch angle set point + 0.2 rad overlap over the switch value
- _v_att_sp->pitch_body = _pitch_transition_start - fabsf(PITCH_TRANSITION_FRONT_P1 - _pitch_transition_start) *
- time_since_trans_start / _params->front_trans_duration;
- _v_att_sp->pitch_body = math::constrain(_v_att_sp->pitch_body, PITCH_TRANSITION_FRONT_P1 - 0.2f,
- _pitch_transition_start);
+ // the intial attitude setpoint for a backtransition is a combination of the current fw pitch setpoint,
+ // the yaw setpoint and zero roll since we want wings level transition
+ _q_trans_start = Eulerf(0.0f, _fw_virtual_att_sp->pitch_body, yaw_sp);
- // disable mc yaw control once the plane has picked up speed
- if (_airspeed->indicated_airspeed_m_s > ARSP_YAW_CTRL_DISABLE) {
- _mc_yaw_weight = 0.0f;
+ // attitude during transitions are controlled by mc attitude control so rotate the desired attitude to the
+ // multirotor frame
+ _q_trans_start = _q_trans_start * Quatf(Eulerf(0, -M_PI_2_F, 0));
- } else {
- _mc_yaw_weight = 1.0f;
+ } else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1) {
+ // initial attitude setpoint for the transition should be with wings level
+ _q_trans_start = Eulerf(0.0f, _mc_virtual_att_sp->pitch_body, _mc_virtual_att_sp->yaw_body);
+ Vector3f x = Dcmf(Quatf(_v_att->q)) * Vector3f(1, 0, 0);
+ _trans_rot_axis = -x.cross(Vector3f(0, 0, -1));
}
- _mc_roll_weight = 1.0f;
- _mc_pitch_weight = 1.0f;
+ _q_trans_sp = _q_trans_start;
+ }
- } else if (_vtol_schedule.flight_mode == TRANSITION_BACK) {
+ // tilt angle (zero if vehicle nose points up (hover))
+ float tilt = acosf(_q_trans_sp(0) * _q_trans_sp(0) - _q_trans_sp(1) * _q_trans_sp(1) - _q_trans_sp(2) * _q_trans_sp(
+ 2) + _q_trans_sp(3) * _q_trans_sp(3));
- if (!flag_idle_mc) {
- flag_idle_mc = set_idle_mc();
- }
+ if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1) {
- // create time dependant pitch angle set point stating at -pi/2 + 0.2 rad overlap over the switch value
- _v_att_sp->pitch_body = M_PI_2_F + _pitch_transition_start + fabsf(PITCH_TRANSITION_BACK + 1.57f) *
- time_since_trans_start / _params->back_trans_duration;
- _v_att_sp->pitch_body = math::constrain(_v_att_sp->pitch_body, -2.0f, PITCH_TRANSITION_BACK + 0.2f);
+ const float trans_pitch_rate = M_PI_2_F / _params->front_trans_duration;
- // keep yaw disabled
- _mc_yaw_weight = 0.0f;
+ if (tilt < M_PI_2_F - _params_tailsitter.fw_pitch_sp_offset) {
+ _q_trans_sp = Quatf(AxisAnglef(_trans_rot_axis,
+ time_since_trans_start * trans_pitch_rate)) * _q_trans_start;
+ }
- // smoothly move control weight to MC
- _mc_roll_weight = _mc_pitch_weight = time_since_trans_start / _params->back_trans_duration;
+ } else if (_vtol_schedule.flight_mode == TRANSITION_BACK) {
- }
+ const float trans_pitch_rate = M_PI_2_F / _params->back_trans_duration;
- if (_v_control_mode->flag_control_climb_rate_enabled) {
- _v_att_sp->thrust = _params->front_trans_throttle;
+ if (!flag_idle_mc) {
+ flag_idle_mc = set_idle_mc();
+ }
- } else {
- _v_att_sp->thrust = _mc_virtual_att_sp->thrust;
+ if (tilt > 0.01f) {
+ _q_trans_sp = Quatf(AxisAnglef(_trans_rot_axis,
+ time_since_trans_start * trans_pitch_rate)) * _q_trans_start;
+ }
}
- _mc_roll_weight = math::constrain(_mc_roll_weight, 0.0f, 1.0f);
- _mc_yaw_weight = math::constrain(_mc_yaw_weight, 0.0f, 1.0f);
- _mc_pitch_weight = math::constrain(_mc_pitch_weight, 0.0f, 1.0f);
+ _v_att_sp->thrust_body[2] = _mc_virtual_att_sp->thrust_body[2];
- // compute desired attitude and thrust setpoint for the transition
+ _mc_roll_weight = 1.0f;
+ _mc_pitch_weight = 1.0f;
+ _mc_yaw_weight = 1.0f;
_v_att_sp->timestamp = hrt_absolute_time();
- _v_att_sp->roll_body = 0.0f;
- _v_att_sp->yaw_body = _yaw_transition;
- matrix::Quatf q_sp = matrix::Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body);
- q_sp.copyTo(_v_att_sp->q_d);
+ const Eulerf euler_sp(_q_trans_sp);
+ _v_att_sp->roll_body = euler_sp.phi();
+ _v_att_sp->pitch_body = euler_sp.theta();
+ _v_att_sp->yaw_body = euler_sp.psi();
+
+ _q_trans_sp.copyTo(_v_att_sp->q_d);
_v_att_sp->q_d_valid = true;
}
void Tailsitter::waiting_on_tecs()
{
// copy the last trust value from the front transition
- _v_att_sp->thrust = _thrust_transition;
-}
-
-void Tailsitter::update_mc_state()
-{
- VtolType::update_mc_state();
+ _v_att_sp->thrust_body[0] = _thrust_transition;
}
void Tailsitter::update_fw_state()
{
VtolType::update_fw_state();
+
+ // allow fw yawrate control via multirotor roll actuation. this is useful for vehicles
+ // which don't have a rudder to coordinate turns
+ if (_params->diff_thrust == 1) {
+ _mc_roll_weight = 1.0f;
+ }
}
/**
@@ -272,67 +280,30 @@ void Tailsitter::fill_actuator_outputs()
_actuators_out_1->timestamp = hrt_absolute_time();
_actuators_out_1->timestamp_sample = _actuators_fw_in->timestamp_sample;
- switch (_vtol_mode) {
- case ROTARY_WING:
- _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL];
- _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] =
- _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH];
- _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW];
- _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] =
- _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE];
-
- if (_params->elevons_mc_lock) {
- _actuators_out_1->control[0] = 0;
- _actuators_out_1->control[1] = 0;
-
- } else {
- // NOTE: There is no mistake in the line below, multicopter yaw axis is controlled by elevon roll actuation!
- _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] =
- _actuators_mc_in->control[actuator_controls_s::INDEX_YAW]; //roll elevon
- _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] =
- _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH]; //pitch elevon
- }
-
- break;
+ _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL]
+ * _mc_roll_weight;
+ _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] =
+ _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight;
+ _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW] *
+ _mc_yaw_weight;
- case FIXED_WING:
- // in fixed wing mode we use engines only for providing thrust, no moments are generated
- _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = 0;
- _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = 0;
- _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = 0;
+ if (_vtol_schedule.flight_mode == FW_MODE) {
_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] =
_actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE];
- _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] =
- -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL]; // roll elevon
- _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] =
- _actuators_fw_in->control[actuator_controls_s::INDEX_PITCH]; // pitch elevon
- _actuators_out_1->control[actuator_controls_s::INDEX_YAW] =
- _actuators_fw_in->control[actuator_controls_s::INDEX_YAW]; // yaw
- _actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] =
- _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; // throttle
- break;
-
- case TRANSITION_TO_FW:
- case TRANSITION_TO_MC:
- // in transition engines are mixed by weight (BACK TRANSITION ONLY)
- _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL]
- * _mc_roll_weight;
- _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] =
- _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight;
- _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW] *
- _mc_yaw_weight;
+ } else {
_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] =
_actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE];
+ }
+
+ if (_params->elevons_mc_lock && _vtol_schedule.flight_mode == MC_MODE) {
+ _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = 0;
+ _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = 0;
- // NOTE: There is no mistake in the line below, multicopter yaw axis is controlled by elevon roll actuation!
- _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL]
- * (1 - _mc_yaw_weight);
+ } else {
+ _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] =
+ -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL];
_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] =
- _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight;
- // **LATER** + (_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim) *(1 - _mc_pitch_weight);
- _actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] =
- _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE];
- break;
+ _actuators_fw_in->control[actuator_controls_s::INDEX_PITCH];
}
}
diff --git a/src/modules/vtol_att_control/tailsitter.h b/src/modules/vtol_att_control/tailsitter.h
index b65a4ef7131d1794e0f65258e84542867e45da02..da5715b2e0c6e131b722dd0f0f6ad956024dd991 100644
--- a/src/modules/vtol_att_control/tailsitter.h
+++ b/src/modules/vtol_att_control/tailsitter.h
@@ -46,30 +46,32 @@
#include <perf/perf_counter.h> /** is it necsacery? **/
#include <parameters/param.h>
#include <drivers/drv_hrt.h>
+#include <matrix/matrix/math.hpp>
class Tailsitter : public VtolType
{
public:
Tailsitter(VtolAttitudeControl *_att_controller);
- ~Tailsitter() = default;
+ ~Tailsitter() override = default;
- virtual void update_vtol_state();
- virtual void update_transition_state();
- virtual void update_mc_state();
- virtual void update_fw_state();
- virtual void fill_actuator_outputs();
- virtual void waiting_on_tecs();
+ void update_vtol_state() override;
+ void update_transition_state() override;
+ void update_fw_state() override;
+ void fill_actuator_outputs() override;
+ void waiting_on_tecs() override;
private:
struct {
float front_trans_dur_p2;
- } _params_tailsitter;
+ float fw_pitch_sp_offset;
+ } _params_tailsitter{};
struct {
param_t front_trans_dur_p2;
- } _params_handles_tailsitter;
+ param_t fw_pitch_sp_offset;
+ } _params_handles_tailsitter{};
enum vtol_mode {
MC_MODE = 0, /**< vtol is in multicopter mode */
@@ -83,14 +85,11 @@ private:
hrt_abstime transition_start; /**< absoulte time at which front transition started */
} _vtol_schedule;
- float _thrust_transition_start; // throttle value when we start the front transition
- float _yaw_transition; // yaw angle in which transition will take place
- float _pitch_transition_start; // pitch angle at the start of transition (tailsitter)
+ matrix::Quatf _q_trans_start;
+ matrix::Quatf _q_trans_sp;
+ matrix::Vector3f _trans_rot_axis;
- /**
- * Update parameters.
- */
- virtual void parameters_update();
+ void parameters_update() override;
};
#endif
diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp
index c6424ed4e43bc5346b66ec95f20a263509aeb393..04a58a46def339c542f67759c192d52737b05b08 100644
--- a/src/modules/vtol_att_control/tiltrotor.cpp
+++ b/src/modules/vtol_att_control/tiltrotor.cpp
@@ -45,8 +45,7 @@
#define ARSP_YAW_CTRL_DISABLE 7.0f // airspeed at which we stop controlling yaw during a front transition
Tiltrotor::Tiltrotor(VtolAttitudeControl *attc) :
- VtolType(attc),
- _tilt_control(0.0f)
+ VtolType(attc)
{
_vtol_schedule.flight_mode = MC_MODE;
_vtol_schedule.transition_start = 0;
@@ -61,8 +60,6 @@ Tiltrotor::Tiltrotor(VtolAttitudeControl *attc) :
_params_handles_tiltrotor.tilt_transition = param_find("VT_TILT_TRANS");
_params_handles_tiltrotor.tilt_fw = param_find("VT_TILT_FW");
_params_handles_tiltrotor.front_trans_dur_p2 = param_find("VT_TRANS_P2_DUR");
- _params_handles_tiltrotor.diff_thrust = param_find("VT_FW_DIFTHR_EN");
- _params_handles_tiltrotor.diff_thrust_scale = param_find("VT_FW_DIFTHR_SC");
}
void
@@ -85,16 +82,10 @@ Tiltrotor::parameters_update()
/* vtol front transition phase 2 duration */
param_get(_params_handles_tiltrotor.front_trans_dur_p2, &v);
_params_tiltrotor.front_trans_dur_p2 = v;
-
- param_get(_params_handles_tiltrotor.diff_thrust, &_params_tiltrotor.diff_thrust);
-
- param_get(_params_handles_tiltrotor.diff_thrust_scale, &v);
- _params_tiltrotor.diff_thrust_scale = math::constrain(v, -1.0f, 1.0f);
}
void Tiltrotor::update_vtol_state()
{
-
/* simple logic using a two way switch to perform transitions.
* after flipping the switch the vehicle will start tilting rotors, picking up
* forward speed. After the vehicle has picked up enough speed the rotors are tilted
@@ -270,7 +261,7 @@ void Tiltrotor::update_transition_state()
_mc_yaw_weight = _mc_roll_weight;
}
- _thrust_transition = _mc_virtual_att_sp->thrust;
+ _thrust_transition = -_mc_virtual_att_sp->thrust_body[2];
} else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P2) {
// the plane is ready to go into fixed wing mode, tilt the rotors forward completely
@@ -289,7 +280,7 @@ void Tiltrotor::update_transition_state()
_motor_state = set_motor_state(_motor_state, VALUE, rear_value);
- _thrust_transition = _mc_virtual_att_sp->thrust;
+ _thrust_transition = -_mc_virtual_att_sp->thrust_body[2];
} else if (_vtol_schedule.flight_mode == TRANSITION_BACK) {
if (_motor_state != ENABLED) {
@@ -335,7 +326,7 @@ void Tiltrotor::update_transition_state()
void Tiltrotor::waiting_on_tecs()
{
// keep multicopter thrust until we get data from TECS
- _v_att_sp->thrust = _thrust_transition;
+ _v_att_sp->thrust_body[0] = _thrust_transition;
}
/**
@@ -358,9 +349,9 @@ void Tiltrotor::fill_actuator_outputs()
_actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE];
/* allow differential thrust if enabled */
- if (_params_tiltrotor.diff_thrust == 1) {
+ if (_params->diff_thrust == 1) {
_actuators_out_0->control[actuator_controls_s::INDEX_ROLL] =
- _actuators_fw_in->control[actuator_controls_s::INDEX_YAW] * _params_tiltrotor.diff_thrust_scale;
+ _actuators_fw_in->control[actuator_controls_s::INDEX_YAW] * _params->diff_thrust_scale;
}
} else {
diff --git a/src/modules/vtol_att_control/tiltrotor.h b/src/modules/vtol_att_control/tiltrotor.h
index 508973642a69f58c5cceefe6bc3dcaa04dafb802..edfe89dd4f75a838272581064338d8372aed8c74 100644
--- a/src/modules/vtol_att_control/tiltrotor.h
+++ b/src/modules/vtol_att_control/tiltrotor.h
@@ -50,14 +50,14 @@ class Tiltrotor : public VtolType
public:
Tiltrotor(VtolAttitudeControl *_att_controller);
- ~Tiltrotor() = default;
+ ~Tiltrotor() override = default;
- virtual void update_vtol_state();
- virtual void update_transition_state();
- virtual void fill_actuator_outputs();
- virtual void update_mc_state();
- virtual void update_fw_state();
- virtual void waiting_on_tecs();
+ void update_vtol_state() override;
+ void update_transition_state() override;
+ void fill_actuator_outputs() override;
+ void update_mc_state() override;
+ void update_fw_state() override;
+ void waiting_on_tecs() override;
private:
@@ -66,8 +66,6 @@ private:
float tilt_transition; /**< actuator value corresponding to transition tilt (e.g 45 degrees) */
float tilt_fw; /**< actuator value corresponding to fw tilt */
float front_trans_dur_p2;
- int32_t diff_thrust;
- float diff_thrust_scale;
} _params_tiltrotor;
struct {
@@ -75,8 +73,6 @@ private:
param_t tilt_transition;
param_t tilt_fw;
param_t front_trans_dur_p2;
- param_t diff_thrust;
- param_t diff_thrust_scale;
} _params_handles_tiltrotor;
enum vtol_mode {
@@ -99,12 +95,9 @@ private:
hrt_abstime transition_start; /**< absoulte time at which front transition started */
} _vtol_schedule;
- float _tilt_control; /**< actuator value for the tilt servo */
+ float _tilt_control{0.0f}; /**< actuator value for the tilt servo */
- /**
- * Update parameters.
- */
- virtual void parameters_update();
+ void parameters_update() override;
};
#endif
diff --git a/src/modules/vtol_att_control/tiltrotor_params.c b/src/modules/vtol_att_control/tiltrotor_params.c
index d3cb6df264e47a123cc19d3b0b6206cc16f46c9d..bf57b9687e630382cfe284390405a5278c1aeb23 100644
--- a/src/modules/vtol_att_control/tiltrotor_params.c
+++ b/src/modules/vtol_att_control/tiltrotor_params.c
@@ -85,28 +85,3 @@ PARAM_DEFINE_FLOAT(VT_TILT_FW, 1.0f);
* @group VTOL Attitude Control
*/
PARAM_DEFINE_FLOAT(VT_TRANS_P2_DUR, 0.5f);
-
-/**
- * Differential thrust in forwards flight.
- *
- * Set to 1 to enable differential thrust in fixed-wing flight.
- *
- * @min 0
- * @max 1
- * @decimal 0
- * @group VTOL Attitude Control
- */
-PARAM_DEFINE_INT32(VT_FW_DIFTHR_EN, 0);
-
-/**
- * Differential thrust scaling factor
- *
- * This factor specifies how the yaw input gets mapped to differential thrust in forwards flight.
- *
- * @min 0.0
- * @max 1.0
- * @decimal 2
- * @increment 0.1
- * @group VTOL Attitude Control
- */
-PARAM_DEFINE_FLOAT(VT_FW_DIFTHR_SC, 0.1f);
diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp
index a6e673b50a5a71b71fd0a5aad295d25908c68e08..1180875cc24c2c9c1a04b1a753fdfd4420c270b0 100644
--- a/src/modules/vtol_att_control/vtol_att_control_main.cpp
+++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp
@@ -86,6 +86,8 @@ VtolAttitudeControl::VtolAttitudeControl()
_params_handles.front_trans_timeout = param_find("VT_TRANS_TIMEOUT");
_params_handles.mpc_xy_cruise = param_find("MPC_XY_CRUISE");
_params_handles.fw_motors_off = param_find("VT_FW_MOT_OFFID");
+ _params_handles.diff_thrust = param_find("VT_FW_DIFTHR_EN");
+ _params_handles.diff_thrust_scale = param_find("VT_FW_DIFTHR_SC");
/* fetch initial parameter values */
parameters_update();
@@ -486,6 +488,10 @@ VtolAttitudeControl::parameters_update()
param_get(_params_handles.front_trans_timeout, &_params.front_trans_timeout);
param_get(_params_handles.mpc_xy_cruise, &_params.mpc_xy_cruise);
param_get(_params_handles.fw_motors_off, &_params.fw_motors_off);
+ param_get(_params_handles.diff_thrust, &_params.diff_thrust);
+
+ param_get(_params_handles.diff_thrust_scale, &v);
+ _params.diff_thrust_scale = math::constrain(v, -1.0f, 1.0f);
// standard vtol always needs to turn all mc motors off when going into fixed wing mode
// normally the parameter fw_motors_off can be used to specify this, however, since historically standard vtol code
@@ -506,52 +512,6 @@ VtolAttitudeControl::parameters_update()
return OK;
}
-/**
-* Prepare message for mc attitude rates setpoint topic
-*/
-void VtolAttitudeControl::fill_mc_att_rates_sp()
-{
- bool updated;
- orb_check(_mc_virtual_v_rates_sp_sub, &updated);
-
- if (updated) {
- vehicle_rates_setpoint_s v_rates_sp;
-
- if (orb_copy(ORB_ID(mc_virtual_rates_setpoint), _mc_virtual_v_rates_sp_sub, &v_rates_sp) == PX4_OK) {
- // publish the attitude rates setpoint
- if (_v_rates_sp_pub != nullptr) {
- orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &v_rates_sp);
-
- } else {
- _v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &v_rates_sp);
- }
- }
- }
-}
-
-/**
-* Prepare message for fw attitude rates setpoint topic
-*/
-void VtolAttitudeControl::fill_fw_att_rates_sp()
-{
- bool updated;
- orb_check(_fw_virtual_v_rates_sp_sub, &updated);
-
- if (updated) {
- vehicle_rates_setpoint_s v_rates_sp;
-
- if (orb_copy(ORB_ID(fw_virtual_rates_setpoint), _fw_virtual_v_rates_sp_sub, &v_rates_sp) == PX4_OK) {
- // publish the attitude rates setpoint
- if (_v_rates_sp_pub != nullptr) {
- orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &v_rates_sp);
-
- } else {
- _v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &v_rates_sp);
- }
- }
- }
-}
-
int
VtolAttitudeControl::task_main_trampoline(int argc, char *argv[])
{
@@ -567,8 +527,6 @@ void VtolAttitudeControl::task_main()
_v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
_mc_virtual_att_sp_sub = orb_subscribe(ORB_ID(mc_virtual_attitude_setpoint));
_fw_virtual_att_sp_sub = orb_subscribe(ORB_ID(fw_virtual_attitude_setpoint));
- _mc_virtual_v_rates_sp_sub = orb_subscribe(ORB_ID(mc_virtual_rates_setpoint));
- _fw_virtual_v_rates_sp_sub = orb_subscribe(ORB_ID(fw_virtual_rates_setpoint));
_v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
@@ -680,7 +638,6 @@ void VtolAttitudeControl::task_main()
// got data from mc attitude controller
_vtol_type->update_mc_state();
- fill_mc_att_rates_sp();
} else if (_vtol_type->get_mode() == FIXED_WING) {
@@ -692,7 +649,6 @@ void VtolAttitudeControl::task_main()
_vtol_vehicle_status.in_transition_to_fw = false;
_vtol_type->update_fw_state();
- fill_fw_att_rates_sp();
} else if (_vtol_type->get_mode() == TRANSITION_TO_MC || _vtol_type->get_mode() == TRANSITION_TO_FW) {
@@ -705,7 +661,6 @@ void VtolAttitudeControl::task_main()
_vtol_vehicle_status.in_transition_to_fw = (_vtol_type->get_mode() == TRANSITION_TO_FW);
_vtol_type->update_transition_state();
- fill_mc_att_rates_sp();
}
_vtol_type->fill_actuator_outputs();
diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h
index 44298866649c214a15e83cf2db824dbba064c698..40dbeec9f497cfc8bc6f5fa71644b31d262b6bf0 100644
--- a/src/modules/vtol_att_control/vtol_att_control_main.h
+++ b/src/modules/vtol_att_control/vtol_att_control_main.h
@@ -76,7 +76,6 @@
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/position_setpoint_triplet.h>
-#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vtol_vehicle_status.h>
#include "tiltrotor.h"
@@ -128,13 +127,11 @@ private:
int _actuator_inputs_mc{-1}; //topic on which the mc_att_controller publishes actuator inputs
int _airspeed_sub{-1}; // airspeed subscription
int _fw_virtual_att_sp_sub{-1};
- int _fw_virtual_v_rates_sp_sub{-1}; //vehicle rates setpoint subscription
int _land_detected_sub{-1};
int _local_pos_sp_sub{-1}; // setpoint subscription
int _local_pos_sub{-1}; // sensor subscription
int _manual_control_sp_sub{-1}; //manual control setpoint subscription
int _mc_virtual_att_sp_sub{-1};
- int _mc_virtual_v_rates_sp_sub{-1}; //vehicle rates setpoint subscription
int _params_sub{-1}; //parameter updates subscription
int _pos_sp_triplet_sub{-1}; // local position setpoint subscription
int _tecs_status_sub{-1};
@@ -148,7 +145,6 @@ private:
orb_advert_t _mavlink_log_pub{nullptr}; // mavlink log uORB handle
orb_advert_t _v_att_sp_pub{nullptr};
orb_advert_t _v_cmd_ack_pub{nullptr};
- orb_advert_t _v_rates_sp_pub{nullptr};
orb_advert_t _vtol_vehicle_status_pub{nullptr};
orb_advert_t _actuators_1_pub{nullptr};
@@ -199,6 +195,8 @@ private:
param_t front_trans_timeout;
param_t mpc_xy_cruise;
param_t fw_motors_off;
+ param_t diff_thrust;
+ param_t diff_thrust_scale;
} _params_handles{};
/* for multicopters it is usual to have a non-zero idle speed of the engines
@@ -229,10 +227,7 @@ private:
void vehicle_local_pos_poll(); // Check for changes in sensor values
void vehicle_local_pos_sp_poll(); // Check for changes in setpoint values
- int parameters_update(); //Update local paraemter cache
-
- void fill_mc_att_rates_sp();
- void fill_fw_att_rates_sp();
+ int parameters_update(); //Update local parameter cache
void handle_command();
};
diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c
index f27da247d4d319f42a6cefb7c1317a8f2f9bd608..e8af9384fd9508030c1608125cca57c2010534f3 100644
--- a/src/modules/vtol_att_control/vtol_att_control_params.c
+++ b/src/modules/vtol_att_control/vtol_att_control_params.c
@@ -289,3 +289,39 @@ PARAM_DEFINE_FLOAT(VT_F_TR_OL_TM, 6.0f);
* @group VTOL Attitude Control
*/
PARAM_DEFINE_INT32(VT_FW_MOT_OFFID, 0);
+
+/**
+ * Differential thrust in forwards flight.
+ *
+ * Set to 1 to enable differential thrust in fixed-wing flight.
+ *
+ * @min 0
+ * @max 1
+ * @decimal 0
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_INT32(VT_FW_DIFTHR_EN, 0);
+
+/**
+ * Differential thrust scaling factor
+ *
+ * This factor specifies how the yaw input gets mapped to differential thrust in forwards flight.
+ *
+ * @min 0.0
+ * @max 1.0
+ * @decimal 2
+ * @increment 0.1
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_FLOAT(VT_FW_DIFTHR_SC, 0.1f);
+
+/**
+ * Airspeed rules regarding fixed wing control surface scaling.
+ *
+ * @value 0 No special rules
+ * @value 1 During hover (excluding transitions) use the lowest possible airspeed value.
+ * @min 0.0
+ * @increment 1
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_INT32(VT_AIRSPD_RULE, 0);
diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h
index 1a10d4c1081d0fbbf5f332c8fdec8a072cdf8a35..230e615bdda013f7b98b36a66b6b1c68d1cdda13 100644
--- a/src/modules/vtol_att_control/vtol_type.h
+++ b/src/modules/vtol_att_control/vtol_type.h
@@ -68,6 +68,8 @@ struct Params {
float front_trans_timeout;
float mpc_xy_cruise;
int32_t fw_motors_off; /**< bitmask of all motors that should be off in fixed wing mode */
+ int32_t diff_thrust;
+ float diff_thrust_scale;
};
// Has to match 1:1 msg/vtol_vehicle_status.msg
diff --git a/src/systemcmds/tests/test_controlmath.cpp b/src/systemcmds/tests/test_controlmath.cpp
index 7be4d0909b5a6a7d993b834a21a9f086b292efd0..fb5c3d6a2a5a11486c523d02329dc9da4bab89e4 100644
--- a/src/systemcmds/tests/test_controlmath.cpp
+++ b/src/systemcmds/tests/test_controlmath.cpp
@@ -37,7 +37,7 @@ bool ControlMathTest::testThrAttMapping()
ut_assert_true(att.roll_body < SIGMA_SINGLE_OP);
ut_assert_true(att.pitch_body < SIGMA_SINGLE_OP);
ut_assert_true(att.yaw_body < SIGMA_SINGLE_OP);
- ut_assert_true(att.thrust - 1.0f < SIGMA_SINGLE_OP);
+ ut_assert_true(-att.thrust_body[2] - 1.0f < SIGMA_SINGLE_OP);
/* expected: same as before but with 90 yaw
* reason: only yaw changed
@@ -47,7 +47,7 @@ bool ControlMathTest::testThrAttMapping()
ut_assert_true(att.roll_body < SIGMA_SINGLE_OP);
ut_assert_true(att.pitch_body < SIGMA_SINGLE_OP);
ut_assert_true(att.yaw_body - M_PI_2_F < SIGMA_SINGLE_OP);
- ut_assert_true(att.thrust - 1.0f < SIGMA_SINGLE_OP);
+ ut_assert_true(-att.thrust_body[2] - 1.0f < SIGMA_SINGLE_OP);
/* expected: same as before but roll 180
* reason: thrust points straight down and order Euler
@@ -58,7 +58,7 @@ bool ControlMathTest::testThrAttMapping()
ut_assert_true(fabsf(att.roll_body) - M_PI_F < SIGMA_SINGLE_OP);
ut_assert_true(fabsf(att.pitch_body) < SIGMA_SINGLE_OP);
ut_assert_true(att.yaw_body - M_PI_2_F < SIGMA_SINGLE_OP);
- ut_assert_true(att.thrust - 1.0f < SIGMA_SINGLE_OP);
+ ut_assert_true(-att.thrust_body[2] - 1.0f < SIGMA_SINGLE_OP);
/* TODO: find a good way to test it */