Copter: Heli: Add swashplate logging

ArduPilot · Mar 25, 2024 · 88bef14 · 88bef14
1 parent 3786c60
commit 88bef14
Show file tree

Hide file tree

Showing 4 changed files with 40 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;
+    uint8_t  instance;
+    float    collective_angle;
+    float    cyclic_angle;
+};
+
+// Write a helicopter swashplate packet
+void Copter::Log_Write_Heli_Swash()
+{
+    for (uint8_t i=0; i<motors->MAX_SWASHPLATES; i++) {
+        // If we have written ndata to that instance of swashplate we log it
+        float col = 0.0, cyc = 0.0;
+        if (motors->get_swash_angles(i, col, cyc)) {
+            struct log_Heli_Swash pkt_heli_swash = {
+                LOG_PACKET_HEADER_INIT(LOG_HELI_SWASH_MSG),
+                time_us                 : AP_HAL::micros64(),
+                instance                : i,
+                collective_angle        : col,
+                cyclic_angle            : cyc,
+            };
+            logger.WriteBlock(&pkt_heli_swash, sizeof(pkt_heli_swash));
+        }
+    }
+}
 #endif
 
 // guided position target logging
@@ -492,6 +519,15 @@ 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: I: Swashplate instance
+// @Field: Col: Blade pitch angle contribution from collective
+// @Field: Cyc: Blade pitch angle contribution from cyclic
+    { LOG_HELI_SWASH_MSG, sizeof(log_Heli_Swash),
+      "SWSH",  "QBff",        "TimeUS,I,Col,Cyc", "s#dd", "F-00" , 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)