Copter: Heli: Add swashplate logging

MattKear · Mar 24, 2024 · 523ecd2 · 523ecd2
1 parent d2944f5
commit 523ecd2
Show file tree

Hide file tree

Showing 4 changed files with 44 additions and 1 deletion.
diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp
@@ -577,6 +577,7 @@ void Copter::ten_hz_logging_loop()
     }
 #if FRAME_CONFIG == HELI_FRAME
     Log_Write_Heli();
+    Log_Write_Heli_Swash();
 #endif
 #if AP_WINCH_ENABLED
     if (should_log(MASK_LOG_ANY)) {

diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h
@@ -857,6 +857,7 @@ class Copter : public AP_Vehicle {
     void Log_Video_Stabilisation();
 #if FRAME_CONFIG == HELI_FRAME
     void Log_Write_Heli(void);
+    void Log_Write_Heli_Swash(void);
 #endif
     void Log_Write_Guided_Position_Target(ModeGuided::SubMode submode, const Vector3f& pos_target, bool terrain_alt, const Vector3f& vel_target, const Vector3f& accel_target);
     void Log_Write_Guided_Attitude_Target(ModeGuided::SubMode target_type, float roll, float pitch, float yaw, const Vector3f &ang_vel, float thrust, float climb_rate);

diff --git a/ArduCopter/Log.cpp b/ArduCopter/Log.cpp
@@ -324,6 +324,33 @@ void Copter::Log_Write_Heli()
     };
     logger.WriteBlock(&pkt_heli, sizeof(pkt_heli));
 }
+
+struct PACKED log_Heli_Swash {
+    LOG_PACKET_HEADER;
+    uint64_t time_us;
+    float    collective1_angle;
+    float    collective2_angle;
+    float    collective3_angle;
+    float    collective4_angle;
+    float    cyclic1_angle;
+    float    cyclic2_angle;
+};
+
+// Write a helicopter swashplate packet
+void Copter::Log_Write_Heli_Swash()
+{
+    struct log_Heli_Swash pkt_heli_swash = {
+        LOG_PACKET_HEADER_INIT(LOG_HELI_SWASH_MSG),
+        time_us                 : AP_HAL::micros64(),
+        collective1_angle       : motors->get_coll_angle(0),
+        collective2_angle       : motors->get_coll_angle(1),
+        collective3_angle       : motors->get_coll_angle(2),
+        collective4_angle       : motors->get_coll_angle(3),
+        cyclic1_angle           : motors->get_cyc_angle(0),
+        cyclic2_angle           : motors->get_cyc_angle(1),
+    };
+    logger.WriteBlock(&pkt_heli_swash, sizeof(pkt_heli_swash));
+}
 #endif
 
 // guided position target logging
@@ -492,6 +519,19 @@ const struct LogStructure Copter::log_structure[] = {
 #if FRAME_CONFIG == HELI_FRAME
     { LOG_HELI_MSG, sizeof(log_Heli),
       "HELI",  "Qffff",        "TimeUS,DRRPM,ERRPM,Gov,Throt", "s----", "F----" , true },
+
+
+// @LoggerMessage: SWSH
+// @Description: Helicopter swashplate logging
+// @Field: TimeUS: Time since system startup
+// @Field: Col1: Collective blade pitch angle contribution from collective 1
+// @Field: Col2: Collective blade pitch angle contribution from collective 2 (Dual)
+// @Field: Col3: Collective blade pitch angle contribution from collective 3 (Heli Quad)
+// @Field: Col4: Collective blade pitch angle contribution from collective 4 (Heli Quad)
+// @Field: Cyc1: Cyclic blade pitch angle contribution from first swashplate (Dual-Front)
+// @Field: Cyc2: Cyclic blade pitch angle contribution from second swashplate (Dual-Aft)
+    { LOG_HELI_SWASH_MSG, sizeof(log_Heli_Swash),
+      "SWSH",  "Qffffff",        "TimeUS,Col1,Col2,Col3,Col4,Cyc1,Cyc2", "sdddddd", "F000000" , true },
 #endif
 
 // @LoggerMessage: SIDD

diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h
@@ -95,7 +95,8 @@ enum LoggingParameters {
      LOG_GUIDED_POSITION_TARGET_MSG,
      LOG_SYSIDD_MSG,
      LOG_SYSIDS_MSG,
-     LOG_GUIDED_ATTITUDE_TARGET_MSG
+     LOG_GUIDED_ATTITUDE_TARGET_MSG,
+     LOG_HELI_SWASH_MSG
 };
 
 #define MASK_LOG_ATTITUDE_FAST          (1<<0)