AP_Winch: add daiwa driver

libraries/AP_Winch/AP_Winch.cpp

@@ -1,5 +1,6 @@
 #include "AP_Winch.h"
 #include "AP_Winch_Servo.h"
+#include "AP_Winch_Daiwa.h"
 
 extern const AP_HAL::HAL& hal;
 
@@ -10,7 +11,7 @@ const AP_Param::GroupInfo AP_Winch::var_info[] = {
     // @DisplayName: Winch Type
     // @Description: Winch Type
     // @User: Standard
-    // @Values: 0:None, 1:Servo
+    // @Values: 0:None, 1:Servo, 2:Daiwa
     AP_GROUPINFO_FLAGS("_TYPE", 1, AP_Winch, config.type, (int8_t)WinchType::NONE, AP_PARAM_FLAG_ENABLE),
 
     // @Param: _RATE_MAX
@@ -69,6 +70,9 @@ void AP_Winch::init()
     case WinchType::SERVO:
         backend = new AP_Winch_Servo(config);
         break;
+    case WinchType::DAIWA:
+        backend = new AP_Winch_Daiwa(config);
+        break;
     default:
         break;
     }

libraries/AP_Winch/AP_Winch.h

@@ -25,6 +25,7 @@ class AP_Winch_Backend;
 class AP_Winch {
     friend class AP_Winch_Backend;
     friend class AP_Winch_Servo;
+    friend class AP_Winch_Daiwa;
 
 public:
     AP_Winch();
@@ -75,6 +76,7 @@ private:
     enum class WinchType {
         NONE = 0,
         SERVO = 1,
+        DAIWA = 2
     };
 
     // winch states

libraries/AP_Winch/AP_Winch_Daiwa.cpp

@@ -0,0 +1,227 @@
+#include <AP_Winch/AP_Winch_Daiwa.h>
+#include <GCS_MAVLink/GCS.h>
+
+extern const AP_HAL::HAL& hal;
+
+// true if winch is healthy
+bool AP_Winch_Daiwa::healthy() const
+{
+    // healthy if we have received data within 3 seconds
+    return (AP_HAL::millis() - data_update_ms < 3000);
+}
+
+void AP_Winch_Daiwa::init()
+{
+    // initialise rc input and output
+    init_input_and_output();
+
+    // initialise serial connection to winch
+    const AP_SerialManager &serial_manager = AP::serialmanager();
+    uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Winch, 0);
+    if (uart != nullptr) {
+        // always use baudrate of 115200
+        uart->begin(115200);
+    }
+}
+
+void AP_Winch_Daiwa::update()
+{
+    // return immediately if no servo is assigned to control the winch
+    if (!SRV_Channels::function_assigned(SRV_Channel::k_winch)) {
+        return;
+    }
+
+    // read latest data from winch
+    read_data_from_winch();
+
+    // read pilot input
+    read_pilot_desired_rate();
+
+    // send outputs to winch
+    control_winch();
+}
+
+//send generator status
+void AP_Winch_Daiwa::send_status(const GCS_MAVLINK &channel)
+{
+    // prepare status bitmask
+    uint32_t status_bitmask = 0;
+    if (healthy()) {
+        status_bitmask |= MAV_WINCH_STATUS_HEALTHY;
+    }
+    if (latest.thread_end) {
+        status_bitmask |= MAV_WINCH_STATUS_FULLY_RETRACTED;
+    }
+    if (latest.moving > 0) {
+        status_bitmask |= MAV_WINCH_STATUS_MOVING;
+    }
+    if (latest.clutch > 0) {
+        status_bitmask |= MAV_WINCH_STATUS_CLUTCH_ENGAGED;
+    }
+
+    // convert speed percentage to absolute speed
+    const float speed_ms = fabsf(config.rate_max) * (float)latest.speed_pct;
+
+    // send status
+    mavlink_msg_winch_status_send(
+        channel.get_chan(),
+        AP_HAL::micros64(),
+        latest.line_length,
+        speed_ms,
+        (float)latest.tension_corrected * 0.01f,
+        latest.voltage,
+        latest.current,
+        latest.motor_temp,
+        status_bitmask);
+}
+
+// write log
+void AP_Winch_Daiwa::write_log()
+{
+    AP::logger().Write_Winch(
+            healthy(),
+            latest.thread_end,
+            latest.moving,
+            latest.clutch,
+            (uint8_t)config.control_mode,
+            config.length_desired,
+            get_current_length(),
+            config.rate_desired,
+            latest.tension_corrected,
+            latest.voltage,
+            constrain_int16(latest.motor_temp, INT8_MIN, INT8_MAX));
+}
+
+// read incoming data from winch and update intermediate and latest structures
+void AP_Winch_Daiwa::read_data_from_winch()
+{
+    // return immediately if serial port is not configured
+    if (uart == nullptr) {
+        return;
+    }
+
+    // read any available characters from serial port and send to GCS
+    int16_t nbytes = uart->available();
+    while (nbytes-- > 0) {
+        int16_t b = uart->read();
+
+        if ((b >= '0' && b <= '9') || (b >= 'A' && b <= 'F') || (b >= 'a' && b <= 'f')) {
+            // add digits to buffer
+            buff[buff_len] = b;
+            buff_len++;
+            if (buff_len >= buff_len_max) {
+                buff_len = 0;
+                parse_state = ParseState::WAITING_FOR_TIME;
+            }
+        } else if (b == ',' || b == '\r') {
+            // comma or carriage return signals end of current value
+            buff[buff_len] = '\0';
+            long int ret = (int32_t)strtol(buff, nullptr, 16);
+            if (ret >= (long)INT32_MAX || ret <= (long)INT32_MIN) {
+                // failed to get valid number, throw away packet
+                parse_state = ParseState::WAITING_FOR_TIME;
+            } else {
+                // parse number received and empty buffer
+                buff_len = 0;
+                switch (parse_state) {
+                case ParseState::WAITING_FOR_TIME:
+                    intermediate.time_ms = (uint32_t)ret;
+                    parse_state = ParseState::WAITING_FOR_SPOOL;
+                    break;
+                case ParseState::WAITING_FOR_SPOOL:
+                    intermediate.line_length = (int32_t)ret * line_length_correction_factor;
+                    parse_state = ParseState::WAITING_FOR_TENSION1;
+                    break;
+                case ParseState::WAITING_FOR_TENSION1:
+                    intermediate.tension_uncorrected = (uint16_t)ret;
+                    parse_state = ParseState::WAITING_FOR_TENSION2;
+                    break;
+                case ParseState::WAITING_FOR_TENSION2:
+                    intermediate.tension_corrected = (uint16_t)ret;
+                    parse_state = ParseState::WAITING_FOR_THREAD_END;
+                    break;
+                case ParseState::WAITING_FOR_THREAD_END:
+                    intermediate.thread_end = (ret > 0);
+                    parse_state = ParseState::WAITING_FOR_MOVING;
+                    break;
+                case ParseState::WAITING_FOR_MOVING:
+                    intermediate.moving = constrain_int16(ret, 0, UINT8_MAX);
+                    parse_state = ParseState::WAITING_FOR_CLUTCH;
+                    break;
+                case ParseState::WAITING_FOR_CLUTCH:
+                    intermediate.clutch = constrain_int16(ret, 0, UINT8_MAX);
+                    parse_state = ParseState::WAITING_FOR_SPEED;
+                    break;
+                case ParseState::WAITING_FOR_SPEED:
+                    intermediate.speed_pct = constrain_int16(ret, 0, UINT8_MAX);
+                    parse_state = ParseState::WAITING_FOR_VOLTAGE;
+                    break;
+                case ParseState::WAITING_FOR_VOLTAGE:
+                    intermediate.voltage = (float)ret * 0.1f;
+                    parse_state = ParseState::WAITING_FOR_CURRENT;
+                    break;
+                case ParseState::WAITING_FOR_CURRENT:
+                    intermediate.current = (float)ret * 0.1f;
+                    parse_state = ParseState::WAITING_FOR_PCB_TEMP;
+                    break;
+                case ParseState::WAITING_FOR_PCB_TEMP:
+                    intermediate.pcb_temp = (float)ret * 0.1f;
+                    parse_state = ParseState::WAITING_FOR_MOTOR_TEMP;
+                    break;
+                case ParseState::WAITING_FOR_MOTOR_TEMP:
+                    intermediate.motor_temp = (float)ret * 0.1f;
+                    // successfully parsed a complete message
+                    latest = intermediate;
+                    data_update_ms = AP_HAL::millis();
+                    parse_state = ParseState::WAITING_FOR_TIME;
+                    break;
+                }
+            }
+        } else {
+            // carriage return or unexpected characters
+            buff_len = 0;
+            parse_state = ParseState::WAITING_FOR_TIME;
+        }
+    }
+}
+
+// update pwm outputs to control winch
+void AP_Winch_Daiwa::control_winch()
+{
+    uint32_t now_ms = AP_HAL::millis();
+    float dt = (now_ms - control_update_ms) / 1000.0f;
+    if (dt > 1.0f) {
+        dt = 0.0f;
+    }
+    control_update_ms = now_ms;
+
+    // if real doing any control output trim value
+    if (config.control_mode == AP_Winch::ControlMode::RELAXED) {
+        // if not doing any control output release clutch and move winch to trim
+        SRV_Channels::set_output_limit(SRV_Channel::k_winch_clutch, SRV_Channel::Limit::MAX);
+        SRV_Channels::set_output_scaled(SRV_Channel::k_winch, 0);
+
+        // rate used for acceleration limiting reset to zero
+        set_previous_rate(0.0f);
+        return;
+    }
+
+    // release clutch
+    SRV_Channels::set_output_limit(SRV_Channel::k_winch_clutch, SRV_Channel::Limit::MIN);
+
+    // if doing position control, calculate position error to desired rate
+    if ((config.control_mode == AP_Winch::ControlMode::POSITION) && healthy()) {
+        float position_error = config.length_desired - latest.line_length;
+        config.rate_desired = constrain_float(position_error * config.pos_p, -config.rate_max, config.rate_max);
+    }
+
+    // apply acceleration limited to rate
+    const float rate_limited = get_rate_limited_by_accel(config.rate_desired, dt);
+
+    // use linear interpolation to calculate output to move winch at desired rate
+    int16_t scaled_output = 0;
+    if (!is_zero(rate_limited)) {
+        scaled_output = linear_interpolate(output_dz, 1000, fabsf(rate_limited), 0, config.rate_max) * (is_positive(rate_limited) ? 1.0f : -1.0f);
+    }
+    SRV_Channels::set_output_scaled(SRV_Channel::k_winch, scaled_output);
+}

