From 90bfdb6f0a2832268baa4d91f88a9e3130a335ff Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Thu, 22 Nov 2018 02:32:40 +0100 Subject: [PATCH] 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 --- Tools/uorb_graph/create.py | 2 - msg/vehicle_attitude_setpoint.msg | 4 +- msg/vehicle_rates_setpoint.msg | 5 +- src/examples/rover_steering_control/main.cpp | 2 +- .../FixedwingAttitudeControl.cpp | 226 +++++++++++------- .../FixedwingAttitudeControl.hpp | 17 +- .../FixedwingPositionControl.cpp | 14 +- .../GroundRoverAttitudeControl.cpp | 2 +- .../GroundRoverPositionControl.cpp | 8 +- src/modules/mavlink/mavlink_messages.cpp | 2 +- src/modules/mavlink/mavlink_receiver.cpp | 7 +- src/modules/mc_att_control/mc_att_control.hpp | 4 +- .../mc_att_control/mc_att_control_main.cpp | 24 +- .../mc_pos_control/Utility/ControlMath.cpp | 2 +- .../mc_pos_control/mc_pos_control_main.cpp | 2 +- src/modules/navigator/gpsfailure.cpp | 2 +- src/modules/vtol_att_control/standard.cpp | 41 ++-- src/modules/vtol_att_control/standard.h | 22 +- src/modules/vtol_att_control/tailsitter.cpp | 201 +++++++--------- src/modules/vtol_att_control/tailsitter.h | 31 ++- src/modules/vtol_att_control/tiltrotor.cpp | 21 +- src/modules/vtol_att_control/tiltrotor.h | 25 +- .../vtol_att_control/tiltrotor_params.c | 25 -- .../vtol_att_control_main.cpp | 57 +---- .../vtol_att_control/vtol_att_control_main.h | 11 +- .../vtol_att_control_params.c | 36 +++ src/modules/vtol_att_control/vtol_type.h | 2 + src/systemcmds/tests/test_controlmath.cpp | 6 +- 28 files changed, 387 insertions(+), 414 deletions(-) diff --git a/Tools/uorb_graph/create.py b/Tools/uorb_graph/create.py index d5fb89b52a..7f4dd2c67a 100755 --- a/Tools/uorb_graph/create.py +++ b/Tools/uorb_graph/create.py @@ -246,10 +246,8 @@ class Graph: ('mc_pos_control', r'mc_pos_control_main\.cpp$', r'\b_attitude_setpoint_id=([^,)]+)', r'^_attitude_setpoint_id$'), - ('mc_att_control', r'mc_att_control_main\.cpp$', r'\b_rates_sp_id=([^,)]+)', r'^_rates_sp_id$'), ('mc_att_control', r'mc_att_control_main\.cpp$', r'\b_actuators_id=([^,)]+)', r'^_actuators_id$'), - ('fw_att_control', r'FixedwingAttitudeControl\.cpp$', r'\b_rates_sp_id=([^,)]+)', r'^_rates_sp_id$'), ('fw_att_control', r'FixedwingAttitudeControl\.cpp$', r'\b_actuators_id=([^,)]+)', r'^_actuators_id$'), ('fw_att_control', r'FixedwingAttitudeControl\.cpp$', r'\b_attitude_setpoint_id=([^,)]+)', r'^_attitude_setpoint_id$'), diff --git a/msg/vehicle_attitude_setpoint.msg b/msg/vehicle_attitude_setpoint.msg index da0b01b97f..9b5c5db015 100644 --- a/msg/vehicle_attitude_setpoint.msg +++ b/msg/vehicle_attitude_setpoint.msg @@ -12,7 +12,9 @@ float32 yaw_sp_move_rate # rad/s (commanded by user) float32[4] q_d # Desired quaternion for quaternion control bool q_d_valid # Set to true if quaternion vector is valid -float32 thrust # Thrust in Newton the power system should generate +# For clarification: For multicopters thrust_body[0] and thrust[1] are usually 0 and thrust[2] is the negative throttle demand. +# For fixed wings thrust_x is the throttle demand and thrust_y, thrust_z will usually be zero. +float32[3] thrust_body # Normalized thrust command in body NED frame [-1,1] bool roll_reset_integral # Reset roll integral part (navigation logic change) bool pitch_reset_integral # Reset pitch integral part (navigation logic change) diff --git a/msg/vehicle_rates_setpoint.msg b/msg/vehicle_rates_setpoint.msg index 14753f4be8..ae7a3c7822 100644 --- a/msg/vehicle_rates_setpoint.msg +++ b/msg/vehicle_rates_setpoint.msg @@ -3,6 +3,7 @@ uint64 timestamp # time since system start (microseconds) float32 roll # body angular rates in NED frame float32 pitch # body angular rates in NED frame float32 yaw # body angular rates in NED frame -float32 thrust # thrust normalized to 0..1 -# TOPICS vehicle_rates_setpoint mc_virtual_rates_setpoint fw_virtual_rates_setpoint +# For clarification: For multicopters thrust_body[0] and thrust[1] are usually 0 and thrust[2] is the negative throttle demand. +# For fixed wings thrust_x is the throttle demand and thrust_y, thrust_z will usually be zero. +float32[3] thrust_body # Normalized thrust command in body NED frame [-1,1] diff --git a/src/examples/rover_steering_control/main.cpp b/src/examples/rover_steering_control/main.cpp index d72f1cd9cb..723a371b05 100644 --- a/src/examples/rover_steering_control/main.cpp +++ b/src/examples/rover_steering_control/main.cpp @@ -182,7 +182,7 @@ void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const st actuators->control[2] = yaw_err * pp.yaw_p; /* copy throttle */ - actuators->control[3] = att_sp->thrust; + actuators->control[3] = att_sp->thrust_body[0]; actuators->timestamp = hrt_absolute_time(); } diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index 2511eef767..acc3e5bb9f 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -48,6 +48,9 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _nonfinite_input_perf(perf_alloc(PC_COUNT, "fwa_nani")), _nonfinite_output_perf(perf_alloc(PC_COUNT, "fwa_nano")) { + // check if VTOL first + vehicle_status_poll(); + _parameter_handles.p_tc = param_find("FW_P_TC"); _parameter_handles.p_p = param_find("FW_PR_P"); _parameter_handles.p_i = param_find("FW_PR_I"); @@ -116,13 +119,43 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : // initialize to invalid VTOL type _parameters.vtol_type = -1; + _parameters.vtol_airspeed_rule = 0; /* fetch initial parameter values */ parameters_update(); + + // set initial maximum body rate setpoints + _roll_ctrl.set_max_rate(_parameters.acro_max_x_rate_rad); + _pitch_ctrl.set_max_rate_pos(_parameters.acro_max_y_rate_rad); + _pitch_ctrl.set_max_rate_neg(_parameters.acro_max_y_rate_rad); + _yaw_ctrl.set_max_rate(_parameters.acro_max_z_rate_rad); + + // subscriptions + _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); + _params_sub = orb_subscribe(ORB_ID(parameter_update)); + _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); + _vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected)); + _battery_status_sub = orb_subscribe(ORB_ID(battery_status)); + _rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); } FixedwingAttitudeControl::~FixedwingAttitudeControl() { + orb_unsubscribe(_att_sub); + orb_unsubscribe(_att_sp_sub); + orb_unsubscribe(_vcontrol_mode_sub); + orb_unsubscribe(_params_sub); + orb_unsubscribe(_manual_sub); + orb_unsubscribe(_global_pos_sub); + orb_unsubscribe(_vehicle_status_sub); + orb_unsubscribe(_vehicle_land_detected_sub); + orb_unsubscribe(_battery_status_sub); + orb_unsubscribe(_rates_sp_sub); + perf_free(_loop_perf); perf_free(_nonfinite_input_perf); perf_free(_nonfinite_output_perf); @@ -131,7 +164,6 @@ FixedwingAttitudeControl::~FixedwingAttitudeControl() int FixedwingAttitudeControl::parameters_update() { - int32_t tmp = 0; param_get(_parameter_handles.p_tc, &(_parameters.p_tc)); param_get(_parameter_handles.p_p, &(_parameters.p_p)); @@ -209,6 +241,7 @@ FixedwingAttitudeControl::parameters_update() if (_vehicle_status.is_vtol) { param_get(_parameter_handles.vtol_type, &_parameters.vtol_type); + param_get(_parameter_handles.vtol_airspeed_rule, &_parameters.vtol_airspeed_rule); } param_get(_parameter_handles.bat_scale_en, &_parameters.bat_scale_en); @@ -249,13 +282,12 @@ FixedwingAttitudeControl::parameters_update() void FixedwingAttitudeControl::vehicle_control_mode_poll() { - bool vcontrol_mode_updated; + bool updated = false; /* Check if vehicle control mode has changed */ - orb_check(_vcontrol_mode_sub, &vcontrol_mode_updated); - - if (vcontrol_mode_updated) { + orb_check(_vcontrol_mode_sub, &updated); + if (updated) { orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &_vcontrol_mode); } } @@ -263,8 +295,8 @@ FixedwingAttitudeControl::vehicle_control_mode_poll() void FixedwingAttitudeControl::vehicle_manual_poll() { - // only update manual if in a manual mode - if (_vcontrol_mode.flag_control_manual_enabled) { + + if (_vcontrol_mode.flag_control_manual_enabled && !_vehicle_status.is_rotary_wing) { // Always copy the new manual setpoint, even if it wasn't updated, to fill the _actuators with valid values if (orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &_manual) == PX4_OK) { @@ -287,7 +319,7 @@ FixedwingAttitudeControl::vehicle_manual_poll() _att_sp.pitch_body = -_manual.x * _parameters.man_pitch_max + _parameters.pitchsp_offset_rad; _att_sp.pitch_body = math::constrain(_att_sp.pitch_body, -_parameters.man_pitch_max, _parameters.man_pitch_max); _att_sp.yaw_body = 0.0f; - _att_sp.thrust = _manual.z; + _att_sp.thrust_body[0] = _manual.z; Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body)); q.copyTo(_att_sp.q_d); @@ -310,15 +342,15 @@ FixedwingAttitudeControl::vehicle_manual_poll() _rates_sp.roll = _manual.y * _parameters.acro_max_x_rate_rad; _rates_sp.pitch = -_manual.x * _parameters.acro_max_y_rate_rad; _rates_sp.yaw = _manual.r * _parameters.acro_max_z_rate_rad; - _rates_sp.thrust = _manual.z; + _rates_sp.thrust_body[0] = _manual.z; if (_rate_sp_pub != nullptr) { /* publish the attitude rates setpoint */ - orb_publish(_rates_sp_id, _rate_sp_pub, &_rates_sp); + orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &_rates_sp); - } else if (_rates_sp_id) { + } else { /* advertise the attitude rates setpoint */ - _rate_sp_pub = orb_advertise(_rates_sp_id, &_rates_sp); + _rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_rates_sp); } } else { @@ -335,14 +367,36 @@ FixedwingAttitudeControl::vehicle_manual_poll() } void -FixedwingAttitudeControl::vehicle_setpoint_poll() +FixedwingAttitudeControl::vehicle_attitude_setpoint_poll() { /* check if there is a new setpoint */ - bool att_sp_updated; - orb_check(_att_sp_sub, &att_sp_updated); + bool updated = false; + orb_check(_att_sp_sub, &updated); - if (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; diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp index b4d7e14e28..d9c414f78d 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp @@ -95,6 +95,7 @@ private: int _att_sub{-1}; /**< vehicle attitude */ int _att_sp_sub{-1}; /**< vehicle attitude setpoint */ + int _rates_sp_sub{-1}; /**< vehicle rates setpoint */ int _battery_status_sub{-1}; /**< battery status subscription */ int _global_pos_sub{-1}; /**< global position subscription */ int _manual_sub{-1}; /**< notification of manual control updates */ @@ -109,7 +110,6 @@ private: orb_advert_t _actuators_2_pub{nullptr}; /**< actuator control group 1 setpoint (Airframe) */ orb_advert_t _rate_ctrl_status_pub{nullptr}; /**< rate controller status publication */ - orb_id_t _rates_sp_id{nullptr}; // pointer to correct rates setpoint uORB metadata structure orb_id_t _actuators_id{nullptr}; // pointer to correct actuator controls0 uORB metadata structure orb_id_t _attitude_setpoint_id{nullptr}; @@ -185,7 +185,7 @@ private: float pitchsp_offset_deg; /**< Pitch Setpoint Offset in deg */ float rollsp_offset_rad; /**< Roll Setpoint Offset in rad */ float pitchsp_offset_rad; /**< Pitch Setpoint Offset in rad */ - float man_roll_max; /**< Max Roll in rad */ + float man_roll_max; /**< Max Roll in rad */ float man_pitch_max; /**< Max Pitch in rad */ float man_roll_scale; /**< scale factor applied to roll actuator control in pure manual mode */ float man_pitch_scale; /**< scale factor applied to pitch actuator control in pure manual mode */ @@ -195,13 +195,14 @@ private: float acro_max_y_rate_rad; float acro_max_z_rate_rad; - float flaps_scale; /**< Scale factor for flaps */ - float flaps_takeoff_scale; /**< Scale factor for flaps on take-off */ + float flaps_scale; /**< Scale factor for flaps */ + float flaps_takeoff_scale; /**< Scale factor for flaps on take-off */ float flaperon_scale; /**< Scale factor for flaperons */ float rattitude_thres; - int32_t vtol_type; /**< VTOL type: 0 = tailsitter, 1 = tiltrotor */ + int32_t vtol_type; /**< VTOL type: 0 = tailsitter, 1 = tiltrotor */ + int32_t vtol_airspeed_rule; int32_t bat_scale_en; /**< Battery scaling enabled */ bool airspeed_disabled; @@ -271,6 +272,7 @@ private: param_t rattitude_thres; param_t vtol_type; + param_t vtol_airspeed_rule; param_t bat_scale_en; param_t airspeed_mode; @@ -291,9 +293,10 @@ private: void vehicle_control_mode_poll(); void vehicle_manual_poll(); - void vehicle_setpoint_poll(); + void vehicle_attitude_setpoint_poll(); + void vehicle_rates_setpoint_poll(); void global_pos_poll(); void vehicle_status_poll(); void vehicle_land_detected_poll(); - + void get_airspeed_and_scaling(float &airspeed, float &airspeed_scaling); }; diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 45314e9397..23a0ee172f 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -916,7 +916,7 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto } if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { - _att_sp.thrust = 0.0f; + _att_sp.thrust_body[0] = 0.0f; _att_sp.roll_body = 0.0f; _att_sp.pitch_body = 0.0f; @@ -1185,30 +1185,30 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto /* making sure again that the correct thrust is used, * without depending on library calls for safety reasons. the pre-takeoff throttle and the idle throttle normally map to the same parameter. */ - _att_sp.thrust = _parameters.throttle_idle; + _att_sp.thrust_body[0] = _parameters.throttle_idle; } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && _runway_takeoff.runwayTakeoffEnabled()) { - _att_sp.thrust = _runway_takeoff.getThrottle(min(get_tecs_thrust(), throttle_max)); + _att_sp.thrust_body[0] = _runway_takeoff.getThrottle(min(get_tecs_thrust(), throttle_max)); } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { - _att_sp.thrust = 0.0f; + _att_sp.thrust_body[0] = 0.0f; } else if (_control_mode_current == FW_POSCTRL_MODE_OTHER) { - _att_sp.thrust = min(_att_sp.thrust, _parameters.throttle_max); + _att_sp.thrust_body[0] = min(_att_sp.thrust_body[0], _parameters.throttle_max); } else { /* Copy thrust and pitch values from tecs */ if (_vehicle_land_detected.landed) { // when we are landed state we want the motor to spin at idle speed - _att_sp.thrust = min(_parameters.throttle_idle, throttle_max); + _att_sp.thrust_body[0] = min(_parameters.throttle_idle, throttle_max); } else { - _att_sp.thrust = min(get_tecs_thrust(), throttle_max); + _att_sp.thrust_body[0] = min(get_tecs_thrust(), throttle_max); } } diff --git a/src/modules/gnd_att_control/GroundRoverAttitudeControl.cpp b/src/modules/gnd_att_control/GroundRoverAttitudeControl.cpp index 5787052427..fe2f12b0e7 100644 --- a/src/modules/gnd_att_control/GroundRoverAttitudeControl.cpp +++ b/src/modules/gnd_att_control/GroundRoverAttitudeControl.cpp @@ -303,7 +303,7 @@ GroundRoverAttitudeControl::task_main() } /* throttle passed through if it is finite and if no engine failure was detected */ - _actuators.control[actuator_controls_s::INDEX_THROTTLE] = _att_sp.thrust; + _actuators.control[actuator_controls_s::INDEX_THROTTLE] = _att_sp.thrust_body[0]; /* scale effort by battery status */ if (_parameters.bat_scale_en && _battery_status.scale > 0.0f && diff --git a/src/modules/gnd_pos_control/GroundRoverPositionControl.cpp b/src/modules/gnd_pos_control/GroundRoverPositionControl.cpp index 8593b807a0..4960b788e9 100644 --- a/src/modules/gnd_pos_control/GroundRoverPositionControl.cpp +++ b/src/modules/gnd_pos_control/GroundRoverPositionControl.cpp @@ -261,7 +261,7 @@ GroundRoverPositionControl::control_position(const matrix::Vector2f ¤t_pos _att_sp.roll_body = 0.0f; _att_sp.pitch_body = 0.0f; _att_sp.yaw_body = 0.0f; - _att_sp.thrust = 0.0f; + _att_sp.thrust_body[0] = 0.0f; } else if ((pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) || (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF)) { @@ -272,7 +272,7 @@ GroundRoverPositionControl::control_position(const matrix::Vector2f ¤t_pos _att_sp.pitch_body = 0.0f; _att_sp.yaw_body = _gnd_control.nav_bearing(); _att_sp.fw_control_yaw = true; - _att_sp.thrust = mission_throttle; + _att_sp.thrust_body[0] = mission_throttle; } else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) { @@ -284,7 +284,7 @@ GroundRoverPositionControl::control_position(const matrix::Vector2f ¤t_pos _att_sp.pitch_body = 0.0f; _att_sp.yaw_body = _gnd_control.nav_bearing(); _att_sp.fw_control_yaw = true; - _att_sp.thrust = 0.0f; + _att_sp.thrust_body[0] = 0.0f; } if (was_circle_mode && !_gnd_control.circle_mode()) { @@ -299,7 +299,7 @@ GroundRoverPositionControl::control_position(const matrix::Vector2f ¤t_pos _att_sp.pitch_body = 0.0f; _att_sp.yaw_body = 0.0f; _att_sp.fw_control_yaw = true; - _att_sp.thrust = 0.0f; + _att_sp.thrust_body[0] = 0.0f; /* do not publish the setpoint */ setpoint = false; diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 7817e884c8..5ae3b873c1 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -3138,7 +3138,7 @@ protected: msg.body_pitch_rate = att_rates_sp.pitch; msg.body_yaw_rate = att_rates_sp.yaw; - msg.thrust = att_sp.thrust; + msg.thrust = att_sp.thrust_body[0]; mavlink_msg_attitude_target_send_struct(_mavlink->get_channel(), &msg); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index b062aa9f08..c7875f37c1 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1447,8 +1447,11 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) att_sp.yaw_sp_move_rate = 0.0f; } + // TODO: We assume offboard is only used for multicopters which produce thrust along the + // body z axis. If we want to support fixed wing as well we need to handle it differently here, e.g. + // in that case we should assign att_sp.thrust_body[0] if (!_offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid - att_sp.thrust = set_attitude_target.thrust; + att_sp.thrust_body[2] = -set_attitude_target.thrust; } if (_att_sp_pub == nullptr) { @@ -1472,7 +1475,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) } if (!_offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid - rates_sp.thrust = set_attitude_target.thrust; + rates_sp.thrust_body[2] = -set_attitude_target.thrust; } if (_rates_sp_pub == nullptr) { diff --git a/src/modules/mc_att_control/mc_att_control.hpp b/src/modules/mc_att_control/mc_att_control.hpp index eb95da8d18..c3dad3c7ab 100644 --- a/src/modules/mc_att_control/mc_att_control.hpp +++ b/src/modules/mc_att_control/mc_att_control.hpp @@ -166,7 +166,6 @@ private: orb_advert_t _controller_status_pub{nullptr}; /**< controller status publication */ orb_advert_t _vehicle_attitude_setpoint_pub{nullptr}; - orb_id_t _rates_sp_id{nullptr}; /**< pointer to correct rates setpoint uORB metadata structure */ orb_id_t _actuators_id{nullptr}; /**< pointer to correct actuator controls0 uORB metadata structure */ bool _actuators_0_circuit_breaker_enabled{false}; /**< circuit breaker to suppress output */ @@ -196,8 +195,9 @@ private: matrix::Vector3f _rates_prev_filtered; /**< angular rates on previous step (low-pass filtered) */ matrix::Vector3f _rates_sp; /**< angular rates setpoint */ matrix::Vector3f _rates_int; /**< angular rates integral error */ - float _thrust_sp; /**< thrust setpoint */ + matrix::Vector3f _att_control; /**< attitude control vector */ + float _thrust_sp{0.0f}; /**< thrust setpoint */ matrix::Dcmf _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index c4858a5db3..020096ac63 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -273,13 +273,11 @@ MulticopterAttitudeControl::vehicle_status_poll() orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); /* set correct uORB ID, depending on if vehicle is VTOL or not */ - if (_rates_sp_id == nullptr) { + if (_actuators_id == nullptr) { if (_vehicle_status.is_vtol) { - _rates_sp_id = ORB_ID(mc_virtual_rates_setpoint); _actuators_id = ORB_ID(actuator_controls_virtual_mc); } else { - _rates_sp_id = ORB_ID(vehicle_rates_setpoint); _actuators_id = ORB_ID(actuator_controls_0); } } @@ -508,7 +506,7 @@ MulticopterAttitudeControl::generate_attitude_setpoint(float dt, bool reset_yaw_ q_sp.copyTo(attitude_setpoint.q_d); attitude_setpoint.q_d_valid = true; - attitude_setpoint.thrust = throttle_curve(_manual_control_sp.z); + attitude_setpoint.thrust_body[2] = -throttle_curve(_manual_control_sp.z); attitude_setpoint.landing_gear = get_landing_gear_state(); attitude_setpoint.timestamp = hrt_absolute_time(); @@ -524,7 +522,9 @@ void MulticopterAttitudeControl::control_attitude() { vehicle_attitude_setpoint_poll(); - _thrust_sp = _v_att_sp.thrust; + + // physical thrust axis is the negative of body z axis + _thrust_sp = -_v_att_sp.thrust_body[2]; /* prepare yaw weight from the ratio between roll/pitch and yaw gains */ Vector3f attitude_gain = _attitude_p; @@ -607,7 +607,7 @@ Vector3f MulticopterAttitudeControl::pid_attenuations(float tpa_breakpoint, float tpa_rate) { /* throttle pid attenuation factor */ - float tpa = 1.0f - tpa_rate * (fabsf(_v_rates_sp.thrust) - tpa_breakpoint) / (1.0f - tpa_breakpoint); + float tpa = 1.0f - tpa_rate * (fabsf(_thrust_sp) - tpa_breakpoint) / (1.0f - tpa_breakpoint); tpa = fmaxf(TPA_RATE_LOWER_LIMIT, fminf(1.0f, tpa)); Vector3f pidAttenuationPerAxis; @@ -731,9 +731,11 @@ MulticopterAttitudeControl::publish_rates_setpoint() _v_rates_sp.roll = _rates_sp(0); _v_rates_sp.pitch = _rates_sp(1); _v_rates_sp.yaw = _rates_sp(2); - _v_rates_sp.thrust = _thrust_sp; + _v_rates_sp.thrust_body[0] = 0.0f; + _v_rates_sp.thrust_body[1] = 0.0f; + _v_rates_sp.thrust_body[2] = -_thrust_sp; _v_rates_sp.timestamp = hrt_absolute_time(); - orb_publish_auto(_rates_sp_id, &_v_rates_sp_pub, &_v_rates_sp, nullptr, ORB_PRIO_DEFAULT); + orb_publish_auto(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp_pub, &_v_rates_sp, nullptr, ORB_PRIO_DEFAULT); } void @@ -886,7 +888,7 @@ MulticopterAttitudeControl::run() bool attitude_setpoint_generated = false; - if (_v_control_mode.flag_control_attitude_enabled) { + if (_v_control_mode.flag_control_attitude_enabled && _vehicle_status.is_rotary_wing) { if (attitude_updated) { // Generate the attitude setpoint from stick inputs if we are in Manual/Stabilized mode if (_v_control_mode.flag_control_manual_enabled && @@ -903,7 +905,7 @@ MulticopterAttitudeControl::run() } else { /* attitude controller disabled, poll rates setpoint topic */ - if (_v_control_mode.flag_control_manual_enabled) { + if (_v_control_mode.flag_control_manual_enabled && _vehicle_status.is_rotary_wing) { if (manual_control_updated) { /* manual rates control - ACRO mode */ Vector3f man_rate_sp( @@ -921,7 +923,7 @@ MulticopterAttitudeControl::run() _rates_sp(0) = _v_rates_sp.roll; _rates_sp(1) = _v_rates_sp.pitch; _rates_sp(2) = _v_rates_sp.yaw; - _thrust_sp = _v_rates_sp.thrust; + _thrust_sp = -_v_rates_sp.thrust_body[2]; } } } diff --git a/src/modules/mc_pos_control/Utility/ControlMath.cpp b/src/modules/mc_pos_control/Utility/ControlMath.cpp index f5d28d9dbe..b3cfa8afa1 100644 --- a/src/modules/mc_pos_control/Utility/ControlMath.cpp +++ b/src/modules/mc_pos_control/Utility/ControlMath.cpp @@ -103,7 +103,7 @@ vehicle_attitude_setpoint_s thrustToAttitude(const Vector3f &thr_sp, const float Eulerf euler = R_sp; att_sp.roll_body = euler(0); att_sp.pitch_body = euler(1); - att_sp.thrust = thr_sp.length(); + att_sp.thrust_body[2] = -thr_sp.length(); return att_sp; } diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 0b50c5d7b4..94e1380cc3 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -827,7 +827,7 @@ MulticopterPositionControl::run() matrix::Quatf q_sp = matrix::Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body); q_sp.copyTo(_att_sp.q_d); _att_sp.q_d_valid = true; - _att_sp.thrust = 0.0f; + _att_sp.thrust_body[2] = 0.0f; } } diff --git a/src/modules/navigator/gpsfailure.cpp b/src/modules/navigator/gpsfailure.cpp index d51e543384..69b37c793f 100644 --- a/src/modules/navigator/gpsfailure.cpp +++ b/src/modules/navigator/gpsfailure.cpp @@ -86,7 +86,7 @@ GpsFailure::on_active() att_sp.timestamp = hrt_absolute_time(); att_sp.roll_body = math::radians(_param_openlooploiter_roll.get()); att_sp.pitch_body = math::radians(_param_openlooploiter_pitch.get()); - att_sp.thrust = _param_openlooploiter_thrust.get(); + att_sp.thrust_body[0] = _param_openlooploiter_thrust.get(); Quatf q(Eulerf(att_sp.roll_body, att_sp.pitch_body, 0.0f)); q.copyTo(att_sp.q_d); diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index 506f810f40..7f37bc2f69 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -46,13 +46,10 @@ #include -using matrix::wrap_pi; +using namespace matrix; Standard::Standard(VtolAttitudeControl *attc) : - VtolType(attc), - _pusher_throttle(0.0f), - _reverse_output(0.0f), - _airspeed_trans_blend_margin(0.0f) + VtolType(attc) { _vtol_schedule.flight_mode = MC_MODE; _vtol_schedule.transition_start = 0; @@ -156,8 +153,8 @@ void Standard::update_vtol_state() } else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) { // transition to MC mode if transition time has passed or forward velocity drops below MPC cruise speed - const matrix::Dcmf R_to_body(matrix::Quatf(_v_att->q).inversed()); - const matrix::Vector3f vel = R_to_body * matrix::Vector3f(_local_pos->vx, _local_pos->vy, _local_pos->vz); + const Dcmf R_to_body(Quatf(_v_att->q).inversed()); + const Vector3f vel = R_to_body * Vector3f(_local_pos->vx, _local_pos->vy, _local_pos->vz); float x_vel = vel(0); @@ -261,7 +258,7 @@ void Standard::update_transition_state() // ramp up FW_PSP_OFF _v_att_sp->pitch_body = _params_standard.pitch_setpoint_offset * (1.0f - mc_weight); - matrix::Quatf q_sp(matrix::Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body)); + const Quatf q_sp(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body)); q_sp.copyTo(_v_att_sp->q_d); _v_att_sp->q_d_valid = true; @@ -277,7 +274,7 @@ void Standard::update_transition_state() // maintain FW_PSP_OFF _v_att_sp->pitch_body = _params_standard.pitch_setpoint_offset; - matrix::Quatf q_sp(matrix::Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body)); + const Quatf q_sp(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body)); q_sp.copyTo(_v_att_sp->q_d); _v_att_sp->q_d_valid = true; @@ -333,18 +330,18 @@ void Standard::update_mc_state() return; } - matrix::Dcmf R(matrix::Quatf(_v_att->q)); - matrix::Dcmf R_sp(matrix::Quatf(_v_att_sp->q_d)); - matrix::Eulerf euler(R); - matrix::Eulerf euler_sp(R_sp); + const Dcmf R(Quatf(_v_att->q)); + const Dcmf R_sp(Quatf(_v_att_sp->q_d)); + const Eulerf euler(R); + const Eulerf euler_sp(R_sp); _pusher_throttle = 0.0f; // direction of desired body z axis represented in earth frame - matrix::Vector3f body_z_sp(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2)); + Vector3f body_z_sp(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2)); // rotate desired body z axis into new frame which is rotated in z by the current // heading of the vehicle. we refer to this as the heading frame. - matrix::Dcmf R_yaw = matrix::Eulerf(0.0f, 0.0f, -euler(2)); + Dcmf R_yaw = Eulerf(0.0f, 0.0f, -euler(2)); body_z_sp = R_yaw * body_z_sp; body_z_sp.normalize(); @@ -365,25 +362,25 @@ void Standard::update_mc_state() float pitch_new = 0.0f; // create corrected desired body z axis in heading frame - matrix::Dcmf R_tmp = matrix::Eulerf(roll_new, pitch_new, 0.0f); - matrix::Vector3f tilt_new(R_tmp(0, 2), R_tmp(1, 2), R_tmp(2, 2)); + const Dcmf R_tmp = Eulerf(roll_new, pitch_new, 0.0f); + Vector3f tilt_new(R_tmp(0, 2), R_tmp(1, 2), R_tmp(2, 2)); // rotate the vector into a new frame which is rotated in z by the desired heading // with respect to the earh frame. const float yaw_error = wrap_pi(euler_sp(2) - euler(2)); - matrix::Dcmf R_yaw_correction = matrix::Eulerf(0.0f, 0.0f, -yaw_error); + const Dcmf R_yaw_correction = Eulerf(0.0f, 0.0f, -yaw_error); tilt_new = R_yaw_correction * tilt_new; // now extract roll and pitch setpoints _v_att_sp->pitch_body = atan2f(tilt_new(0), tilt_new(2)); _v_att_sp->roll_body = -asinf(tilt_new(1)); - R_sp = matrix::Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, euler_sp(2)); - matrix::Quatf q_sp(R_sp); + + const Quatf q_sp(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, euler_sp(2))); q_sp.copyTo(_v_att_sp->q_d); + _v_att_sp->q_d_valid = true; } _pusher_throttle = _pusher_throttle < 0.0f ? 0.0f : _pusher_throttle; - } void Standard::update_fw_state() @@ -475,5 +472,5 @@ void Standard::waiting_on_tecs() { // keep thrust from transition - _v_att_sp->thrust = _pusher_throttle; + _v_att_sp->thrust_body[0] = _pusher_throttle; }; diff --git a/src/modules/vtol_att_control/standard.h b/src/modules/vtol_att_control/standard.h index 884753fa6f..27ada9c8e6 100644 --- a/src/modules/vtol_att_control/standard.h +++ b/src/modules/vtol_att_control/standard.h @@ -55,14 +55,14 @@ class Standard : public VtolType public: Standard(VtolAttitudeControl *_att_controller); - ~Standard() = default; + ~Standard() override = default; - virtual void update_vtol_state(); - virtual void update_transition_state(); - virtual void update_fw_state(); - virtual void update_mc_state(); - virtual void fill_actuator_outputs(); - virtual void waiting_on_tecs(); + void update_vtol_state() override; + void update_transition_state() override; + void update_fw_state() override; + void update_mc_state() override; + void fill_actuator_outputs() override; + void waiting_on_tecs() override; private: @@ -98,10 +98,10 @@ private: hrt_abstime transition_start; // at what time did we start a transition (front- or backtransition) } _vtol_schedule; - float _pusher_throttle; - float _reverse_output; - float _airspeed_trans_blend_margin; + float _pusher_throttle{0.0f}; + float _reverse_output{0.0f}; + float _airspeed_trans_blend_margin{0.0f}; - virtual void parameters_update(); + void parameters_update() override; }; #endif diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index be21121bc6..974f1c2826 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -47,11 +47,10 @@ #define PITCH_TRANSITION_FRONT_P1 -1.1f // pitch angle to switch to TRANSITION_P2 #define PITCH_TRANSITION_BACK -0.25f // pitch angle to switch to MC +using namespace matrix; + Tailsitter::Tailsitter(VtolAttitudeControl *attc) : - VtolType(attc), - _thrust_transition_start(0.0f), - _yaw_transition(0.0f), - _pitch_transition_start(0.0f) + VtolType(attc) { _vtol_schedule.flight_mode = MC_MODE; _vtol_schedule.transition_start = 0; @@ -63,6 +62,7 @@ Tailsitter::Tailsitter(VtolAttitudeControl *attc) : _flag_was_in_trans_mode = false; _params_handles_tailsitter.front_trans_dur_p2 = param_find("VT_TRANS_P2_DUR"); + _params_handles_tailsitter.fw_pitch_sp_offset = param_find("FW_PSP_OFF"); } void @@ -73,18 +73,20 @@ Tailsitter::parameters_update() /* vtol front transition phase 2 duration */ param_get(_params_handles_tailsitter.front_trans_dur_p2, &v); _params_tailsitter.front_trans_dur_p2 = v; + + param_get(_params_handles_tailsitter.fw_pitch_sp_offset, &v); + _params_tailsitter.fw_pitch_sp_offset = math::radians(v); } void Tailsitter::update_vtol_state() { - /* simple logic using a two way switch to perform transitions. * after flipping the switch the vehicle will start tilting in MC control mode, picking up * forward speed. After the vehicle has picked up enough and sufficient pitch angle the uav will go into FW mode. * For the backtransition the pitch is controlled in MC mode again and switches to full MC control reaching the sufficient pitch angle. */ - matrix::Eulerf euler = matrix::Quatf(_v_att->q); + Eulerf euler = Quatf(_v_att->q); float pitch = euler.theta(); if (!_attc->is_fixed_wing_requested()) { @@ -104,9 +106,10 @@ void Tailsitter::update_vtol_state() break; case TRANSITION_BACK: + float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f; // check if we have reached pitch angle to switch to MC mode - if (pitch >= PITCH_TRANSITION_BACK) { + if (pitch >= PITCH_TRANSITION_BACK || time_since_trans_start > _params->back_trans_duration) { _vtol_schedule.flight_mode = MC_MODE; } @@ -176,89 +179,94 @@ void Tailsitter::update_transition_state() float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f; if (!_flag_was_in_trans_mode) { - // save desired heading for transition and last thrust value - _yaw_transition = _v_att_sp->yaw_body; - //transition should start from current attitude instead of current setpoint - matrix::Eulerf euler = matrix::Quatf(_v_att->q); - _pitch_transition_start = euler.theta(); - _thrust_transition_start = _v_att_sp->thrust; _flag_was_in_trans_mode = true; + + if (_vtol_schedule.flight_mode == TRANSITION_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]; } } diff --git a/src/modules/vtol_att_control/tailsitter.h b/src/modules/vtol_att_control/tailsitter.h index b65a4ef713..da5715b2e0 100644 --- a/src/modules/vtol_att_control/tailsitter.h +++ b/src/modules/vtol_att_control/tailsitter.h @@ -46,30 +46,32 @@ #include /** is it necsacery? **/ #include #include +#include class Tailsitter : public VtolType { public: Tailsitter(VtolAttitudeControl *_att_controller); - ~Tailsitter() = default; + ~Tailsitter() override = default; - virtual void update_vtol_state(); - virtual void update_transition_state(); - virtual void update_mc_state(); - virtual void update_fw_state(); - virtual void fill_actuator_outputs(); - virtual void waiting_on_tecs(); + void update_vtol_state() override; + void update_transition_state() override; + void update_fw_state() override; + void fill_actuator_outputs() override; + void waiting_on_tecs() override; private: struct { float front_trans_dur_p2; - } _params_tailsitter; + float fw_pitch_sp_offset; + } _params_tailsitter{}; struct { param_t front_trans_dur_p2; - } _params_handles_tailsitter; + param_t fw_pitch_sp_offset; + } _params_handles_tailsitter{}; enum vtol_mode { MC_MODE = 0, /**< vtol is in multicopter mode */ @@ -83,14 +85,11 @@ private: hrt_abstime transition_start; /**< absoulte time at which front transition started */ } _vtol_schedule; - float _thrust_transition_start; // throttle value when we start the front transition - float _yaw_transition; // yaw angle in which transition will take place - float _pitch_transition_start; // pitch angle at the start of transition (tailsitter) + matrix::Quatf _q_trans_start; + matrix::Quatf _q_trans_sp; + matrix::Vector3f _trans_rot_axis; - /** - * Update parameters. - */ - virtual void parameters_update(); + void parameters_update() override; }; #endif diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index c6424ed4e4..04a58a46de 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -45,8 +45,7 @@ #define ARSP_YAW_CTRL_DISABLE 7.0f // airspeed at which we stop controlling yaw during a front transition Tiltrotor::Tiltrotor(VtolAttitudeControl *attc) : - VtolType(attc), - _tilt_control(0.0f) + VtolType(attc) { _vtol_schedule.flight_mode = MC_MODE; _vtol_schedule.transition_start = 0; @@ -61,8 +60,6 @@ Tiltrotor::Tiltrotor(VtolAttitudeControl *attc) : _params_handles_tiltrotor.tilt_transition = param_find("VT_TILT_TRANS"); _params_handles_tiltrotor.tilt_fw = param_find("VT_TILT_FW"); _params_handles_tiltrotor.front_trans_dur_p2 = param_find("VT_TRANS_P2_DUR"); - _params_handles_tiltrotor.diff_thrust = param_find("VT_FW_DIFTHR_EN"); - _params_handles_tiltrotor.diff_thrust_scale = param_find("VT_FW_DIFTHR_SC"); } void @@ -85,16 +82,10 @@ Tiltrotor::parameters_update() /* vtol front transition phase 2 duration */ param_get(_params_handles_tiltrotor.front_trans_dur_p2, &v); _params_tiltrotor.front_trans_dur_p2 = v; - - param_get(_params_handles_tiltrotor.diff_thrust, &_params_tiltrotor.diff_thrust); - - param_get(_params_handles_tiltrotor.diff_thrust_scale, &v); - _params_tiltrotor.diff_thrust_scale = math::constrain(v, -1.0f, 1.0f); } void Tiltrotor::update_vtol_state() { - /* simple logic using a two way switch to perform transitions. * after flipping the switch the vehicle will start tilting rotors, picking up * forward speed. After the vehicle has picked up enough speed the rotors are tilted @@ -270,7 +261,7 @@ void Tiltrotor::update_transition_state() _mc_yaw_weight = _mc_roll_weight; } - _thrust_transition = _mc_virtual_att_sp->thrust; + _thrust_transition = -_mc_virtual_att_sp->thrust_body[2]; } else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P2) { // the plane is ready to go into fixed wing mode, tilt the rotors forward completely @@ -289,7 +280,7 @@ void Tiltrotor::update_transition_state() _motor_state = set_motor_state(_motor_state, VALUE, rear_value); - _thrust_transition = _mc_virtual_att_sp->thrust; + _thrust_transition = -_mc_virtual_att_sp->thrust_body[2]; } else if (_vtol_schedule.flight_mode == TRANSITION_BACK) { if (_motor_state != ENABLED) { @@ -335,7 +326,7 @@ void Tiltrotor::update_transition_state() void Tiltrotor::waiting_on_tecs() { // keep multicopter thrust until we get data from TECS - _v_att_sp->thrust = _thrust_transition; + _v_att_sp->thrust_body[0] = _thrust_transition; } /** @@ -358,9 +349,9 @@ void Tiltrotor::fill_actuator_outputs() _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; /* allow differential thrust if enabled */ - if (_params_tiltrotor.diff_thrust == 1) { + if (_params->diff_thrust == 1) { _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = - _actuators_fw_in->control[actuator_controls_s::INDEX_YAW] * _params_tiltrotor.diff_thrust_scale; + _actuators_fw_in->control[actuator_controls_s::INDEX_YAW] * _params->diff_thrust_scale; } } else { diff --git a/src/modules/vtol_att_control/tiltrotor.h b/src/modules/vtol_att_control/tiltrotor.h index 508973642a..edfe89dd4f 100644 --- a/src/modules/vtol_att_control/tiltrotor.h +++ b/src/modules/vtol_att_control/tiltrotor.h @@ -50,14 +50,14 @@ class Tiltrotor : public VtolType public: Tiltrotor(VtolAttitudeControl *_att_controller); - ~Tiltrotor() = default; + ~Tiltrotor() override = default; - virtual void update_vtol_state(); - virtual void update_transition_state(); - virtual void fill_actuator_outputs(); - virtual void update_mc_state(); - virtual void update_fw_state(); - virtual void waiting_on_tecs(); + void update_vtol_state() override; + void update_transition_state() override; + void fill_actuator_outputs() override; + void update_mc_state() override; + void update_fw_state() override; + void waiting_on_tecs() override; private: @@ -66,8 +66,6 @@ private: float tilt_transition; /**< actuator value corresponding to transition tilt (e.g 45 degrees) */ float tilt_fw; /**< actuator value corresponding to fw tilt */ float front_trans_dur_p2; - int32_t diff_thrust; - float diff_thrust_scale; } _params_tiltrotor; struct { @@ -75,8 +73,6 @@ private: param_t tilt_transition; param_t tilt_fw; param_t front_trans_dur_p2; - param_t diff_thrust; - param_t diff_thrust_scale; } _params_handles_tiltrotor; enum vtol_mode { @@ -99,12 +95,9 @@ private: hrt_abstime transition_start; /**< absoulte time at which front transition started */ } _vtol_schedule; - float _tilt_control; /**< actuator value for the tilt servo */ + float _tilt_control{0.0f}; /**< actuator value for the tilt servo */ - /** - * Update parameters. - */ - virtual void parameters_update(); + void parameters_update() override; }; #endif diff --git a/src/modules/vtol_att_control/tiltrotor_params.c b/src/modules/vtol_att_control/tiltrotor_params.c index d3cb6df264..bf57b9687e 100644 --- a/src/modules/vtol_att_control/tiltrotor_params.c +++ b/src/modules/vtol_att_control/tiltrotor_params.c @@ -85,28 +85,3 @@ PARAM_DEFINE_FLOAT(VT_TILT_FW, 1.0f); * @group VTOL Attitude Control */ PARAM_DEFINE_FLOAT(VT_TRANS_P2_DUR, 0.5f); - -/** - * Differential thrust in forwards flight. - * - * Set to 1 to enable differential thrust in fixed-wing flight. - * - * @min 0 - * @max 1 - * @decimal 0 - * @group VTOL Attitude Control - */ -PARAM_DEFINE_INT32(VT_FW_DIFTHR_EN, 0); - -/** - * Differential thrust scaling factor - * - * This factor specifies how the yaw input gets mapped to differential thrust in forwards flight. - * - * @min 0.0 - * @max 1.0 - * @decimal 2 - * @increment 0.1 - * @group VTOL Attitude Control - */ -PARAM_DEFINE_FLOAT(VT_FW_DIFTHR_SC, 0.1f); diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index a6e673b50a..1180875cc2 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -86,6 +86,8 @@ VtolAttitudeControl::VtolAttitudeControl() _params_handles.front_trans_timeout = param_find("VT_TRANS_TIMEOUT"); _params_handles.mpc_xy_cruise = param_find("MPC_XY_CRUISE"); _params_handles.fw_motors_off = param_find("VT_FW_MOT_OFFID"); + _params_handles.diff_thrust = param_find("VT_FW_DIFTHR_EN"); + _params_handles.diff_thrust_scale = param_find("VT_FW_DIFTHR_SC"); /* fetch initial parameter values */ parameters_update(); @@ -486,6 +488,10 @@ VtolAttitudeControl::parameters_update() param_get(_params_handles.front_trans_timeout, &_params.front_trans_timeout); param_get(_params_handles.mpc_xy_cruise, &_params.mpc_xy_cruise); param_get(_params_handles.fw_motors_off, &_params.fw_motors_off); + param_get(_params_handles.diff_thrust, &_params.diff_thrust); + + param_get(_params_handles.diff_thrust_scale, &v); + _params.diff_thrust_scale = math::constrain(v, -1.0f, 1.0f); // standard vtol always needs to turn all mc motors off when going into fixed wing mode // normally the parameter fw_motors_off can be used to specify this, however, since historically standard vtol code @@ -506,52 +512,6 @@ VtolAttitudeControl::parameters_update() return OK; } -/** -* Prepare message for mc attitude rates setpoint topic -*/ -void VtolAttitudeControl::fill_mc_att_rates_sp() -{ - bool updated; - orb_check(_mc_virtual_v_rates_sp_sub, &updated); - - if (updated) { - vehicle_rates_setpoint_s v_rates_sp; - - if (orb_copy(ORB_ID(mc_virtual_rates_setpoint), _mc_virtual_v_rates_sp_sub, &v_rates_sp) == PX4_OK) { - // publish the attitude rates setpoint - if (_v_rates_sp_pub != nullptr) { - orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &v_rates_sp); - - } else { - _v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &v_rates_sp); - } - } - } -} - -/** -* Prepare message for fw attitude rates setpoint topic -*/ -void VtolAttitudeControl::fill_fw_att_rates_sp() -{ - bool updated; - orb_check(_fw_virtual_v_rates_sp_sub, &updated); - - if (updated) { - vehicle_rates_setpoint_s v_rates_sp; - - if (orb_copy(ORB_ID(fw_virtual_rates_setpoint), _fw_virtual_v_rates_sp_sub, &v_rates_sp) == PX4_OK) { - // publish the attitude rates setpoint - if (_v_rates_sp_pub != nullptr) { - orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &v_rates_sp); - - } else { - _v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &v_rates_sp); - } - } - } -} - int VtolAttitudeControl::task_main_trampoline(int argc, char *argv[]) { @@ -567,8 +527,6 @@ void VtolAttitudeControl::task_main() _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); _mc_virtual_att_sp_sub = orb_subscribe(ORB_ID(mc_virtual_attitude_setpoint)); _fw_virtual_att_sp_sub = orb_subscribe(ORB_ID(fw_virtual_attitude_setpoint)); - _mc_virtual_v_rates_sp_sub = orb_subscribe(ORB_ID(mc_virtual_rates_setpoint)); - _fw_virtual_v_rates_sp_sub = orb_subscribe(ORB_ID(fw_virtual_rates_setpoint)); _v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); _v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); @@ -680,7 +638,6 @@ void VtolAttitudeControl::task_main() // got data from mc attitude controller _vtol_type->update_mc_state(); - fill_mc_att_rates_sp(); } else if (_vtol_type->get_mode() == FIXED_WING) { @@ -692,7 +649,6 @@ void VtolAttitudeControl::task_main() _vtol_vehicle_status.in_transition_to_fw = false; _vtol_type->update_fw_state(); - fill_fw_att_rates_sp(); } else if (_vtol_type->get_mode() == TRANSITION_TO_MC || _vtol_type->get_mode() == TRANSITION_TO_FW) { @@ -705,7 +661,6 @@ void VtolAttitudeControl::task_main() _vtol_vehicle_status.in_transition_to_fw = (_vtol_type->get_mode() == TRANSITION_TO_FW); _vtol_type->update_transition_state(); - fill_mc_att_rates_sp(); } _vtol_type->fill_actuator_outputs(); diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index 4429886664..40dbeec9f4 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -76,7 +76,6 @@ #include #include #include -#include #include #include "tiltrotor.h" @@ -128,13 +127,11 @@ private: int _actuator_inputs_mc{-1}; //topic on which the mc_att_controller publishes actuator inputs int _airspeed_sub{-1}; // airspeed subscription int _fw_virtual_att_sp_sub{-1}; - int _fw_virtual_v_rates_sp_sub{-1}; //vehicle rates setpoint subscription int _land_detected_sub{-1}; int _local_pos_sp_sub{-1}; // setpoint subscription int _local_pos_sub{-1}; // sensor subscription int _manual_control_sp_sub{-1}; //manual control setpoint subscription int _mc_virtual_att_sp_sub{-1}; - int _mc_virtual_v_rates_sp_sub{-1}; //vehicle rates setpoint subscription int _params_sub{-1}; //parameter updates subscription int _pos_sp_triplet_sub{-1}; // local position setpoint subscription int _tecs_status_sub{-1}; @@ -148,7 +145,6 @@ private: orb_advert_t _mavlink_log_pub{nullptr}; // mavlink log uORB handle orb_advert_t _v_att_sp_pub{nullptr}; orb_advert_t _v_cmd_ack_pub{nullptr}; - orb_advert_t _v_rates_sp_pub{nullptr}; orb_advert_t _vtol_vehicle_status_pub{nullptr}; orb_advert_t _actuators_1_pub{nullptr}; @@ -199,6 +195,8 @@ private: param_t front_trans_timeout; param_t mpc_xy_cruise; param_t fw_motors_off; + param_t diff_thrust; + param_t diff_thrust_scale; } _params_handles{}; /* for multicopters it is usual to have a non-zero idle speed of the engines @@ -229,10 +227,7 @@ private: void vehicle_local_pos_poll(); // Check for changes in sensor values void vehicle_local_pos_sp_poll(); // Check for changes in setpoint values - int parameters_update(); //Update local paraemter cache - - void fill_mc_att_rates_sp(); - void fill_fw_att_rates_sp(); + int parameters_update(); //Update local parameter cache void handle_command(); }; diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c index f27da247d4..e8af9384fd 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -289,3 +289,39 @@ PARAM_DEFINE_FLOAT(VT_F_TR_OL_TM, 6.0f); * @group VTOL Attitude Control */ PARAM_DEFINE_INT32(VT_FW_MOT_OFFID, 0); + +/** + * Differential thrust in forwards flight. + * + * Set to 1 to enable differential thrust in fixed-wing flight. + * + * @min 0 + * @max 1 + * @decimal 0 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_INT32(VT_FW_DIFTHR_EN, 0); + +/** + * Differential thrust scaling factor + * + * This factor specifies how the yaw input gets mapped to differential thrust in forwards flight. + * + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.1 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_FW_DIFTHR_SC, 0.1f); + +/** + * Airspeed rules regarding fixed wing control surface scaling. + * + * @value 0 No special rules + * @value 1 During hover (excluding transitions) use the lowest possible airspeed value. + * @min 0.0 + * @increment 1 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_INT32(VT_AIRSPD_RULE, 0); diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index 1a10d4c108..230e615bdd 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -68,6 +68,8 @@ struct Params { float front_trans_timeout; float mpc_xy_cruise; int32_t fw_motors_off; /**< bitmask of all motors that should be off in fixed wing mode */ + int32_t diff_thrust; + float diff_thrust_scale; }; // Has to match 1:1 msg/vtol_vehicle_status.msg diff --git a/src/systemcmds/tests/test_controlmath.cpp b/src/systemcmds/tests/test_controlmath.cpp index 7be4d0909b..fb5c3d6a2a 100644 --- a/src/systemcmds/tests/test_controlmath.cpp +++ b/src/systemcmds/tests/test_controlmath.cpp @@ -37,7 +37,7 @@ bool ControlMathTest::testThrAttMapping() ut_assert_true(att.roll_body < SIGMA_SINGLE_OP); ut_assert_true(att.pitch_body < SIGMA_SINGLE_OP); ut_assert_true(att.yaw_body < SIGMA_SINGLE_OP); - ut_assert_true(att.thrust - 1.0f < SIGMA_SINGLE_OP); + ut_assert_true(-att.thrust_body[2] - 1.0f < SIGMA_SINGLE_OP); /* expected: same as before but with 90 yaw * reason: only yaw changed @@ -47,7 +47,7 @@ bool ControlMathTest::testThrAttMapping() ut_assert_true(att.roll_body < SIGMA_SINGLE_OP); ut_assert_true(att.pitch_body < SIGMA_SINGLE_OP); ut_assert_true(att.yaw_body - M_PI_2_F < SIGMA_SINGLE_OP); - ut_assert_true(att.thrust - 1.0f < SIGMA_SINGLE_OP); + ut_assert_true(-att.thrust_body[2] - 1.0f < SIGMA_SINGLE_OP); /* expected: same as before but roll 180 * reason: thrust points straight down and order Euler @@ -58,7 +58,7 @@ bool ControlMathTest::testThrAttMapping() ut_assert_true(fabsf(att.roll_body) - M_PI_F < SIGMA_SINGLE_OP); ut_assert_true(fabsf(att.pitch_body) < SIGMA_SINGLE_OP); ut_assert_true(att.yaw_body - M_PI_2_F < SIGMA_SINGLE_OP); - ut_assert_true(att.thrust - 1.0f < SIGMA_SINGLE_OP); + ut_assert_true(-att.thrust_body[2] - 1.0f < SIGMA_SINGLE_OP); /* TODO: find a good way to test it */