AP_Winch: Options param for init state and verbose output

libraries/AP_Winch/AP_Winch.cpp

@@ -2,6 +2,7 @@
 
 #if AP_WINCH_ENABLED
 
+#include <GCS_MAVLink/GCS.h>
 #include "AP_Winch_PWM.h"
 #include "AP_Winch_Daiwa.h"
 
@@ -32,6 +33,13 @@ const AP_Param::GroupInfo AP_Winch::var_info[] = {
     // @User: Standard
     AP_GROUPINFO("_POS_P", 3, AP_Winch, config.pos_p, 1.0f),
 
+    // @Param: _OPTIONS
+    // @DisplayName: Winch options
+    // @Description: Winch options
+    // @Bitmask:  0:Spin freely on startup, 1:Verbose output
+    // @User: Standard
+    AP_GROUPINFO("_OPTIONS", 4, AP_Winch, config.options, 3.0f),
+
     // 4 was _RATE_PID
 
     AP_GROUPEND
@@ -81,6 +89,13 @@ void AP_Winch::init()
     }
     if (backend != nullptr) {
         backend->init();
+
+        // initialise control mode
+        if (backend->option_enabled(Options::SpinFreelyOnStartup)) {
+            relax();
+        } else {
+            set_desired_rate(0);
+        }
     }
 }
 
@@ -92,6 +107,11 @@ void AP_Winch::release_length(float length)
     }
     config.length_desired = backend->get_current_length() + length;
     config.control_mode = ControlMode::POSITION;
+
+    // display verbose output to user
+    if (backend->option_enabled(Options::VerboseOutput)) {
+        GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Winch: lowering %4.1fm to %4.1fm", (double)length, (double)config.length_desired);
+    }
 }
 
 // deploy line at specified speed in m/s (+ve deploys line, -ve retracts line, 0 stops)

libraries/AP_Winch/AP_Winch.h

@@ -82,6 +82,12 @@ private:
         DAIWA = 2
     };
 
+    // enum for OPTIONS parameter
+    enum class Options : int16_t {
+        SpinFreelyOnStartup = (1U << 0),
+        VerboseOutput = (1U << 1),
+    };
+
     // winch states
     enum class ControlMode : uint8_t {
         RELAXED = 0,    // winch is realxed
@@ -94,6 +100,7 @@ private:
         AP_Int8     type;               // winch type
         AP_Float    rate_max;           // deploy or retract rate maximum (in m/s).
         AP_Float    pos_p;              // position error P gain
+        AP_Int16    options;            // options bitmask
         ControlMode control_mode;       // state of winch control (using target position or target rate)
         float       length_desired;     // target desired length (in meters)
         float       rate_desired;       // target deploy rate (in m/s, +ve = deploying, -ve = retracting)

libraries/AP_Winch/AP_Winch_Backend.h

@@ -42,6 +42,11 @@ public:
     // write log
     virtual void write_log() = 0;
 
+    // helper to check if option enabled
+    bool option_enabled(AP_Winch::Options option) const {
+        return (config.options & uint16_t(option)) != 0;
+    }
+
 protected:
 
     // calculate the pilot desired rate (+ve deploys line, -ve retracts line, 0 stops) from rc input

libraries/AP_Winch/AP_Winch_Daiwa.cpp

@@ -8,6 +8,8 @@
 
 extern const AP_HAL::HAL& hal;
 
+const char* AP_Winch_Daiwa::send_text_prefix = "Winch:";
+
 // true if winch is healthy
 bool AP_Winch_Daiwa::healthy() const
 {
@@ -40,6 +42,9 @@ void AP_Winch_Daiwa::update()
 
     // send outputs to winch
     control_winch();
+
+    // update_user
+    update_user();
 }
 
 //send generator status
@@ -222,4 +227,74 @@ void AP_Winch_Daiwa::control_winch()
     SRV_Channels::set_output_scaled(SRV_Channel::k_winch, scaled_output);
 }
 
+// update user with changes to winch state via send text messages
+void AP_Winch_Daiwa::update_user()
+{
+    // exit immediately if verbose output disabled
+    if (!option_enabled(AP_Winch::Options::VerboseOutput)) {
+        return;
+    }
+
+    // send updates at no more than 2hz
+    uint32_t now_ms = AP_HAL::millis();
+    if (now_ms - user_update.last_ms < 500) {
+        return;
+    }
+    bool update_sent = false;
+
+    // health change
+    const bool now_healthy = healthy();
+    if (user_update.healthy != now_healthy) {
+        user_update.healthy = now_healthy;
+        GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s %s", send_text_prefix, now_healthy ? "healthy" : "not healthy");
+        update_sent = true;
+    }
+
+    // thread end
+    if (latest.thread_end && (user_update.thread_end != latest.thread_end)) {
+        GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s thread end", send_text_prefix);
+        update_sent = true;
+    }
+    user_update.thread_end = latest.thread_end;
+
+    // moving state
+    if (user_update.moving != latest.moving) {
+        // 0:stopped, 1:retracting line, 2:extending line, 3:clutch engaged, 4:zero reset
+        static const char* moving_str[] = {"stopped", "raising", "lowering", "free spinning", "zero reset"};
+        if (latest.moving < ARRAY_SIZE(moving_str)) {
+            GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s %s", send_text_prefix, moving_str[latest.moving]);
+        } else {
+            GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s move state uknown", send_text_prefix);
+        }
+        update_sent = true;
+    }
+    user_update.moving = latest.moving;
+
+    // clutch state
+    if (user_update.clutch != latest.clutch) {
+        // 0:clutch off, 1:clutch engaged weakly, 2:clutch engaged strongly, motor can spin freely
+        static const char* clutch_str[] = {"off", "weak", "strong (free)"};
+        if (user_update.clutch < ARRAY_SIZE(clutch_str)) {
+            GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s clutch %s", send_text_prefix, clutch_str[latest.moving]);
+        } else {
+            GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s clutch state uknown", send_text_prefix);
+        }
+        update_sent = true;
+    }
+    user_update.clutch = latest.clutch;
+
+    // length in meters
+    const float latest_line_length_rounded = roundf(latest.line_length);
+    if (!is_equal(user_update.line_length, latest_line_length_rounded)) {
+        GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s %dm", send_text_prefix, (int)latest_line_length_rounded);
+        update_sent = true;
+    }
+    user_update.line_length = latest_line_length_rounded;
+
+    // record time message last sent to user
+    if (update_sent) {
+        user_update.last_ms = now_ms;
+    }
+}
+
 #endif  // AP_WINCH_DAIWA_ENABLED

libraries/AP_Winch/AP_Winch_Daiwa.h

@@ -106,6 +106,18 @@ private:
         WAITING_FOR_PCB_TEMP,
         WAITING_FOR_MOTOR_TEMP
     } parse_state;
+
+    // update user with changes to winch state via send text messages
+    static const char* send_text_prefix;// send text prefix string to reduce flash cost
+    void update_user();
+    struct {
+        uint32_t last_ms;               // system time of last update to user
+        bool healthy;                   // latest reported health
+        float line_length;
+        bool thread_end;                // true if end of thread has been detected
+        uint8_t moving;                 // 0:stopped, 1:retracting line, 2:extending line, 3:clutch engaged, 4:zero reset
+        uint8_t clutch;                 // 0:clutch off, 1:clutch engaged weakly, 2:clutch engaged strongly, motor can spin freely
+    } user_update;
 };
 
 #endif  // AP_WINCH_DAIWA_ENABLED