Copter: make libraries get EKF control limits themselves

ArduCopter/Copter.h

@@ -294,12 +294,6 @@ private:
     OpticalFlow optflow{ahrs};
 #endif
 
-    // gnd speed limit required to observe optical flow sensor limits
-    float ekfGndSpdLimit;
-
-    // scale factor applied to velocity controller gain to prevent optical flow noise causing excessive angle demand noise
-    float ekfNavVelGainScaler;
-
     // system time in milliseconds of last recorded yaw reset from ekf
     uint32_t ekfYawReset_ms;
     int8_t ekf_primary_core;

ArduCopter/mode.cpp

@@ -23,12 +23,10 @@ Copter::Mode::Mode(void) :
     channel_throttle(copter.channel_throttle),
     channel_yaw(copter.channel_yaw),
     G_Dt(copter.G_Dt),
-    ap(copter.ap),
-    ekfGndSpdLimit(copter.ekfGndSpdLimit),
 #if FRAME_CONFIG == HELI_FRAME
     heli_flags(copter.heli_flags),
 #endif
-    ekfNavVelGainScaler(copter.ekfNavVelGainScaler)
+    ap(copter.ap)
 { };
 
 float Copter::Mode::auto_takeoff_no_nav_alt_cm = 0;
@@ -251,9 +249,6 @@ bool Copter::set_mode(control_mode_t mode, mode_reason_t reason)
 // called at 100hz or more
 void Copter::update_flight_mode()
 {
-    // Update EKF speed limit - used to limit speed when we are using optical flow
-    ahrs.getEkfControlLimits(ekfGndSpdLimit, ekfNavVelGainScaler);
-
     target_rangefinder_alt_used = false;
 
     flightmode->run();
@@ -519,7 +514,7 @@ void Copter::Mode::land_run_horizontal_control()
     loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch, G_Dt);
 
     // run loiter controller
-    loiter_nav->update(ekfGndSpdLimit, ekfNavVelGainScaler);
+    loiter_nav->update();
 
     int32_t nav_roll  = loiter_nav->get_roll();
     int32_t nav_pitch = loiter_nav->get_pitch();

ArduCopter/mode.h

@@ -173,12 +173,6 @@ protected:
     // altitude below which we do no navigation in auto takeoff
     static float auto_takeoff_no_nav_alt_cm;
 
-    // gnd speed limit required to observe optical flow sensor limits
-    float &ekfGndSpdLimit;
-
-    // scale factor applied to velocity controller gain to prevent optical flow noise causing excessive angle demand noise
-    float &ekfNavVelGainScaler;
-
 #if FRAME_CONFIG == HELI_FRAME
     heli_flags_t &heli_flags;
 #endif

ArduCopter/mode_auto.cpp

@@ -980,7 +980,7 @@ void Copter::ModeAuto::payload_place_run_loiter()
     land_run_horizontal_control();
 
     // run loiter controller
-    loiter_nav->update(ekfGndSpdLimit, ekfNavVelGainScaler);
+    loiter_nav->update();
 
     // call attitude controller
     const float target_yaw_rate = 0;

ArduCopter/mode_brake.cpp

@@ -56,7 +56,7 @@ void Copter::ModeBrake::run()
     motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
 
     // run brake controller
-    wp_nav->update_brake(ekfGndSpdLimit, ekfNavVelGainScaler);
+    wp_nav->update_brake();
 
     // call attitude controller
     attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), 0.0f);

ArduCopter/mode_guided.cpp

@@ -497,7 +497,7 @@ void Copter::ModeGuided::vel_control_run()
     }
 
     // call velocity controller which includes z axis controller
-    pos_control->update_vel_controller_xyz(ekfNavVelGainScaler);
+    pos_control->update_vel_controller_xyz();
 
     // call attitude controller
     if (auto_yaw.mode() == AUTO_YAW_HOLD) {
@@ -564,7 +564,7 @@ void Copter::ModeGuided::posvel_control_run()
     pos_control->set_desired_velocity_xy(guided_vel_target_cms.x, guided_vel_target_cms.y);
 
     // run position controllers
-    pos_control->update_xy_controller(ekfNavVelGainScaler);
+    pos_control->update_xy_controller();
     pos_control->update_z_controller();
 
     // call attitude controller

ArduCopter/mode_loiter.cpp

@@ -143,7 +143,7 @@ void Copter::ModeLoiter::run()
         attitude_control->set_yaw_target_to_current_heading();
         pos_control->relax_alt_hold_controllers(0.0f);   // forces throttle output to go to zero
 #endif
-        loiter_nav->update(ekfGndSpdLimit, ekfNavVelGainScaler);
+        loiter_nav->update();
         attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(loiter_nav->get_roll(), loiter_nav->get_pitch(), target_yaw_rate);
         pos_control->update_z_controller();
         break;
@@ -168,7 +168,7 @@ void Copter::ModeLoiter::run()
         target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);
 
         // run loiter controller
-        loiter_nav->update(ekfGndSpdLimit, ekfNavVelGainScaler);
+        loiter_nav->update();
 
         // call attitude controller
         attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(loiter_nav->get_roll(), loiter_nav->get_pitch(), target_yaw_rate);
@@ -206,7 +206,7 @@ void Copter::ModeLoiter::run()
 #endif
 
         // run loiter controller
-        loiter_nav->update(ekfGndSpdLimit, ekfNavVelGainScaler);
+        loiter_nav->update();
 
         // call attitude controller
         attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(loiter_nav->get_roll(), loiter_nav->get_pitch(), target_yaw_rate);

ArduCopter/mode_poshold.cpp

@@ -442,7 +442,7 @@ void Copter::ModePosHold::run()
                     poshold_update_brake_angle_from_velocity(poshold.brake_pitch, -vel_fw);
 
                     // run loiter controller
-                    loiter_nav->update(ekfGndSpdLimit, ekfNavVelGainScaler);
+                    loiter_nav->update();
 
                     // calculate final roll and pitch output by mixing loiter and brake controls
                     poshold.roll = poshold_mix_controls(brake_to_loiter_mix, poshold.brake_roll + poshold.wind_comp_roll, loiter_nav->get_roll());
@@ -473,7 +473,7 @@ void Copter::ModePosHold::run()
 
                 case POSHOLD_LOITER:
                     // run loiter controller
-                    loiter_nav->update(ekfGndSpdLimit, ekfNavVelGainScaler);
+                    loiter_nav->update();
 
                     // set roll angle based on loiter controller outputs
                     poshold.roll = loiter_nav->get_roll();

ArduCopter/mode_rtl.cpp

@@ -304,7 +304,7 @@ void Copter::ModeRTL::descent_run()
     loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch, G_Dt);
 
     // run loiter controller
-    loiter_nav->update(ekfGndSpdLimit, ekfNavVelGainScaler);
+    loiter_nav->update();
 
     // call z-axis position controller
     pos_control->set_alt_target_with_slew(rtl_path.descent_target.alt, G_Dt);

ArduCopter/mode_throw.cpp

@@ -169,7 +169,7 @@ void Copter::ModeThrow::run()
         motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
 
         // run loiter controller
-        loiter_nav->update(ekfGndSpdLimit, ekfNavVelGainScaler);
+        loiter_nav->update();
 
         // call attitude controller
         attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(loiter_nav->get_roll(), loiter_nav->get_pitch(), 0.0f);