Move RATE logging to attitude controller and log sample time

ArduCopter/Log.cpp

@@ -76,8 +76,8 @@ void Copter::Log_Write_Control_Tuning()
 void Copter::Log_Write_Attitude()
 {
     ahrs.Write_Attitude(attitude_control->get_att_target_euler_rad() * RAD_TO_DEG);
-    ahrs_view->Write_Rate(*motors, *attitude_control, *pos_control);
- }
+    attitude_control->Write_Rate(*pos_control);
+}
 
 // Write PIDS packets
 void Copter::Log_Write_PIDS()

ArduPlane/quadplane.cpp

@@ -1764,7 +1764,7 @@ void QuadPlane::update(void)
         const bool motors_active = in_vtol_mode() || assisted_flight;
         if (motors_active && (motors->get_spool_state() != AP_Motors::SpoolState::SHUT_DOWN)) {
             // log RATE at main loop rate
-            ahrs_view->Write_Rate(*motors, *attitude_control, *pos_control);
+            attitude_control->Write_Rate(*pos_control);
 
             // log CTRL and MOTB at 10 Hz
             if (now - last_ctrl_log_ms > 100) {

ArduSub/ArduSub.cpp

@@ -183,7 +183,7 @@ void Sub::ten_hz_logging_loop()
     // log attitude data if we're not already logging at the higher rate
     if (should_log(MASK_LOG_ATTITUDE_MED) && !should_log(MASK_LOG_ATTITUDE_FAST)) {
         Log_Write_Attitude();
-        ahrs_view.Write_Rate(motors, attitude_control, pos_control);
+        attitude_control.Write_Rate(pos_control);
         if (should_log(MASK_LOG_PID)) {
             logger.Write_PID(LOG_PIDR_MSG, attitude_control.get_rate_roll_pid().get_pid_info());
             logger.Write_PID(LOG_PIDP_MSG, attitude_control.get_rate_pitch_pid().get_pid_info());
@@ -222,7 +222,7 @@ void Sub::twentyfive_hz_logging()
 {
     if (should_log(MASK_LOG_ATTITUDE_FAST)) {
         Log_Write_Attitude();
-        ahrs_view.Write_Rate(motors, attitude_control, pos_control);
+        attitude_control.Write_Rate(pos_control);
         if (should_log(MASK_LOG_PID)) {
             logger.Write_PID(LOG_PIDR_MSG, attitude_control.get_rate_roll_pid().get_pid_info());
             logger.Write_PID(LOG_PIDP_MSG, attitude_control.get_rate_pitch_pid().get_pid_info());

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,51 @@ 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 AC_PosControl &pos_control) const
+{
+    const Vector3f &rate_targets = rate_bf_targets();
+    const Vector3f &accel_target = pos_control.get_accel_target_cmss();
+    const Vector3f &gyro_rate = _rate_gyro;
+    const struct log_Rate pkt_rate{
+        LOG_PACKET_HEADER_INIT(LOG_RATE_MSG),
+        time_us         : _rate_gyro_time_us,
+        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 = _pd_scale_used;
+    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  : _rate_gyro_time_us,
+            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

@@ -428,8 +428,8 @@ class AC_AttitudeControl {
     // purposes
     void set_PD_scale_mult(const Vector3f &pd_scale) { _pd_scale *= pd_scale; }
 
-    // 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; }
+    // write RATE message
+    void Write_Rate(const AC_PosControl &pos_control) const;
 
     // User settable parameters
     static const struct AP_Param::GroupInfo var_info[];
@@ -487,6 +487,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 }

libraries/AC_AutoTune/AC_AutoTune.cpp

@@ -443,7 +443,7 @@ void AC_AutoTune::control_attitude()
 #if HAL_LOGGING_ENABLED
         // log this iterations lean angle and rotation rate
         Log_AutoTuneDetails();
-        ahrs_view->Write_Rate(*motors, *attitude_control, *pos_control);
+        attitude_control->Write_Rate(*pos_control);
         log_pids();
 #endif
 

libraries/AP_AHRS/AP_AHRS_Logging.cpp

@@ -139,50 +139,4 @@ void AP_AHRS_View::Write_AttitudeView(const Vector3f &targets) const
     AP::logger().WriteBlock(&pkt, sizeof(pkt));
 }
 
-// Write a rate packet
-void AP_AHRS_View::Write_Rate(const AP_Motors &motors, const AC_AttitudeControl &attitude_control,
-                                const AC_PosControl &pos_control) const
-{
-    const Vector3f &rate_targets = attitude_control.rate_bf_targets();
-    const Vector3f &accel_target = pos_control.get_accel_target_cmss();
-    const auto timeus = AP_HAL::micros64();
-    const struct log_Rate pkt_rate{
-        LOG_PACKET_HEADER_INIT(LOG_RATE_MSG),
-        time_us         : timeus,
-        control_roll    : degrees(rate_targets.x),
-        roll            : degrees(get_gyro().x),
-        roll_out        : motors.get_roll()+motors.get_roll_ff(),
-        control_pitch   : degrees(rate_targets.y),
-        pitch           : degrees(get_gyro().y),
-        pitch_out       : motors.get_pitch()+motors.get_pitch_ff(),
-        control_yaw     : degrees(rate_targets.z),
-        yaw             : degrees(get_gyro().z),
-        yaw_out         : motors.get_yaw()+motors.get_yaw_ff(),
-        control_accel   : (float)accel_target.z,
-        accel           : (float)(-(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 = attitude_control.get_last_angle_P_scale();
-    const Vector3f &pd_scale = attitude_control.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/AP_AHRS/AP_AHRS_View.h

@@ -174,9 +174,6 @@ class AP_AHRS_View
 
     // Logging Functions
     void Write_AttitudeView(const Vector3f &targets) const;    
-    void Write_Rate(const class AP_Motors &motors,
-                    const class AC_AttitudeControl &attitude_control,
-                    const AC_PosControl &pos_control) const;
 
     float roll;
     float pitch;

libraries/AP_AHRS/LogStructure.h

@@ -105,40 +105,6 @@ struct PACKED log_POS {
     float rel_origin_alt;
 };
 
-// @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;
-};
-
 // @LoggerMessage: VSTB
 // @Description: Log message for video stabilisation software such as Gyroflow
 // @Field: TimeUS: Time since system startup
@@ -200,8 +166,6 @@ struct PACKED log_ATSC {
         "ORGN","QBLLe","TimeUS,Type,Lat,Lng,Alt", "s#DUm", "F-GGB" }, \
     { LOG_POS_MSG, sizeof(log_POS), \
         "POS","QLLfff","TimeUS,Lat,Lng,Alt,RelHomeAlt,RelOriginAlt", "sDUmmm", "FGG000" , true }, \
-    { 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 }, \
     { LOG_ATSC_MSG, sizeof(log_ATSC), \
         "ATSC", "Qffffff",  "TimeUS,AngPScX,AngPScY,AngPScZ,PDScX,PDScY,PDScZ", "s------", "F000000" , true }, \
     { LOG_VIDEO_STABILISATION_MSG, sizeof(log_Video_Stabilisation), \