mirror of https://github.com/ArduPilot/ardupilot
Sub: seperate kinimatic shaping from pid limit setting
This commit is contained in:
parent
30aa636ba1
commit
e1000d25ed
|
@ -15,9 +15,7 @@ bool Sub::althold_init()
|
|||
// initialize vertical maximum speeds and acceleration
|
||||
// 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);
|
||||
|
||||
// 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_correction_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
|
||||
|
||||
// initialise position and desired velocity
|
||||
pos_control.init_z_controller();
|
||||
|
|
|
@ -572,6 +572,7 @@ bool Sub::auto_terrain_recover_start()
|
|||
|
||||
// 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_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");
|
||||
return true;
|
||||
|
|
|
@ -15,7 +15,9 @@ bool Sub::circle_init()
|
|||
|
||||
// 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_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_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
|
||||
circle_nav.init();
|
||||
|
|
|
@ -74,6 +74,7 @@ void Sub::guided_vel_control_start()
|
|||
|
||||
// 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_correction_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
|
||||
|
||||
// initialise velocity controller
|
||||
pos_control.init_z_controller();
|
||||
|
@ -88,6 +89,7 @@ void Sub::guided_posvel_control_start()
|
|||
|
||||
// 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_correction_speed_accel_z(wp_nav.get_default_speed_down(), wp_nav.get_default_speed_up(), wp_nav.get_accel_z());
|
||||
|
||||
// initialise velocity controller
|
||||
pos_control.init_z_controller();
|
||||
|
@ -105,6 +107,7 @@ void Sub::guided_angle_control_start()
|
|||
|
||||
// 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_correction_speed_accel_z(wp_nav.get_default_speed_down(), wp_nav.get_default_speed_up(), wp_nav.get_accel_z());
|
||||
|
||||
// initialise velocity controller
|
||||
pos_control.init_z_controller();
|
||||
|
|
|
@ -16,7 +16,9 @@ bool Sub::poshold_init()
|
|||
|
||||
// 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_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_correction_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
|
||||
|
||||
// initialise position and desired velocity
|
||||
pos_control.init_xy_controller_stopping_point();
|
||||
|
|
|
@ -9,6 +9,7 @@ bool Sub::surface_init()
|
|||
|
||||
// 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_correction_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
|
||||
|
||||
// initialise position and desired velocity
|
||||
pos_control.init_z_controller();
|
||||
|
|
Loading…
Reference in New Issue