zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

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
sbg
Roman Bapst 6 years ago committed by Daniel Agar
parent
75c1396ed7
commit
90bfdb6f0a
28 changed files with 382 additions and 409 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      Tools/uorb_graph/create.py
  2. 4
      msg/vehicle_attitude_setpoint.msg
  3. 5
      msg/vehicle_rates_setpoint.msg
  4. 2
      src/examples/rover_steering_control/main.cpp
  5. 222
      src/modules/fw_att_control/FixedwingAttitudeControl.cpp
  6. 17
      src/modules/fw_att_control/FixedwingAttitudeControl.hpp
  7. 14
      src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
  8. 2
      src/modules/gnd_att_control/GroundRoverAttitudeControl.cpp
  9. 8
      src/modules/gnd_pos_control/GroundRoverPositionControl.cpp
  10. 2
      src/modules/mavlink/mavlink_messages.cpp
  11. 7
      src/modules/mavlink/mavlink_receiver.cpp
  12. 4
      src/modules/mc_att_control/mc_att_control.hpp
  13. 24
      src/modules/mc_att_control/mc_att_control_main.cpp
  14. 2
      src/modules/mc_pos_control/Utility/ControlMath.cpp
  15. 2
      src/modules/mc_pos_control/mc_pos_control_main.cpp
  16. 2
      src/modules/navigator/gpsfailure.cpp
  17. 41
      src/modules/vtol_att_control/standard.cpp
  18. 22
      src/modules/vtol_att_control/standard.h
  19. 195
      src/modules/vtol_att_control/tailsitter.cpp
  20. 31
      src/modules/vtol_att_control/tailsitter.h
  21. 21
      src/modules/vtol_att_control/tiltrotor.cpp
  22. 25
      src/modules/vtol_att_control/tiltrotor.h
  23. 25
      src/modules/vtol_att_control/tiltrotor_params.c
  24. 57
      src/modules/vtol_att_control/vtol_att_control_main.cpp
  25. 11
      src/modules/vtol_att_control/vtol_att_control_main.h
  26. 36
      src/modules/vtol_att_control/vtol_att_control_params.c
  27. 2
      src/modules/vtol_att_control/vtol_type.h
  28. 6
      src/systemcmds/tests/test_controlmath.cpp

2
Tools/uorb_graph/create.py
Unescape 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_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$'), ('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_actuators_id=([^,)]+)', r'^_actuators_id$'),
('fw_att_control', r'FixedwingAttitudeControl\.cpp$', r'\b_attitude_setpoint_id=([^,)]+)', r'^_attitude_setpoint_id$'), ('fw_att_control', r'FixedwingAttitudeControl\.cpp$', r'\b_attitude_setpoint_id=([^,)]+)', r'^_attitude_setpoint_id$'),

4
msg/vehicle_attitude_setpoint.msg
Unescape 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 float32[4] q_d # Desired quaternion for quaternion control
bool q_d_valid # Set to true if quaternion vector is valid 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 roll_reset_integral # Reset roll integral part (navigation logic change)
bool pitch_reset_integral # Reset pitch integral part (navigation logic change) bool pitch_reset_integral # Reset pitch integral part (navigation logic change)

5
msg/vehicle_rates_setpoint.msg
Unescape View File

@ -3,6 +3,7 @@ uint64 timestamp # time since system start (microseconds)
float32 roll # body angular rates in NED frame float32 roll # body angular rates in NED frame
float32 pitch # body angular rates in NED frame float32 pitch # body angular rates in NED frame
float32 yaw # 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
Unescape 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; actuators->control[2] = yaw_err * pp.yaw_p;
/* copy throttle */ /* copy throttle */
actuators->control[3] = att_sp->thrust; actuators->control[3] = att_sp->thrust_body[0];
actuators->timestamp = hrt_absolute_time(); actuators->timestamp = hrt_absolute_time();
} }

222
src/modules/fw_att_control/FixedwingAttitudeControl.cpp
Unescape View File

