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 5350413756..ec42455951 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 @@ -291,6 +291,8 @@ private: float throttle_idle; float throttle_cruise; float throttle_slew_max; + float man_roll_max_rad; + float rollsp_offset_rad; float throttle_land_max; @@ -342,6 +344,8 @@ private: param_t throttle_idle; param_t throttle_cruise; param_t throttle_slew_max; + param_t man_roll_max_deg; + param_t rollsp_offset_deg; param_t throttle_land_max; @@ -611,6 +615,8 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.throttle_slew_max = param_find("FW_THR_SLEW_MAX"); _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.rollsp_offset_deg = param_find("FW_RSP_OFF"); _parameter_handles.land_slope_angle = param_find("FW_LND_ANG"); _parameter_handles.land_H1_virt = param_find("FW_LND_HVIRT"); @@ -693,6 +699,12 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.throttle_land_max, &(_parameters.throttle_land_max)); + 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.rollsp_offset_deg, &_parameters.rollsp_offset_rad); + _parameters.rollsp_offset_rad = math::radians(_parameters.rollsp_offset_rad); + + param_get(_parameter_handles.time_const, &(_parameters.time_const)); param_get(_parameter_handles.time_const_throt, &(_parameters.time_const_throt)); param_get(_parameter_handles.min_sink_rate, &(_parameters.min_sink_rate)); @@ -1941,6 +1953,16 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi ground_speed, tecs_status_s::TECS_MODE_NORMAL); + // calculate roll setpoint from user input + // this is already calculated in fw_att_control_main.cpp but we do it here for the sake of logging + if (fabsf(_manual.y) < 0.01f && fabsf(_roll) < 0.2f) { + _att_sp.roll_body = _parameters.rollsp_offset_rad; + + } else { + _att_sp.roll_body = (_manual.y * _parameters.man_roll_max_rad) + + _parameters.rollsp_offset_rad; + } + } else { _control_mode_current = FW_POSCTRL_MODE_OTHER;