From 8b1381ead9803f8fdcbae6fc2f79087a0cbbef3b Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Wed, 1 Aug 2018 20:01:41 +0200 Subject: [PATCH] update ecl l1 and usage for new roll angle setpoint slew rate limit (#10005) * added ability to slew rate limit the roll angle output of the l1 controller * FixedWingPositionControl: this avoids the steps in roll angle setpoint which occur when the controller switches to a new waypoint * GroundRoverPositionControl: adapted to new l1 API --- src/lib/ecl | 2 +- .../FixedwingPositionControl.cpp | 20 ++++++++++++------- .../FixedwingPositionControl.hpp | 1 + .../fw_pos_control_l1_params.c | 12 +++++++++++ .../GroundRoverPositionControl.cpp | 6 +++--- 5 files changed, 30 insertions(+), 11 deletions(-) diff --git a/src/lib/ecl b/src/lib/ecl index e0bcfeb533..48a17b5234 160000 --- a/src/lib/ecl +++ b/src/lib/ecl @@ -1 +1 @@ -Subproject commit e0bcfeb533d872c6bf10c83491b760d2882aed01 +Subproject commit 48a17b52342f8c97dcb6af193233f616ace9f5f6 diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index f5f3ebb7bb..5a29bc480f 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -45,6 +45,7 @@ FixedwingPositionControl::FixedwingPositionControl() : { _parameter_handles.l1_period = param_find("FW_L1_PERIOD"); _parameter_handles.l1_damping = param_find("FW_L1_DAMPING"); + _parameter_handles.roll_slew_deg_sec = param_find("FW_L1_R_SLEW_MAX"); _parameter_handles.airspeed_min = param_find("FW_AIRSPD_MIN"); _parameter_handles.airspeed_trim = param_find("FW_AIRSPD_TRIM"); @@ -176,6 +177,9 @@ FixedwingPositionControl::parameters_update() _l1_control.set_l1_roll_limit(radians(v)); } + if (param_get(_parameter_handles.roll_slew_deg_sec, &v) == PX4_OK) { + _l1_control.set_roll_slew_rate(radians(v)); + } // TECS parameters @@ -700,6 +704,8 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto _control_position_last_called = hrt_absolute_time(); + _l1_control.set_dt(dt); + /* only run position controller in fixed-wing mode and during transitions for VTOL */ if (_vehicle_status.is_rotary_wing && !_vehicle_status.in_transition_mode) { _control_mode_current = FW_POSCTRL_MODE_OTHER; @@ -815,7 +821,7 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto } else if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_POSITION) { /* waypoint is a plain navigation waypoint */ _l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, nav_speed_2d); - _att_sp.roll_body = _l1_control.nav_roll(); + _att_sp.roll_body = _l1_control.get_roll_setpoint(); _att_sp.yaw_body = _l1_control.nav_bearing(); tecs_update_pitch_throttle(pos_sp_curr.alt, @@ -833,7 +839,7 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto /* waypoint is a loiter waypoint */ _l1_control.navigate_loiter(curr_wp, curr_pos, pos_sp_curr.loiter_radius, pos_sp_curr.loiter_direction, nav_speed_2d); - _att_sp.roll_body = _l1_control.nav_roll(); + _att_sp.roll_body = _l1_control.get_roll_setpoint(); _att_sp.yaw_body = _l1_control.nav_bearing(); float alt_sp = pos_sp_curr.alt; @@ -978,7 +984,7 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto /* populate l1 control setpoint */ _l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, ground_speed); - _att_sp.roll_body = _l1_control.nav_roll(); + _att_sp.roll_body = _l1_control.get_roll_setpoint(); _att_sp.yaw_body = _l1_control.nav_bearing(); if (in_takeoff_situation()) { @@ -1192,7 +1198,7 @@ FixedwingPositionControl::control_takeoff(const Vector2f &curr_pos, const Vector tecs_status_s::TECS_MODE_TAKEOFF); // assign values - _att_sp.roll_body = _runway_takeoff.getRoll(_l1_control.nav_roll()); + _att_sp.roll_body = _runway_takeoff.getRoll(_l1_control.get_roll_setpoint()); _att_sp.yaw_body = _runway_takeoff.getYaw(_l1_control.nav_bearing()); _att_sp.fw_control_yaw = _runway_takeoff.controlYaw(); _att_sp.pitch_body = _runway_takeoff.getPitch(get_tecs_pitch()); @@ -1232,7 +1238,7 @@ FixedwingPositionControl::control_takeoff(const Vector2f &curr_pos, const Vector /* Launch has been detected, hence we have to control the plane. */ _l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, ground_speed); - _att_sp.roll_body = _l1_control.nav_roll(); + _att_sp.roll_body = _l1_control.get_roll_setpoint(); _att_sp.yaw_body = _l1_control.nav_bearing(); /* Select throttle: only in LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS we want to use @@ -1386,7 +1392,7 @@ FixedwingPositionControl::control_landing(const Vector2f &curr_pos, const Vector _l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, ground_speed); } - _att_sp.roll_body = _l1_control.nav_roll(); + _att_sp.roll_body = _l1_control.get_roll_setpoint(); _att_sp.yaw_body = _l1_control.nav_bearing(); if (_land_noreturn_horizontal) { @@ -1746,7 +1752,7 @@ FixedwingPositionControl::run() /* set new turn distance */ _fw_pos_ctrl_status.turn_distance = turn_distance; - _fw_pos_ctrl_status.nav_roll = _l1_control.nav_roll(); + _fw_pos_ctrl_status.nav_roll = _l1_control.get_roll_setpoint(); _fw_pos_ctrl_status.nav_pitch = get_tecs_pitch(); _fw_pos_ctrl_status.nav_bearing = _l1_control.nav_bearing(); diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index 264a7ee209..ddf6ed5e02 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -306,6 +306,7 @@ private: param_t l1_period; param_t l1_damping; param_t roll_limit; + param_t roll_slew_deg_sec; param_t time_const; param_t time_const_throt; diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index e9398a4134..ab7f465b24 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -73,6 +73,18 @@ PARAM_DEFINE_FLOAT(FW_L1_PERIOD, 20.0f); */ PARAM_DEFINE_FLOAT(FW_L1_DAMPING, 0.75f); +/** + * L1 controller roll slew rate limit. + * + * The maxium change in roll angle setpoint per second. + * + * @unit deg/s + * @min 0 + * @increment 1 + * @group FW L1 Control + */ +PARAM_DEFINE_FLOAT(FW_L1_R_SLEW_MAX, 90.0f); + /** * Cruise throttle * diff --git a/src/modules/gnd_pos_control/GroundRoverPositionControl.cpp b/src/modules/gnd_pos_control/GroundRoverPositionControl.cpp index 5ab284c48b..23e03e4009 100644 --- a/src/modules/gnd_pos_control/GroundRoverPositionControl.cpp +++ b/src/modules/gnd_pos_control/GroundRoverPositionControl.cpp @@ -286,7 +286,7 @@ GroundRoverPositionControl::control_position(const matrix::Vector2f ¤t_pos /* waypoint is a plain navigation waypoint or the takeoff waypoint, does not matter */ _gnd_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); - _att_sp.roll_body = _gnd_control.nav_roll(); + _att_sp.roll_body = _gnd_control.get_roll_setpoint(); _att_sp.pitch_body = 0.0f; _att_sp.yaw_body = _gnd_control.nav_bearing(); _att_sp.fw_control_yaw = true; @@ -298,7 +298,7 @@ GroundRoverPositionControl::control_position(const matrix::Vector2f ¤t_pos _gnd_control.navigate_loiter(curr_wp, current_position, pos_sp_triplet.current.loiter_radius, pos_sp_triplet.current.loiter_direction, ground_speed_2d); - _att_sp.roll_body = _gnd_control.nav_roll(); + _att_sp.roll_body = _gnd_control.get_roll_setpoint(); _att_sp.pitch_body = 0.0f; _att_sp.yaw_body = _gnd_control.nav_bearing(); _att_sp.fw_control_yaw = true; @@ -453,7 +453,7 @@ GroundRoverPositionControl::task_main() /* set new turn distance */ _gnd_pos_ctrl_status.turn_distance = turn_distance; - _gnd_pos_ctrl_status.nav_roll = _gnd_control.nav_roll(); + _gnd_pos_ctrl_status.nav_roll = _gnd_control.get_roll_setpoint(); _gnd_pos_ctrl_status.nav_pitch = 0.0f; _gnd_pos_ctrl_status.nav_bearing = _gnd_control.nav_bearing();