@ -48,6 +48,9 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_nonfinite_input_perf(perf_alloc(PC_COUNT, "fwa_nani")), _nonfinite_input_perf(perf_alloc(PC_COUNT, "fwa_nani")),
_nonfinite_output_perf(perf_alloc(PC_COUNT, "fwa_nano")) _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_tc = param_find("FW_P_TC");
_parameter_handles.p_p = param_find("FW_PR_P"); _parameter_handles.p_p = param_find("FW_PR_P");
_parameter_handles.p_i = param_find("FW_PR_I"); _parameter_handles.p_i = param_find("FW_PR_I");
@ -116,13 +119,43 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
// initialize to invalid VTOL type // initialize to invalid VTOL type
_parameters.vtol_type = -1; _parameters.vtol_type = -1;
_parameters.vtol_airspeed_rule = 0;
/* fetch initial parameter values */ /* fetch initial parameter values */
parameters_update(); 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() 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(_loop_perf);
perf_free(_nonfinite_input_perf); perf_free(_nonfinite_input_perf);
perf_free(_nonfinite_output_perf); perf_free(_nonfinite_output_perf);
@ -131,7 +164,6 @@ FixedwingAttitudeControl::~FixedwingAttitudeControl()
int int
FixedwingAttitudeControl::parameters_update() FixedwingAttitudeControl::parameters_update()
{ {
int32_t tmp = 0; int32_t tmp = 0;
param_get(_parameter_handles.p_tc, &(_parameters.p_tc)); param_get(_parameter_handles.p_tc, &(_parameters.p_tc));
param_get(_parameter_handles.p_p, &(_parameters.p_p)); param_get(_parameter_handles.p_p, &(_parameters.p_p));
@ -209,6 +241,7 @@ FixedwingAttitudeControl::parameters_update()
if (_vehicle_status.is_vtol) { if (_vehicle_status.is_vtol) {
param_get(_parameter_handles.vtol_type, &_parameters.vtol_type); 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); param_get(_parameter_handles.bat_scale_en, &_parameters.bat_scale_en);
@ -249,13 +282,12 @@ FixedwingAttitudeControl::parameters_update()
void void
FixedwingAttitudeControl::vehicle_control_mode_poll() FixedwingAttitudeControl::vehicle_control_mode_poll()
{ {
bool vcontrol_mode_updated; bool updated = false;
/* Check if vehicle control mode has changed */ /* Check if vehicle control mode has changed */
orb_check(_vcontrol_mode_sub, &vcontrol_mode_updated); orb_check(_vcontrol_mode_sub, &updated);
if (vcontrol_mode_updated) {
if (updated) {
orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &_vcontrol_mode); orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &_vcontrol_mode);
} }
} }
@ -263,8 +295,8 @@ FixedwingAttitudeControl::vehicle_control_mode_poll()
void void
FixedwingAttitudeControl::vehicle_manual_poll() 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 // 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) { 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 = -_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.pitch_body = math::constrain(_att_sp.pitch_body, -_parameters.man_pitch_max, _parameters.man_pitch_max);
_att_sp.yaw_body = 0.0f; _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)); Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body));
q.copyTo(_att_sp.q_d); 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.roll = _manual.y * _parameters.acro_max_x_rate_rad;
_rates_sp.pitch = -_manual.x * _parameters.acro_max_y_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.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) { if (_rate_sp_pub != nullptr) {
/* publish the attitude rates setpoint */ /* 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 */ /* 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 { } else {
@ -335,14 +367,36 @@ FixedwingAttitudeControl::vehicle_manual_poll()
} }
void void
FixedwingAttitudeControl::vehicle_setpoint_poll() FixedwingAttitudeControl::vehicle_attitude_setpoint_poll()
{ {
/* check if there is a new setpoint */ /* check if there is a new setpoint */
bool att_sp_updated; bool updated = false;
orb_check(_att_sp_sub, &att_sp_updated); 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) { void
orb_copy(ORB_ID(vehicle_attitude_setpoint), _att_sp_sub, &_att_sp); 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) { if (vehicle_status_updated) {
orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); 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 */ /* set correct uORB ID, depending on if vehicle is VTOL or not */
if (!_rates_sp_id) { if (!_actuators_id) {
if (_vehicle_status.is_vtol) { if (_vehicle_status.is_vtol) {
_rates_sp_id = ORB_ID(fw_virtual_rates_setpoint);
_actuators_id = ORB_ID(actuator_controls_virtual_fw); _actuators_id = ORB_ID(actuator_controls_virtual_fw);
_attitude_setpoint_id = ORB_ID(fw_virtual_attitude_setpoint); _attitude_setpoint_id = ORB_ID(fw_virtual_attitude_setpoint);
_parameter_handles.vtol_type = param_find("VT_TYPE"); _parameter_handles.vtol_type = param_find("VT_TYPE");
_parameter_handles.vtol_airspeed_rule = param_find("VT_AIRSPD_RULE");
parameters_update(); parameters_update();
} else { } else {
_rates_sp_id = ORB_ID(vehicle_rates_setpoint);
_actuators_id = ORB_ID(actuator_controls_0); _actuators_id = ORB_ID(actuator_controls_0);
_attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint); _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)
{ {
/* const bool airspeed_valid = PX4_ISFINITE(_airspeed_sub.get().indicated_airspeed_m_s)
* do subscriptions && (_airspeed_sub.get().indicated_airspeed_m_s > 0.0f)
*/ && (hrt_elapsed_time(&_airspeed_sub.get().timestamp) < 1e6);
_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(); 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 */ } else {
vehicle_setpoint_poll(); // if no airspeed measurement is available out best guess is to use the trim airspeed
vehicle_control_mode_poll(); airspeed = _parameters.airspeed_trim;
vehicle_manual_poll();
vehicle_status_poll(); // VTOL: if we have no airspeed available and we are in hover mode then assume the lowest airspeed possible
vehicle_land_detected_poll(); // 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 */ /* wakeup source */
px4_pollfd_struct_t fds[1]; px4_pollfd_struct_t fds[1];
@ -529,7 +606,7 @@ void FixedwingAttitudeControl::run()
matrix::Eulerf euler_angles(R); matrix::Eulerf euler_angles(R);
_airspeed_sub.update(); _airspeed_sub.update();
vehicle_setpoint_poll(); vehicle_attitude_setpoint_poll();
vehicle_control_mode_poll(); vehicle_control_mode_poll();
vehicle_manual_poll(); vehicle_manual_poll();
global_pos_poll(); global_pos_poll();
@ -560,35 +637,10 @@ void FixedwingAttitudeControl::run()
/* decide if in stabilized or full manual control */ /* decide if in stabilized or full manual control */
if (_vcontrol_mode.flag_control_rates_enabled) { 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);
}
}
/* float airspeed;
* For scaling our actuators using anything less than the min (close to stall) float airspeed_scaling;
* speed doesn't make any sense - its the strongest reasonable deflection we get_airspeed_and_scaling(airspeed, airspeed_scaling);
* 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);
/* Use min airspeed to calculate ground speed scaling region. /* Use min airspeed to calculate ground speed scaling region.
* Don't scale below gspd_scaling_trim * Don't scale below gspd_scaling_trim
@ -624,10 +676,6 @@ void FixedwingAttitudeControl::run()
_wheel_ctrl.reset_integrator(); _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 */ /* Prepare data for attitude controllers */
struct ECL_ControlData control_input = {}; struct ECL_ControlData control_input = {};
control_input.roll = euler_angles.phi(); control_input.roll = euler_angles.phi();
@ -636,9 +684,9 @@ void FixedwingAttitudeControl::run()
control_input.body_x_rate = _att.rollspeed; control_input.body_x_rate = _att.rollspeed;
control_input.body_y_rate = _att.pitchspeed; control_input.body_y_rate = _att.pitchspeed;
control_input.body_z_rate = _att.yawspeed; control_input.body_z_rate = _att.yawspeed;
control_input.roll_setpoint = roll_sp; control_input.roll_setpoint = _att_sp.roll_body;
control_input.pitch_setpoint = pitch_sp; control_input.pitch_setpoint = _att_sp.pitch_body;
control_input.yaw_setpoint = yaw_sp; control_input.yaw_setpoint = _att_sp.yaw_body;
control_input.airspeed_min = _parameters.airspeed_min; control_input.airspeed_min = _parameters.airspeed_min;
control_input.airspeed_max = _parameters.airspeed_max; control_input.airspeed_max = _parameters.airspeed_max;
control_input.airspeed = airspeed; control_input.airspeed = airspeed;
@ -649,7 +697,7 @@ void FixedwingAttitudeControl::run()
/* reset body angular rate limits on mode change */ /* 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 != _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)); _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_pos(math::radians(_parameters.p_rmax_pos));
_pitch_ctrl.set_max_rate_neg(math::radians(_parameters.p_rmax_neg)); _pitch_ctrl.set_max_rate_neg(math::radians(_parameters.p_rmax_neg));
@ -693,7 +741,7 @@ void FixedwingAttitudeControl::run()
/* Run attitude controllers */ /* Run attitude controllers */
if (_vcontrol_mode.flag_control_attitude_enabled) { 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); _roll_ctrl.control_attitude(control_input);
_pitch_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 _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 */ /* 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) _actuators.control[actuator_controls_s::INDEX_THROTTLE] = (PX4_ISFINITE(_att_sp.thrust_body[0])
&& !_vehicle_status.engine_failure) ? _att_sp.thrust : 0.0f; && !_vehicle_status.engine_failure) ? _att_sp.thrust_body[0] : 0.0f;
/* scale effort by battery status */ /* scale effort by battery status */
if (_parameters.bat_scale_en && if (_parameters.bat_scale_en &&
@ -783,18 +831,19 @@ void FixedwingAttitudeControl::run()
if (_rate_sp_pub != nullptr) { if (_rate_sp_pub != nullptr) {
/* publish the attitude rates setpoint */ /* 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 */ /* 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 { } else {
// pure rate control vehicle_rates_setpoint_poll();
_roll_ctrl.set_bodyrate_setpoint(_rates_sp.roll); _roll_ctrl.set_bodyrate_setpoint(_rates_sp.roll);
_pitch_ctrl.set_bodyrate_setpoint(_rates_sp.pitch);
_yaw_ctrl.set_bodyrate_setpoint(_rates_sp.yaw); _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); 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; _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); 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_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; rate_ctrl_status_s rate_ctrl_status;

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

@ -95,6 +95,7 @@ private:
int _att_sub{-1}; /**< vehicle attitude */ int _att_sub{-1}; /**< vehicle attitude */
int _att_sp_sub{-1}; /**< vehicle attitude setpoint */ 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 _battery_status_sub{-1}; /**< battery status subscription */
int _global_pos_sub{-1}; /**< global position subscription */ int _global_pos_sub{-1}; /**< global position subscription */
int _manual_sub{-1}; /**< notification of manual control updates */ 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 _actuators_2_pub{nullptr}; /**< actuator control group 1 setpoint (Airframe) */
orb_advert_t _rate_ctrl_status_pub{nullptr}; /**< rate controller status publication */ 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 _actuators_id{nullptr}; // pointer to correct actuator controls0 uORB metadata structure
orb_id_t _attitude_setpoint_id{nullptr}; orb_id_t _attitude_setpoint_id{nullptr};
@ -185,7 +185,7 @@ private:
float pitchsp_offset_deg; /**< Pitch Setpoint Offset in deg */ float pitchsp_offset_deg; /**< Pitch Setpoint Offset in deg */
float rollsp_offset_rad; /**< Roll Setpoint Offset in rad */ float rollsp_offset_rad; /**< Roll Setpoint Offset in rad */
float pitchsp_offset_rad; /**< Pitch 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_pitch_max; /**< Max Pitch in rad */
float man_roll_scale; /**< scale factor applied to roll actuator control in pure manual mode */ 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 */ 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_y_rate_rad;
float acro_max_z_rate_rad; float acro_max_z_rate_rad;
float flaps_scale; /**< Scale factor for flaps */ float flaps_scale; /**< Scale factor for flaps */
float flaps_takeoff_scale; /**< Scale factor for flaps on take-off */ float flaps_takeoff_scale; /**< Scale factor for flaps on take-off */
float flaperon_scale; /**< Scale factor for flaperons */ float flaperon_scale; /**< Scale factor for flaperons */
float rattitude_thres; 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 */ int32_t bat_scale_en; /**< Battery scaling enabled */
bool airspeed_disabled; bool airspeed_disabled;
@ -271,6 +272,7 @@ private:
param_t rattitude_thres; param_t rattitude_thres;
param_t vtol_type; param_t vtol_type;
param_t vtol_airspeed_rule;
param_t bat_scale_en; param_t bat_scale_en;
param_t airspeed_mode; param_t airspeed_mode;
@ -291,9 +293,10 @@ private:
void vehicle_control_mode_poll(); void vehicle_control_mode_poll();
void vehicle_manual_poll(); void vehicle_manual_poll();
void vehicle_setpoint_poll(); void vehicle_attitude_setpoint_poll();
void vehicle_rates_setpoint_poll();
void global_pos_poll(); void global_pos_poll();
void vehicle_status_poll(); void vehicle_status_poll();
void vehicle_land_detected_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
Unescape 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) { 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.roll_body = 0.0f;
_att_sp.pitch_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, /* making sure again that the correct thrust is used,
* without depending on library calls for safety reasons. * without depending on library calls for safety reasons.
the pre-takeoff throttle and the idle throttle normally map to the same parameter. */ 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 && } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO &&
pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF &&
_runway_takeoff.runwayTakeoffEnabled()) { _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 && } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO &&
pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { 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) { } 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 { } else {
/* Copy thrust and pitch values from tecs */ /* Copy thrust and pitch values from tecs */
if (_vehicle_land_detected.landed) { if (_vehicle_land_detected.landed) {
// when we are landed state we want the motor to spin at idle speed // 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 { } 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
Unescape View File

@ -303,7 +303,7 @@ GroundRoverAttitudeControl::task_main()
} }
/* throttle passed through if it is finite and if no engine failure was detected */ /* 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 */ /* scale effort by battery status */
if (_parameters.bat_scale_en && _battery_status.scale > 0.0f && if (_parameters.bat_scale_en && _battery_status.scale > 0.0f &&

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

@ -261,7 +261,7 @@ GroundRoverPositionControl::control_position(const matrix::Vector2f &current_pos
_att_sp.roll_body = 0.0f; _att_sp.roll_body = 0.0f;
_att_sp.pitch_body = 0.0f; _att_sp.pitch_body = 0.0f;
_att_sp.yaw_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) } else if ((pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION)
|| (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF)) { || (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.pitch_body = 0.0f;
_att_sp.yaw_body = _gnd_control.nav_bearing(); _att_sp.yaw_body = _gnd_control.nav_bearing();
_att_sp.fw_control_yaw = true; _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) { } 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.pitch_body = 0.0f;
_att_sp.yaw_body = _gnd_control.nav_bearing(); _att_sp.yaw_body = _gnd_control.nav_bearing();
_att_sp.fw_control_yaw = true; _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()) { 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.pitch_body = 0.0f;
_att_sp.yaw_body = 0.0f; _att_sp.yaw_body = 0.0f;
_att_sp.fw_control_yaw = true; _att_sp.fw_control_yaw = true;
_att_sp.thrust = 0.0f; _att_sp.thrust_body[0] = 0.0f;
/* do not publish the setpoint */ /* do not publish the setpoint */
setpoint = false; setpoint = false;

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

@ -3138,7 +3138,7 @@ protected:
msg.body_pitch_rate = att_rates_sp.pitch; msg.body_pitch_rate = att_rates_sp.pitch;
msg.body_yaw_rate = att_rates_sp.yaw; 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); mavlink_msg_attitude_target_send_struct(_mavlink->get_channel(), &msg);

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

@ -1447,8 +1447,11 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
att_sp.yaw_sp_move_rate = 0.0f; 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 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) { 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 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) { if (_rates_sp_pub == nullptr) {

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

@ -166,7 +166,6 @@ private:
orb_advert_t _controller_status_pub{nullptr}; /**< controller status publication */ orb_advert_t _controller_status_pub{nullptr}; /**< controller status publication */
orb_advert_t _vehicle_attitude_setpoint_pub{nullptr}; 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 */ 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 */ 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_prev_filtered; /**< angular rates on previous step (low-pass filtered) */
matrix::Vector3f _rates_sp; /**< angular rates setpoint */ matrix::Vector3f _rates_sp; /**< angular rates setpoint */
matrix::Vector3f _rates_int; /**< angular rates integral error */ matrix::Vector3f _rates_int; /**< angular rates integral error */
float _thrust_sp; /**< thrust setpoint */
matrix::Vector3f _att_control; /**< attitude control vector */ 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 */ 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
Unescape View File

@ -273,13 +273,11 @@ MulticopterAttitudeControl::vehicle_status_poll()
orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
/* set correct uORB ID, depending on if vehicle is VTOL or not */ /* 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) { if (_vehicle_status.is_vtol) {
_rates_sp_id = ORB_ID(mc_virtual_rates_setpoint);
_actuators_id = ORB_ID(actuator_controls_virtual_mc); _actuators_id = ORB_ID(actuator_controls_virtual_mc);
} else { } else {
_rates_sp_id = ORB_ID(vehicle_rates_setpoint);
_actuators_id = ORB_ID(actuator_controls_0); _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); q_sp.copyTo(attitude_setpoint.q_d);
attitude_setpoint.q_d_valid = true; 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.landing_gear = get_landing_gear_state();
attitude_setpoint.timestamp = hrt_absolute_time(); attitude_setpoint.timestamp = hrt_absolute_time();
@ -524,7 +522,9 @@ void
MulticopterAttitudeControl::control_attitude() MulticopterAttitudeControl::control_attitude()
{ {
vehicle_attitude_setpoint_poll(); 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 */ /* prepare yaw weight from the ratio between roll/pitch and yaw gains */
Vector3f attitude_gain = _attitude_p; Vector3f attitude_gain = _attitude_p;
@ -607,7 +607,7 @@ Vector3f
MulticopterAttitudeControl::pid_attenuations(float tpa_breakpoint, float tpa_rate) MulticopterAttitudeControl::pid_attenuations(float tpa_breakpoint, float tpa_rate)
{ {
/* throttle pid attenuation factor */ /* 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)); tpa = fmaxf(TPA_RATE_LOWER_LIMIT, fminf(1.0f, tpa));
Vector3f pidAttenuationPerAxis; Vector3f pidAttenuationPerAxis;
@ -731,9 +731,11 @@ MulticopterAttitudeControl::publish_rates_setpoint()
_v_rates_sp.roll = _rates_sp(0); _v_rates_sp.roll = _rates_sp(0);
_v_rates_sp.pitch = _rates_sp(1); _v_rates_sp.pitch = _rates_sp(1);
_v_rates_sp.yaw = _rates_sp(2); _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(); _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 void
@ -886,7 +888,7 @@ MulticopterAttitudeControl::run()
bool attitude_setpoint_generated = false; 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) { if (attitude_updated) {
// Generate the attitude setpoint from stick inputs if we are in Manual/Stabilized mode // Generate the attitude setpoint from stick inputs if we are in Manual/Stabilized mode
if (_v_control_mode.flag_control_manual_enabled && if (_v_control_mode.flag_control_manual_enabled &&
@ -903,7 +905,7 @@ MulticopterAttitudeControl::run()
} else { } else {
/* attitude controller disabled, poll rates setpoint topic */ /* 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) { if (manual_control_updated) {
/* manual rates control - ACRO mode */ /* manual rates control - ACRO mode */
Vector3f man_rate_sp( Vector3f man_rate_sp(
@ -921,7 +923,7 @@ MulticopterAttitudeControl::run()
_rates_sp(0) = _v_rates_sp.roll; _rates_sp(0) = _v_rates_sp.roll;
_rates_sp(1) = _v_rates_sp.pitch; _rates_sp(1) = _v_rates_sp.pitch;
_rates_sp(2) = _v_rates_sp.yaw; _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
Unescape View File

@ -103,7 +103,7 @@ vehicle_attitude_setpoint_s thrustToAttitude(const Vector3f &thr_sp, const float
Eulerf euler = R_sp; Eulerf euler = R_sp;
att_sp.roll_body = euler(0); att_sp.roll_body = euler(0);
att_sp.pitch_body = euler(1); att_sp.pitch_body = euler(1);
att_sp.thrust = thr_sp.length(); att_sp.thrust_body[2] = -thr_sp.length();
return att_sp; return att_sp;
} }

2
src/modules/mc_pos_control/mc_pos_control_main.cpp
Unescape 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); 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); q_sp.copyTo(_att_sp.q_d);
_att_sp.q_d_valid = true; _att_sp.q_d_valid = true;
_att_sp.thrust = 0.0f; _att_sp.thrust_body[2] = 0.0f;
} }
} }

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

@ -86,7 +86,7 @@ GpsFailure::on_active()
att_sp.timestamp = hrt_absolute_time(); att_sp.timestamp = hrt_absolute_time();
att_sp.roll_body = math::radians(_param_openlooploiter_roll.get()); att_sp.roll_body = math::radians(_param_openlooploiter_roll.get());
att_sp.pitch_body = math::radians(_param_openlooploiter_pitch.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)); Quatf q(Eulerf(att_sp.roll_body, att_sp.pitch_body, 0.0f));
q.copyTo(att_sp.q_d); q.copyTo(att_sp.q_d);

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

@ -46,13 +46,10 @@
#include <float.h> #include <float.h>
using matrix::wrap_pi; using namespace matrix;
Standard::Standard(VtolAttitudeControl *attc) : Standard::Standard(VtolAttitudeControl *attc) :
VtolType(attc), VtolType(attc)
_pusher_throttle(0.0f),
_reverse_output(0.0f),
_airspeed_trans_blend_margin(0.0f)
{ {
_vtol_schedule.flight_mode = MC_MODE; _vtol_schedule.flight_mode = MC_MODE;
_vtol_schedule.transition_start = 0; _vtol_schedule.transition_start = 0;
@ -156,8 +153,8 @@ void Standard::update_vtol_state()
} else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) { } 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 // 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 Dcmf R_to_body(Quatf(_v_att->q).inversed());
const matrix::Vector3f vel = R_to_body * matrix::Vector3f(_local_pos->vx, _local_pos->vy, _local_pos->vz); const Vector3f vel = R_to_body * Vector3f(_local_pos->vx, _local_pos->vy, _local_pos->vz);
float x_vel = vel(0); float x_vel = vel(0);
@ -261,7 +258,7 @@ void Standard::update_transition_state()
// ramp up FW_PSP_OFF // ramp up FW_PSP_OFF
_v_att_sp->pitch_body = _params_standard.pitch_setpoint_offset * (1.0f - mc_weight); _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); q_sp.copyTo(_v_att_sp->q_d);
_v_att_sp->q_d_valid = true; _v_att_sp->q_d_valid = true;
@ -277,7 +274,7 @@ void Standard::update_transition_state()
// maintain FW_PSP_OFF // maintain FW_PSP_OFF
_v_att_sp->pitch_body = _params_standard.pitch_setpoint_offset; _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); q_sp.copyTo(_v_att_sp->q_d);
_v_att_sp->q_d_valid = true; _v_att_sp->q_d_valid = true;
@ -333,18 +330,18 @@ void Standard::update_mc_state()
return; return;
} }
matrix::Dcmf R(matrix::Quatf(_v_att->q)); const Dcmf R(Quatf(_v_att->q));
matrix::Dcmf R_sp(matrix::Quatf(_v_att_sp->q_d)); const Dcmf R_sp(Quatf(_v_att_sp->q_d));
matrix::Eulerf euler(R); const Eulerf euler(R);
matrix::Eulerf euler_sp(R_sp); const Eulerf euler_sp(R_sp);
_pusher_throttle = 0.0f; _pusher_throttle = 0.0f;
// direction of desired body z axis represented in earth frame // 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 // 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. // 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 = R_yaw * body_z_sp;
body_z_sp.normalize(); body_z_sp.normalize();
@ -365,25 +362,25 @@ void Standard::update_mc_state()
float pitch_new = 0.0f; float pitch_new = 0.0f;
// create corrected desired body z axis in heading frame // create corrected desired body z axis in heading frame
matrix::Dcmf R_tmp = matrix::Eulerf(roll_new, pitch_new, 0.0f); const Dcmf R_tmp = Eulerf(roll_new, pitch_new, 0.0f);
matrix::Vector3f tilt_new(R_tmp(0, 2), R_tmp(1, 2), R_tmp(2, 2)); 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 // rotate the vector into a new frame which is rotated in z by the desired heading
// with respect to the earh frame. // with respect to the earh frame.
const float yaw_error = wrap_pi(euler_sp(2) - euler(2)); 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; tilt_new = R_yaw_correction * tilt_new;
// now extract roll and pitch setpoints // now extract roll and pitch setpoints
_v_att_sp->pitch_body = atan2f(tilt_new(0), tilt_new(2)); _v_att_sp->pitch_body = atan2f(tilt_new(0), tilt_new(2));
_v_att_sp->roll_body = -asinf(tilt_new(1)); _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); 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; _pusher_throttle = _pusher_throttle < 0.0f ? 0.0f : _pusher_throttle;
} }
void Standard::update_fw_state() void Standard::update_fw_state()
@ -475,5 +472,5 @@ void
Standard::waiting_on_tecs() Standard::waiting_on_tecs()
{ {
// keep thrust from transition // 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
Unescape View File

@ -55,14 +55,14 @@ class Standard : public VtolType
public: public:
Standard(VtolAttitudeControl *_att_controller); Standard(VtolAttitudeControl *_att_controller);
~Standard() = default; ~Standard() override = default;
virtual void update_vtol_state(); void update_vtol_state() override;
virtual void update_transition_state(); void update_transition_state() override;
virtual void update_fw_state(); void update_fw_state() override;
virtual void update_mc_state(); void update_mc_state() override;
virtual void fill_actuator_outputs(); void fill_actuator_outputs() override;
virtual void waiting_on_tecs(); void waiting_on_tecs() override;
private: private:
@ -98,10 +98,10 @@ private:
hrt_abstime transition_start; // at what time did we start a transition (front- or backtransition) hrt_abstime transition_start; // at what time did we start a transition (front- or backtransition)
} _vtol_schedule; } _vtol_schedule;
float _pusher_throttle; float _pusher_throttle{0.0f};
float _reverse_output; float _reverse_output{0.0f};
float _airspeed_trans_blend_margin; float _airspeed_trans_blend_margin{0.0f};
virtual void parameters_update(); void parameters_update() override;
}; };
#endif #endif

