From a7fbfe776713d8788c8162f263e70e2359368892 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 22 May 2018 10:04:53 +0900 Subject: [PATCH] AR_AttitudeControl: caller provides dt instead of calculated internally This allows the vehicle's main loop rate to be used instead of an internally calculated dt which suffers from jitter --- libraries/APM_Control/AR_AttitudeControl.cpp | 67 ++++++++++---------- libraries/APM_Control/AR_AttitudeControl.h | 14 ++-- 2 files changed, 42 insertions(+), 39 deletions(-) diff --git a/libraries/APM_Control/AR_AttitudeControl.cpp b/libraries/APM_Control/AR_AttitudeControl.cpp index 54db868a57..60312bc19c 100644 --- a/libraries/APM_Control/AR_AttitudeControl.cpp +++ b/libraries/APM_Control/AR_AttitudeControl.cpp @@ -182,7 +182,7 @@ AR_AttitudeControl::AR_AttitudeControl(AP_AHRS &ahrs) : // return a steering servo output from -1.0 to +1.0 given a desired lateral acceleration rate in m/s/s. // positive lateral acceleration is to the right. -float AR_AttitudeControl::get_steering_out_lat_accel(float desired_accel, bool motor_limit_left, bool motor_limit_right) +float AR_AttitudeControl::get_steering_out_lat_accel(float desired_accel, bool motor_limit_left, bool motor_limit_right, float dt) { // record desired accel for reporting purposes _steer_lat_accel_last_ms = AP_HAL::millis(); @@ -208,11 +208,11 @@ float AR_AttitudeControl::get_steering_out_lat_accel(float desired_accel, bool m // Calculate the desired steering rate given desired_accel and speed const float desired_rate = desired_accel / speed; - return get_steering_out_rate(desired_rate, motor_limit_left, motor_limit_right); + return get_steering_out_rate(desired_rate, motor_limit_left, motor_limit_right, dt); } // return a steering servo output from -1 to +1 given a heading in radians -float AR_AttitudeControl::get_steering_out_heading(float heading_rad, float rate_max, bool motor_limit_left, bool motor_limit_right) +float AR_AttitudeControl::get_steering_out_heading(float heading_rad, float rate_max, bool motor_limit_left, bool motor_limit_right, float dt) { // calculate heading error (in radians) const float yaw_error = wrap_PI(heading_rad - _ahrs.yaw); @@ -224,29 +224,27 @@ float AR_AttitudeControl::get_steering_out_heading(float heading_rad, float rate desired_rate = constrain_float(desired_rate, -rate_max, rate_max); } - return get_steering_out_rate(desired_rate, motor_limit_left, motor_limit_right); + return get_steering_out_rate(desired_rate, motor_limit_left, motor_limit_right, dt); } // return a steering servo output from -1 to +1 given a // desired yaw rate in radians/sec. Positive yaw is to the right. -float AR_AttitudeControl::get_steering_out_rate(float desired_rate, bool motor_limit_left, bool motor_limit_right) +float AR_AttitudeControl::get_steering_out_rate(float desired_rate, bool motor_limit_left, bool motor_limit_right, float dt) { - // calculate dt + // sanity check dt + dt = constrain_float(dt, 0.0f, 1.0f); + + // if not called recently, reset input filter and desired turn rate to actual turn rate (used for accel limiting) const uint32_t now = AP_HAL::millis(); - float dt = (now - _steer_turn_last_ms) / 1000.0f; - if ((_steer_turn_last_ms == 0) || (dt > (AR_ATTCONTROL_TIMEOUT_MS / 1000.0f))) { - dt = 0.0f; + if ((_steer_turn_last_ms == 0) || ((now - _steer_turn_last_ms) > AR_ATTCONTROL_TIMEOUT_MS)) { _steer_rate_pid.reset_filter(); - // reset desired turn rate to actual turn rate for accel limiting _desired_turn_rate = _ahrs.get_yaw_rate_earth(); - } else { - _steer_rate_pid.set_dt(dt); } _steer_turn_last_ms = now; // acceleration limit desired turn rate - const float change_max = radians(_steer_accel_max) * dt; - if (is_positive(dt) && is_positive(change_max)) { + if (is_positive(_steer_accel_max)) { + const float change_max = radians(_steer_accel_max) * dt; desired_rate = constrain_float(desired_rate, _desired_turn_rate - change_max, _desired_turn_rate + change_max); } _desired_turn_rate = desired_rate; @@ -261,6 +259,9 @@ float AR_AttitudeControl::get_steering_out_rate(float desired_rate, bool motor_l // We do this in earth frame to allow for rover leaning over in hard corners const float rate_error = (_desired_turn_rate - _ahrs.get_yaw_rate_earth()); + // set PID's dt + _steer_rate_pid.set_dt(dt); + // record desired rate for logging purposes only _steer_rate_pid.set_desired_rate(_desired_turn_rate); @@ -320,8 +321,11 @@ bool AR_AttitudeControl::get_lat_accel(float &lat_accel) const // return a throttle output from -1 to +1 given a desired speed in m/s (use negative speeds to travel backwards) // motor_limit should be true if motors have hit their upper or lower limits // cruise speed should be in m/s, cruise throttle should be a number from -1 to +1 -float AR_AttitudeControl::get_throttle_out_speed(float desired_speed, bool motor_limit_low, bool motor_limit_high, float cruise_speed, float cruise_throttle) +float AR_AttitudeControl::get_throttle_out_speed(float desired_speed, bool motor_limit_low, bool motor_limit_high, float cruise_speed, float cruise_throttle, float dt) { + // sanity check dt + dt = constrain_float(dt, 0.0f, 1.0f); + // get speed forward float speed; if (!get_forward_speed(speed)) { @@ -330,19 +334,19 @@ float AR_AttitudeControl::get_throttle_out_speed(float desired_speed, bool motor return 0.0f; } - // calculate dt + // if not called recently, reset input filter and desired speed to actual speed (used for accel limiting) const uint32_t now = AP_HAL::millis(); - float dt = (now - _speed_last_ms) / 1000.0f; - if ((_speed_last_ms == 0) || (dt > (AR_ATTCONTROL_TIMEOUT_MS / 1000.0f))) { - dt = 0.0f; + if ((_speed_last_ms == 0) || ((now - _speed_last_ms) > AR_ATTCONTROL_TIMEOUT_MS)) { _throttle_speed_pid.reset_filter(); - } else { - _throttle_speed_pid.set_dt(dt); + _desired_speed = speed; } _speed_last_ms = now; - // record desired speed for next iteration - _desired_speed = desired_speed; + // acceleration limit desired speed + _desired_speed = get_desired_speed_accel_limited(desired_speed, dt); + + // set PID's dt + _throttle_speed_pid.set_dt(dt); // calculate speed error and pass to PID controller const float speed_error = desired_speed - speed; @@ -397,7 +401,7 @@ float AR_AttitudeControl::get_throttle_out_speed(float desired_speed, bool motor } // return a throttle output from -1 to +1 to perform a controlled stop. returns true once the vehicle has stopped -float AR_AttitudeControl::get_throttle_out_stop(bool motor_limit_low, bool motor_limit_high, float cruise_speed, float cruise_throttle, bool &stopped) +float AR_AttitudeControl::get_throttle_out_stop(bool motor_limit_low, bool motor_limit_high, float cruise_speed, float cruise_throttle, float dt, bool &stopped) { // get current system time const uint32_t now = AP_HAL::millis(); @@ -406,7 +410,7 @@ float AR_AttitudeControl::get_throttle_out_stop(bool motor_limit_low, bool motor bool _stopped = (_stop_last_ms != 0) && (now - _stop_last_ms) < 300; // get deceleration limited speed - float desired_speed_limited = get_desired_speed_accel_limited(0.0f); + float desired_speed_limited = get_desired_speed_accel_limited(0.0f, dt); // get speed forward float speed; @@ -432,7 +436,7 @@ float AR_AttitudeControl::get_throttle_out_stop(bool motor_limit_low, bool motor // clear stopped system time _stop_last_ms = 0; // run speed controller to bring vehicle to stop - return get_throttle_out_speed(desired_speed_limited, motor_limit_low, motor_limit_high, cruise_speed, cruise_throttle); + return get_throttle_out_speed(desired_speed_limited, motor_limit_low, motor_limit_high, cruise_speed, cruise_throttle, dt); } } @@ -478,18 +482,17 @@ float AR_AttitudeControl::get_desired_speed() const } // get acceleration limited desired speed -float AR_AttitudeControl::get_desired_speed_accel_limited(float desired_speed) const +float AR_AttitudeControl::get_desired_speed_accel_limited(float desired_speed, float dt) const { - // calculate dt - const uint32_t now = AP_HAL::millis(); - float dt = (now - _speed_last_ms) / 1000.0f; + // sanity check dt + dt = constrain_float(dt, 0.0f, 1.0f); // use previous desired speed as basis for accel limiting float speed_prev = _desired_speed; // if no recent calls to speed controller limit based on current speed - if (is_negative(dt) || (dt > AR_ATTCONTROL_TIMEOUT_MS / 1000.0f)) { - dt = 0.0f; + const uint32_t now = AP_HAL::millis(); + if ((_speed_last_ms == 0) || ((now - _speed_last_ms) > AR_ATTCONTROL_TIMEOUT_MS)) { get_forward_speed(speed_prev); } diff --git a/libraries/APM_Control/AR_AttitudeControl.h b/libraries/APM_Control/AR_AttitudeControl.h index 2917ab749c..af03ad1405 100644 --- a/libraries/APM_Control/AR_AttitudeControl.h +++ b/libraries/APM_Control/AR_AttitudeControl.h @@ -44,15 +44,15 @@ public: // // return a steering servo output from -1.0 to +1.0 given a desired lateral acceleration rate in m/s/s. - // positive lateral acceleration is to the right. - float get_steering_out_lat_accel(float desired_accel, bool motor_limit_left, bool motor_limit_right); + // positive lateral acceleration is to the right. dt should normally be the main loop rate. + float get_steering_out_lat_accel(float desired_accel, bool motor_limit_left, bool motor_limit_right, float dt); // return a steering servo output from -1 to +1 given a heading in radians - float get_steering_out_heading(float heading_rad, float rate_max, bool motor_limit_left, bool motor_limit_right); + float get_steering_out_heading(float heading_rad, float rate_max, bool motor_limit_left, bool motor_limit_right, float dt); // return a steering servo output from -1 to +1 given a // desired yaw rate in radians/sec. Positive yaw is to the right. - float get_steering_out_rate(float desired_rate, bool motor_limit_left, bool motor_limit_right); + float get_steering_out_rate(float desired_rate, bool motor_limit_left, bool motor_limit_right, float dt); // get latest desired turn rate in rad/sec recorded during calls to get_steering_out_rate. For reporting purposes only float get_desired_turn_rate() const; @@ -76,10 +76,10 @@ public: // desired_speed argument should already have been passed through get_desired_speed_accel_limited function // motor_limit should be true if motors have hit their upper or lower limits // cruise speed should be in m/s, cruise throttle should be a number from -1 to +1 - float get_throttle_out_speed(float desired_speed, bool motor_limit_low, bool motor_limit_high, float cruise_speed, float cruise_throttle); + float get_throttle_out_speed(float desired_speed, bool motor_limit_low, bool motor_limit_high, float cruise_speed, float cruise_throttle, float dt); // return a throttle output from -1 to +1 to perform a controlled stop. stopped is set to true once stop has been completed - float get_throttle_out_stop(bool motor_limit_low, bool motor_limit_high, float cruise_speed, float cruise_throttle, bool &stopped); + float get_throttle_out_stop(bool motor_limit_low, bool motor_limit_high, float cruise_speed, float cruise_throttle, float dt, bool &stopped); // low level control accessors for reporting and logging AC_P& get_steering_angle_p() { return _steer_angle_p; } @@ -99,7 +99,7 @@ public: float get_desired_speed() const; // get acceleration limited desired speed - float get_desired_speed_accel_limited(float desired_speed) const; + float get_desired_speed_accel_limited(float desired_speed, float dt) const; // get minimum stopping distance (in meters) given a speed (in m/s) float get_stopping_distance(float speed);