From 26ca75efae245b7883e6a2fd03973770740cfd3b Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 5 Sep 2018 16:47:50 +1000 Subject: [PATCH] Copter: make libraries get EKF control limits themselves --- ArduCopter/Copter.h | 6 ------ ArduCopter/mode.cpp | 9 ++------- ArduCopter/mode.h | 6 ------ ArduCopter/mode_auto.cpp | 2 +- ArduCopter/mode_brake.cpp | 2 +- ArduCopter/mode_guided.cpp | 4 ++-- ArduCopter/mode_loiter.cpp | 6 +++--- ArduCopter/mode_poshold.cpp | 4 ++-- ArduCopter/mode_rtl.cpp | 2 +- ArduCopter/mode_throw.cpp | 2 +- 10 files changed, 13 insertions(+), 30 deletions(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 9777614f93..dad5cabb6a 100644 --- a/ArduCopter/Copter.h +++ b/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; diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 0cb822f51d..c7134f157a 100644 --- a/ArduCopter/mode.cpp +++ b/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(); diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index ef55e4f1e6..a82d3811cf 100644 --- a/ArduCopter/mode.h +++ b/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 diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index 7cc435c2f3..e406e60192 100644 --- a/ArduCopter/mode_auto.cpp +++ b/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; diff --git a/ArduCopter/mode_brake.cpp b/ArduCopter/mode_brake.cpp index 6acebf3c0f..8d64d618e3 100644 --- a/ArduCopter/mode_brake.cpp +++ b/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); diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index cd0a779a20..96ae5df141 100644 --- a/ArduCopter/mode_guided.cpp +++ b/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 diff --git a/ArduCopter/mode_loiter.cpp b/ArduCopter/mode_loiter.cpp index 0a5df5dd37..6b1ca34ec9 100644 --- a/ArduCopter/mode_loiter.cpp +++ b/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); diff --git a/ArduCopter/mode_poshold.cpp b/ArduCopter/mode_poshold.cpp index 79792f7c93..a3fd32b927 100644 --- a/ArduCopter/mode_poshold.cpp +++ b/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(); diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp index d1c983f35e..9be244cb9c 100644 --- a/ArduCopter/mode_rtl.cpp +++ b/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); diff --git a/ArduCopter/mode_throw.cpp b/ArduCopter/mode_throw.cpp index a0f93d93d0..431e0741c8 100644 --- a/ArduCopter/mode_throw.cpp +++ b/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);