dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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
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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/lib/ecl

@ -1 +1 @@
Subproject commit e0bcfeb533d872c6bf10c83491b760d2882aed01
Subproject commit 48a17b52342f8c97dcb6af193233f616ace9f5f6

20
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
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();

1
src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
View File

@ -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;

12
src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
View File

@ -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
*

6
src/modules/gnd_pos_control/GroundRoverPositionControl.cpp
View File

@ -286,7 +286,7 @@ GroundRoverPositionControl::control_position(const matrix::Vector2f &current_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 &current_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();