195
src/modules/vtol_att_control/tailsitter.cpp
Unescape View File

@ -47,11 +47,10 @@
#define PITCH_TRANSITION_FRONT_P1 -1.1f // pitch angle to switch to TRANSITION_P2 #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 #define PITCH_TRANSITION_BACK -0.25f // pitch angle to switch to MC
using namespace matrix;
Tailsitter::Tailsitter(VtolAttitudeControl *attc) : Tailsitter::Tailsitter(VtolAttitudeControl *attc) :
VtolType(attc), VtolType(attc)
_thrust_transition_start(0.0f),
_yaw_transition(0.0f),
_pitch_transition_start(0.0f)
{ {
_vtol_schedule.flight_mode = MC_MODE; _vtol_schedule.flight_mode = MC_MODE;
_vtol_schedule.transition_start = 0; _vtol_schedule.transition_start = 0;
@ -63,6 +62,7 @@ Tailsitter::Tailsitter(VtolAttitudeControl *attc) :
_flag_was_in_trans_mode = false; _flag_was_in_trans_mode = false;
_params_handles_tailsitter.front_trans_dur_p2 = param_find("VT_TRANS_P2_DUR"); _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 void
@ -73,18 +73,20 @@ Tailsitter::parameters_update()
/* vtol front transition phase 2 duration */ /* vtol front transition phase 2 duration */
param_get(_params_handles_tailsitter.front_trans_dur_p2, &v); param_get(_params_handles_tailsitter.front_trans_dur_p2, &v);
_params_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() void Tailsitter::update_vtol_state()
{ {
/* simple logic using a two way switch to perform transitions. /* 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 * 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. * 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. * 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(); float pitch = euler.theta();
if (!_attc->is_fixed_wing_requested()) { if (!_attc->is_fixed_wing_requested()) {
@ -104,9 +106,10 @@ void Tailsitter::update_vtol_state()
break; break;
case TRANSITION_BACK: 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 // 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; _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; float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f;
if (!_flag_was_in_trans_mode) { 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; _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 // the intial attitude setpoint for a backtransition is a combination of the current fw pitch setpoint,
_v_att_sp->pitch_body = _pitch_transition_start - fabsf(PITCH_TRANSITION_FRONT_P1 - _pitch_transition_start) * // the yaw setpoint and zero roll since we want wings level transition
time_since_trans_start / _params->front_trans_duration; _q_trans_start = Eulerf(0.0f, _fw_virtual_att_sp->pitch_body, yaw_sp);
_v_att_sp->pitch_body = math::constrain(_v_att_sp->pitch_body, PITCH_TRANSITION_FRONT_P1 - 0.2f,
_pitch_transition_start);
// disable mc yaw control once the plane has picked up speed // attitude during transitions are controlled by mc attitude control so rotate the desired attitude to the
if (_airspeed->indicated_airspeed_m_s > ARSP_YAW_CTRL_DISABLE) { // multirotor frame
_mc_yaw_weight = 0.0f; _q_trans_start = _q_trans_start * Quatf(Eulerf(0, -M_PI_2_F, 0));
} else { } else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1) {
_mc_yaw_weight = 1.0f; // 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; _q_trans_sp = _q_trans_start;
_mc_pitch_weight = 1.0f; }
} 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) { if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1) {
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 const float trans_pitch_rate = M_PI_2_F / _params->front_trans_duration;
_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 if (tilt < M_PI_2_F - _params_tailsitter.fw_pitch_sp_offset) {
_mc_yaw_weight = 0.0f; _q_trans_sp = Quatf(AxisAnglef(_trans_rot_axis,
time_since_trans_start * trans_pitch_rate)) * _q_trans_start;
}
// smoothly move control weight to MC } else if (_vtol_schedule.flight_mode == TRANSITION_BACK) {
_mc_roll_weight = _mc_pitch_weight = time_since_trans_start / _params->back_trans_duration;
} const float trans_pitch_rate = M_PI_2_F / _params->back_trans_duration;
if (_v_control_mode->flag_control_climb_rate_enabled) { if (!flag_idle_mc) {
_v_att_sp->thrust = _params->front_trans_throttle; flag_idle_mc = set_idle_mc();
}
} else { if (tilt > 0.01f) {
_v_att_sp->thrust = _mc_virtual_att_sp->thrust; _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); _v_att_sp->thrust_body[2] = _mc_virtual_att_sp->thrust_body[2];
_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->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); const Eulerf euler_sp(_q_trans_sp);
q_sp.copyTo(_v_att_sp->q_d); _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; _v_att_sp->q_d_valid = true;
} }
void Tailsitter::waiting_on_tecs() void Tailsitter::waiting_on_tecs()
{ {
// copy the last trust value from the front transition // copy the last trust value from the front transition
_v_att_sp->thrust = _thrust_transition; _v_att_sp->thrust_body[0] = _thrust_transition;
}
void Tailsitter::update_mc_state()
{
VtolType::update_mc_state();
} }
void Tailsitter::update_fw_state() void Tailsitter::update_fw_state()
{ {
VtolType::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 = hrt_absolute_time();
_actuators_out_1->timestamp_sample = _actuators_fw_in->timestamp_sample; _actuators_out_1->timestamp_sample = _actuators_fw_in->timestamp_sample;
switch (_vtol_mode) { _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL]
case ROTARY_WING: * _mc_roll_weight;
_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_out_0->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight;
_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_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];
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: if (_vtol_schedule.flight_mode == FW_MODE) {
// 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;
_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] =
_actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE];
_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = } else {
-_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;
_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] =
_actuators_mc_in->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! } 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_ROLL] =
* (1 - _mc_yaw_weight); -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL];
_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] =
_actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight; _actuators_fw_in->control[actuator_controls_s::INDEX_PITCH];
// **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;
} }
} }

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

@ -46,30 +46,32 @@
#include <perf/perf_counter.h> /** is it necsacery? **/ #include <perf/perf_counter.h> /** is it necsacery? **/
#include <parameters/param.h> #include <parameters/param.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <matrix/matrix/math.hpp>
class Tailsitter : public VtolType class Tailsitter : public VtolType
{ {
public: public:
Tailsitter(VtolAttitudeControl *_att_controller); Tailsitter(VtolAttitudeControl *_att_controller);
~Tailsitter() = default; ~Tailsitter() override = default;
virtual void update_vtol_state(); void update_vtol_state() override;
virtual void update_transition_state(); void update_transition_state() override;
virtual void update_mc_state(); void update_fw_state() override;
virtual void update_fw_state(); void fill_actuator_outputs() override;
virtual void fill_actuator_outputs(); void waiting_on_tecs() override;
virtual void waiting_on_tecs();
private: private:
struct { struct {
float front_trans_dur_p2; float front_trans_dur_p2;
} _params_tailsitter; float fw_pitch_sp_offset;
} _params_tailsitter{};
struct { struct {
param_t front_trans_dur_p2; param_t front_trans_dur_p2;
} _params_handles_tailsitter; param_t fw_pitch_sp_offset;
} _params_handles_tailsitter{};
enum vtol_mode { enum vtol_mode {
MC_MODE = 0, /**< vtol is in multicopter 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 */ hrt_abstime transition_start; /**< absoulte time at which front transition started */
} _vtol_schedule; } _vtol_schedule;
float _thrust_transition_start; // throttle value when we start the front transition matrix::Quatf _q_trans_start;
float _yaw_transition; // yaw angle in which transition will take place matrix::Quatf _q_trans_sp;
float _pitch_transition_start; // pitch angle at the start of transition (tailsitter) matrix::Vector3f _trans_rot_axis;
/** void parameters_update() override;
* Update parameters.
*/
virtual void parameters_update();
}; };
#endif #endif

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

@ -45,8 +45,7 @@
#define ARSP_YAW_CTRL_DISABLE 7.0f // airspeed at which we stop controlling yaw during a front transition #define ARSP_YAW_CTRL_DISABLE 7.0f // airspeed at which we stop controlling yaw during a front transition
Tiltrotor::Tiltrotor(VtolAttitudeControl *attc) : Tiltrotor::Tiltrotor(VtolAttitudeControl *attc) :
VtolType(attc), VtolType(attc)
_tilt_control(0.0f)
{ {
_vtol_schedule.flight_mode = MC_MODE; _vtol_schedule.flight_mode = MC_MODE;
_vtol_schedule.transition_start = 0; _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_transition = param_find("VT_TILT_TRANS");
_params_handles_tiltrotor.tilt_fw = param_find("VT_TILT_FW"); _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.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 void
@ -85,16 +82,10 @@ Tiltrotor::parameters_update()
/* vtol front transition phase 2 duration */ /* vtol front transition phase 2 duration */
param_get(_params_handles_tiltrotor.front_trans_dur_p2, &v); param_get(_params_handles_tiltrotor.front_trans_dur_p2, &v);
_params_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() void Tiltrotor::update_vtol_state()
{ {
/* simple logic using a two way switch to perform transitions. /* simple logic using a two way switch to perform transitions.
* after flipping the switch the vehicle will start tilting rotors, picking up * 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 * 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; _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) { } else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P2) {
// the plane is ready to go into fixed wing mode, tilt the rotors forward completely // 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); _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) { } else if (_vtol_schedule.flight_mode == TRANSITION_BACK) {
if (_motor_state != ENABLED) { if (_motor_state != ENABLED) {
@ -335,7 +326,7 @@ void Tiltrotor::update_transition_state()
void Tiltrotor::waiting_on_tecs() void Tiltrotor::waiting_on_tecs()
{ {
// keep multicopter thrust until we get data from 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]; _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE];
/* allow differential thrust if enabled */ /* 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_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 { } else {

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

@ -50,14 +50,14 @@ class Tiltrotor : public VtolType
public: public:
Tiltrotor(VtolAttitudeControl *_att_controller); Tiltrotor(VtolAttitudeControl *_att_controller);
~Tiltrotor() = default; ~Tiltrotor() override = default;
virtual void update_vtol_state(); void update_vtol_state() override;
virtual void update_transition_state(); void update_transition_state() override;
virtual void fill_actuator_outputs(); void fill_actuator_outputs() override;
virtual void update_mc_state(); void update_mc_state() override;
virtual void update_fw_state(); void update_fw_state() override;
virtual void waiting_on_tecs(); void waiting_on_tecs() override;
private: private:
@ -66,8 +66,6 @@ private:
float tilt_transition; /**< actuator value corresponding to transition tilt (e.g 45 degrees) */ float tilt_transition; /**< actuator value corresponding to transition tilt (e.g 45 degrees) */
float tilt_fw; /**< actuator value corresponding to fw tilt */ float tilt_fw; /**< actuator value corresponding to fw tilt */
float front_trans_dur_p2; float front_trans_dur_p2;
int32_t diff_thrust;
float diff_thrust_scale;
} _params_tiltrotor; } _params_tiltrotor;
struct { struct {
@ -75,8 +73,6 @@ private:
param_t tilt_transition; param_t tilt_transition;
param_t tilt_fw; param_t tilt_fw;
param_t front_trans_dur_p2; param_t front_trans_dur_p2;
param_t diff_thrust;
param_t diff_thrust_scale;
} _params_handles_tiltrotor; } _params_handles_tiltrotor;
enum vtol_mode { enum vtol_mode {
@ -99,12 +95,9 @@ private:
hrt_abstime transition_start; /**< absoulte time at which front transition started */ hrt_abstime transition_start; /**< absoulte time at which front transition started */
} _vtol_schedule; } _vtol_schedule;
float _tilt_control; /**< actuator value for the tilt servo */ float _tilt_control{0.0f}; /**< actuator value for the tilt servo */
/** void parameters_update() override;
* Update parameters.
*/
virtual void parameters_update();
}; };
#endif #endif

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

@ -85,28 +85,3 @@ PARAM_DEFINE_FLOAT(VT_TILT_FW, 1.0f);
* @group VTOL Attitude Control * @group VTOL Attitude Control
*/ */
PARAM_DEFINE_FLOAT(VT_TRANS_P2_DUR, 0.5f); 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
Unescape View File

@ -86,6 +86,8 @@ VtolAttitudeControl::VtolAttitudeControl()
_params_handles.front_trans_timeout = param_find("VT_TRANS_TIMEOUT"); _params_handles.front_trans_timeout = param_find("VT_TRANS_TIMEOUT");
_params_handles.mpc_xy_cruise = param_find("MPC_XY_CRUISE"); _params_handles.mpc_xy_cruise = param_find("MPC_XY_CRUISE");
_params_handles.fw_motors_off = param_find("VT_FW_MOT_OFFID"); _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 */ /* fetch initial parameter values */
parameters_update(); parameters_update();
@ -486,6 +488,10 @@ VtolAttitudeControl::parameters_update()
param_get(_params_handles.front_trans_timeout, &_params.front_trans_timeout); 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.mpc_xy_cruise, &_params.mpc_xy_cruise);
param_get(_params_handles.fw_motors_off, &_params.fw_motors_off); 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 // 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 // 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; 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 int
VtolAttitudeControl::task_main_trampoline(int argc, char *argv[]) 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)); _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
_mc_virtual_att_sp_sub = orb_subscribe(ORB_ID(mc_virtual_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)); _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_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub = orb_subscribe(ORB_ID(parameter_update)); _params_sub = orb_subscribe(ORB_ID(parameter_update));
@ -680,7 +638,6 @@ void VtolAttitudeControl::task_main()
// got data from mc attitude controller // got data from mc attitude controller
_vtol_type->update_mc_state(); _vtol_type->update_mc_state();
fill_mc_att_rates_sp();
} else if (_vtol_type->get_mode() == FIXED_WING) { } 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_vehicle_status.in_transition_to_fw = false;
_vtol_type->update_fw_state(); _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) { } 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_vehicle_status.in_transition_to_fw = (_vtol_type->get_mode() == TRANSITION_TO_FW);
_vtol_type->update_transition_state(); _vtol_type->update_transition_state();
fill_mc_att_rates_sp();
} }
_vtol_type->fill_actuator_outputs(); _vtol_type->fill_actuator_outputs();

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

