AP_Torqeedo: support connection to motor or tiller

libraries/AP_Torqeedo/AP_Torqeedo.cpp

@@ -27,19 +27,20 @@
 #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
+#define TORQEEDO_SEND_MOTOR_SPEED_INTERVAL_US   300000  // motor speed sent every 0.3sec if connected to motor
 
 extern const AP_HAL::HAL& hal;
 
 // parameters
 const AP_Param::GroupInfo AP_Torqeedo::var_info[] = {
 
-    // @Param: ENABLE
+    // @Param: TYPE
-    // @DisplayName: Torqeedo enable
+    // @DisplayName: Torqeedo connection type
-    // @Description: Torqeedo enable
+    // @Description: Torqeedo connection type
-    // @Values: 0:Disabled, 1:Enabled
+    // @Values: 0:Disabled, 1:Tiller, 2:Motor
     // @User: Standard
     // @RebootRequired: True
-    AP_GROUPINFO_FLAGS("ENABLE", 1, AP_Torqeedo, _enable, 0, AP_PARAM_FLAG_ENABLE),
+    AP_GROUPINFO_FLAGS("TYPE", 1, AP_Torqeedo, _type, (int8_t)ConnectionType::TYPE_DISABLED, AP_PARAM_FLAG_ENABLE),
 
     // @Param: ONOFF_PIN
     // @DisplayName: Torqeedo ON/Off pin
@@ -76,6 +77,11 @@ AP_Torqeedo::AP_Torqeedo()
 // initialise driver
 void AP_Torqeedo::init()
 {
+    // exit immediately if not enabled
+    if (!enabled()) {
+        return;
+    }
+
     // only init once
     if (_initialised) {
         return;
@@ -100,17 +106,19 @@ bool AP_Torqeedo::init_internals()
     _uart->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE);
     _uart->set_unbuffered_writes(true);
 
-    if (_pin_onoff > -1) {
+    // if using tiller connection set on/off pin for 0.5 sec to turn on battery
-        // initialise on/off pin and set for 0.5 sec to turn on motor
+    if (_type == ConnectionType::TYPE_TILLER) {
-        hal.gpio->pinMode(_pin_onoff, HAL_GPIO_OUTPUT);
+        if (_pin_onoff > -1) {
-        hal.gpio->write(_pin_onoff, 1);
+            hal.gpio->pinMode(_pin_onoff, HAL_GPIO_OUTPUT);
-        hal.scheduler->delay(500);
+            hal.gpio->write(_pin_onoff, 1);
-        hal.gpio->write(_pin_onoff, 0);
+            hal.scheduler->delay(500);
-    } else {
+            hal.gpio->write(_pin_onoff, 0);
-        // user serial port's RTS pin to turn on motor
+        } else {
-        _uart->set_RTS_pin(true);
+            // use serial port's RTS pin to turn on battery
-        hal.scheduler->delay(500);
+            _uart->set_RTS_pin(true);
-        _uart->set_RTS_pin(false);
+            hal.scheduler->delay(500);
+            _uart->set_RTS_pin(false);
+        }
     }
 
     // initialise RS485 DE pin (when high, allows send to motor)
@@ -124,6 +132,15 @@ bool AP_Torqeedo::init_internals()
     return true;
 }
 
+// returns true if the driver is enabled
+bool AP_Torqeedo::enabled() const
+{
+    if (_type == ConnectionType::TYPE_TILLER || _type == ConnectionType::TYPE_MOTOR) {
+        return true;
+    }
+    return false;
+}
+
 // consume incoming messages from motor, reply with latest motor speed
 // runs in background thread
 void AP_Torqeedo::thread_main()
@@ -143,20 +160,31 @@ void AP_Torqeedo::thread_main()
 
         // parse incoming characters
         uint32_t nbytes = MIN(_uart->available(), 1024U);
-        bool motor_speed_request_received = false;
         while (nbytes-- > 0) {
             int16_t b = _uart->read();
             if (b >= 0 ) {
                 if (parse_byte((uint8_t)b)) {
                     // request received to send updated motor speed
-                    motor_speed_request_received = true;
+                    if (_type == ConnectionType::TYPE_TILLER) {
+                        _send_motor_speed = true;
+                    }
                 }
             }
         }
 
         // send motor speed
-        if (motor_speed_request_received) {
+        if (safe_to_send()) {
-            send_motor_speed_cmd();
+            // if connected to motor send motor speed every 0.5sec
+            if (_type == ConnectionType::TYPE_MOTOR &&
+                (AP_HAL::micros() - _last_send_motor_us > TORQEEDO_SEND_MOTOR_SPEED_INTERVAL_US)) {\
+                _send_motor_speed = true;
+            }
+
+            // send motor speed
+            if (_send_motor_speed) {
+                send_motor_speed_cmd();
+                _send_motor_speed = false;
+            }
         }
 
         // logging and debug output
@@ -179,6 +207,11 @@ bool AP_Torqeedo::healthy()
 // run pre-arm check.  returns false on failure and fills in failure_msg
 bool AP_Torqeedo::pre_arm_checks(char *failure_msg, uint8_t failure_msg_len)
 {
+    // exit immediately if not enabled
+    if (!enabled()) {
+        return true;
+    }
+
     const char *failure_prefix = "Torqeedo:";
     if (!_initialised) {
         hal.util->snprintf(failure_msg, failure_msg_len, "%s not initialised", failure_prefix);
@@ -322,9 +355,18 @@ void AP_Torqeedo::send_motor_speed_cmd()
     // set send pin
     send_start();
 
+    // by default use tiller connection command
     uint8_t mot_speed_cmd_buff[] = {TORQEEDO_PACKET_HEADER, 0x0, 0x0, 0x5, 0x0, HIGHBYTE(_motor_speed), LOWBYTE(_motor_speed), 0x0, TORQEEDO_PACKET_FOOTER};
     uint8_t buff_size = ARRAY_SIZE(mot_speed_cmd_buff);
 
+    // update message if using motor connection
+    if (_type == ConnectionType::TYPE_MOTOR) {
+        mot_speed_cmd_buff[1] = 0x30;
+        mot_speed_cmd_buff[2] = 0x82;
+        mot_speed_cmd_buff[3] = _motor_speed == 0 ? 0 : 0x1;    // enable motor
+        mot_speed_cmd_buff[4] = _motor_speed == 0 ? 0 : 0x64;   // motor power from 0 to 100
+    }
+
     // calculate crc and add to buffer
     const uint8_t crc = crc8_maxim(&mot_speed_cmd_buff[1], buff_size-3);
     mot_speed_cmd_buff[buff_size-2] = crc;

libraries/AP_Torqeedo/AP_Torqeedo.h

@@ -67,6 +67,13 @@ private:
         WAITING_FOR_FOOTER,
     };
 
+    // TYPE parameter values
+    enum class ConnectionType {
+        TYPE_DISABLED = 0,
+        TYPE_TILLER = 1,
+        TYPE_MOTOR = 2
+    };
+
     enum class DebugLevel {
         NONE = 0,
         LOGGING_ONLY = 1,
@@ -77,10 +84,16 @@ private:
     // returns true on success
     bool init_internals();
 
+    // returns true if the driver is enabled
+    bool enabled() const;
+
     // process a single byte received on serial port
     // return true if a this driver should send a set-motor-speed message
     bool parse_byte(uint8_t b);
 
+    // returns true if it is safe to send a message
+    bool safe_to_send() const { return (_send_delay_us == 0); }
+
     // set pin to enable sending commands to motor
     void send_start();
 
@@ -98,7 +111,7 @@ private:
     void log_and_debug();
 
     // parameters
-    AP_Int8 _enable;        // 1 if torqeedo feature is enabled
+    AP_Enum<ConnectionType> _type;      // connector type used (0:disabled, 1:tiller connector, 2: motor connector)
     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
@@ -106,6 +119,7 @@ private:
     // members
     AP_HAL::UARTDriver *_uart;      // serial port to communicate with motor
     bool _initialised;              // true once driver has been initialised
+    bool _send_motor_speed;         // true if motor speed should be sent at next opportunity
     int16_t _motor_speed;           // desired motor speed (set from within update method)
     uint32_t _last_send_motor_us;   // system time (in micros) last motor speed command was sent
     uint32_t _send_delay_us;        // delay (in micros) to allow bytes to be sent after which pin can be unset.  0 if not delaying