mirror of https://github.com/ArduPilot/ardupilot
Copter: integrate AttControl's set_yaw_target_to_current_heading
This commit is contained in:
parent
2bb30b3ef9
commit
08801eebf2
|
@ -21,6 +21,7 @@ static void acro_run()
|
|||
// if motors not running reset angle targets
|
||||
if(!motors.armed() || g.rc_3.control_in <= 0) {
|
||||
attitude_control.relax_bf_rate_controller();
|
||||
attitude_control.set_yaw_target_to_current_heading();
|
||||
attitude_control.set_throttle_out(0, false);
|
||||
return;
|
||||
}
|
||||
|
|
|
@ -29,6 +29,7 @@ static void althold_run()
|
|||
if(!ap.auto_armed) {
|
||||
// To-Do: reset altitude target if we're somehow not landed?
|
||||
attitude_control.relax_bf_rate_controller();
|
||||
attitude_control.set_yaw_target_to_current_heading();
|
||||
attitude_control.set_throttle_out(0, false);
|
||||
return;
|
||||
}
|
||||
|
@ -57,6 +58,7 @@ static void althold_run()
|
|||
// reset target lean angles and heading while landed
|
||||
if (ap.land_complete) {
|
||||
attitude_control.relax_bf_rate_controller();
|
||||
attitude_control.set_yaw_target_to_current_heading();
|
||||
// move throttle to minimum to keep us on the ground
|
||||
attitude_control.set_throttle_out(0, false);
|
||||
}else{
|
||||
|
|
|
@ -98,6 +98,7 @@ static void auto_takeoff_run()
|
|||
if(!ap.auto_armed) {
|
||||
// reset attitude control targets
|
||||
attitude_control.relax_bf_rate_controller();
|
||||
attitude_control.set_yaw_target_to_current_heading();
|
||||
attitude_control.set_throttle_out(0, false);
|
||||
// tell motors to do a slow start
|
||||
motors.slow_start(true);
|
||||
|
@ -146,6 +147,7 @@ static void auto_wp_run()
|
|||
// To-Do: reset waypoint origin to current location because copter is probably on the ground so we don't want it lurching left or right on take-off
|
||||
// (of course it would be better if people just used take-off)
|
||||
attitude_control.relax_bf_rate_controller();
|
||||
attitude_control.set_yaw_target_to_current_heading();
|
||||
attitude_control.set_throttle_out(0, false);
|
||||
// tell motors to do a slow start
|
||||
motors.slow_start(true);
|
||||
|
@ -203,6 +205,7 @@ static void auto_spline_run()
|
|||
// To-Do: reset waypoint origin to current location because copter is probably on the ground so we don't want it lurching left or right on take-off
|
||||
// (of course it would be better if people just used take-off)
|
||||
attitude_control.relax_bf_rate_controller();
|
||||
attitude_control.set_yaw_target_to_current_heading();
|
||||
attitude_control.set_throttle_out(0, false);
|
||||
// tell motors to do a slow start
|
||||
motors.slow_start(true);
|
||||
|
@ -268,6 +271,7 @@ static void auto_land_run()
|
|||
// if not auto armed set throttle to zero and exit immediately
|
||||
if(!ap.auto_armed) {
|
||||
attitude_control.relax_bf_rate_controller();
|
||||
attitude_control.set_yaw_target_to_current_heading();
|
||||
attitude_control.set_throttle_out(0, false);
|
||||
// set target to current position
|
||||
wp_nav.init_loiter_target();
|
||||
|
|
|
@ -214,6 +214,7 @@ static void autotune_run()
|
|||
// this should not actually be possible because of the autotune_init() checks
|
||||
if(!ap.auto_armed) {
|
||||
attitude_control.relax_bf_rate_controller();
|
||||
attitude_control.set_yaw_target_to_current_heading();
|
||||
attitude_control.set_throttle_out(0, false);
|
||||
return;
|
||||
}
|
||||
|
@ -241,6 +242,7 @@ static void autotune_run()
|
|||
// reset target lean angles and heading while landed
|
||||
if (ap.land_complete) {
|
||||
attitude_control.relax_bf_rate_controller();
|
||||
attitude_control.set_yaw_target_to_current_heading();
|
||||
// move throttle to minimum to keep us on the ground
|
||||
attitude_control.set_throttle_out(0, false);
|
||||
}else{
|
||||
|
|
|
@ -36,6 +36,7 @@ static void circle_run()
|
|||
if(!ap.auto_armed || ap.land_complete) {
|
||||
// To-Do: add some initialisation of position controllers
|
||||
attitude_control.relax_bf_rate_controller();
|
||||
attitude_control.set_yaw_target_to_current_heading();
|
||||
attitude_control.set_throttle_out(0, false);
|
||||
return;
|
||||
}
|
||||
|
|
|
@ -45,6 +45,7 @@ static void drift_run()
|
|||
// if not armed or landed and throttle at zero, set throttle to zero and exit immediately
|
||||
if(!motors.armed() || (ap.land_complete && g.rc_3.control_in <= 0)) {
|
||||
attitude_control.relax_bf_rate_controller();
|
||||
attitude_control.set_yaw_target_to_current_heading();
|
||||
attitude_control.set_throttle_out(0, false);
|
||||
return;
|
||||
}
|
||||
|
|
|
@ -54,6 +54,7 @@ static void guided_run()
|
|||
if(!ap.auto_armed) {
|
||||
// To-Do: reset waypoint controller?
|
||||
attitude_control.relax_bf_rate_controller();
|
||||
attitude_control.set_yaw_target_to_current_heading();
|
||||
attitude_control.set_throttle_out(0, false);
|
||||
// To-Do: handle take-offs - these may not only be immediately after auto_armed becomes true
|
||||
return;
|
||||
|
|
|
@ -158,6 +158,7 @@ static void hybrid_run()
|
|||
if(!ap.auto_armed || !inertial_nav.position_ok()) {
|
||||
wp_nav.init_loiter_target();
|
||||
attitude_control.relax_bf_rate_controller();
|
||||
attitude_control.set_yaw_target_to_current_heading();
|
||||
attitude_control.set_throttle_out(0, false);
|
||||
return;
|
||||
}
|
||||
|
@ -186,6 +187,7 @@ static void hybrid_run()
|
|||
if (ap.land_complete) {
|
||||
wp_nav.init_loiter_target();
|
||||
attitude_control.relax_bf_rate_controller();
|
||||
attitude_control.set_yaw_target_to_current_heading();
|
||||
attitude_control.set_throttle_out(0, false);
|
||||
return;
|
||||
}else{
|
||||
|
|
|
@ -48,6 +48,7 @@ static void land_gps_run()
|
|||
// if not auto armed or landed set throttle to zero and exit immediately
|
||||
if(!ap.auto_armed || ap.land_complete) {
|
||||
attitude_control.relax_bf_rate_controller();
|
||||
attitude_control.set_yaw_target_to_current_heading();
|
||||
attitude_control.set_throttle_out(0, false);
|
||||
wp_nav.init_loiter_target();
|
||||
|
||||
|
@ -103,6 +104,7 @@ static void land_nogps_run()
|
|||
// if not auto armed or landed set throttle to zero and exit immediately
|
||||
if(!ap.auto_armed || ap.land_complete) {
|
||||
attitude_control.relax_bf_rate_controller();
|
||||
attitude_control.set_yaw_target_to_current_heading();
|
||||
attitude_control.set_throttle_out(0, false);
|
||||
#if LAND_REQUIRE_MIN_THROTTLE_TO_DISARM == ENABLED
|
||||
// disarm when the landing detector says we've landed and throttle is at minimum
|
||||
|
|
|
@ -36,6 +36,7 @@ static void loiter_run()
|
|||
if(!ap.auto_armed || !inertial_nav.position_ok()) {
|
||||
wp_nav.init_loiter_target();
|
||||
attitude_control.relax_bf_rate_controller();
|
||||
attitude_control.set_yaw_target_to_current_heading();
|
||||
attitude_control.set_throttle_out(0, false);
|
||||
return;
|
||||
}
|
||||
|
@ -70,6 +71,7 @@ static void loiter_run()
|
|||
if (ap.land_complete) {
|
||||
wp_nav.init_loiter_target();
|
||||
attitude_control.relax_bf_rate_controller();
|
||||
attitude_control.set_yaw_target_to_current_heading();
|
||||
attitude_control.set_throttle_out(0, false);
|
||||
}else{
|
||||
// run loiter controller
|
||||
|
|
|
@ -37,6 +37,7 @@ static void ofloiter_run()
|
|||
// if not auto armed set throttle to zero and exit immediately
|
||||
if(!ap.auto_armed) {
|
||||
attitude_control.relax_bf_rate_controller();
|
||||
attitude_control.set_yaw_target_to_current_heading();
|
||||
attitude_control.set_throttle_out(0, false);
|
||||
reset_optflow_I();
|
||||
return;
|
||||
|
@ -68,6 +69,7 @@ static void ofloiter_run()
|
|||
// when landed reset targets and output zero throttle
|
||||
if (ap.land_complete) {
|
||||
attitude_control.relax_bf_rate_controller();
|
||||
attitude_control.set_yaw_target_to_current_heading();
|
||||
attitude_control.set_throttle_out(0, false);
|
||||
reset_optflow_I();
|
||||
}else{
|
||||
|
|
|
@ -128,6 +128,7 @@ static void rtl_climb_return_run()
|
|||
if(!ap.auto_armed) {
|
||||
// reset attitude control targets
|
||||
attitude_control.relax_bf_rate_controller();
|
||||
attitude_control.set_yaw_target_to_current_heading();
|
||||
attitude_control.set_throttle_out(0, false);
|
||||
// To-Do: re-initialise wpnav targets
|
||||
return;
|
||||
|
@ -185,6 +186,7 @@ static void rtl_loiterathome_run()
|
|||
if(!ap.auto_armed) {
|
||||
// reset attitude control targets
|
||||
attitude_control.relax_bf_rate_controller();
|
||||
attitude_control.set_yaw_target_to_current_heading();
|
||||
attitude_control.set_throttle_out(0, false);
|
||||
// To-Do: re-initialise wpnav targets
|
||||
return;
|
||||
|
@ -252,6 +254,7 @@ static void rtl_descent_run()
|
|||
// if not auto armed set throttle to zero and exit immediately
|
||||
if(!ap.auto_armed || !inertial_nav.position_ok()) {
|
||||
attitude_control.relax_bf_rate_controller();
|
||||
attitude_control.set_yaw_target_to_current_heading();
|
||||
attitude_control.set_throttle_out(0, false);
|
||||
// set target to current position
|
||||
wp_nav.init_loiter_target();
|
||||
|
@ -311,6 +314,7 @@ static void rtl_land_run()
|
|||
// if not auto armed set throttle to zero and exit immediately
|
||||
if(!ap.auto_armed || !inertial_nav.position_ok()) {
|
||||
attitude_control.relax_bf_rate_controller();
|
||||
attitude_control.set_yaw_target_to_current_heading();
|
||||
attitude_control.set_throttle_out(0, false);
|
||||
// set target to current position
|
||||
wp_nav.init_loiter_target();
|
||||
|
|
|
@ -27,6 +27,7 @@ static void sport_run()
|
|||
// if not armed or throttle at zero, set throttle to zero and exit immediately
|
||||
if(!motors.armed() || g.rc_3.control_in <= 0) {
|
||||
attitude_control.relax_bf_rate_controller();
|
||||
attitude_control.set_yaw_target_to_current_heading();
|
||||
attitude_control.set_throttle_out(0, false);
|
||||
return;
|
||||
}
|
||||
|
@ -76,6 +77,7 @@ static void sport_run()
|
|||
// reset target lean angles and heading while landed
|
||||
if (ap.land_complete) {
|
||||
attitude_control.relax_bf_rate_controller();
|
||||
attitude_control.set_yaw_target_to_current_heading();
|
||||
// move throttle to minimum to keep us on the ground
|
||||
attitude_control.set_throttle_out(0, false);
|
||||
}else{
|
||||
|
|
|
@ -26,6 +26,7 @@ static void stabilize_run()
|
|||
// if not armed or throttle at zero, set throttle to zero and exit immediately
|
||||
if(!motors.armed() || g.rc_3.control_in <= 0) {
|
||||
attitude_control.relax_bf_rate_controller();
|
||||
attitude_control.set_yaw_target_to_current_heading();
|
||||
attitude_control.set_throttle_out(0, false);
|
||||
return;
|
||||
}
|
||||
|
@ -46,6 +47,7 @@ static void stabilize_run()
|
|||
// reset target lean angles and heading while landed
|
||||
if (ap.land_complete) {
|
||||
attitude_control.relax_bf_rate_controller();
|
||||
attitude_control.set_yaw_target_to_current_heading();
|
||||
}else{
|
||||
// call attitude controller
|
||||
attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
|
||||
|
|
Loading…
Reference in New Issue