From da34209626032db3c5cc27e194e8acd7866b2002 Mon Sep 17 00:00:00 2001 From: Gone4Dirt Date: Sun, 24 Mar 2024 13:12:44 +0000 Subject: [PATCH] Copter: Heli: Add swashplate logging --- ArduCopter/Copter.cpp | 1 + ArduCopter/Copter.h | 1 + ArduCopter/Log.cpp | 42 +++++++++++++++++++++++++++++++++++++++++- ArduCopter/defines.h | 3 ++- 4 files changed, 45 insertions(+), 2 deletions(-) diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index 0460de12ecf821..74446a3f9ebc67 100644 --- 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 index b3be8285f7f9b1..a4d2422e673422 100644 --- 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 index d1d530ca1355e4..da58598d9b8262 100644 --- 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 @@ -491,7 +518,20 @@ const struct LogStructure Copter::log_structure[] = { // @Field: Throt: Throttle output #if FRAME_CONFIG == HELI_FRAME { LOG_HELI_MSG, sizeof(log_Heli), - "HELI", "Qffff", "TimeUS,DRRPM,ERRPM,Gov,Throt", "s----", "F----" , true }, + "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 index 7f1175552537b7..4771a408c96f82 100644 --- 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)