Browse Source

Copter: integrate init_targets rename to relax_bf_rate_controller

Also remove unnecessary init_targets from flight mode init functions.
master
Randy Mackay 11 years ago
parent
commit
77d5d682c1
  1. 5
      ArduCopter/control_acro.pde
  2. 4
      ArduCopter/control_althold.pde
  3. 8
      ArduCopter/control_auto.pde
  4. 4
      ArduCopter/control_autotune.pde
  5. 2
      ArduCopter/control_circle.pde
  6. 2
      ArduCopter/control_drift.pde
  7. 2
      ArduCopter/control_guided.pde
  8. 4
      ArduCopter/control_hybrid.pde
  9. 4
      ArduCopter/control_land.pde
  10. 4
      ArduCopter/control_loiter.pde
  11. 4
      ArduCopter/control_ofloiter.pde
  12. 8
      ArduCopter/control_rtl.pde
  13. 4
      ArduCopter/control_sport.pde
  14. 4
      ArduCopter/control_stabilize.pde

5
ArduCopter/control_acro.pde

@ -7,8 +7,7 @@ @@ -7,8 +7,7 @@
// acro_init - initialise acro controller
static bool acro_init(bool ignore_checks)
{
// clear stabilized rate errors
attitude_control.init_targets();
// always successfully enter acro
return true;
}
@ -21,7 +20,7 @@ static void acro_run() @@ -21,7 +20,7 @@ static void acro_run()
// if motors not running reset angle targets
if(!motors.armed() || g.rc_3.control_in <= 0) {
attitude_control.init_targets();
attitude_control.relax_bf_rate_controller();
attitude_control.set_throttle_out(0, false);
return;
}

4
ArduCopter/control_althold.pde

