Plane: Add tiltrotor specific log message

ArduPlane/Log.cpp

@@ -439,6 +439,17 @@ const struct LogStructure Plane::log_structure[] = {
       "TSIT", "Qfff",  "TimeUS,Ts,Ss,Tmin", "s---", "F---" , true },
 #endif
 
+// @LoggerMessage: TILT
+// @Description: Tiltrotor tilt values
+// @Field: TimeUS: Time since system startup
+// @Field: Tilt: Current tilt angle, 0 deg vertical, 90 deg horizontal
+// @Field: FL: Front left tilt angle, 0 deg vertical, 90 deg horizontal
+// @Field: FR: Front right tilt angle, 0 deg vertical, 90 deg horizontal
+#if HAL_QUADPLANE_ENABLED
+    { LOG_TILT_MSG, sizeof(Tiltrotor::log_tiltrotor),
+      "TILT", "Qfff",  "TimeUS,Tilt,FL,FR", "sddd", "F---" , true },
+#endif
+
 // @LoggerMessage: PIDG
 // @Description: Plane Proportional/Integral/Derivative gain values for Heading when using COMMAND_INT control.
 // @Field: TimeUS: Time since system startup

ArduPlane/defines.h

@@ -103,6 +103,7 @@ enum log_messages {
     LOG_AETR_MSG,
     LOG_OFG_MSG,
     LOG_TSIT_MSG,
+    LOG_TILT_MSG,
 };
 
 #define MASK_LOG_ATTITUDE_FAST          (1<<0)

ArduPlane/quadplane.cpp

@@ -3658,6 +3658,9 @@ void QuadPlane::Log_Write_QControl_Tuning()
 
     // write multicopter position control message
     pos_control->write_log();
+
+    // Write tiltrotor tilt angle log
+    tiltrotor.write_log();
 }
 #endif
 

ArduPlane/tiltrotor.cpp

@@ -394,6 +394,33 @@ void Tiltrotor::update(void)
     }
 }
 
+#if HAL_LOGGING_ENABLED
+// Write tiltrotor specific log
+void Tiltrotor::write_log()
+{
+    struct log_tiltrotor pkt {
+        LOG_PACKET_HEADER_INIT(LOG_TILT_MSG),
+        time_us      : AP_HAL::micros64(),
+        current_tilt : current_tilt * 90.0,
+    };
+
+    if (type != TILT_TYPE_VECTORED_YAW) {
+        // Left and right tilt are invalid
+        pkt.front_left_tilt = plane.logger.quiet_nanf();
+        pkt.front_right_tilt = plane.logger.quiet_nanf();
+
+    } else {
+        // Calculate tilt angle from servo outputs
+        const float total_angle = 90.0 + tilt_yaw_angle + fixed_angle;
+        const float scale = total_angle * 0.001;
+        pkt.front_left_tilt = (SRV_Channels::get_output_scaled(SRV_Channel::k_tiltMotorLeft) * scale) - tilt_yaw_angle;
+        pkt.front_right_tilt = (SRV_Channels::get_output_scaled(SRV_Channel::k_tiltMotorRight) * scale) - tilt_yaw_angle;
+    }
+
+    plane.logger.WriteBlock(&pkt, sizeof(pkt));
+}
+#endif
+
 /*
   tilt compensation for angle of tilt. When the rotors are tilted the
   roll effect of differential thrust on the tilted rotors is decreased

ArduPlane/tiltrotor.h

@@ -16,6 +16,7 @@
 
 #include <AP_Param/AP_Param.h>
 #include "transition.h"
+#include <AP_Logger/LogStructure.h>
 
 class QuadPlane;
 class AP_MotorsMulticopter;
@@ -69,6 +70,9 @@ public:
     // always return true if not enabled or not a continuous type
     bool tilt_angle_achieved() const { return !enabled() || (type != TILT_TYPE_CONTINUOUS) || angle_achieved; }
 
+    // Write tiltrotor specific log
+    void write_log();
+
     AP_Int8 enable;
     AP_Int16 tilt_mask;
     AP_Int16 max_rate_up_dps;
@@ -98,6 +102,15 @@ public:
 
 private:
 
+    // Tiltrotor specific log message
+    struct PACKED log_tiltrotor {
+        LOG_PACKET_HEADER;
+        uint64_t time_us;
+        float current_tilt;
+        float front_left_tilt;
+        float front_right_tilt;
+    };
+
     bool setup_complete;
 
     // true if a fixed forward motor is setup