From 37108870e15a7b25f7aa06b98c6fbcb7f1ccfd0a Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Wed, 22 Jun 2016 15:19:11 +0200 Subject: [PATCH] fw_pos_control_l1: added roll setpoint for logging (#4869) in altitude control mode for fixed wings the roll setpoint was not logged because the position controller publishes the attitude setpoint but the desired roll setpoint is calculated in the attitude control module. Now the position controller calculates the roll setpoint as well for the sake of logging. Signed-off-by: tumbili --- .../fw_pos_control_l1_main.cpp | 22 +++++++++++++++++++ 1 file changed, 22 insertions(+) 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;