@ -28,7 +28,7 @@ static void althold_run() @@ -28,7 +28,7 @@ static void althold_run()
// if not auto armed set throttle to zero and exit immediately
if(!ap.auto_armed) {
// To-Do: reset altitude target if we're somehow not landed?
attitude_control.init_targets();
attitude_control.relax_bf_rate_controller();
attitude_control.set_throttle_out(0, false);
return;
}
@ -56,7 +56,7 @@ static void althold_run() @@ -56,7 +56,7 @@ static void althold_run()
// reset target lean angles and heading while landed
if (ap.land_complete) {
attitude_control.init_targets();
attitude_control.relax_bf_rate_controller();
// move throttle to minimum to keep us on the ground
attitude_control.set_throttle_out(0, false);
}else{

8
ArduCopter/control_auto.pde

@ -97,7 +97,7 @@ static void auto_takeoff_run() @@ -97,7 +97,7 @@ static void auto_takeoff_run()
// if not auto armed set throttle to zero and exit immediately
if(!ap.auto_armed) {
// reset attitude control targets
attitude_control.init_targets();
attitude_control.relax_bf_rate_controller();
attitude_control.set_throttle_out(0, false);
// tell motors to do a slow start
motors.slow_start(true);
@ -145,7 +145,7 @@ static void auto_wp_run() @@ -145,7 +145,7 @@ static void auto_wp_run()
if(!ap.auto_armed) {
// 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.init_targets();
attitude_control.relax_bf_rate_controller();
attitude_control.set_throttle_out(0, false);
// tell motors to do a slow start
motors.slow_start(true);
@ -202,7 +202,7 @@ static void auto_spline_run() @@ -202,7 +202,7 @@ static void auto_spline_run()
if(!ap.auto_armed) {
// 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.init_targets();
attitude_control.relax_bf_rate_controller();
attitude_control.set_throttle_out(0, false);
// tell motors to do a slow start
motors.slow_start(true);
@ -267,7 +267,7 @@ static void auto_land_run() @@ -267,7 +267,7 @@ static void auto_land_run()
{
// if not auto armed set throttle to zero and exit immediately
if(!ap.auto_armed) {
attitude_control.init_targets();
attitude_control.relax_bf_rate_controller();
attitude_control.set_throttle_out(0, false);
// set target to current position
wp_nav.init_loiter_target();

4
ArduCopter/control_autotune.pde

@ -213,7 +213,7 @@ static void autotune_run() @@ -213,7 +213,7 @@ static void autotune_run()
// if not auto armed set throttle to zero and exit immediately
// this should not actually be possible because of the autotune_init() checks
if(!ap.auto_armed) {
attitude_control.init_targets();
attitude_control.relax_bf_rate_controller();
attitude_control.set_throttle_out(0, false);
return;
}
@ -240,7 +240,7 @@ static void autotune_run() @@ -240,7 +240,7 @@ static void autotune_run()
// reset target lean angles and heading while landed
if (ap.land_complete) {
attitude_control.init_targets();
attitude_control.relax_bf_rate_controller();
// move throttle to minimum to keep us on the ground
attitude_control.set_throttle_out(0, false);
}else{

2
ArduCopter/control_circle.pde

@ -35,7 +35,7 @@ static void circle_run() @@ -35,7 +35,7 @@ static void circle_run()
// if not auto armed set throttle to zero and exit immediately
if(!ap.auto_armed || ap.land_complete) {
// To-Do: add some initialisation of position controllers
attitude_control.init_targets();
attitude_control.relax_bf_rate_controller();
attitude_control.set_throttle_out(0, false);
return;
}

2
ArduCopter/control_drift.pde

@ -44,7 +44,7 @@ static void drift_run() @@ -44,7 +44,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.init_targets();
attitude_control.relax_bf_rate_controller();
attitude_control.set_throttle_out(0, false);
return;
}

2
ArduCopter/control_guided.pde

@ -53,7 +53,7 @@ static void guided_run() @@ -53,7 +53,7 @@ static void guided_run()
// if not auto armed set throttle to zero and exit immediately
if(!ap.auto_armed) {
// To-Do: reset waypoint controller?
attitude_control.init_targets();
attitude_control.relax_bf_rate_controller();
attitude_control.set_throttle_out(0, false);
// To-Do: handle take-offs - these may not only be immediately after auto_armed becomes true
return;

4
ArduCopter/control_hybrid.pde

@ -157,7 +157,7 @@ static void hybrid_run() @@ -157,7 +157,7 @@ static void hybrid_run()
// if not auto armed set throttle to zero and exit immediately
if(!ap.auto_armed || !inertial_nav.position_ok()) {
wp_nav.init_loiter_target();
attitude_control.init_targets();
attitude_control.relax_bf_rate_controller();
attitude_control.set_throttle_out(0, false);
return;
}
@ -185,7 +185,7 @@ static void hybrid_run() @@ -185,7 +185,7 @@ static void hybrid_run()
// if landed initialise loiter targets, set throttle to zero and exit
if (ap.land_complete) {
wp_nav.init_loiter_target();
attitude_control.init_targets();
attitude_control.relax_bf_rate_controller();
attitude_control.set_throttle_out(0, false);
return;
}else{

4
ArduCopter/control_land.pde

@ -47,7 +47,7 @@ static void land_gps_run() @@ -47,7 +47,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.init_targets();
attitude_control.relax_bf_rate_controller();
attitude_control.set_throttle_out(0, false);
wp_nav.init_loiter_target();
@ -102,7 +102,7 @@ static void land_nogps_run() @@ -102,7 +102,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.init_targets();
attitude_control.relax_bf_rate_controller();
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

4
ArduCopter/control_loiter.pde

@ -35,7 +35,7 @@ static void loiter_run() @@ -35,7 +35,7 @@ static void loiter_run()
// if not auto armed set throttle to zero and exit immediately
if(!ap.auto_armed || !inertial_nav.position_ok()) {
wp_nav.init_loiter_target();
attitude_control.init_targets();
attitude_control.relax_bf_rate_controller();
attitude_control.set_throttle_out(0, false);
return;
}
@ -69,7 +69,7 @@ static void loiter_run() @@ -69,7 +69,7 @@ static void loiter_run()
// when landed reset targets and output zero throttle
if (ap.land_complete) {
wp_nav.init_loiter_target();
attitude_control.init_targets();
attitude_control.relax_bf_rate_controller();
attitude_control.set_throttle_out(0, false);
}else{
// run loiter controller

4
ArduCopter/control_ofloiter.pde

@ -36,7 +36,7 @@ static void ofloiter_run() @@ -36,7 +36,7 @@ static void ofloiter_run()
// if not auto armed set throttle to zero and exit immediately
if(!ap.auto_armed) {
attitude_control.init_targets();
attitude_control.relax_bf_rate_controller();
attitude_control.set_throttle_out(0, false);
reset_optflow_I();
return;
@ -67,7 +67,7 @@ static void ofloiter_run() @@ -67,7 +67,7 @@ static void ofloiter_run()
// when landed reset targets and output zero throttle
if (ap.land_complete) {
attitude_control.init_targets();
attitude_control.relax_bf_rate_controller();
attitude_control.set_throttle_out(0, false);
reset_optflow_I();
}else{

8
ArduCopter/control_rtl.pde

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

4
ArduCopter/control_sport.pde

@ -26,7 +26,7 @@ static void sport_run() @@ -26,7 +26,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.init_targets();
attitude_control.relax_bf_rate_controller();
attitude_control.set_throttle_out(0, false);
return;
}
@ -75,7 +75,7 @@ static void sport_run() @@ -75,7 +75,7 @@ static void sport_run()
// reset target lean angles and heading while landed
if (ap.land_complete) {
attitude_control.init_targets();
attitude_control.relax_bf_rate_controller();
// move throttle to minimum to keep us on the ground
attitude_control.set_throttle_out(0, false);
}else{

4
ArduCopter/control_stabilize.pde

@ -25,7 +25,7 @@ static void stabilize_run() @@ -25,7 +25,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.init_targets();
attitude_control.relax_bf_rate_controller();
attitude_control.set_throttle_out(0, false);
return;
}
@ -45,7 +45,7 @@ static void stabilize_run() @@ -45,7 +45,7 @@ static void stabilize_run()
// reset target lean angles and heading while landed
if (ap.land_complete) {
attitude_control.init_targets();
attitude_control.relax_bf_rate_controller();
}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…
Cancel
Save