diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index b0b63d818c..3faa23a9c5 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -622,10 +622,6 @@ static byte superslow_loopCounter; // Counter to trigger execution of 1 Hz processes static byte counter_one_herz; -// used to track the elapsed time for navigation PID integral terms -static uint32_t nav_loopTimer; -// Elapsed time since last call to navigation pid functions -static uint32_t dTnav; // % MCU cycles used static float load; @@ -815,9 +811,6 @@ Serial.println(tempaccel.z, DEC); if(g_gps->new_data){ g_gps->new_data = false; - dTnav = millis() - nav_loopTimer; - nav_loopTimer = millis(); - // calculate the plane's desired bearing // ------------------------------------- navigate(); diff --git a/ArduPlane/Attitude.pde b/ArduPlane/Attitude.pde index a1b598dd24..4633366aea 100644 --- a/ArduPlane/Attitude.pde +++ b/ArduPlane/Attitude.pde @@ -48,7 +48,7 @@ static void stabilize() // Calculate dersired servo output for the roll // --------------------------------------------- - g.channel_roll.servo_out = g.pidServoRoll.get_pid((nav_roll - ahrs.roll_sensor), delta_ms_fast_loop, speed_scaler); + g.channel_roll.servo_out = g.pidServoRoll.get_pid((nav_roll - ahrs.roll_sensor), speed_scaler); long tempcalc = nav_pitch + fabs(ahrs.roll_sensor * g.kff_pitch_compensation) + (g.channel_throttle.servo_out * g.kff_throttle_to_pitch) - @@ -57,7 +57,7 @@ static void stabilize() // when flying upside down the elevator control is inverted tempcalc = -tempcalc; } - g.channel_pitch.servo_out = g.pidServoPitch.get_pid(tempcalc, delta_ms_fast_loop, speed_scaler); + g.channel_pitch.servo_out = g.pidServoPitch.get_pid(tempcalc, speed_scaler); // Mix Stick input to allow users to override control surfaces // ----------------------------------------------------------- @@ -154,7 +154,7 @@ static void calc_throttle() energy_error = airspeed_energy_error + (float)altitude_error * 0.098f; // positive energy errors make the throttle go higher - g.channel_throttle.servo_out = g.throttle_cruise + g.pidTeThrottle.get_pid(energy_error, dTnav); + g.channel_throttle.servo_out = g.throttle_cruise + g.pidTeThrottle.get_pid(energy_error); g.channel_throttle.servo_out += (g.channel_pitch.servo_out * g.kff_pitch_to_throttle); g.channel_throttle.servo_out = constrain(g.channel_throttle.servo_out, @@ -176,7 +176,7 @@ static void calc_nav_yaw(float speed_scaler) long error = (long)(-temp.y*100.0); // Control is a feedforward from the aileron control + a PID to coordinate the turn (drive y axis accel to zero) - g.channel_rudder.servo_out = g.kff_rudder_mix * g.channel_roll.servo_out + g.pidServoRudder.get_pid(error, delta_ms_fast_loop, speed_scaler); + g.channel_rudder.servo_out = g.kff_rudder_mix * g.channel_roll.servo_out + g.pidServoRudder.get_pid(error, speed_scaler); #else g.channel_rudder.servo_out = g.kff_rudder_mix * g.channel_roll.servo_out; // XXX probably need something here based on heading @@ -189,9 +189,9 @@ static void calc_nav_pitch() // Calculate the Pitch of the plane // -------------------------------- if (g.airspeed_enabled == true && g.airspeed_use == true) { - nav_pitch = -g.pidNavPitchAirspeed.get_pid(airspeed_error, dTnav); + nav_pitch = -g.pidNavPitchAirspeed.get_pid(airspeed_error); } else { - nav_pitch = g.pidNavPitchAltitude.get_pid(altitude_error, dTnav); + nav_pitch = g.pidNavPitchAltitude.get_pid(altitude_error); } nav_pitch = constrain(nav_pitch, g.pitch_limit_min.get(), g.pitch_limit_max.get()); } @@ -211,7 +211,7 @@ static void calc_nav_roll() // positive error = right turn // Calculate the required roll of the plane // ---------------------------------------- - nav_roll = g.pidNavRoll.get_pid(bearing_error, dTnav, nav_gain_scaler); //returns desired bank angle in degrees*100 + nav_roll = g.pidNavRoll.get_pid(bearing_error, nav_gain_scaler); //returns desired bank angle in degrees*100 nav_roll = constrain(nav_roll, -g.roll_limit.get(), g.roll_limit.get()); Vector3f omega; diff --git a/ArduPlane/config.h b/ArduPlane/config.h index 6cc417ab52..a0860fd2bf 100644 --- a/ArduPlane/config.h +++ b/ArduPlane/config.h @@ -650,10 +650,10 @@ # define NAV_ROLL_P 0.7 #endif #ifndef NAV_ROLL_I -# define NAV_ROLL_I 0.1 +# define NAV_ROLL_I 0.02 #endif #ifndef NAV_ROLL_D -# define NAV_ROLL_D 0.02 +# define NAV_ROLL_D 0.1 #endif #ifndef NAV_ROLL_INT_MAX # define NAV_ROLL_INT_MAX 5 diff --git a/libraries/PID/PID.cpp b/libraries/PID/PID.cpp index 879dd8f47f..931cc52872 100644 --- a/libraries/PID/PID.cpp +++ b/libraries/PID/PID.cpp @@ -15,11 +15,20 @@ const AP_Param::GroupInfo PID::var_info[] PROGMEM = { AP_GROUPEND }; -long -PID::get_pid(int32_t error, uint16_t dt, float scaler) +int32_t +PID::get_pid(int32_t error, float scaler) { + uint32_t tnow = millis(); + uint32_t dt = tnow - _last_t; float output = 0; - float delta_time = (float)dt / 1000.0; + float delta_time; + + if (_last_t == 0 || dt > 1000) { + dt = 0; + } + _last_t = tnow; + + delta_time = (float)dt / 1000.0; // Compute proportional component output += error * _kp; diff --git a/libraries/PID/PID.h b/libraries/PID/PID.h index 384ac0381f..9de654f8e1 100644 --- a/libraries/PID/PID.h +++ b/libraries/PID/PID.h @@ -38,7 +38,7 @@ public: /// /// @returns The updated control output. /// - long get_pid(int32_t error, uint16_t dt, float scaler = 1.0); + int32_t get_pid(int32_t error, float scaler = 1.0); /// Reset the PID integrator /// @@ -86,6 +86,9 @@ private: float _integrator; ///< integrator value int32_t _last_error; ///< last error for derivative float _last_derivative; ///< last derivative for low-pass filter + uint32_t _last_t; + + int32_t _get_pid(int32_t error, uint16_t dt, float scaler); /// Low pass filter cut frequency for derivative calculation. /// diff --git a/libraries/PID/examples/pid/pid.pde b/libraries/PID/examples/pid/pid.pde index 509ed17166..0ec899d489 100644 --- a/libraries/PID/examples/pid/pid.pde +++ b/libraries/PID/examples/pid/pid.pde @@ -6,6 +6,7 @@ #include #include #include +#include #include // ArduPilot RC Library #include // ArduPilot Mega RC Library #include @@ -21,7 +22,7 @@ Arduino_Mega_ISR_Registry isr_registry; APM_RC_APM1 APM_RC; #endif -PID pid(AP_Var::k_key_none, NULL); +PID pid; void setup() { @@ -51,7 +52,7 @@ void loop() delay(20); //rc.read_pwm(); long error = APM_RC.InputCh(0) - radio_trim; - long control = pid.get_pid(error, 20, 1); + long control = pid.get_pid(error, 1); Serial.print("control: "); Serial.println(control,DEC);