AC_AttitudeControl: record latest gyro value and time used for logging

move Write_Rate() to AC_AttitudeControl
move RATE log structure to AC_AttitudeControl

libraries/AC_AttitudeControl/AC_AttitudeControl.cpp

@@ -1,7 +1,9 @@
 #include "AC_AttitudeControl.h"
+#include "AC_PosControl.h"
 #include <AP_HAL/AP_HAL.h>
 #include <AP_Vehicle/AP_Vehicle_Type.h>
 #include <AP_Scheduler/AP_Scheduler.h>
+#include <AP_Logger/AP_Logger.h>
 
 extern const AP_HAL::HAL& hal;
 
@@ -1240,3 +1242,52 @@ void AC_AttitudeControl::get_rpy_srate(float &roll_srate, float &pitch_srate, fl
     pitch_srate = get_rate_pitch_pid().get_pid_info().slew_rate;
     yaw_srate = get_rate_yaw_pid().get_pid_info().slew_rate;
 }
+
+#if HAL_LOGGING_ENABLED
+// Write a rate packet
+void AC_AttitudeControl::Write_Rate(const AP_Motors &motors, const AC_PosControl &pos_control) const
+{
+    Vector3f rate_targets = rate_bf_targets();
+    const Vector3f &accel_target = pos_control.get_accel_target_cmss();
+    const Vector3f gyro_rate = get_gyro_latest();
+    const auto timeus = get_gyro_latest_timestamp_us();
+    const struct log_Rate pkt_rate{
+        LOG_PACKET_HEADER_INIT(LOG_RATE_MSG),
+        time_us         : timeus,
+        control_roll    : degrees(rate_targets.x),
+        roll            : degrees(gyro_rate.x),
+        roll_out        : motors.get_roll()+motors.get_roll_ff(),
+        control_pitch   : degrees(rate_targets.y),
+        pitch           : degrees(gyro_rate.y),
+        pitch_out       : motors.get_pitch()+motors.get_pitch_ff(),
+        control_yaw     : degrees(rate_targets.z),
+        yaw             : degrees(gyro_rate.z),
+        yaw_out         : motors.get_yaw()+motors.get_yaw_ff(),
+        control_accel   : (float)accel_target.z,
+        accel           : (float)(-(_ahrs.get_accel_ef().z + GRAVITY_MSS) * 100.0f),
+        accel_out       : motors.get_throttle(),
+        throttle_slew   : motors.get_throttle_slew_rate()
+    };
+    AP::logger().WriteBlock(&pkt_rate, sizeof(pkt_rate));
+
+    /*
+      log P/PD gain scale if not == 1.0
+     */
+    const Vector3f &scale = get_last_angle_P_scale();
+    const Vector3f &pd_scale = get_PD_scale_logging();
+    if (scale != AC_AttitudeControl::VECTORF_111 || pd_scale != AC_AttitudeControl::VECTORF_111) {
+        const struct log_ATSC pkt_ATSC {
+            LOG_PACKET_HEADER_INIT(LOG_ATSC_MSG),
+            time_us  : timeus,
+            scaleP_x : scale.x,
+            scaleP_y : scale.y,
+            scaleP_z : scale.z,
+            scalePD_x : pd_scale.x,
+            scalePD_y : pd_scale.y,
+            scalePD_z : pd_scale.z,
+        };
+        AP::logger().WriteBlock(&pkt_ATSC, sizeof(pkt_ATSC));
+    }
+}
+
+#endif  // HAL_LOGGING_ENABLED

libraries/AC_AttitudeControl/AC_AttitudeControl.h

