AR_AttitudeControl: add get_desired_turn_rate lat_accel and speed

for reporting use only

libraries/APM_Control/AR_AttitudeControl.cpp

@@ -133,6 +133,10 @@ AR_AttitudeControl::AR_AttitudeControl(AP_AHRS &ahrs) :
 // positive lateral acceleration is to the right.
 float AR_AttitudeControl::get_steering_out_lat_accel(float desired_accel, bool skid_steering, bool motor_limit_left, bool motor_limit_right, bool reversed)
 {
+    // record desired accel for reporting purposes
+    _steer_lat_accel_last_ms = AP_HAL::millis();
+    _desired_lat_accel = desired_accel;
+
     // get speed forward
     float speed;
     if (!get_forward_speed(speed)) {
@@ -173,10 +177,13 @@ float AR_AttitudeControl::get_steering_out_angle_error(float angle_err, bool ski
 // desired yaw rate in radians/sec. Positive yaw is to the right.
 float AR_AttitudeControl::get_steering_out_rate(float desired_rate, bool skid_steering, bool motor_limit_left, bool motor_limit_right, bool reversed)
 {
+    // record desired turn rate for reporting purposes
+    _desired_turn_rate = desired_rate;
+
     // calculate dt
     const uint32_t now = AP_HAL::millis();
     float dt = (now - _steer_turn_last_ms) / 1000.0f;
-    if (_steer_turn_last_ms == 0 || dt > 0.1f) {
+    if (_steer_turn_last_ms == 0 || dt > AR_ATTCONTROL_TIMEOUT) {
         dt = 0.0f;
         _steer_rate_pid.reset_filter();
     } else {
@@ -239,6 +246,37 @@ float AR_AttitudeControl::get_steering_out_rate(float desired_rate, bool skid_st
     return constrain_float(p + i + d, -1.0f, 1.0f);
 }
 
+// get latest desired turn rate in rad/sec (recorded during calls to get_steering_out_rate)
+float AR_AttitudeControl::get_desired_turn_rate() const
+{
+    // return zero if no recent calls to turn rate controller
+    if ((_steer_turn_last_ms == 0) || ((AP_HAL::millis() - _steer_turn_last_ms) > AR_ATTCONTROL_TIMEOUT)) {
+        return 0.0f;
+    }
+    return _desired_turn_rate;
+}
+
+// get latest desired lateral acceleration in m/s/s (recorded during calls to get_steering_out_lat_accel)
+float AR_AttitudeControl::get_desired_lat_accel() const
+{
+    // return zero if no recent calls to lateral acceleration controller
+    if ((_steer_lat_accel_last_ms == 0) || ((AP_HAL::millis() - _steer_lat_accel_last_ms) > AR_ATTCONTROL_TIMEOUT)) {
+        return 0.0f;
+    }
+    return _desired_lat_accel;
+}
+
+// get actual lateral acceleration in m/s/s.  returns true on success
+bool AR_AttitudeControl::get_lat_accel(float &lat_accel) const
+{
+    float speed;
+    if (!get_forward_speed(speed)) {
+        return false;
+    }
+    lat_accel = speed * _ahrs.get_yaw_rate_earth();
+    return true;
+}
+
 // 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
@@ -381,3 +419,13 @@ bool AR_AttitudeControl::get_forward_speed(float &speed) const
     speed = velocity.x*_ahrs.cos_yaw() + velocity.y*_ahrs.sin_yaw();
     return true;
 }
+
+// get latest desired speed recorded during call to get_throttle_out_speed.  For reporting purposes only
+float AR_AttitudeControl::get_desired_speed() const
+{
+    // return zero if no recent calls to speed controller
+    if ((_speed_last_ms == 0) || ((AP_HAL::millis() - _speed_last_ms) > AR_ATTCONTROL_TIMEOUT)) {
+        return 0.0f;
+    }
+    return _desired_speed;
+}

libraries/APM_Control/AR_AttitudeControl.h

@@ -18,6 +18,7 @@
 #define AR_ATTCONTROL_THR_SPEED_D       0.00f
 #define AR_ATTCONTROL_THR_SPEED_FILT    5.00f
 #define AR_ATTCONTROL_DT                0.02f
+#define AR_ATTCONTROL_TIMEOUT           0.1f
 
 // throttle/speed control maximum acceleration/deceleration (in m/s) (_ACCEL_MAX parameter default)
 #define AR_ATTCONTROL_THR_ACCEL_MAX     5.00f
@@ -50,6 +51,15 @@ public:
     // desired yaw rate in radians/sec. Positive yaw is to the right.
     float get_steering_out_rate(float desired_rate, bool skid_steering, bool motor_limit_left, bool motor_limit_right, bool reverse);
 
+    // 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;
+
+    // get latest desired lateral acceleration in m/s/s recorded during calls to get_steering_out_lat_accel.  For reporting purposes only
+    float get_desired_lat_accel() const;
+
+    // get actual lateral acceleration in m/s/s.  returns true on success.  For reporting purposes only
+    bool get_lat_accel(float &lat_accel) const;
+
     //
     // throttle / speed controller
     //
@@ -78,6 +88,9 @@ public:
     // get throttle/speed controller maximum acceleration (also used for deceleration)
     float get_accel_max() const { return MAX(_throttle_accel_max, 0.0f); }
 
+    // get latest desired speed recorded during call to get_throttle_out_speed.  For reporting purposes only
+    float get_desired_speed() const;
+
     // parameter var table
     static const struct AP_Param::GroupInfo var_info[];
 
@@ -95,7 +108,10 @@ private:
     AP_Float _stop_speed;           // speed control stop speed.  Motor outputs to zero once vehicle speed falls below this value
 
     // steering control
+    uint32_t _steer_lat_accel_last_ms;  // system time of last call to lateral acceleration controller (i.e. get_steering_out_lat_accel)
     uint32_t _steer_turn_last_ms;   // system time of last call to steering rate controller
+    float    _desired_lat_accel;    // desired lateral acceleration from latest call to get_steering_out_lat_accel (for reporting purposes)
+    float    _desired_turn_rate;    // desired turn rate either from external caller or from lateral acceleration controller (for reporting purpose)
 
     // throttle control
     uint32_t _speed_last_ms;        // system time of last call to get_throttle_out_speed