Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Sub: seperate kinimatic shaping from pid limit setting

This commit is contained in:
Leonard Hall 2021-07-08 13:46:08 +09:30 committed by Andrew Tridgell
parent 30aa636ba1
commit e1000d25ed
6 changed files with 10 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduSub/control_althold.cpp
View File

@ -15,9 +15,7 @@ bool Sub::althold_init()
// initialize vertical maximum speeds and acceleration // initialize vertical maximum speeds and acceleration
// sets the maximum speed up and down returned by position controller // sets the maximum speed up and down returned by position controller
pos_control.set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); pos_control.set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
pos_control.set_correction_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
// initialize vertical speeds and acceleration
pos_control.set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
// initialise position and desired velocity // initialise position and desired velocity
pos_control.init_z_controller(); pos_control.init_z_controller();

1
ArduSub/control_auto.cpp
View File

@ -572,6 +572,7 @@ bool Sub::auto_terrain_recover_start()
// initialize vertical maximum speeds and acceleration // initialize vertical maximum speeds and acceleration
pos_control.set_max_speed_accel_z(wp_nav.get_default_speed_down(), wp_nav.get_default_speed_up(), wp_nav.get_accel_z()); pos_control.set_max_speed_accel_z(wp_nav.get_default_speed_down(), wp_nav.get_default_speed_up(), wp_nav.get_accel_z());
pos_control.set_correction_speed_accel_z(wp_nav.get_default_speed_down(), wp_nav.get_default_speed_up(), wp_nav.get_accel_z());
gcs().send_text(MAV_SEVERITY_WARNING, "Attempting auto failsafe recovery"); gcs().send_text(MAV_SEVERITY_WARNING, "Attempting auto failsafe recovery");
return true; return true;

2
ArduSub/control_circle.cpp
View File

@ -15,7 +15,9 @@ bool Sub::circle_init()
// initialize speeds and accelerations // initialize speeds and accelerations
pos_control.set_max_speed_accel_xy(wp_nav.get_default_speed_xy(), wp_nav.get_wp_acceleration()); pos_control.set_max_speed_accel_xy(wp_nav.get_default_speed_xy(), wp_nav.get_wp_acceleration());
pos_control.set_correction_speed_accel_xy(wp_nav.get_default_speed_xy(), wp_nav.get_wp_acceleration());
pos_control.set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); pos_control.set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
pos_control.set_correction_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
// initialise circle controller including setting the circle center based on vehicle speed // initialise circle controller including setting the circle center based on vehicle speed
circle_nav.init(); circle_nav.init();

3
ArduSub/control_guided.cpp
View File

@ -74,6 +74,7 @@ void Sub::guided_vel_control_start()
// initialize vertical maximum speeds and acceleration // initialize vertical maximum speeds and acceleration
pos_control.set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); pos_control.set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
pos_control.set_correction_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
// initialise velocity controller // initialise velocity controller
pos_control.init_z_controller(); pos_control.init_z_controller();
@ -88,6 +89,7 @@ void Sub::guided_posvel_control_start()
// set vertical speed and acceleration // set vertical speed and acceleration
pos_control.set_max_speed_accel_z(wp_nav.get_default_speed_down(), wp_nav.get_default_speed_up(), wp_nav.get_accel_z()); pos_control.set_max_speed_accel_z(wp_nav.get_default_speed_down(), wp_nav.get_default_speed_up(), wp_nav.get_accel_z());
pos_control.set_correction_speed_accel_z(wp_nav.get_default_speed_down(), wp_nav.get_default_speed_up(), wp_nav.get_accel_z());
// initialise velocity controller // initialise velocity controller
pos_control.init_z_controller(); pos_control.init_z_controller();
@ -105,6 +107,7 @@ void Sub::guided_angle_control_start()
// set vertical speed and acceleration // set vertical speed and acceleration
pos_control.set_max_speed_accel_z(wp_nav.get_default_speed_down(), wp_nav.get_default_speed_up(), wp_nav.get_accel_z()); pos_control.set_max_speed_accel_z(wp_nav.get_default_speed_down(), wp_nav.get_default_speed_up(), wp_nav.get_accel_z());
pos_control.set_correction_speed_accel_z(wp_nav.get_default_speed_down(), wp_nav.get_default_speed_up(), wp_nav.get_accel_z());
// initialise velocity controller // initialise velocity controller
pos_control.init_z_controller(); pos_control.init_z_controller();

2
ArduSub/control_poshold.cpp
View File

@ -16,7 +16,9 @@ bool Sub::poshold_init()
// initialize vertical speeds and acceleration // initialize vertical speeds and acceleration
pos_control.set_max_speed_accel_xy(wp_nav.get_default_speed_xy(), wp_nav.get_wp_acceleration()); pos_control.set_max_speed_accel_xy(wp_nav.get_default_speed_xy(), wp_nav.get_wp_acceleration());
pos_control.set_correction_speed_accel_xy(wp_nav.get_default_speed_xy(), wp_nav.get_wp_acceleration());
pos_control.set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); pos_control.set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
pos_control.set_correction_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
// initialise position and desired velocity // initialise position and desired velocity
pos_control.init_xy_controller_stopping_point(); pos_control.init_xy_controller_stopping_point();

1
ArduSub/control_surface.cpp
View File

@ -9,6 +9,7 @@ bool Sub::surface_init()
// initialize vertical speeds and acceleration // initialize vertical speeds and acceleration
pos_control.set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); pos_control.set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
pos_control.set_correction_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
// initialise position and desired velocity // initialise position and desired velocity
pos_control.init_z_controller(); pos_control.init_z_controller();