Plane: Quadplane: log tailsitter speed scailing in TSIT msg

ArduPilot · Dec 15, 2023 · b8c840e · b8c840e
1 parent 56cc78b
commit b8c840e
Show file tree

Hide file tree

Showing 6 changed files with 39 additions and 8 deletions.
diff --git a/ArduPlane/Log.cpp b/ArduPlane/Log.cpp
@@ -383,12 +383,11 @@ const struct LogStructure Plane::log_structure[] = {
 // @Field: DCRt: desired climb rate
 // @Field: CRt: climb rate
 // @Field: TMix: transition throttle mix value
-// @Field: Sscl: speed scalar for tailsitter control surfaces
 // @Field: Trn: Transition state: 0-AirspeedWait,1-Timer,2-Done / TailSitter: 0-FW Wait,1-VTOL Wait,2-Done
 // @Field: Ast: Q assist active
 #if HAL_QUADPLANE_ENABLED
     { LOG_QTUN_MSG, sizeof(QuadPlane::log_QControl_Tuning),
-      "QTUN", "QffffffeccffBB", "TimeUS,ThI,ABst,ThO,ThH,DAlt,Alt,BAlt,DCRt,CRt,TMix,Sscl,Trn,Ast", "s----mmmnn----", "F----00000-0--" , true },
+      "QTUN", "QffffffeccfBB", "TimeUS,ThI,ABst,ThO,ThH,DAlt,Alt,BAlt,DCRt,CRt,TMix,Trn,Ast", "s----mmmnn---", "F----00000---" , true },
 #endif
 
 // @LoggerMessage: PIQR,PIQP,PIQY,PIQA
@@ -406,6 +405,7 @@ const struct LogStructure Plane::log_structure[] = {
 // @Field: SRate: slew rate
 // @Field: Flags: bitmask of PID state flags
 // @FieldBitmaskEnum: Flags: log_PID_Flags
+#if HAL_QUADPLANE_ENABLED
     { LOG_PIQR_MSG, sizeof(log_PID),
       "PIQR", PID_FMT,  PID_LABELS, PID_UNITS, PID_MULTS , true },
     { LOG_PIQP_MSG, sizeof(log_PID),
@@ -414,6 +414,18 @@ const struct LogStructure Plane::log_structure[] = {
       "PIQY", PID_FMT,  PID_LABELS, PID_UNITS, PID_MULTS , true },
     { LOG_PIQA_MSG, sizeof(log_PID),
       "PIQA", PID_FMT,  PID_LABELS, PID_UNITS, PID_MULTS , true },
+#endif
+
+// @LoggerMessage: TSIT
+// @Description: tailsitter speed scailing values
+// @Field: TimeUS: Time since system startup
+// @Field: Ts: throttle scailing used for tilt motors
+// @Field: Ss: speed scailing used for control surfaces method from Q_TAILSIT_GSCMSK
+// @Field: Tmin: minimum output throttle caculated from disk thoery gain scale with Q_TAILSIT_MIN_VO
+#if HAL_QUADPLANE_ENABLED
+    { LOG_TSIT_MSG, sizeof(Tailsitter::log_tailsitter),
+      "TSIT", "Qfff",  "TimeUS,Ts,Ss,Tmin", "s---", "F---" , true },
+#endif
 
 // @LoggerMessage: PIDG
 // @Description: Plane Proportional/Integral/Derivative gain values for Heading when using COMMAND_INT control.

diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h
@@ -102,6 +102,7 @@ enum log_messages {
     LOG_PIDG_MSG,
     LOG_AETR_MSG,
     LOG_OFG_MSG,
+    LOG_TSIT_MSG,
 };
 
 #define MASK_LOG_ATTITUDE_FAST          (1<<0)

diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp
@@ -3663,7 +3663,6 @@ void QuadPlane::Log_Write_QControl_Tuning()
         target_climb_rate   : target_climb_rate_cms,
         climb_rate          : int16_t(inertial_nav.get_velocity_z_up_cms()),
         throttle_mix        : attitude_control->get_throttle_mix(),
-        speed_scaler        : tailsitter.log_spd_scaler,
         transition_state    : transition->get_log_transition_state(),
         assist              : assisted_flight,
     };

diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h
@@ -156,7 +156,6 @@ class QuadPlane
         int16_t  target_climb_rate;
         int16_t  climb_rate;
         float    throttle_mix;
-        float    speed_scaler;
         uint8_t  transition_state;
         uint8_t  assist;
     };

diff --git a/ArduPlane/tailsitter.cpp b/ArduPlane/tailsitter.cpp
@@ -755,9 +755,6 @@ void Tailsitter::speed_scaling(void)
         spd_scaler /= plane.barometer.get_air_density_ratio();
     }
 
-    // record for QTUN log
-    log_spd_scaler = spd_scaler;
-
     const SRV_Channel::Aux_servo_function_t functions[] = {
         SRV_Channel::Aux_servo_function_t::k_aileron,
         SRV_Channel::Aux_servo_function_t::k_elevator,
@@ -778,6 +775,18 @@ void Tailsitter::speed_scaling(void)
     if (tailsitter_motors != nullptr) {
         tailsitter_motors->set_min_throttle(disk_loading_min_throttle);
     }
+
+    // Write log, this is currently at loop rate
+    // we may want to consider reate limiting in the future
+    struct log_tailsitter pkt = {
+        LOG_PACKET_HEADER_INIT(LOG_TSIT_MSG),
+        time_us             : AP_HAL::micros64(),
+        throttle_scaler     : throttle_scaler,
+        speed_scaler        : spd_scaler,
+        min_throttle        : disk_loading_min_throttle,
+    };
+    plane.logger.WriteBlock(&pkt, sizeof(pkt));
+
 }
 
 // return true if pitch control should be relaxed

diff --git a/ArduPlane/tailsitter.h b/ArduPlane/tailsitter.h
@@ -17,6 +17,7 @@
 #include <AP_Param/AP_Param.h>
 #include "transition.h"
 #include <AP_Motors/AP_MotorsTailsitter.h>
+#include <AP_Logger/LogStructure.h>
 
 class QuadPlane;
 class AP_MotorsMulticopter;
@@ -68,7 +69,6 @@ friend class Plane;
 
     // tailsitter speed scaler
     float last_spd_scaler = 1.0f; // used to slew rate limiting with TAILSITTER_GSCL_ATT_THR option
-    float log_spd_scaler; // for QTUN log
 
     static const struct AP_Param::GroupInfo var_info[];
 
@@ -110,6 +110,17 @@ friend class Plane;
 
     AP_MotorsTailsitter* tailsitter_motors;
 
+protected:
+
+    // Tailsitter specific log message
+    struct PACKED log_tailsitter {
+        LOG_PACKET_HEADER;
+        uint64_t time_us;
+        float throttle_scaler;
+        float speed_scaler;
+        float min_throttle;
+    };
+
 private:
 
     bool setup_complete;