AC_AttitudeControl: introduce ANG log message for high resolution att…

…itude logging

Move RATE message to AC_AttitudeControl_Logging.cpp

libraries/AC_AttitudeControl/AC_AttitudeControl.cpp

@@ -1,9 +1,7 @@
 #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;
 
@@ -1242,51 +1240,3 @@ 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

@@ -431,6 +431,9 @@ class AC_AttitudeControl {
     // write RATE message
     void Write_Rate(const AC_PosControl &pos_control) const;
 
+    // write ANG message
+    void Write_Attitude() const;
+
     // User settable parameters
     static const struct AP_Param::GroupInfo var_info[];
 

libraries/AC_AttitudeControl/AC_AttitudeControl_Logging.cpp

@@ -0,0 +1,82 @@
+#include <AP_Logger/AP_Logger_config.h>
+
+#if HAL_LOGGING_ENABLED
+
+#include "AC_AttitudeControl.h"
+#include "AC_PosControl.h"
+#include <AP_Logger/AP_Logger.h>
+#include <AP_Scheduler/AP_Scheduler.h>
+#include "LogStructure.h"
+
+// Write an ANG packet
+void AC_AttitudeControl::Write_Attitude() const
+{
+    Vector3f targets = get_att_target_euler_rad() * RAD_TO_DEG;
+    // Retrieve quaternion body attitude
+    Quaternion attitude_body;
+    _ahrs.get_quat_body_to_ned(attitude_body);
+    // calculate the attitude body euler angles
+    Vector3f attitude_body_angles;
+    attitude_body.to_euler(attitude_body_angles);
+    attitude_body_angles *= RAD_TO_DEG;
+
+    const struct log_ANG pkt{
+        LOG_PACKET_HEADER_INIT(LOG_ANG_MSG),
+        time_us         : AP::scheduler().get_loop_start_time_us(),
+        control_roll    : targets.x,
+        roll            : attitude_body_angles.x,
+        control_pitch   : targets.y,
+        pitch           : attitude_body_angles.y,
+        control_yaw     : wrap_360(targets.z),
+        yaw             : wrap_360(attitude_body_angles.z),
+        sensor_dt       : AP::scheduler().get_last_loop_time_s()
+    };
+    AP::logger().WriteBlock(&pkt, sizeof(pkt));
+}
+
+// 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/LogStructure.h

@@ -5,7 +5,8 @@
 #define LOG_IDS_FROM_AC_ATTITUDECONTROL \
     LOG_PSCN_MSG, \
     LOG_PSCE_MSG, \
-    LOG_PSCD_MSG
+    LOG_PSCD_MSG, \
+    LOG_ANG_MSG
 
 // @LoggerMessage: PSCN
 // @Description: Position Control North
@@ -92,6 +93,28 @@ struct PACKED log_Rate {
     float   throttle_slew;
 };
 
+// @LoggerMessage: ANG
+// @Description: Canonical vehicle attitude
+// @Field: TimeUS: Time of gyro sample
+// @Field: DesRoll: vehicle desired roll
+// @Field: Roll: achieved vehicle roll
+// @Field: DesPitch: vehicle desired pitch
+// @Field: Pitch: achieved vehicle pitch
+// @Field: DesYaw: vehicle desired yaw
+// @Field: Yaw: achieved vehicle yaw
+// @Field: Dt: attitude delta time
+struct PACKED log_ANG {
+    LOG_PACKET_HEADER;
+    uint64_t time_us;
+    float control_roll;
+    float roll;
+    float control_pitch;
+    float pitch;
+    float control_yaw;
+    float yaw;
+    float sensor_dt;
+};
+
 #define PSCx_FMT "Qffffffff"
 #define PSCx_UNITS "smmnnnooo"
 #define PSCx_MULTS "F00000000"
@@ -104,4 +127,6 @@ struct PACKED log_Rate {
     { LOG_PSCD_MSG, sizeof(log_PSCx), \
       "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 }
+        "RATE", "Qfffffffffffff",  "TimeUS,RDes,R,ROut,PDes,P,POut,YDes,Y,YOut,ADes,A,AOut,AOutSlew", "skk-kk-kk-oo--", "F?????????BB--" , true }, \
+    { LOG_ANG_MSG, sizeof(log_ANG),\
+        "ANG", "Qfffffff", "TimeUS,DesRoll,Roll,DesPitch,Pitch,DesYaw,Yaw,Dt", "sddddhhs", "F0000000" , true }