diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 06b126895a..199b3189cb 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -975,129 +975,27 @@ FixedwingAttitudeControl::task_main() float yaw_manual = 0.0f; float throttle_sp = 0.0f; - /* Read attitude setpoint from uorb if - * - velocity control or position control is enabled (pos controller is running) - * - manual control is disabled (another app may send the setpoint, but it should - * for sure not be set from the remote control values) - */ - if (_vcontrol_mode.flag_control_auto_enabled || - !_vcontrol_mode.flag_control_manual_enabled) { - /* read in attitude setpoint from attitude setpoint uorb topic */ - roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad; - pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad; - yaw_sp = _att_sp.yaw_body; - throttle_sp = _att_sp.thrust; + roll_sp = _att_sp.roll_body; + pitch_sp = _att_sp.pitch_body; + yaw_sp = _att_sp.yaw_body; + throttle_sp = _att_sp.thrust; - /* reset integrals where needed */ - if (_att_sp.roll_reset_integral) { - _roll_ctrl.reset_integrator(); - } - - if (_att_sp.pitch_reset_integral) { - _pitch_ctrl.reset_integrator(); - } - - if (_att_sp.yaw_reset_integral) { - _yaw_ctrl.reset_integrator(); - _wheel_ctrl.reset_integrator(); - } - - } else if (_vcontrol_mode.flag_control_velocity_enabled) { - - /* the pilot does not want to change direction, - * take straight attitude setpoint from position controller - */ - if (fabsf(_manual.y) < 0.01f && fabsf(_roll) < 0.2f) { - roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad; - - } else { - roll_sp = (_manual.y * _parameters.man_roll_max) - + _parameters.rollsp_offset_rad; - } - - pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad; - throttle_sp = _att_sp.thrust; - - /* reset integrals where needed */ - if (_att_sp.roll_reset_integral) { - _roll_ctrl.reset_integrator(); - } - - if (_att_sp.pitch_reset_integral) { - _pitch_ctrl.reset_integrator(); - } - - if (_att_sp.yaw_reset_integral) { - _yaw_ctrl.reset_integrator(); - _wheel_ctrl.reset_integrator(); - } - - } else if (_vcontrol_mode.flag_control_altitude_enabled) { - /* - * Velocity should be controlled and manual is enabled. - */ - roll_sp = (_manual.y * _parameters.man_roll_max) + _parameters.rollsp_offset_rad; - pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad; - throttle_sp = _att_sp.thrust; - - /* reset integrals where needed */ - if (_att_sp.roll_reset_integral) { - _roll_ctrl.reset_integrator(); - } - - if (_att_sp.pitch_reset_integral) { - _pitch_ctrl.reset_integrator(); - } - - if (_att_sp.yaw_reset_integral) { - _yaw_ctrl.reset_integrator(); - _wheel_ctrl.reset_integrator(); - } - - } else { - /* - * Scale down roll and pitch as the setpoints are radians - * and a typical remote can only do around 45 degrees, the mapping is - * -1..+1 to -man_roll_max rad..+man_roll_max rad (equivalent for pitch) - * - * With this mapping the stick angle is a 1:1 representation of - * the commanded attitude. - * - * The trim gets subtracted here from the manual setpoint to get - * the intended attitude setpoint. Later, after the rate control step the - * trim is added again to the control signal. - */ - roll_sp = (_manual.y * _parameters.man_roll_max) + _parameters.rollsp_offset_rad; - pitch_sp = -(_manual.x * _parameters.man_pitch_max) + _parameters.pitchsp_offset_rad; - /* allow manual control of rudder deflection */ + if (!_vcontrol_mode.flag_control_velocity_enabled) { yaw_manual = _manual.r; - throttle_sp = _manual.z; + } - /* - * in manual mode no external source should / does emit attitude setpoints. - * emit the manual setpoint here to allow attitude controller tuning - * in attitude control mode. - */ - struct vehicle_attitude_setpoint_s att_sp = {}; - att_sp.timestamp = hrt_absolute_time(); - att_sp.roll_body = roll_sp; - att_sp.pitch_body = pitch_sp; - att_sp.yaw_body = 0.0f - _parameters.trim_yaw; - att_sp.thrust = throttle_sp; + /* reset integrals where needed */ + if (_att_sp.roll_reset_integral) { + _roll_ctrl.reset_integrator(); + } - att_sp.roll_reset_integral = false; - att_sp.pitch_reset_integral = false; - att_sp.yaw_reset_integral = false; + if (_att_sp.pitch_reset_integral) { + _pitch_ctrl.reset_integrator(); + } - /* lazily publish the setpoint only once available */ - if (_attitude_sp_pub != nullptr) { - /* publish the attitude setpoint */ - orb_publish(_attitude_setpoint_id, _attitude_sp_pub, &att_sp); - - } else if (_attitude_setpoint_id) { - /* advertise and publish */ - _attitude_sp_pub = orb_advertise(_attitude_setpoint_id, &att_sp); - } + if (_att_sp.yaw_reset_integral) { + _yaw_ctrl.reset_integrator(); + _wheel_ctrl.reset_integrator(); } /* If the aircraft is on ground reset the integrators */ diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 96584a788d..8390266d9f 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -290,6 +290,7 @@ private: float throttle_cruise; float throttle_slew_max; float man_roll_max_rad; + float man_pitch_max_rad; float rollsp_offset_rad; float pitchsp_offset_rad; @@ -345,6 +346,7 @@ private: param_t throttle_cruise; param_t throttle_slew_max; param_t man_roll_max_deg; + param_t man_pitch_max_deg; param_t rollsp_offset_deg; param_t pitchsp_offset_deg; @@ -624,6 +626,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.throttle_cruise = param_find("FW_THR_CRUISE"); _parameter_handles.throttle_land_max = param_find("FW_THR_LND_MAX"); _parameter_handles.man_roll_max_deg = param_find("FW_MAN_R_MAX"); + _parameter_handles.man_pitch_max_deg = param_find("FW_MAN_P_MAX"); _parameter_handles.rollsp_offset_deg = param_find("FW_RSP_OFF"); _parameter_handles.pitchsp_offset_deg = param_find("FW_PSP_OFF"); @@ -710,6 +713,8 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.man_roll_max_deg, &_parameters.man_roll_max_rad); _parameters.man_roll_max_rad = math::radians(_parameters.man_roll_max_rad); + param_get(_parameter_handles.man_pitch_max_deg, &_parameters.man_pitch_max_rad); + _parameters.man_pitch_max_rad = math::radians(_parameters.man_pitch_max_rad); param_get(_parameter_handles.rollsp_offset_deg, &_parameters.rollsp_offset_rad); _parameters.rollsp_offset_rad = math::radians(_parameters.rollsp_offset_rad); param_get(_parameter_handles.pitchsp_offset_deg, &_parameters.pitchsp_offset_rad); @@ -1996,6 +2001,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _control_mode_current = FW_POSCTRL_MODE_OTHER; /** MANUAL FLIGHT **/ + setpoint = false; + if (_control_mode.flag_control_attitude_enabled) { + _att_sp.roll_body = _manual.y * _parameters.man_roll_max_rad; + _att_sp.pitch_body = -_manual.x * _parameters.man_pitch_max_rad; + _att_sp.yaw_body = 0.0f; + _att_sp.thrust = _manual.z; + setpoint = true; + } // reset hold altitude _hold_alt = _global_pos.alt; @@ -2007,9 +2020,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // reset lading abort state _fw_pos_ctrl_status.abort_landing = false; - /* no flight mode applies, do not publish an attitude setpoint */ - setpoint = false; - /* reset landing and takeoff state */ if (!_last_manual) { reset_landing_state(); @@ -2040,7 +2050,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { _att_sp.thrust = 0.0f; - + } else if (_control_mode_current == FW_POSCTRL_MODE_OTHER) { + _att_sp.thrust = math::min(_att_sp.thrust, _parameters.throttle_max); } else { /* Copy thrust and pitch values from tecs */ if (_vehicle_land_detected.landed) {