|
|
@ -127,7 +127,7 @@ static void rtl_climb_return_run() |
|
|
|
// if not auto armed set throttle to zero and exit immediately |
|
|
|
// if not auto armed set throttle to zero and exit immediately |
|
|
|
if(!ap.auto_armed) { |
|
|
|
if(!ap.auto_armed) { |
|
|
|
// reset attitude control targets |
|
|
|
// reset attitude control targets |
|
|
|
attitude_control.init_targets(); |
|
|
|
attitude_control.relax_bf_rate_controller(); |
|
|
|
attitude_control.set_throttle_out(0, false); |
|
|
|
attitude_control.set_throttle_out(0, false); |
|
|
|
// To-Do: re-initialise wpnav targets |
|
|
|
// To-Do: re-initialise wpnav targets |
|
|
|
return; |
|
|
|
return; |
|
|
@ -184,7 +184,7 @@ static void rtl_loiterathome_run() |
|
|
|
// if not auto armed set throttle to zero and exit immediately |
|
|
|
// if not auto armed set throttle to zero and exit immediately |
|
|
|
if(!ap.auto_armed) { |
|
|
|
if(!ap.auto_armed) { |
|
|
|
// reset attitude control targets |
|
|
|
// reset attitude control targets |
|
|
|
attitude_control.init_targets(); |
|
|
|
attitude_control.relax_bf_rate_controller(); |
|
|
|
attitude_control.set_throttle_out(0, false); |
|
|
|
attitude_control.set_throttle_out(0, false); |
|
|
|
// To-Do: re-initialise wpnav targets |
|
|
|
// To-Do: re-initialise wpnav targets |
|
|
|
return; |
|
|
|
return; |
|
|
@ -251,7 +251,7 @@ static void rtl_descent_run() |
|
|
|
{ |
|
|
|
{ |
|
|
|
// if not auto armed set throttle to zero and exit immediately |
|
|
|
// if not auto armed set throttle to zero and exit immediately |
|
|
|
if(!ap.auto_armed || !inertial_nav.position_ok()) { |
|
|
|
if(!ap.auto_armed || !inertial_nav.position_ok()) { |
|
|
|
attitude_control.init_targets(); |
|
|
|
attitude_control.relax_bf_rate_controller(); |
|
|
|
attitude_control.set_throttle_out(0, false); |
|
|
|
attitude_control.set_throttle_out(0, false); |
|
|
|
// set target to current position |
|
|
|
// set target to current position |
|
|
|
wp_nav.init_loiter_target(); |
|
|
|
wp_nav.init_loiter_target(); |
|
|
@ -310,7 +310,7 @@ static void rtl_land_run() |
|
|
|
{ |
|
|
|
{ |
|
|
|
// if not auto armed set throttle to zero and exit immediately |
|
|
|
// if not auto armed set throttle to zero and exit immediately |
|
|
|
if(!ap.auto_armed || !inertial_nav.position_ok()) { |
|
|
|
if(!ap.auto_armed || !inertial_nav.position_ok()) { |
|
|
|
attitude_control.init_targets(); |
|
|
|
attitude_control.relax_bf_rate_controller(); |
|
|
|
attitude_control.set_throttle_out(0, false); |
|
|
|
attitude_control.set_throttle_out(0, false); |
|
|
|
// set target to current position |
|
|
|
// set target to current position |
|
|
|
wp_nav.init_loiter_target(); |
|
|
|
wp_nav.init_loiter_target(); |
|
|
|