fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-13 23:37:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

update ecl l1 and usage for new roll angle setpoint slew rate limit (#10005)

Browse Source 
* 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
This commit is contained in:
Roman Bapst
2018-08-01 20:01:41 +02:00
committed by Daniel Agar
parent ac0988d519
commit 8b1381ead9
5 changed files with 30 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+13 -7
View File
@@ -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();