AP_Torqeedo: logging and debug output

libraries/AP_Torqeedo/AP_Torqeedo.cpp

@@ -20,10 +20,13 @@
 #include <AP_Common/AP_Common.h>
 #include <AP_Math/AP_Math.h>
 #include <SRV_Channel/SRV_Channel.h>
+#include <AP_Logger/AP_Logger.h>
+#include <GCS_MAVLink/GCS.h>
 
 #define TORQEEDO_SERIAL_BAUD        19200   // communication is always at 19200
 #define TORQEEDO_PACKET_HEADER      0xAC    // communication packet header
 #define TORQEEDO_PACKET_FOOTER      0xAD    // communication packer footer
+#define TORQEEDO_LOG_INTERVAL       5       // log debug info at this interval in seconds
 
 extern const AP_HAL::HAL& hal;
 
@@ -54,6 +57,13 @@ const AP_Param::GroupInfo AP_Torqeedo::var_info[] = {
     // @RebootRequired: True
     AP_GROUPINFO("DE_PIN", 3, AP_Torqeedo, _pin_de, -1),
 
+    // @Param: DEBUG
+    // @DisplayName: Torqeedo Debug Level
+    // @Description: Debug level
+    // @Values: 0:None, 1:Logging only, 2:Logging and GCS
+    // @User: Advanced
+    AP_GROUPINFO("DEBUG", 4, AP_Torqeedo, _debug_level, (int8_t)DebugLevel::LOGGING_ONLY),
+
     AP_GROUPEND
 };
 
@@ -148,6 +158,9 @@ void AP_Torqeedo::thread_main()
         if (motor_speed_request_received) {
             send_motor_speed_cmd();
         }
+
+        // logging and debug output
+        log_and_debug();
     }
 }
 
@@ -330,6 +343,48 @@ void AP_Torqeedo::send_motor_speed_cmd()
 
 }
 
+// output logging and debug messages (if required)
+void AP_Torqeedo::log_and_debug()
+{
+    // exit immediately if debugging is disabled
+    if (_debug_level == DebugLevel::NONE) {
+        return;
+    }
+
+    // return if not enough time has passed since last output
+    const uint32_t now_ms = AP_HAL::millis();
+    if (now_ms - _last_debug_ms < (1000 * TORQEEDO_LOG_INTERVAL)) {
+        return;
+    }
+
+    const bool health = healthy();
+    const int16_t mot_speed = health ? _motor_speed : 0;
+
+    // @LoggerMessage: TRQD
+    // @Description: Torqeedo Status
+    // @Field: TimeUS: Time since system startup
+    // @Field: Health: Health
+    // @Field: MotSpeed: Motor Speed (-1000 to 1000)
+    // @Field: SuccCnt: Success Count
+    // @Field: ErrCnt: Error Count
+    AP::logger().Write("TRQD", "TimeUS,Health,MotSpeed,SuccCnt,ErrCnt", "QbhII",
+                       AP_HAL::micros64(),
+                       (int)health,
+                       (int)mot_speed,
+                       (unsigned long)_parse_success_count,
+                       (unsigned long)_parse_error_count);
+
+    if (_debug_level == DebugLevel::LOGGING_AND_GCS) {
+        gcs().send_text(MAV_SEVERITY_INFO,"Trqd h:%u spd:%d succ:%ld err:%ld",
+                (unsigned)health,
+                (int)mot_speed,
+                (unsigned long)_parse_success_count,
+                (unsigned long)_parse_error_count);
+    }
+
+    _last_debug_ms = now_ms;
+}
+
 // get the AP_Torqeedo singleton
 AP_Torqeedo *AP_Torqeedo::get_singleton()
 {

libraries/AP_Torqeedo/AP_Torqeedo.h

@@ -67,6 +67,12 @@ private:
         WAITING_FOR_FOOTER,
     };
 
+    enum class DebugLevel {
+        NONE = 0,
+        LOGGING_ONLY = 1,
+        LOGGING_AND_GCS = 2
+    };
+
     // initialise serial port and gpio pins (run from background thread)
     // returns true on success
     bool init_internals();
@@ -88,10 +94,14 @@ private:
     // value is taken directly from SRV_Channel
     void send_motor_speed_cmd();
 
+    // output logging and debug messages (if required)
+    void log_and_debug();
+
     // parameters
     AP_Int8 _enable;        // 1 if torqeedo feature is enabled
     AP_Int8 _pin_onoff;     // Pin number connected to Torqeedo's on/off pin. -1 to disable turning motor on/off from autopilot
     AP_Int8 _pin_de;        // Pin number connected to RS485 to Serial converter's DE pin. -1 to disable sending commands to motor
+    AP_Enum<DebugLevel> _debug_level;  // debug level
 
     // members
     AP_HAL::UARTDriver *_uart;      // serial port to communicate with motor
@@ -103,6 +113,7 @@ private:
     // health reporting
     uint32_t _last_healthy_ms;      // system time (in millis) that driver was last considered healthy
     HAL_Semaphore _last_healthy_sem;// semaphore protecting reading and updating of _last_healthy_ms
+    uint32_t _last_debug_ms;        // system time (in millis) that debug was last output
 
     // message parsing members
     ParseState _parse_state;        // current state of parsing