libraries/AP_Winch/AP_Winch_Daiwa.h

@@ -0,0 +1,96 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include <AP_Winch/AP_Winch_Backend.h>
+#include <SRV_Channel/SRV_Channel.h>
+
+class AP_Winch_Daiwa : public AP_Winch_Backend {
+public:
+
+    AP_Winch_Daiwa(struct AP_Winch::Backend_Config &_config) :
+        AP_Winch_Backend(_config) { }
+
+    // true if winch is healthy
+    bool healthy() const override;
+
+    // initialise the winch
+    void init() override;
+
+    // control the winch
+    void update() override;
+
+    // returns current length of line deployed
+    float get_current_length() const override { return latest.line_length; }
+
+    // send status to ground station
+    void send_status(const GCS_MAVLINK &channel) override;
+
+    // write log
+    void write_log() override;
+
+private:
+
+    // read incoming data from winch and update intermediate and latest structures
+    void read_data_from_winch();
+
+    // update pwm outputs to control winch
+    void control_winch();
+
+    static const uint8_t buff_len_max = 20; // buffer maximum length
+    static const int16_t output_dz = 100;   // output deadzone in scale of -1000 to +1000
+    const float line_length_correction_factor = 0.0357f;    // convert winch counter to meters
+
+    AP_HAL::UARTDriver *uart;
+    char buff[buff_len_max];    // buffer holding latest data from winch
+    uint8_t buff_len;           // number of bytes in buff
+
+    // winch data
+    // latest holds most recent complete data received
+    // intermediate holds partial results currently being processed
+    struct WinchData {
+        uint32_t time_ms;               // winch system time in milliseconds
+        float line_length;              // length of line released in meters
+        uint16_t tension_uncorrected;   // uncorrected tension in grams (0 to 1024)
+        uint16_t tension_corrected;     // corrected tension in grams (0 to 1024)
+        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
+        uint8_t speed_pct;              // speed motor is moving as a percentage
+        float voltage;                  // battery voltage (in voltes)
+        float current;                  // current draw (in amps)
+        float pcb_temp;                 // PCB temp in C
+        float motor_temp;               // motor temp in C
+    } latest, intermediate;
+    uint32_t data_update_ms;            // system time that latest was last updated
+    uint32_t control_update_ms;         // last time control_winch was called
+
+    // parsing state
+    enum class ParseState : uint8_t {
+        WAITING_FOR_TIME = 0,
+        WAITING_FOR_SPOOL,
+        WAITING_FOR_TENSION1,
+        WAITING_FOR_TENSION2,
+        WAITING_FOR_THREAD_END,
+        WAITING_FOR_MOVING,
+        WAITING_FOR_CLUTCH,
+        WAITING_FOR_SPEED,
+        WAITING_FOR_VOLTAGE,
+        WAITING_FOR_CURRENT,
+        WAITING_FOR_PCB_TEMP,
+        WAITING_FOR_MOTOR_TEMP
+    } parse_state;
+};