@@ -431,6 +431,16 @@ class AC_AttitudeControl {
     // get the value of the PD scale that was used in the last loop, for logging
     const Vector3f &get_PD_scale_logging(void) const { return _pd_scale_used; }
 
+    // get the latest gyro value that was used by the rate controller
+    const Vector3f &get_gyro_latest(void) const { return _rate_gyro; }
+
+    // get the timestamp of the latest gyro value that was used by the rate controller
+    // primarily used by logging
+    uint64_t get_gyro_latest_timestamp_us(void) const { return _rate_gyro_time_us; }
+
+    // write RATE message
+    void Write_Rate(const class AP_Motors &motors, const AC_PosControl &pos_control) const;
+
     // User settable parameters
     static const struct AP_Param::GroupInfo var_info[];
 
@@ -487,6 +497,11 @@ class AC_AttitudeControl {
     AP_Float            _land_pitch_mult;
     AP_Float            _land_yaw_mult;
 
+    // latest gyro value use by the rate_controller
+    Vector3f            _rate_gyro;
+    // timestamp of the latest gyro value used by the rate controller
+    uint64_t            _rate_gyro_time_us;
+
     // Intersampling period in seconds
     float               _dt;
 

libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp

@@ -422,20 +422,21 @@ void AC_AttitudeControl_Heli::rate_controller_run()
 {	
     _ang_vel_body += _sysid_ang_vel_body;
 
-    Vector3f gyro_latest = _ahrs.get_gyro_latest();
+    _rate_gyro = _ahrs.get_gyro_latest();
+    _rate_gyro_time_us = AP_HAL::micros64();
 
     // call rate controllers and send output to motors object
     // if using a flybar passthrough roll and pitch directly to motors
     if (_flags_heli.flybar_passthrough) {
         _motors.set_roll(_passthrough_roll / 4500.0f);
         _motors.set_pitch(_passthrough_pitch / 4500.0f);
     } else {
-        rate_bf_to_motor_roll_pitch(gyro_latest, _ang_vel_body.x, _ang_vel_body.y);
+        rate_bf_to_motor_roll_pitch(_rate_gyro, _ang_vel_body.x, _ang_vel_body.y);
     }
     if (_flags_heli.tail_passthrough) {
         _motors.set_yaw(_passthrough_yaw / 4500.0f);
     } else {
-        _motors.set_yaw(rate_target_to_motor_yaw(gyro_latest.z, _ang_vel_body.z));
+        _motors.set_yaw(rate_target_to_motor_yaw(_rate_gyro.z, _ang_vel_body.z));
     }
 
     _sysid_ang_vel_body.zero();

libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp

@@ -449,15 +449,16 @@ void AC_AttitudeControl_Multi::rate_controller_run()
 
     _ang_vel_body += _sysid_ang_vel_body;
 
-    Vector3f gyro_latest = _ahrs.get_gyro_latest();
+    _rate_gyro = _ahrs.get_gyro_latest();
+    _rate_gyro_time_us = AP_HAL::micros64();
 
-    _motors.set_roll(get_rate_roll_pid().update_all(_ang_vel_body.x, gyro_latest.x,  _dt, _motors.limit.roll, _pd_scale.x) + _actuator_sysid.x);
+    _motors.set_roll(get_rate_roll_pid().update_all(_ang_vel_body.x, _rate_gyro.x,  _dt, _motors.limit.roll, _pd_scale.x) + _actuator_sysid.x);
     _motors.set_roll_ff(get_rate_roll_pid().get_ff());
 
-    _motors.set_pitch(get_rate_pitch_pid().update_all(_ang_vel_body.y, gyro_latest.y,  _dt, _motors.limit.pitch, _pd_scale.y) + _actuator_sysid.y);
+    _motors.set_pitch(get_rate_pitch_pid().update_all(_ang_vel_body.y, _rate_gyro.y,  _dt, _motors.limit.pitch, _pd_scale.y) + _actuator_sysid.y);
     _motors.set_pitch_ff(get_rate_pitch_pid().get_ff());
 
-    _motors.set_yaw(get_rate_yaw_pid().update_all(_ang_vel_body.z, gyro_latest.z,  _dt, _motors.limit.yaw, _pd_scale.z) + _actuator_sysid.z);
+    _motors.set_yaw(get_rate_yaw_pid().update_all(_ang_vel_body.z, _rate_gyro.z,  _dt, _motors.limit.yaw, _pd_scale.z) + _actuator_sysid.z);
     _motors.set_yaw_ff(get_rate_yaw_pid().get_ff()*_feedforward_scalar);
 
     _sysid_ang_vel_body.zero();

libraries/AC_AttitudeControl/AC_AttitudeControl_Sub.cpp

@@ -423,10 +423,12 @@ void AC_AttitudeControl_Sub::rate_controller_run()
     // move throttle vs attitude mixing towards desired (called from here because this is conveniently called on every iteration)
     update_throttle_rpy_mix();
 
-    Vector3f gyro_latest = _ahrs.get_gyro_latest();
-    _motors.set_roll(get_rate_roll_pid().update_all(_ang_vel_body.x, gyro_latest.x, _dt, _motors.limit.roll));
-    _motors.set_pitch(get_rate_pitch_pid().update_all(_ang_vel_body.y, gyro_latest.y, _dt, _motors.limit.pitch));
-    _motors.set_yaw(get_rate_yaw_pid().update_all(_ang_vel_body.z, gyro_latest.z, _dt, _motors.limit.yaw));
+    _rate_gyro = _ahrs.get_gyro_latest();
+    _rate_gyro_time_us = AP_HAL::micros64();
+
+    _motors.set_roll(get_rate_roll_pid().update_all(_ang_vel_body.x, _rate_gyro.x, _dt, _motors.limit.roll));
+    _motors.set_pitch(get_rate_pitch_pid().update_all(_ang_vel_body.y, _rate_gyro.y, _dt, _motors.limit.pitch));
+    _motors.set_yaw(get_rate_yaw_pid().update_all(_ang_vel_body.z, _rate_gyro.z, _dt, _motors.limit.yaw));
 
     control_monitor_update();
 }

libraries/AC_AttitudeControl/LogStructure.h

@@ -58,6 +58,40 @@ struct PACKED log_PSCx {
     float accel;
 };
 
+// @LoggerMessage: RATE
+// @Description: Desired and achieved vehicle attitude rates. Not logged in Fixed Wing Plane modes.
+// @Field: TimeUS: Time since system startup
+// @Field: RDes: vehicle desired roll rate
+// @Field: R: achieved vehicle roll rate
+// @Field: ROut: normalized output for Roll
+// @Field: PDes: vehicle desired pitch rate
+// @Field: P: vehicle pitch rate
+// @Field: POut: normalized output for Pitch
+// @Field: Y: achieved vehicle yaw rate
+// @Field: YOut: normalized output for Yaw
+// @Field: YDes: vehicle desired yaw rate
+// @Field: ADes: desired vehicle vertical acceleration
+// @Field: A: achieved vehicle vertical acceleration
+// @Field: AOut: percentage of vertical thrust output current being used
+// @Field: AOutSlew: vertical thrust output slew rate
+struct PACKED log_Rate {
+    LOG_PACKET_HEADER;
+    uint64_t time_us;
+    float   control_roll;
+    float   roll;
+    float   roll_out;
+    float   control_pitch;
+    float   pitch;
+    float   pitch_out;
+    float   control_yaw;
+    float   yaw;
+    float   yaw_out;
+    float   control_accel;
+    float   accel;
+    float   accel_out;
+    float   throttle_slew;
+};
+
 #define PSCx_FMT "Qffffffff"
 #define PSCx_UNITS "smmnnnooo"
 #define PSCx_MULTS "F00000000"
@@ -68,4 +102,6 @@ struct PACKED log_PSCx {
     { LOG_PSCE_MSG, sizeof(log_PSCx), \
       "PSCE", PSCx_FMT, "TimeUS,TPE,PE,DVE,TVE,VE,DAE,TAE,AE", PSCx_UNITS, PSCx_MULTS }, \
     { LOG_PSCD_MSG, sizeof(log_PSCx), \
-      "PSCD", PSCx_FMT, "TimeUS,TPD,PD,DVD,TVD,VD,DAD,TAD,AD", PSCx_UNITS, PSCx_MULTS }
+      "PSCD", PSCx_FMT, "TimeUS,TPD,PD,DVD,TVD,VD,DAD,TAD,AD", PSCx_UNITS, PSCx_MULTS }, \
+    { LOG_RATE_MSG, sizeof(log_Rate), \
+        "RATE", "Qfffffffffffff",  "TimeUS,RDes,R,ROut,PDes,P,POut,YDes,Y,YOut,ADes,A,AOut,AOutSlew", "skk-kk-kk-oo--", "F?????????BB--" , true }