@ -76,7 +76,6 @@
#include <uORB/topics/vehicle_local_position.h> #include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_local_position_setpoint.h> #include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/position_setpoint_triplet.h> #include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vtol_vehicle_status.h> #include <uORB/topics/vtol_vehicle_status.h>
#include "tiltrotor.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 _actuator_inputs_mc{-1}; //topic on which the mc_att_controller publishes actuator inputs
int _airspeed_sub{-1}; // airspeed subscription int _airspeed_sub{-1}; // airspeed subscription
int _fw_virtual_att_sp_sub{-1}; 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 _land_detected_sub{-1};
int _local_pos_sp_sub{-1}; // setpoint subscription int _local_pos_sp_sub{-1}; // setpoint subscription
int _local_pos_sub{-1}; // sensor subscription int _local_pos_sub{-1}; // sensor subscription
int _manual_control_sp_sub{-1}; //manual control setpoint subscription int _manual_control_sp_sub{-1}; //manual control setpoint subscription
int _mc_virtual_att_sp_sub{-1}; 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 _params_sub{-1}; //parameter updates subscription
int _pos_sp_triplet_sub{-1}; // local position setpoint subscription int _pos_sp_triplet_sub{-1}; // local position setpoint subscription
int _tecs_status_sub{-1}; int _tecs_status_sub{-1};
@ -148,7 +145,6 @@ private:
orb_advert_t _mavlink_log_pub{nullptr}; // mavlink log uORB handle orb_advert_t _mavlink_log_pub{nullptr}; // mavlink log uORB handle
orb_advert_t _v_att_sp_pub{nullptr}; orb_advert_t _v_att_sp_pub{nullptr};
orb_advert_t _v_cmd_ack_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 _vtol_vehicle_status_pub{nullptr};
orb_advert_t _actuators_1_pub{nullptr}; orb_advert_t _actuators_1_pub{nullptr};
@ -199,6 +195,8 @@ private:
param_t front_trans_timeout; param_t front_trans_timeout;
param_t mpc_xy_cruise; param_t mpc_xy_cruise;
param_t fw_motors_off; param_t fw_motors_off;
param_t diff_thrust;
param_t diff_thrust_scale;
} _params_handles{}; } _params_handles{};
/* for multicopters it is usual to have a non-zero idle speed of the engines /* 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_poll(); // Check for changes in sensor values
void vehicle_local_pos_sp_poll(); // Check for changes in setpoint values void vehicle_local_pos_sp_poll(); // Check for changes in setpoint values
int parameters_update(); //Update local paraemter cache int parameters_update(); //Update local parameter cache
void fill_mc_att_rates_sp();
void fill_fw_att_rates_sp();
void handle_command(); void handle_command();
}; };

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

@ -289,3 +289,39 @@ PARAM_DEFINE_FLOAT(VT_F_TR_OL_TM, 6.0f);
* @group VTOL Attitude Control * @group VTOL Attitude Control
*/ */
PARAM_DEFINE_INT32(VT_FW_MOT_OFFID, 0); 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
Unescape View File

