AP_MotorsHeli: Move heli logging down to motors

libraries/AP_Motors/AP_MotorsHeli.h

@@ -42,7 +42,7 @@ public:
     /// Constructor
     AP_MotorsHeli( uint16_t speed_hz = AP_MOTORS_HELI_SPEED_DEFAULT) :
         AP_Motors(speed_hz),
-        _main_rotor(SRV_Channel::k_heli_rsc, AP_MOTORS_HELI_RSC)
+        _main_rotor(SRV_Channel::k_heli_rsc, AP_MOTORS_HELI_RSC, 0U)
     {
         AP_Param::setup_object_defaults(this, var_info);
     };
@@ -90,18 +90,9 @@ public:
     // get_desired_rotor_speed - gets target rotor speed as a number from 0 ~ 1
     float get_desired_rotor_speed() const { return _main_rotor.get_desired_speed(); }
 
-    // get_main_rotor_speed - estimated rotor speed when no governor or speed sensor used
-    float get_main_rotor_speed() const { return _main_rotor.get_rotor_speed(); }
-
     // return true if the main rotor is up to speed
     bool rotor_runup_complete() const { return _heliflags.rotor_runup_complete; }
 
-    //get rotor governor output
-    float get_governor_output() const { return _main_rotor.get_governor_output(); }
-
-    //get engine throttle output
-    float get_control_output() const { return _main_rotor.get_control_output(); }
-
     // get_motor_mask - returns a bitmask of which outputs are being used for motors or servos (1 means being used)
     //  this can be used to ensure other pwm outputs (i.e. for servos) do not conflict
     virtual uint32_t get_motor_mask() override;

libraries/AP_Motors/AP_MotorsHeli_Dual.cpp

@@ -597,10 +597,14 @@ bool AP_MotorsHeli_Dual::arming_checks(size_t buflen, char *buffer) const
 }
 
 #if HAL_LOGGING_ENABLED
-// Blade angle logging - called at 10 Hz
+// heli motors logging - called at 10 Hz
 void AP_MotorsHeli_Dual::Log_Write(void)
 {
+    // write swashplate log
     _swashplate1.write_log(get_cyclic_angle_scaler(), _collective_min_deg.get(), _collective_max_deg.get(), _collective_min.get(), _collective_max.get());
     _swashplate2.write_log(get_cyclic_angle_scaler(), _collective_min_deg.get(), _collective_max_deg.get(), _collective2_min.get(), _collective2_max.get());
+
+    // write RSC log
+    _main_rotor.write_log();
 }
 #endif

libraries/AP_Motors/AP_MotorsHeli_Dual.h

@@ -57,7 +57,7 @@ public:
     bool arming_checks(size_t buflen, char *buffer) const override;
 
 #if HAL_LOGGING_ENABLED
-    // Blade angle logging - called at 10 Hz
+    // heli motors logging - called at 10 Hz
     void Log_Write(void) override;
 #endif
 

libraries/AP_Motors/AP_MotorsHeli_Quad.cpp

@@ -279,3 +279,11 @@ void AP_MotorsHeli_Quad::servo_test()
 {
     // not implemented
 }
+
+#if HAL_LOGGING_ENABLED
+// heli motors logging - called at 10 Hz
+void AP_MotorsHeli_Quad::Log_Write(void)
+{
+    _main_rotor.write_log();
+}
+#endif

libraries/AP_Motors/AP_MotorsHeli_Quad.h

@@ -40,6 +40,11 @@ public:
     // servo_test - move servos through full range of movement
     void servo_test() override;
 
+#if HAL_LOGGING_ENABLED
+    // heli motors logging - called at 10 Hz
+    void Log_Write(void) override;
+#endif
+
     // var_info for holding Parameter information
     static const struct AP_Param::GroupInfo var_info[];
 

libraries/AP_Motors/AP_MotorsHeli_RSC.cpp

@@ -480,20 +480,13 @@ void AP_MotorsHeli_RSC::update_rotor_runup(float dt)
         _runup_complete = false;
     }
     // if rotor estimated speed is zero, then spooldown has been completed
-    if (get_rotor_speed() <= 0.0f) {
+    if (_rotor_runup_output <= 0.0f) {
         _spooldown_complete = true;
     } else {
         _spooldown_complete = false;
     }
 }
 
-// get_rotor_speed - gets rotor speed either as an estimate, or (ToDO) a measured value
-float AP_MotorsHeli_RSC::get_rotor_speed() const
-{
-    // if no actual measured rotor speed is available, estimate speed based on rotor runup scalar.
-    return _rotor_runup_output;
-}
-
 // write_rsc - outputs pwm onto output rsc channel
 // servo_out parameter is of the range 0 ~ 1
 void AP_MotorsHeli_RSC::write_rsc(float servo_out)
@@ -616,3 +609,31 @@ void AP_MotorsHeli_RSC::governor_reset()
     _governor_engage = false;
     _governor_fault_count = 0;   // reset fault count when governor reset
 }
