|
|
|
@ -9,12 +9,16 @@ static bool circle_init(bool ignore_checks)
@@ -9,12 +9,16 @@ static bool circle_init(bool ignore_checks)
|
|
|
|
|
{ |
|
|
|
|
if ((GPS_ok() && inertial_nav.position_ok()) || ignore_checks) { |
|
|
|
|
circle_pilot_yaw_override = false; |
|
|
|
|
circle_nav.init(); |
|
|
|
|
|
|
|
|
|
// initialize vertical speeds and leash lengths |
|
|
|
|
// initialize speeds and accelerations |
|
|
|
|
pos_control.set_speed_xy(wp_nav.get_speed_xy()); |
|
|
|
|
pos_control.set_accel_xy(wp_nav.get_wp_acceleration()); |
|
|
|
|
pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max); |
|
|
|
|
pos_control.set_accel_z(g.pilot_accel_z); |
|
|
|
|
|
|
|
|
|
// initialise circle controller including setting the circle center based on vehicle speed |
|
|
|
|
circle_nav.init(); |
|
|
|
|
|
|
|
|
|
return true; |
|
|
|
|
}else{ |
|
|
|
|
return false; |
|
|
|
|