|
|
|
@ -128,7 +128,7 @@ void ModeLoiter::run()
@@ -128,7 +128,7 @@ void ModeLoiter::run()
|
|
|
|
|
attitude_control->reset_yaw_target_and_rate(); |
|
|
|
|
pos_control->relax_z_controller(0.0f); // forces throttle output to decay to zero
|
|
|
|
|
loiter_nav->init_target(); |
|
|
|
|
attitude_control->input_thrust_vector_rate_heading(loiter_nav->get_thrust_vector(), target_yaw_rate); |
|
|
|
|
attitude_control->input_thrust_vector_rate_heading(loiter_nav->get_thrust_vector(), target_yaw_rate, false); |
|
|
|
|
break; |
|
|
|
|
|
|
|
|
|
case AltHold_Takeoff: |
|
|
|
@ -147,7 +147,7 @@ void ModeLoiter::run()
@@ -147,7 +147,7 @@ void ModeLoiter::run()
|
|
|
|
|
loiter_nav->update(); |
|
|
|
|
|
|
|
|
|
// call attitude controller
|
|
|
|
|
attitude_control->input_thrust_vector_rate_heading(loiter_nav->get_thrust_vector(), target_yaw_rate); |
|
|
|
|
attitude_control->input_thrust_vector_rate_heading(loiter_nav->get_thrust_vector(), target_yaw_rate, false); |
|
|
|
|
break; |
|
|
|
|
|
|
|
|
|
case AltHold_Landed_Ground_Idle: |
|
|
|
@ -157,7 +157,7 @@ void ModeLoiter::run()
@@ -157,7 +157,7 @@ void ModeLoiter::run()
|
|
|
|
|
case AltHold_Landed_Pre_Takeoff: |
|
|
|
|
attitude_control->reset_rate_controller_I_terms_smoothly(); |
|
|
|
|
loiter_nav->init_target(); |
|
|
|
|
attitude_control->input_thrust_vector_rate_heading(loiter_nav->get_thrust_vector(), target_yaw_rate); |
|
|
|
|
attitude_control->input_thrust_vector_rate_heading(loiter_nav->get_thrust_vector(), target_yaw_rate, false); |
|
|
|
|
pos_control->relax_z_controller(0.0f); // forces throttle output to decay to zero
|
|
|
|
|
break; |
|
|
|
|
|
|
|
|
@ -186,7 +186,7 @@ void ModeLoiter::run()
@@ -186,7 +186,7 @@ void ModeLoiter::run()
|
|
|
|
|
#endif |
|
|
|
|
|
|
|
|
|
// call attitude controller
|
|
|
|
|
attitude_control->input_thrust_vector_rate_heading(loiter_nav->get_thrust_vector(), target_yaw_rate); |
|
|
|
|
attitude_control->input_thrust_vector_rate_heading(loiter_nav->get_thrust_vector(), target_yaw_rate, false); |
|
|
|
|
|
|
|
|
|
// get avoidance adjusted climb rate
|
|
|
|
|
target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); |
|
|
|
|