@ -68,6 +68,8 @@ struct Params {
float front_trans_timeout; float front_trans_timeout;
float mpc_xy_cruise; float mpc_xy_cruise;
int32_t fw_motors_off; /**< bitmask of all motors that should be off in fixed wing mode */ 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 // Has to match 1:1 msg/vtol_vehicle_status.msg

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

@ -37,7 +37,7 @@ bool ControlMathTest::testThrAttMapping()
ut_assert_true(att.roll_body < SIGMA_SINGLE_OP); ut_assert_true(att.roll_body < SIGMA_SINGLE_OP);
ut_assert_true(att.pitch_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.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 /* expected: same as before but with 90 yaw
* reason: only yaw changed * reason: only yaw changed
@ -47,7 +47,7 @@ bool ControlMathTest::testThrAttMapping()
ut_assert_true(att.roll_body < SIGMA_SINGLE_OP); ut_assert_true(att.roll_body < SIGMA_SINGLE_OP);
ut_assert_true(att.pitch_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.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 /* expected: same as before but roll 180
* reason: thrust points straight down and order Euler * 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.roll_body) - M_PI_F < SIGMA_SINGLE_OP);
ut_assert_true(fabsf(att.pitch_body) < 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.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 */ /* TODO: find a good way to test it */

Write Preview
Loading…
Cancel
Save