+
+#if HAL_LOGGING_ENABLED
+// Write a helicopter motors packet
+void AP_MotorsHeli_RSC::write_log(void) const
+{
+    // @LoggerMessage: HRSC
+    // @Description: Helicopter related messages 
+    // @Field: I: Instance, 0=Main, 1=Tail
+    // @Field: TimeUS: Time since system startup
+    // @Field: DRRPM: Desired rotor speed
+    // @Field: ERRPM: Estimated rotor speed
+    // @Field: Gov: Governor Output
+    // @Field: Throt: Throttle output
+
+    // Write to data flash log
+    AP::logger().WriteStreaming("HRSC",
+                        "TimeUS,I,DRRPM,ERRPM,Gov,Throt",
+                        "s#----",
+                        "F-----",
+                        "QBffff",
+                        AP_HAL::micros64(),
+                        _instance,
+                        get_desired_speed(),
+                        _rotor_runup_output,
+                        _governor_output,
+                        get_control_output());
+}
+#endif

libraries/AP_Motors/AP_MotorsHeli_RSC.h

@@ -4,6 +4,7 @@
 #include <AP_Math/AP_Math.h>            // ArduPilot Mega Vector/Matrix math Library
 #include <RC_Channel/RC_Channel.h>
 #include <SRV_Channel/SRV_Channel.h>
+#include <AP_Logger/AP_Logger.h>
 
 // default main rotor speed (ch8 out) as a number from 0 ~ 100
 #define AP_MOTORS_HELI_RSC_SETPOINT             70
@@ -46,9 +47,11 @@ public:
     friend class AP_MotorsHeli_Quad;
 
     AP_MotorsHeli_RSC(SRV_Channel::Aux_servo_function_t aux_fn,
-                      uint8_t default_channel) :
+                      uint8_t default_channel,
+                      uint8_t inst) :
         _aux_fn(aux_fn),
-        _default_channel(default_channel)
+        _default_channel(default_channel),
+        _instance(inst)
     {
         AP_Param::setup_object_defaults(this, var_info);
     };
@@ -81,12 +84,8 @@ public:
     // set_desired_speed - this requires input to be 0-1
     void        set_desired_speed(float desired_speed) { _desired_speed = desired_speed; }
 
-    // get_rotor_speed - estimated rotor speed when no governor or rpm sensor is used
-    float       get_rotor_speed() const;
-
     // functions for autothrottle, throttle curve, governor, idle speed, output to servo
     void        set_governor_output(float governor_output) {_governor_output = governor_output; }
-    float       get_governor_output() const { return _governor_output; }
     void        governor_reset();
     float       get_control_output() const { return _control_output; }
     void        set_idle_output(float idle_output) { _idle_output.set(idle_output); }
@@ -129,7 +128,12 @@ public:
     uint32_t    get_output_mask() const;
 
     // rotor_speed_above_critical - return true if rotor speed is above that critical for flight
-    bool        rotor_speed_above_critical(void) const { return get_rotor_speed() >= get_critical_speed(); }
+    bool        rotor_speed_above_critical(void) const { return _rotor_runup_output >= get_critical_speed(); }
+
+#if HAL_LOGGING_ENABLED
+    // RSC logging
+    void write_log(void) const;
+#endif
 
     // var_info for holding Parameter information
     static const struct AP_Param::GroupInfo var_info[];
@@ -147,6 +151,7 @@ public:
 
 private:
     uint64_t        _last_update_us;
+    const uint8_t   _instance;
 
     // channel setup for aux function
     SRV_Channel::Aux_servo_function_t _aux_fn;

libraries/AP_Motors/AP_MotorsHeli_Single.cpp

@@ -467,7 +467,7 @@ float AP_MotorsHeli_Single::get_yaw_offset(float collective)
         return 0.0;
     }
 
-    if (_heliflags.in_autorotation || (get_control_output() <= _main_rotor.get_idle_output())) {
+    if (_heliflags.in_autorotation || (_main_rotor.get_control_output() <= _main_rotor.get_idle_output())) {
         // Motor is stopped or at idle, and thus not creating torque
         return 0.0;
     }
@@ -695,10 +695,15 @@ bool AP_MotorsHeli_Single::use_tail_RSC() const
 #if HAL_LOGGING_ENABLED
 void AP_MotorsHeli_Single::Log_Write(void)
 {
+    // Write swash plate logging
     // For single heli we have to apply an additional cyclic scaler of sqrt(2.0) because the
     // definition of when we achieve _cyclic_max is different to dual heli. In single, _cyclic_max
     // is limited at sqrt(2.0), in dual it is limited at 1.0
     float cyclic_angle_scaler = get_cyclic_angle_scaler() * sqrtf(2.0);
     _swashplate.write_log(cyclic_angle_scaler, _collective_min_deg.get(), _collective_max_deg.get(), _collective_min.get(), _collective_max.get());
+
+    // Write RSC logging
+    _main_rotor.write_log();
+    _tail_rotor.write_log();
 }
 #endif

libraries/AP_Motors/AP_MotorsHeli_Single.h

@@ -32,7 +32,7 @@ public:
     // constructor
     AP_MotorsHeli_Single(uint16_t speed_hz = AP_MOTORS_HELI_SPEED_DEFAULT) :
         AP_MotorsHeli(speed_hz),
-        _tail_rotor(SRV_Channel::k_heli_tail_rsc, AP_MOTORS_HELI_SINGLE_TAILRSC),
+        _tail_rotor(SRV_Channel::k_heli_tail_rsc, AP_MOTORS_HELI_SINGLE_TAILRSC, 1U),
         _swashplate(AP_MOTORS_MOT_1, AP_MOTORS_MOT_2, AP_MOTORS_MOT_3, AP_MOTORS_MOT_5, 1U)
     {
         AP_Param::setup_object_defaults(this, var_info);