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

VTOL rate control architecture improvements (#10819) 

* attitude and rate setpoint message: use 3D array for thrust demand
* FixedWingAttitudeControl: rework airspeed scaling
  * move airspeed and scaling calculation into separate method
  * if vtol in hover and airspeed disabled use minimum airspeed instead of trim airspeed
This commit is contained in:
Roman Bapst 2018-11-22 02:32:40 +01:00 committed by Daniel Agar
parent 75c1396ed7
commit 90bfdb6f0a
28 changed files with 387 additions and 414 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Tools/uorb_graph/create.py
View File

@ -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$'),

4
msg/vehicle_attitude_setpoint.msg
View File

@ -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)

5
msg/vehicle_rates_setpoint.msg
View File

@ -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]

2
src/examples/rover_steering_control/main.cpp
View File

@ -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();
}

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

@ -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 (att_sp_updated) {
orb_copy(ORB_ID(vehicle_attitude_setpoint), _att_sp_sub, &_att_sp);
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];
}
}
}
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::get_airspeed_and_scaling(float &airspeed, float &airspeed_scaling)
{
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 {
// 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()
{
/*
* 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));
parameters_update();
/* 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();
/* 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_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;

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

@ -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);
};

14
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
View File

@ -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);
}
}

2
src/modules/gnd_att_control/GroundRoverAttitudeControl.cpp
View File

@ -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 &&

8
src/modules/gnd_pos_control/GroundRoverPositionControl.cpp
View File

@ -261,7 +261,7 @@ GroundRoverPositionControl::control_position(const matrix::Vector2f &current_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 &current_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 &current_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 &current_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;

2
src/modules/mavlink/mavlink_messages.cpp
View File

@ -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);

7
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -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) {

4
src/modules/mc_att_control/mc_att_control.hpp
View File

@ -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 */

24
src/modules/mc_att_control/mc_att_control_main.cpp
View File

@ -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];
}
}
}

2
src/modules/mc_pos_control/Utility/ControlMath.cpp
View File

@ -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;
}

2
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -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;
}
}

2
src/modules/navigator/gpsfailure.cpp
View File

@ -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);

41
src/modules/vtol_att_control/standard.cpp
View File

@ -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;
};

22
src/modules/vtol_att_control/standard.h
View File

@ -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

201
src/modules/vtol_att_control/tailsitter.cpp
View File

@ -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_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));
// 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);
// 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 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));
}
_q_trans_sp = _q_trans_start;
}
// 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 (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1) {
// 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);
const float trans_pitch_rate = M_PI_2_F / _params->front_trans_duration;
// 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;
} else {
_mc_yaw_weight = 1.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;
}
_mc_roll_weight = 1.0f;
_mc_pitch_weight = 1.0f;
} else if (_vtol_schedule.flight_mode == TRANSITION_BACK) {
const float trans_pitch_rate = M_PI_2_F / _params->back_trans_duration;
if (!flag_idle_mc) {
flag_idle_mc = set_idle_mc();
}
// 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);
// keep yaw disabled
_mc_yaw_weight = 0.0f;
// smoothly move control weight to MC
_mc_roll_weight = _mc_pitch_weight = time_since_trans_start / _params->back_trans_duration;
if (tilt > 0.01f) {
_q_trans_sp = Quatf(AxisAnglef(_trans_rot_axis,
time_since_trans_start * trans_pitch_rate)) * _q_trans_start;
}
}
if (_v_control_mode->flag_control_climb_rate_enabled) {
_v_att_sp->thrust = _params->front_trans_throttle;
_v_att_sp->thrust_body[2] = _mc_virtual_att_sp->thrust_body[2];
} else {
_v_att_sp->thrust = _mc_virtual_att_sp->thrust;
}
_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);
// 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];
_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;
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;
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];
} 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;
} else {
_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] =
-_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL]; // roll elevon
-_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL];
_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;
_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] =
_actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE];
// 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);
_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];
}
}

31
src/modules/vtol_att_control/tailsitter.h
View File

@ -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

21
src/modules/vtol_att_control/tiltrotor.cpp
View File

@ -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 {

25
src/modules/vtol_att_control/tiltrotor.h
View File

@ -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

25
src/modules/vtol_att_control/tiltrotor_params.c
View File

@ -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);

57
src/modules/vtol_att_control/vtol_att_control_main.cpp
View File

@ -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();

11
src/modules/vtol_att_control/vtol_att_control_main.h
View File

@ -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();
};

36
src/modules/vtol_att_control/vtol_att_control_params.c
View File

@ -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);

2
src/modules/vtol_att_control/vtol_type.h
View File

@ -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

6
src/systemcmds/tests/test_controlmath.cpp
View File

@ -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 */