From 72718bb783fd582cd5d65af9e959ac82fe77e4f5 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Mon, 11 Mar 2024 21:10:52 +0900
Subject: [PATCH] AP_Torqeedo: multi backend support

---
 libraries/AP_Torqeedo/AP_Torqeedo.cpp         | 1153 ++---------------
 libraries/AP_Torqeedo/AP_Torqeedo.h           |  344 +----
 libraries/AP_Torqeedo/AP_Torqeedo_Backend.cpp |   31 +
 libraries/AP_Torqeedo/AP_Torqeedo_Backend.h   |   84 ++
 libraries/AP_Torqeedo/AP_Torqeedo_Params.cpp  |   79 ++
 libraries/AP_Torqeedo/AP_Torqeedo_Params.h    |   24 +
 libraries/AP_Torqeedo/AP_Torqeedo_TQBus.cpp   | 1077 +++++++++++++++
 libraries/AP_Torqeedo/AP_Torqeedo_TQBus.h     |  332 +++++
 8 files changed, 1771 insertions(+), 1353 deletions(-)
 create mode 100644 libraries/AP_Torqeedo/AP_Torqeedo_Backend.cpp
 create mode 100644 libraries/AP_Torqeedo/AP_Torqeedo_Backend.h
 create mode 100644 libraries/AP_Torqeedo/AP_Torqeedo_Params.cpp
 create mode 100644 libraries/AP_Torqeedo/AP_Torqeedo_Params.h
 create mode 100644 libraries/AP_Torqeedo/AP_Torqeedo_TQBus.cpp
 create mode 100644 libraries/AP_Torqeedo/AP_Torqeedo_TQBus.h

diff --git a/libraries/AP_Torqeedo/AP_Torqeedo.cpp b/libraries/AP_Torqeedo/AP_Torqeedo.cpp
index 5ff6b672b9..1aac8d689c 100644
--- a/libraries/AP_Torqeedo/AP_Torqeedo.cpp
+++ b/libraries/AP_Torqeedo/AP_Torqeedo.cpp
@@ -19,86 +19,25 @@
 
 #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>
-#include <AP_SerialManager/AP_SerialManager.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 packet footer
-#define TORQEEDO_PACKET_ESCAPE      0xAE    // escape character for handling occurrences of header, footer and this escape bytes in original message
-#define TORQEEDO_PACKET_ESCAPE_MASK 0x80    // byte after ESCAPE character should be XOR'd with this value
-#define TORQEEDO_LOG_TRQD_INTERVAL_MS                   5000// log TRQD message at this interval in milliseconds
-#define TORQEEDO_SEND_MOTOR_SPEED_INTERVAL_MS           100 // motor speed sent at 10hz if connected to motor
-#define TORQEEDO_SEND_MOTOR_STATUS_REQUEST_INTERVAL_MS  400 // motor status requested every 0.4sec if connected to motor
-#define TORQEEDO_SEND_MOTOR_PARAM_REQUEST_INTERVAL_MS   400 // motor param requested every 0.4sec if connected to motor
-#define TORQEEDO_BATT_TIMEOUT_MS    5000    // battery info timeouts after 5 seconds
-#define TORQEEDO_REPLY_TIMEOUT_MS   25      // stop waiting for replies after 25ms
-#define TORQEEDO_ERROR_REPORT_INTERVAL_MAX_MS   10000   // errors reported to user at no less than once every 10 seconds
+#include "AP_Torqeedo_Backend.h"
+#include "AP_Torqeedo_TQBus.h"
 
 extern const AP_HAL::HAL& hal;
 
-// parameters
 const AP_Param::GroupInfo AP_Torqeedo::var_info[] = {
 
-    // @Param: TYPE
-    // @DisplayName: Torqeedo connection type
-    // @Description: Torqeedo connection type
-    // @Values: 0:Disabled, 1:Tiller, 2:Motor
-    // @User: Standard
-    // @RebootRequired: True
-    AP_GROUPINFO_FLAGS("TYPE", 1, AP_Torqeedo, _type, (int8_t)ConnectionType::TYPE_DISABLED, AP_PARAM_FLAG_ENABLE),
+    // 1 to 7 were used for parameters before frontend/backend split
 
-    // @Param: ONOFF_PIN
-    // @DisplayName: Torqeedo ON/Off pin
-    // @Description: Pin number connected to Torqeedo's on/off pin. -1 to use serial port's RTS pin if available
-    // @Values: -1:Disabled,50:AUX1,51:AUX2,52:AUX3,53:AUX4,54:AUX5,55:AUX6
-    // @User: Standard
-    // @RebootRequired: True
-    AP_GROUPINFO("ONOFF_PIN", 2, AP_Torqeedo, _pin_onoff, -1),
+    // @Group: 1_
+    // @Path: AP_Torqeedo_Params.cpp
+    AP_SUBGROUPINFO(_params[0], "1_", 8, AP_Torqeedo, AP_Torqeedo_Params),
 
-    // @Param: DE_PIN
-    // @DisplayName: Torqeedo DE pin
-    // @Description: Pin number connected to RS485 to Serial converter's DE pin. -1 to use serial port's CTS pin if available
-    // @Values: -1:Disabled,50:AUX1,51:AUX2,52:AUX3,53:AUX4,54:AUX5,55:AUX6
-    // @User: Standard
-    // @RebootRequired: True
-    AP_GROUPINFO("DE_PIN", 3, AP_Torqeedo, _pin_de, -1),
-
-    // @Param: OPTIONS
-    // @DisplayName: Torqeedo Options
-    // @Description: Torqeedo Options Bitmask
-    // @Bitmask: 0:Log,1:Send debug to GCS
-    // @User: Advanced
-    AP_GROUPINFO("OPTIONS", 4, AP_Torqeedo, _options, (int8_t)options::LOG),
-
-    // @Param: POWER
-    // @DisplayName: Torqeedo Motor Power
-    // @Description: Torqeedo motor power.  Only applied when using motor connection type (e.g. TRQD_TYPE=2)
-    // @Units: %
-    // @Range: 0 100
-    // @Increment: 1
-    // @User: Advanced
-    AP_GROUPINFO("POWER", 5, AP_Torqeedo, _motor_power, 100),
-
-    // @Param: SLEW_TIME
-    // @DisplayName: Torqeedo Throttle Slew Time
-    // @Description: Torqeedo slew rate specified as the minimum number of seconds required to increase the throttle from 0 to 100%.  Higher values cause a slower response, lower values cause a faster response.  A value of zero disables the limit
-    // @Units: s
-    // @Range: 0 5
-    // @Increment: 0.1
-    // @User: Advanced
-    AP_GROUPINFO("SLEW_TIME", 6, AP_Torqeedo, _slew_time, 2.0),
-
-    // @Param: DIR_DELAY
-    // @DisplayName: Torqeedo Direction Change Delay
-    // @Description: Torqeedo direction change delay.  Output will remain at zero for this many seconds when transitioning between forward and backwards rotation
-    // @Units: s
-    // @Range: 0 5
-    // @Increment: 0.1
-    // @User: Advanced
-    AP_GROUPINFO("DIR_DELAY", 7, AP_Torqeedo, _dir_delay, 1.0),
+#if AP_TORQEEDO_MAX_INSTANCES > 1
+    // @Group: 2_
+    // @Path: AP_Torqeedo_Params.cpp
+    AP_SUBGROUPINFO(_params[1], "2_", 9, AP_Torqeedo, AP_Torqeedo_Params),
+#endif
 
     AP_GROUPEND
 };
@@ -112,156 +51,87 @@ AP_Torqeedo::AP_Torqeedo()
 // initialise driver
 void AP_Torqeedo::init()
 {
-    // exit immediately if not enabled
-    if (!enabled()) {
-        return;
-    }
-
-    // only init once
-    // Note: a race condition exists here if init is called multiple times quickly before thread_main has a chance to set _initialise
-    if (_initialised) {
-        return;
-    }
-
-    // create background thread to process serial input and output
-    if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_Torqeedo::thread_main, void), "torqeedo", 2048, AP_HAL::Scheduler::PRIORITY_RCOUT, 1)) {
-        return;
-    }
-}
-
-// initialise serial port and gpio pins (run from background thread)
-bool AP_Torqeedo::init_internals()
-{
-    // find serial driver and initialise
-    const AP_SerialManager &serial_manager = AP::serialmanager();
-    _uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Torqeedo, 0);
-    if (_uart == nullptr) {
-        return false;
-    }
-    _uart->begin(TORQEEDO_SERIAL_BAUD);
-    _uart->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE);
-    _uart->set_unbuffered_writes(true);
-
-    // if using tiller connection set on/off pin for 0.5 sec to turn on battery
-    if (_type == ConnectionType::TYPE_TILLER) {
-        if (_pin_onoff > -1) {
-            hal.gpio->pinMode(_pin_onoff, HAL_GPIO_OUTPUT);
-            hal.gpio->write(_pin_onoff, 1);
-            hal.scheduler->delay(500);
-            hal.gpio->write(_pin_onoff, 0);
-        } else {
-            // use serial port's RTS pin to turn on battery
-            _uart->set_RTS_pin(true);
-            hal.scheduler->delay(500);
-            _uart->set_RTS_pin(false);
+    // check init has not been called before
+    for (uint8_t i = 1; i < AP_TORQEEDO_MAX_INSTANCES; i++) {
+        if (get_instance(i) != nullptr) {
+            return;
         }
     }
 
-    // initialise RS485 DE pin (when high, allows send to motor)
-    if (_pin_de > -1) {
-        hal.gpio->pinMode(_pin_de, HAL_GPIO_OUTPUT);
-        hal.gpio->write(_pin_de, 0);
-    } else {
-        _uart->set_CTS_pin(false);
+    // create each instance
+    uint8_t instance;
+    for (instance = 0; instance < AP_TORQEEDO_MAX_INSTANCES; instance++) {
+        switch ((ConnectionType)_params[instance].type.get()) {
+        case ConnectionType::TYPE_DISABLED:
+            // do nothing
+            break;
+        case ConnectionType::TYPE_TILLER:
+        case ConnectionType::TYPE_MOTOR:
+            _backends[instance] = new AP_Torqeedo_TQBus(_params[instance], instance);
+            break;
+        }
     }
 
-    return true;
+    // init each instance, do it after all instances were created, so that they all know things
+    for (instance = 0; instance < AP_TORQEEDO_MAX_INSTANCES; instance++) {
+        if (_backends[instance] != nullptr) {
+            _backends[instance]->init();
+        }
+    }
 }
 
-// returns true if the driver is enabled
+// returns true if at least one backend has been configured (e.g. TYPE param has been set)
 bool AP_Torqeedo::enabled() const
 {
-    switch ((ConnectionType)_type) {
-    case ConnectionType::TYPE_DISABLED:
-        return false;
-    case ConnectionType::TYPE_TILLER:
-    case ConnectionType::TYPE_MOTOR:
-        return true;
+    for (uint8_t instance = 0; instance < AP_TORQEEDO_MAX_INSTANCES; instance++) {
+        if (enabled(instance)) {
+            return true;
+        }
     }
-
     return false;
 }
 
-// consume incoming messages from motor, reply with latest motor speed
-// runs in background thread
-void AP_Torqeedo::thread_main()
+// returns true if the instance has been configured (e.g. TYPE param has been set)
+bool AP_Torqeedo::enabled(uint8_t instance) const
 {
-    // initialisation
-    if (!init_internals()) {
-        return;
-    }
-    _initialised = true;
-
-    while (true) {
-        // 1ms loop delay
-        hal.scheduler->delay_microseconds(1000);
-
-        // check if transmit pin should be unset
-        check_for_send_end();
-
-        // check for timeout waiting for reply
-        check_for_reply_timeout();
-
-        // parse incoming characters
-        uint32_t nbytes = MIN(_uart->available(), 1024U);
-        while (nbytes-- > 0) {
-            int16_t b = _uart->read();
-            if (b >= 0 ) {
-                if (parse_byte((uint8_t)b)) {
-                    // complete message received, parse it!
-                    parse_message();
-                    // clear wait-for-reply because if we are waiting for a reply, this message must be it
-                    set_reply_received();
-                }
-            }
+    if (instance < AP_TORQEEDO_MAX_INSTANCES) {
+        switch ((ConnectionType)_params[instance].type.get()) {
+        case ConnectionType::TYPE_DISABLED:
+            return false;
+        case ConnectionType::TYPE_TILLER:
+        case ConnectionType::TYPE_MOTOR:
+            return true;
         }
-
-        // send motor speed
-        bool log_update = false;
-        if (safe_to_send()) {
-            uint32_t now_ms = AP_HAL::millis();
-
-            // if connected to motor
-            if (_type == ConnectionType::TYPE_MOTOR) {
-                if (now_ms - _last_send_motor_ms > TORQEEDO_SEND_MOTOR_SPEED_INTERVAL_MS) {
-                    // send motor speed every 0.1sec
-                    _send_motor_speed = true;
-                } else if (now_ms - _last_send_motor_status_request_ms > TORQEEDO_SEND_MOTOR_STATUS_REQUEST_INTERVAL_MS) {
-                    // send request for motor status
-                    send_motor_msg_request(MotorMsgId::STATUS);
-                    _last_send_motor_status_request_ms = now_ms;
-                } else if (now_ms - _last_send_motor_param_request_ms > TORQEEDO_SEND_MOTOR_PARAM_REQUEST_INTERVAL_MS) {
-                    // send request for motor params
-                    send_motor_msg_request(MotorMsgId::PARAM);
-                    _last_send_motor_param_request_ms = now_ms;
-                }
-            }
-
-            // send motor speed
-            if (_send_motor_speed) {
-                send_motor_speed_cmd();
-                _send_motor_speed = false;
-                log_update = true;
-            }
-        }
-
-        // log high level status and motor speed
-        log_TRQD(log_update);
     }
+    return false;
 }
 
-// returns true if communicating with the motor
+// returns true if all backends are communicating with the motor
 bool AP_Torqeedo::healthy()
 {
-    if (!_initialised) {
+    uint8_t num_backends = 0;
+    uint8_t num_healthy = 0;
+    for (uint8_t instance = 0; instance < AP_TORQEEDO_MAX_INSTANCES; instance++) {
+        auto *backend = get_instance(instance);
+        if (backend != nullptr) {
+            num_backends++;
+            if (backend->healthy()) {
+                num_healthy++;
+            }
+        }
+    }
+
+    return ((num_backends > 0) && (num_healthy == num_backends));
+}
+
+// returns true if instance is healthy
+bool AP_Torqeedo::healthy(uint8_t instance)
+{
+    auto *backend = get_instance(instance);
+    if (backend == nullptr) {
         return false;
     }
-    {
-        // healthy if both receive and send have occurred in the last 3 seconds
-        WITH_SEMAPHORE(_last_healthy_sem);
-        const uint32_t now_ms = AP_HAL::millis();
-        return ((now_ms - _last_received_ms < 3000) && (now_ms - _last_send_motor_ms < 3000));
-    }
+    return backend->healthy();
 }
 
 // run pre-arm check.  returns false on failure and fills in failure_msg
@@ -273,10 +143,6 @@ bool AP_Torqeedo::pre_arm_checks(char *failure_msg, uint8_t failure_msg_len)
         return true;
     }
 
-    if (!_initialised) {
-        strncpy(failure_msg, "not initialised", failure_msg_len);
-        return false;
-    }
     if (!healthy()) {
         strncpy(failure_msg, "not healthy", failure_msg_len);
         return false;
@@ -284,870 +150,58 @@ bool AP_Torqeedo::pre_arm_checks(char *failure_msg, uint8_t failure_msg_len)
     return true;
 }
 
-// returns a human-readable string corresponding the passed-in
-// master error code (see page 93 of https://media.torqeedo.com/downloads/manuals/torqeedo-Travel-manual-DE-EN.pdf)
-// If no conversion is available then nullptr is returned
-const char * AP_Torqeedo::map_master_error_code_to_string(uint8_t code) const
+// clear motor errors
+void AP_Torqeedo::clear_motor_error()
 {
-    switch (code) {
-    case 2:
-        return "stator high temp";
-    case 5:
-        return "propeller blocked";
-    case 6:
-        return "motor low voltage";
-    case 7:
-        return "motor high current";
-    case 8:
-        return "pcb temp high";
-    case 21:
-        return "tiller cal bad";
-    case 22:
-        return "mag bad";
-    case 23:
-        return "range incorrect";
-    case 30:
-        return "motor comm error";
-    case 32:
-        return "tiller comm error";
-    case 33:
-        return "general comm error";
-    case 41:
-    case 42:
-        return "charge voltage bad";
-    case 43:
-        return "battery flat";
-    case 45:
-        return "battery high current";
-    case 46:
-        return "battery temp error";
-    case 48:
-        return "charging temp error";
-    }
-
-    return nullptr;
-}
-
-// report changes in error codes to user
-void AP_Torqeedo::report_error_codes()
-{
-    // skip reporting if we have already reported status very recently
-    const uint32_t now_ms = AP_HAL::millis();
-
-    // skip reporting if no changes in flags and already reported within 10 seconds
-    const bool flags_changed = (_display_system_state_flags_prev.value != _display_system_state.flags.value) ||
-                               (_display_system_state_master_error_code_prev != _display_system_state.master_error_code) ||
-                               (_motor_status_prev.status_flags_value != _motor_status.status_flags_value) ||
-                               (_motor_status_prev.error_flags_value != _motor_status.error_flags_value);
-    if (!flags_changed && ((now_ms - _last_error_report_ms) < TORQEEDO_ERROR_REPORT_INTERVAL_MAX_MS)) {
-        return;
-    }
-
-    // report display system errors
-    const char* msg_prefix = "Torqeedo:";
-    (void)msg_prefix;  // sometimes unused when HAL_GCS_ENABLED is false
-    if (_display_system_state.flags.set_throttle_stop) {
-        GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "%s zero throttle required", msg_prefix);
-    }
-    if (_display_system_state.flags.temp_warning) {
-        GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "%s high temp", msg_prefix);
-    }
-    if (_display_system_state.flags.batt_nearly_empty) {
-        GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "%s batt nearly empty", msg_prefix);
-    }
-    if (_display_system_state.master_error_code > 0) {
-        const char *error_string = map_master_error_code_to_string(_display_system_state.master_error_code);
-        if (error_string != nullptr) {
-            GCS_SEND_TEXT(
-                MAV_SEVERITY_CRITICAL, "%s err:%u %s",
-                msg_prefix,
-                _display_system_state.master_error_code,
-                error_string);
-        } else {
-            GCS_SEND_TEXT(
-                MAV_SEVERITY_CRITICAL, "%s err:%u",
-                msg_prefix,
-                _display_system_state.master_error_code);
+    for (uint8_t instance = 0; instance < AP_TORQEEDO_MAX_INSTANCES; instance++) {
+        auto *backend = get_instance(instance);
+        if (backend != nullptr) {
+            backend->clear_motor_error();
         }
     }
-
-    // report motor status errors
-    if (_motor_status.error_flags.overcurrent) {
-        GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "%s overcurrent", msg_prefix);
-    }
-    if (_motor_status.error_flags.blocked) {
-        GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "%s prop blocked", msg_prefix);
-    }
-    if (_motor_status.error_flags.overvoltage_static || _motor_status.error_flags.overvoltage_current) {
-        GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "%s high voltage", msg_prefix);
-    }
-    if (_motor_status.error_flags.undervoltage_static || _motor_status.error_flags.undervoltage_current) {
-        GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "%s low voltage", msg_prefix);
-    }
-    if (_motor_status.error_flags.overtemp_motor || _motor_status.error_flags.overtemp_pcb) {
-        GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "%s high temp", msg_prefix);
-    }
-    if (_motor_status.error_flags.timeout_rs485) {
-        GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "%s comm timeout", msg_prefix);
-    }
-    if (_motor_status.error_flags.temp_sensor_error) {
-        GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "%s temp sensor err", msg_prefix);
-    }
-    if (_motor_status.error_flags.tilt) {
-        GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "%s tilted", msg_prefix);
-    }
-
-    // display OK if all errors cleared
-    const bool prev_errored = (_display_system_state_flags_prev.value != 0) ||
-                              (_display_system_state_master_error_code_prev != 0) ||
-                              (_motor_status_prev.error_flags_value != 0);
-
-    const bool now_errored = (_display_system_state.flags.value != 0) ||
-                             (_display_system_state.master_error_code != 0) ||
-                             (_motor_status.error_flags_value != 0);
-
-    if (!now_errored && prev_errored) {
-        GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s OK", msg_prefix);
-    }
-
-    // record change in state and reporting time
-    _display_system_state_flags_prev.value = _display_system_state.flags.value;
-    _display_system_state_master_error_code_prev = _display_system_state.master_error_code;
-    _motor_status_prev = _motor_status;
-    _last_error_report_ms = now_ms;
 }
 
 // get latest battery status info.  returns true on success and populates arguments
-bool AP_Torqeedo::get_batt_info(float &voltage, float &current_amps, float &temp_C, uint8_t &pct_remaining) const
+// instance is normally 0 or 1, if invalid instances are provided the first instance is used
+bool AP_Torqeedo::get_batt_info(uint8_t instance, float &voltage, float &current_amps, float &temp_C, uint8_t &pct_remaining) const
 {
-
-    // use battery info from display_system_state if available (tiller connection)
-    if ((AP_HAL::millis() - _display_system_state.last_update_ms) <= TORQEEDO_BATT_TIMEOUT_MS) {
-        voltage = _display_system_state.batt_voltage;
-        current_amps = _display_system_state.batt_current;
-        temp_C = MAX(_display_system_state.temp_sw, _display_system_state.temp_rp);
-        pct_remaining = _display_system_state.batt_charge_pct;
-        return true;
+    // for invalid instances use the first instance
+    if (instance > AP_TORQEEDO_MAX_INSTANCES-1) {
+        instance = 0;
     }
 
-    // use battery info from motor_param if available (motor connection)
-    if ((AP_HAL::millis() - _motor_param.last_update_ms) <= TORQEEDO_BATT_TIMEOUT_MS) {
-        voltage = _motor_param.voltage;
-        current_amps = _motor_param.current;
-        temp_C = MAX(_motor_param.pcb_temp, _motor_param.stator_temp);
-        pct_remaining = 0; // motor does not know percent remaining
-        return true;
+    // return battery info for specified instance
+    auto *backend = get_instance(instance);
+    if (backend != nullptr) {
+        return backend->get_batt_info(voltage, current_amps, temp_C, pct_remaining);
     }
-
     return false;
 }
 
 // get battery capacity.  returns true on success and populates argument
-bool AP_Torqeedo::get_batt_capacity_Ah(uint16_t &amp_hours) const
+// instance is normally 0 or 1, if invalid instances are provided the first instance is used
+bool AP_Torqeedo::get_batt_capacity_Ah(uint8_t instance, uint16_t &amp_hours) const
 {
-    if (_display_system_setup.batt_capacity == 0) {
-        return false;
+    // for invalid instances use the first instance
+    if (instance > AP_TORQEEDO_MAX_INSTANCES-1) {
+        instance = 0;
     }
-    amp_hours = _display_system_setup.batt_capacity;
-    return true;
+
+    // return battery capacity from specified instance
+    auto *backend = get_instance(instance);
+    if (backend != nullptr) {
+        return backend->get_batt_capacity_Ah(amp_hours);
+    }
+    return false;
 }
 
-// process a single byte received on serial port
-// return true if a complete message has been received (the message will be held in _received_buff)
-bool AP_Torqeedo::parse_byte(uint8_t b)
+// return pointer to backend given an instance number
+AP_Torqeedo_Backend *AP_Torqeedo::get_instance(uint8_t instance) const
 {
-    bool complete_msg_received = false;
-
-    switch (_parse_state) {
-    case ParseState::WAITING_FOR_HEADER:
-        if (b == TORQEEDO_PACKET_HEADER) {
-            _parse_state = ParseState::WAITING_FOR_FOOTER;
-        }
-        _received_buff_len = 0;
-        _parse_escape_received = false;
-        break;
-    case ParseState::WAITING_FOR_FOOTER:
-        if (b == TORQEEDO_PACKET_FOOTER) {
-            _parse_state = ParseState::WAITING_FOR_HEADER;
-
-            // check message length
-            if (_received_buff_len == 0) {
-                _parse_error_count++;
-                break;
-            }
-
-            // check crc
-            const uint8_t crc_expected = crc8_maxim(_received_buff, _received_buff_len-1);
-            if (_received_buff[_received_buff_len-1] != crc_expected) {
-                _parse_error_count++;
-                break;
-            }
-            _parse_success_count++;
-            {
-                // record time of successful receive for health reporting
-                WITH_SEMAPHORE(_last_healthy_sem);
-                _last_received_ms = AP_HAL::millis();
-            }
-            complete_msg_received = true;
-        } else {
-            // escape character handling
-            if (_parse_escape_received) {
-                b ^= TORQEEDO_PACKET_ESCAPE_MASK;
-                _parse_escape_received = false;
-            } else if (b == TORQEEDO_PACKET_ESCAPE) {
-                // escape character received, record this and ignore this byte
-                _parse_escape_received = true;
-                break;
-            }
-            // add to buffer
-            _received_buff[_received_buff_len] = b;
-            _received_buff_len++;
-            if (_received_buff_len > TORQEEDO_MESSAGE_LEN_MAX) {
-                // message too long
-                _parse_state = ParseState::WAITING_FOR_HEADER;
-                _parse_error_count++;
-            }
-        }
-        break;
+    if (instance < AP_TORQEEDO_MAX_INSTANCES) {
+        return _backends[instance];
     }
-
-    return complete_msg_received;
-}
-
-// process message held in _received_buff
-void AP_Torqeedo::parse_message()
-{
-    // message address (i.e. target of message)
-    const MsgAddress msg_addr = (MsgAddress)_received_buff[0];
-
-    // handle messages sent to "remote" (aka tiller)
-    if ((_type == ConnectionType::TYPE_TILLER) && (msg_addr == MsgAddress::REMOTE1)) {
-        RemoteMsgId msg_id = (RemoteMsgId)_received_buff[1];
-        if (msg_id == RemoteMsgId::REMOTE) {
-            // request received to send updated motor speed
-            _send_motor_speed = true;
-        }
-        return;
-    }
-
-    // handle messages sent to Display
-    if ((_type == ConnectionType::TYPE_TILLER) && (msg_addr == MsgAddress::DISPLAY)) {
-        DisplayMsgId msg_id = (DisplayMsgId)_received_buff[1];
-        switch (msg_id) {
-        case DisplayMsgId::SYSTEM_STATE :
-            if (_received_buff_len == 30) {
-                // fill in _display_system_state
-                _display_system_state.flags.value = UINT16_VALUE(_received_buff[2], _received_buff[3]);
-                _display_system_state.master_state = _received_buff[4]; // deprecated
-                _display_system_state.master_error_code = _received_buff[5];
-                _display_system_state.motor_voltage = UINT16_VALUE(_received_buff[6], _received_buff[7]) * 0.01;
-                _display_system_state.motor_current = UINT16_VALUE(_received_buff[8], _received_buff[9]) * 0.1;
-                _display_system_state.motor_power = UINT16_VALUE(_received_buff[10], _received_buff[11]);
-                _display_system_state.motor_rpm = (int16_t)UINT16_VALUE(_received_buff[12], _received_buff[13]);
-                _display_system_state.motor_pcb_temp = _received_buff[14];
-                _display_system_state.motor_stator_temp = _received_buff[15];
-                _display_system_state.batt_charge_pct = _received_buff[16];
-                _display_system_state.batt_voltage = UINT16_VALUE(_received_buff[17], _received_buff[18]) * 0.01;
-                _display_system_state.batt_current = UINT16_VALUE(_received_buff[19], _received_buff[20]) * 0.1;
-                _display_system_state.gps_speed = UINT16_VALUE(_received_buff[21], _received_buff[22]);
-                _display_system_state.range_miles = UINT16_VALUE(_received_buff[23], _received_buff[24]);
-                _display_system_state.range_minutes = UINT16_VALUE(_received_buff[25], _received_buff[26]);
-                _display_system_state.temp_sw = _received_buff[27];
-                _display_system_state.temp_rp = _received_buff[28];
-                _display_system_state.last_update_ms = AP_HAL::millis();
-
-                // update esc telem sent to ground station
-                const uint8_t esc_temp = MAX(_display_system_state.temp_sw, _display_system_state.temp_rp);
-                const uint8_t motor_temp = MAX(_display_system_state.motor_pcb_temp, _display_system_state.motor_stator_temp);
-                update_esc_telem(_display_system_state.motor_rpm,
-                        _display_system_state.motor_voltage,
-                        _display_system_state.motor_current,
-                        esc_temp,
-                        motor_temp);
-
-#if HAL_LOGGING_ENABLED
-                // log data
-                if ((_options & options::LOG) != 0) {
-                    // @LoggerMessage: TRST
-                    // @Description: Torqeedo System State
-                    // @Field: TimeUS: Time since system startup
-                    // @Field: F: Flags bitmask
-                    // @Field: Err: Master error code
-                    // @Field: MVolt: Motor voltage
-                    // @Field: MCur: Motor current
-                    // @Field: Pow: Motor power
-                    // @Field: RPM: Motor RPM
-                    // @Field: MTemp: Motor Temp (higher of pcb or stator)
-                    // @Field: BPct: Battery charge percentage
-                    // @Field: BVolt: Battery voltage
-                    // @Field: BCur: Battery current
-                    AP::logger().Write("TRST", "TimeUS,F,Err,MVolt,MCur,Pow,RPM,MTemp,BPct,BVolt,BCur", "QHBffHhBBff",
-                                       AP_HAL::micros64(),
-                                       _display_system_state.flags.value,
-                                       _display_system_state.master_error_code,
-                                       _display_system_state.motor_voltage,
-                                       _display_system_state.motor_current,
-                                       _display_system_state.motor_power,
-                                       _display_system_state.motor_rpm,
-                                       motor_temp,
-                                       _display_system_state.batt_charge_pct,
-                                      _display_system_state.batt_voltage,
-                                      _display_system_state.batt_current);
-                }
-#endif
-
-                // send to GCS
-                if ((_options & options::DEBUG_TO_GCS) != 0) {
-                    GCS_SEND_TEXT(MAV_SEVERITY_INFO,"TRST F:%u Err:%u MV:%4.1f MC:%4.1f P:%u MT:%d B%%:%d BV:%4.1f BC:%4.1f",
-                            (unsigned)_display_system_state.flags.value,
-                            (unsigned)_display_system_state.master_error_code,
-                            (double)_display_system_state.motor_voltage,
-                            (double)_display_system_state.motor_current,
-                            (unsigned)_display_system_state.motor_power,
-                            (int)motor_temp,
-                            (unsigned)_display_system_state.batt_charge_pct,
-                            (double)_display_system_state.batt_voltage,
-                            (double)_display_system_state.batt_current);
-                }
-
-                // report any errors
-                report_error_codes();
-            } else {
-                // unexpected length
-                _parse_error_count++;
-            }
-            break;
-        case DisplayMsgId::SYSTEM_SETUP:
-            if (_received_buff_len == 13) {
-                // fill in display system setup
-                _display_system_setup.flags = _received_buff[2];
-                _display_system_setup.motor_type = _received_buff[3];
-                _display_system_setup.motor_sw_version = UINT16_VALUE(_received_buff[4], _received_buff[5]);
-                _display_system_setup.batt_capacity = UINT16_VALUE(_received_buff[6], _received_buff[7]);
-                _display_system_setup.batt_charge_pct = _received_buff[8];
-                _display_system_setup.batt_type = _received_buff[9];
-                _display_system_setup.master_sw_version =  UINT16_VALUE(_received_buff[10], _received_buff[11]);
-
-#if HAL_LOGGING_ENABLED
-                // log data
-                if ((_options & options::LOG) != 0) {
-                    // @LoggerMessage: TRSE
-                    // @Description: Torqeedo System Setup
-                    // @Field: TimeUS: Time since system startup
-                    // @Field: Flag: Flags
-                    // @Field: MotType: Motor type
-                    // @Field: MotVer: Motor software version
-                    // @Field: BattCap: Battery capacity
-                    // @Field: BattPct: Battery charge percentage
-                    // @Field: BattType: Battery type
-                    // @Field: SwVer: Master software version
-                    AP::logger().Write("TRSE", "TimeUS,Flag,MotType,MotVer,BattCap,BattPct,BattType,SwVer", "QBBHHBBH",
-                                       AP_HAL::micros64(),
-                                       _display_system_setup.flags,
-                                       _display_system_setup.motor_type,
-                                       _display_system_setup.motor_sw_version,
-                                       _display_system_setup.batt_capacity,
-                                       _display_system_setup.batt_charge_pct,
-                                       _display_system_setup.batt_type,
-                                       _display_system_setup.master_sw_version);
-                }
-#endif
-
-                // send to GCS
-                if ((_options & options::DEBUG_TO_GCS) != 0) {
-                    GCS_SEND_TEXT(MAV_SEVERITY_INFO,"TRSE:%u F:%u Mot:%u/%u Bat:%u/%u/%u%%",
-                            (unsigned)_display_system_setup.master_sw_version,
-                            (unsigned)_display_system_setup.flags,
-                            (unsigned)_display_system_setup.motor_type,
-                            (unsigned)_display_system_setup.motor_sw_version,
-                            (unsigned)_display_system_setup.batt_type,
-                            (unsigned)_display_system_setup.batt_capacity,
-                            (unsigned)_display_system_setup.batt_charge_pct);
-                }
-            } else {
-                // unexpected length
-                _parse_error_count++;
-            }
-            break;
-        default:
-            // ignore message
-            break;
-        }
-        return;
-    }
-
-    // handle reply from motor
-    if ((_type == ConnectionType::TYPE_MOTOR) && (msg_addr == MsgAddress::BUS_MASTER)) {
-        // replies strangely do not return the msgid so we must have stored it
-        MotorMsgId msg_id = (MotorMsgId)_reply_msgid;
-        switch (msg_id) {
-
-        case MotorMsgId::PARAM:
-            if (_received_buff_len == 15) {
-                _motor_param.rpm = (int16_t)UINT16_VALUE(_received_buff[2], _received_buff[3]);
-                _motor_param.power = UINT16_VALUE(_received_buff[4], _received_buff[5]);
-                _motor_param.voltage = UINT16_VALUE(_received_buff[6], _received_buff[7]) * 0.01;
-                _motor_param.current = UINT16_VALUE(_received_buff[8], _received_buff[9]) * 0.1;
-                _motor_param.pcb_temp = (int16_t)UINT16_VALUE(_received_buff[10], _received_buff[11]) * 0.1;
-                _motor_param.stator_temp = (int16_t)UINT16_VALUE(_received_buff[12], _received_buff[13]) * 0.1;
-                _motor_param.last_update_ms = AP_HAL::millis();
-
-                // update esc telem sent to ground station
-                update_esc_telem(_motor_param.rpm,
-                        _motor_param.voltage,
-                        _motor_param.current,
-                        _motor_param.pcb_temp,      // esc temp
-                        _motor_param.stator_temp);  // motor temp
-
-#if HAL_LOGGING_ENABLED
-                // log data
-                if ((_options & options::LOG) != 0) {
-                    // @LoggerMessage: TRMP
-                    // @Description: Torqeedo Motor Param
-                    // @Field: TimeUS: Time since system startup
-                    // @Field: RPM: Motor RPM
-                    // @Field: Pow: Motor power
-                    // @Field: Volt: Motor voltage
-                    // @Field: Cur: Motor current
-                    // @Field: ETemp: ESC Temp
-                    // @Field: MTemp: Motor Temp
-                    AP::logger().Write("TRMP", "TimeUS,RPM,Pow,Volt,Cur,ETemp,MTemp", "QhHffff",
-                                       AP_HAL::micros64(),
-                                       _motor_param.rpm,
-                                       _motor_param.power,
-                                       _motor_param.voltage,
-                                       _motor_param.current,
-                                       _motor_param.pcb_temp,
-                                       _motor_param.stator_temp);
-                }
-#endif
-
-                // send to GCS
-                if ((_options & options::DEBUG_TO_GCS) != 0) {
-                    GCS_SEND_TEXT(MAV_SEVERITY_INFO, "TRMP: rpm:%d p:%u V:%4.1f C:%4.1f PT:%4.1f MT:%4.1f",
-                                                       (int)_motor_param.rpm,
-                                                       (unsigned)_motor_param.power,
-                                                       (double)_motor_param.voltage,
-                                                       (double)_motor_param.current,
-                                                       (double)_motor_param.pcb_temp,
-                                                       (double)_motor_param.stator_temp);
-                }
-            } else {
-                // unexpected length
-                _parse_error_count++;
-            }
-            break;
-
-        case MotorMsgId::STATUS:
-            if (_received_buff_len == 6) {
-                _motor_status.status_flags_value = _received_buff[2];
-                _motor_status.error_flags_value = UINT16_VALUE(_received_buff[3], _received_buff[4]);
-
-#if HAL_LOGGING_ENABLED
-                // log data
-                if ((_options & options::LOG) != 0) {
-                    // @LoggerMessage: TRMS
-                    // @Description: Torqeedo Motor Status
-                    // @Field: TimeUS: Time since system startup
-                    // @Field: State: Motor status flags
-                    // @Field: Err: Motor error flags
-                    AP::logger().Write("TRMS", "TimeUS,State,Err", "QBH",
-                                       AP_HAL::micros64(),
-                                       _motor_status.status_flags_value,
-                                       _motor_status.error_flags_value);
-                }
-#endif
-
-                // send to GCS
-                if ((_options & options::DEBUG_TO_GCS) != 0) {
-                    GCS_SEND_TEXT(MAV_SEVERITY_INFO,"TRMS S:%d Err:%d",
-                                   _motor_status.status_flags_value,
-                                   _motor_status.error_flags_value);
-                }
-
-                // report any errors
-                report_error_codes();
-            } else {
-                // unexpected length
-                _parse_error_count++;
-            }
-            break;
-
-        case MotorMsgId::INFO:
-        case MotorMsgId::DRIVE:
-        case MotorMsgId::CONFIG:
-            // we do not process these replies
-            break;
-
-        default:
-            // ignore unknown messages
-            break;
-        }
-    }
-}
-
-// set DE Serial CTS pin to enable sending commands to motor
-void AP_Torqeedo::send_start()
-{
-    // set gpio pin or serial port's CTS pin
-    if (_pin_de > -1) {
-        hal.gpio->write(_pin_de, 1);
-    } else {
-        _uart->set_CTS_pin(true);
-    }
-}
-
-// check for timeout after sending and unset pin if required
-void AP_Torqeedo::check_for_send_end()
-{
-    if (_send_delay_us == 0) {
-        // not sending
-        return;
-    }
-
-    if (AP_HAL::micros() - _send_start_us < _send_delay_us) {
-        // return if delay has not yet elapsed
-        return;
-    }
-    _send_delay_us = 0;
-
-    // unset gpio or serial port's CTS pin
-    if (_pin_de > -1) {
-        hal.gpio->write(_pin_de, 0);
-    } else {
-        _uart->set_CTS_pin(false);
-    }
-}
-
-// calculate delay require to allow bytes to be sent
-uint32_t AP_Torqeedo::calc_send_delay_us(uint8_t num_bytes)
-{
-    // baud rate of 19200 bits/sec
-    // total number of bits = 10 x num_bytes
-    // convert from seconds to micros by multiplying by 1,000,000
-    // plus additional 300us safety margin
-    const uint32_t delay_us = 1e6 * num_bytes * 10 / TORQEEDO_SERIAL_BAUD + 300;
-    return delay_us;
-}
-
-// record msgid of message to wait for and set timer for timeout handling
-void AP_Torqeedo::set_expected_reply_msgid(uint8_t msg_id)
-{
-    _reply_msgid = msg_id;
-    _reply_wait_start_ms = AP_HAL::millis();
-}
-
-// check for timeout waiting for reply message
-void AP_Torqeedo::check_for_reply_timeout()
-{
-    // return immediately if not waiting for reply
-    if (_reply_wait_start_ms == 0) {
-        return;
-    }
-    if (AP_HAL::millis() - _reply_wait_start_ms > TORQEEDO_REPLY_TIMEOUT_MS) {
-        _reply_wait_start_ms = 0;
-        _parse_error_count++;
-    }
-}
-
-// mark reply received. should be called whenever a message is received regardless of whether we are actually waiting for a reply
-void AP_Torqeedo::set_reply_received()
-{
-    _reply_wait_start_ms = 0;
-}
-
-// send a message to the motor with the specified message contents
-// msg_contents should not include the header, footer or CRC
-// returns true on success
-bool AP_Torqeedo::send_message(const uint8_t msg_contents[], uint8_t num_bytes)
-{
-    // buffer for outgoing message
-    uint8_t send_buff[TORQEEDO_MESSAGE_LEN_MAX];
-    uint8_t send_buff_num_bytes = 0;
-
-    // calculate crc
-    const uint8_t crc = crc8_maxim(msg_contents, num_bytes);
-
-    // add header
-    send_buff[send_buff_num_bytes++] = TORQEEDO_PACKET_HEADER;
-
-    // add contents
-    for (uint8_t i=0; i<num_bytes; i++) {
-        if (!add_byte_to_message(msg_contents[i], send_buff, ARRAY_SIZE(send_buff), send_buff_num_bytes)) {
-            _parse_error_count++;
-            return false;
-        }
-    }
-
-    // add crc
-    if (!add_byte_to_message(crc, send_buff, ARRAY_SIZE(send_buff), send_buff_num_bytes)) {
-        _parse_error_count++;
-        return false;
-    }
-
-    // add footer
-    if (send_buff_num_bytes >= ARRAY_SIZE(send_buff)) {
-        _parse_error_count++;
-        return false;
-    }
-    send_buff[send_buff_num_bytes++] = TORQEEDO_PACKET_FOOTER;
-
-    // set send pin
-    send_start();
-
-    // write message
-    _uart->write(send_buff, send_buff_num_bytes);
-
-    // record start and expected delay to send message
-    _send_start_us = AP_HAL::micros();
-    _send_delay_us = calc_send_delay_us(send_buff_num_bytes);
-
-    return true;
-}
-
-// add a byte to a message buffer including adding the escape character (0xAE) if necessary
-// this should only be used when adding the contents to the buffer, not the header and footer
-// num_bytes is updated to the next free byte
-bool AP_Torqeedo::add_byte_to_message(uint8_t byte_to_add, uint8_t msg_buff[], uint8_t msg_buff_size, uint8_t &num_bytes) const
-{
-    bool escape_required = (byte_to_add == TORQEEDO_PACKET_HEADER ||
-                            byte_to_add == TORQEEDO_PACKET_FOOTER ||
-                            byte_to_add == TORQEEDO_PACKET_ESCAPE);
-
-    // check if we have enough space
-    if (num_bytes + (escape_required ? 2 : 1) >= msg_buff_size) {
-        return false;
-    }
-
-    // add byte
-    if (escape_required) {
-        msg_buff[num_bytes++] = TORQEEDO_PACKET_ESCAPE;
-        msg_buff[num_bytes++] = byte_to_add ^ TORQEEDO_PACKET_ESCAPE_MASK;
-    } else {
-        msg_buff[num_bytes++] = byte_to_add;
-    }
-    return true;
-}
-
-// Example "Remote (0x01)" reply message to allow tiller to control motor speed
-// Byte     Field Definition    Example Value   Comments
-// ---------------------------------------------------------------------------------
-// byte 0   Header              0xAC
-// byte 1   TargetAddress       0x00            see MsgAddress enum
-// byte 2   Message ID          0x00            only master populates this. replies have this set to zero
-// byte 3   Flags               0x05            bit0=pin present, bit2=motor speed valid
-// byte 4   Status              0x00            0x20 if byte3=4, 0x0 is byte3=5
-// byte 5   Motor Speed MSB     ----            Motor Speed MSB (-1000 to +1000)
-// byte 6   Motor Speed LSB     ----            Motor Speed LSB (-1000 to +1000)
-// byte 7   CRC-Maxim           ----            CRC-Maxim value
-// byte 8   Footer              0xAD
-//
-// example message when rotating tiller handle forwards:  "AC 00 00 05 00 00 ED 95 AD"    (+237)
-// example message when rotating tiller handle backwards: "AC 00 00 05 00 FF AE 2C 0C AD" (-82)
-
-// send a motor speed command as a value from -1000 to +1000
-// value is taken directly from SRV_Channel
-// for tiller connection this sends the "Remote (0x01)" message
-// for motor connection this sends the "Motor Drive (0x82)" message
-void AP_Torqeedo::send_motor_speed_cmd()
-{
-    // calculate desired motor speed
-    if (!hal.util->get_soft_armed()) {
-        _motor_speed_desired = 0;
-    } else {
-        // convert throttle output to motor output in range -1000 to +1000
-        // ToDo: convert PWM output to motor output so that SERVOx_MIN, MAX and TRIM take effect
-        _motor_speed_desired = constrain_int16(SRV_Channels::get_output_norm(SRV_Channel::Aux_servo_function_t::k_throttle) * 1000.0, -1000, 1000);
-    }
-
-    // updated limited motor speed
-    int16_t mot_speed_limited = calc_motor_speed_limited(_motor_speed_desired);
-
-    // by default use tiller connection command
-    uint8_t mot_speed_cmd_buff[] = {(uint8_t)MsgAddress::BUS_MASTER, 0x0, 0x5, 0x0, HIGHBYTE(mot_speed_limited), LOWBYTE(mot_speed_limited)};
-
-    // update message if using motor connection
-    if (_type == ConnectionType::TYPE_MOTOR) {
-        const uint8_t motor_power = (uint8_t)constrain_int16(_motor_power, 0, 100);
-        mot_speed_cmd_buff[0] = (uint8_t)MsgAddress::MOTOR;
-        mot_speed_cmd_buff[1] = (uint8_t)MotorMsgId::DRIVE;
-        mot_speed_cmd_buff[2] = (mot_speed_limited == 0 ? 0 : 0x01) | (_motor_clear_error ? 0x04 : 0);  // 1:enable motor, 2:fast off, 4:clear error
-        mot_speed_cmd_buff[3] = mot_speed_limited == 0 ? 0 : motor_power;   // motor power from 0 to 100
-
-        // set expected reply message id
-        set_expected_reply_msgid((uint8_t)MotorMsgId::DRIVE);
-
-        // reset motor clear error request
-        _motor_clear_error = false;
-    }
-
-    // send a message
-    if (send_message(mot_speed_cmd_buff, ARRAY_SIZE(mot_speed_cmd_buff))) {
-        // record time of send for health reporting
-        WITH_SEMAPHORE(_last_healthy_sem);
-        _last_send_motor_ms = AP_HAL::millis();
-    }
-}
-
-// send request to motor to reply with a particular message
-// msg_id can be INFO, STATUS or PARAM
-void AP_Torqeedo::send_motor_msg_request(MotorMsgId msg_id)
-{
-    // prepare message
-    uint8_t mot_status_request_buff[] = {(uint8_t)MsgAddress::MOTOR, (uint8_t)msg_id};
-
-    // send a message
-    if (send_message(mot_status_request_buff, ARRAY_SIZE(mot_status_request_buff))) {
-        // record waiting for reply
-        set_expected_reply_msgid((uint8_t)msg_id);
-    }
-}
-
-// calculate the limited motor speed that is sent to the motors
-// desired_motor_speed argument and returned value are in the range -1000 to 1000
-int16_t AP_Torqeedo::calc_motor_speed_limited(int16_t desired_motor_speed)
-{
-    const uint32_t now_ms = AP_HAL::millis();
-
-    // update dir_limit flag for forward-reverse transition delay
-    const bool dir_delay_active = is_positive(_dir_delay);
-    if (!dir_delay_active) {
-        // allow movement in either direction
-        _dir_limit = 0;
-    } else {
-        // by default limit motor direction to previous iteration's direction
-        if (is_positive(_motor_speed_limited)) {
-            _dir_limit = 1;
-        } else if (is_negative(_motor_speed_limited)) {
-            _dir_limit = -1;
-        } else {
-            // motor speed is zero
-            if ((_motor_speed_zero_ms != 0) && ((now_ms - _motor_speed_zero_ms) > (_dir_delay * 1000))) {
-                // delay has passed so allow movement in either direction
-                _dir_limit = 0;
-                _motor_speed_zero_ms = 0;
-            }
-        }
-    }
-
-    // calculate upper and lower limits for forward-reverse transition delay
-    int16_t lower_limit = -1000;
-    int16_t upper_limit = 1000;
-    if (_dir_limit < 0) {
-        upper_limit = 0;
-    }
-    if (_dir_limit > 0) {
-        lower_limit = 0;
-    }
-
-    // calculate dt since last update
-    float dt = (now_ms - _motor_speed_limited_ms) * 0.001f;
-    if (dt > 1.0) {
-        // after a long delay limit motor output to zero to avoid sudden starts
-        lower_limit = 0;
-        upper_limit = 0;
-    }
-    _motor_speed_limited_ms = now_ms;
-
-    // apply slew limit
-    if (_slew_time > 0) {
-       const float chg_max = 1000.0 * dt / _slew_time;
-       _motor_speed_limited = constrain_float(desired_motor_speed, _motor_speed_limited - chg_max, _motor_speed_limited + chg_max);
-    } else {
-        // no slew limit
-        _motor_speed_limited = desired_motor_speed;
-    }
-
-    // apply upper and lower limits
-    _motor_speed_limited = constrain_float(_motor_speed_limited, lower_limit, upper_limit);
-
-    // record time motor speed becomes zero
-    if (is_zero(_motor_speed_limited)) {
-        if (_motor_speed_zero_ms == 0) {
-            _motor_speed_zero_ms = now_ms;
-        }
-    } else {
-        // clear timer
-        _motor_speed_zero_ms = 0;
-    }
-
-    return (int16_t)_motor_speed_limited;
-}
-
-// output logging and debug messages (if required)
-// force_logging should be true if caller wants to ensure the latest status is logged
-void AP_Torqeedo::log_TRQD(bool force_logging)
-{
-    // exit immediately if options are all unset
-    if (_options == 0) {
-        return;
-    }
-
-    // return if not enough time has passed since last output
-    const uint32_t now_ms = AP_HAL::millis();
-    if (!force_logging && (now_ms - _last_log_TRQD_ms < TORQEEDO_LOG_TRQD_INTERVAL_MS)) {
-        return;
-    }
-    _last_log_TRQD_ms = now_ms;
-
-#if HAL_LOGGING_ENABLED || HAL_GCS_ENABLED
-    const bool health = healthy();
-    int16_t actual_motor_speed = get_motor_speed_limited();
-
-#if HAL_LOGGING_ENABLED
-    if ((_options & options::LOG) != 0) {
-        // @LoggerMessage: TRQD
-        // @Description: Torqeedo Status
-        // @Field: TimeUS: Time since system startup
-        // @Field: Health: Health
-        // @Field: DesMotSpeed: Desired Motor Speed (-1000 to 1000)
-        // @Field: MotSpeed: Motor Speed (-1000 to 1000)
-        // @Field: SuccCnt: Success Count
-        // @Field: ErrCnt: Error Count
-        AP::logger().Write("TRQD", "TimeUS,Health,DesMotSpeed,MotSpeed,SuccCnt,ErrCnt", "QBhhII",
-                           AP_HAL::micros64(),
-                           health,
-                           _motor_speed_desired,
-                           actual_motor_speed,
-                           _parse_success_count,
-                           _parse_error_count);
-    }
-#endif
-
-    if ((_options & options::DEBUG_TO_GCS) != 0) {
-        GCS_SEND_TEXT(MAV_SEVERITY_INFO,"TRQD h:%u dspd:%d spd:%d succ:%ld err:%ld",
-                (unsigned)health,
-                (int)_motor_speed_desired,
-                (int)actual_motor_speed,
-                (unsigned long)_parse_success_count,
-                (unsigned long)_parse_error_count);
-    }
-#endif  // HAL_LOGGING_ENABLED || HAL_GCS_ENABLED
-}
-
-// send ESC telemetry
-void AP_Torqeedo::update_esc_telem(float rpm, float voltage, float current_amps, float esc_tempC, float motor_tempC)
-{
-#if HAL_WITH_ESC_TELEM
-    // find servo output channel
-    uint8_t telem_esc_index = 0;
-    IGNORE_RETURN(SRV_Channels::find_channel(SRV_Channel::Aux_servo_function_t::k_throttle, telem_esc_index));
-
-    // fill in telemetry data structure
-    AP_ESC_Telem_Backend::TelemetryData telem_dat {};
-    telem_dat.temperature_cdeg = esc_tempC * 100;   // temperature in centi-degrees
-    telem_dat.voltage = voltage;                    // voltage in volts
-    telem_dat.current = current_amps;               // current in amps
-    telem_dat.motor_temp_cdeg = motor_tempC * 100;  // motor temperature in centi-degrees
-
-    // send telem and rpm data
-    update_telem_data(telem_esc_index, telem_dat, AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE |
-                                                  AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE |
-                                                  AP_ESC_Telem_Backend::TelemetryType::CURRENT |
-                                                  AP_ESC_Telem_Backend::TelemetryType::VOLTAGE);
-
-    update_rpm(telem_esc_index, rpm);
-#endif
+    return nullptr;
 }
 
 // get the AP_Torqeedo singleton
@@ -1159,10 +213,11 @@ AP_Torqeedo *AP_Torqeedo::get_singleton()
 AP_Torqeedo *AP_Torqeedo::_singleton = nullptr;
 
 namespace AP {
+
 AP_Torqeedo *torqeedo()
 {
     return AP_Torqeedo::get_singleton();
 }
-};
+}
 
 #endif // HAL_TORQEEDO_ENABLED
diff --git a/libraries/AP_Torqeedo/AP_Torqeedo.h b/libraries/AP_Torqeedo/AP_Torqeedo.h
index 32d2a002c2..f5c4eb1cb8 100644
--- a/libraries/AP_Torqeedo/AP_Torqeedo.h
+++ b/libraries/AP_Torqeedo/AP_Torqeedo.h
@@ -15,32 +15,7 @@
  
 /*
    This driver supports communicating with Torqeedo motors that implement the "TQ Bus" protocol
-   which includes the Ultralight, Cruise 2.0 R, Cruise 4.0 R, Travel 503, Travel 1003 and Cruise 10kW
-
-   The autopilot should be connected either to the battery's tiller connector or directly to the motor
-   as described on the ArduPilot wiki. https://ardupilot.org/rover/docs/common-torqeedo.html
-   TQ Bus is a serial protocol over RS-485 meaning that a serial to RS-485 converter is required.
-
-       Tiller connection: Autopilot <-> Battery (master) <-> Motor
-       Motor connection:  Autopilot (master) <-> Motor
-
-    Communication between the components is always initiated by the master with replies sent within 25ms
-
-    Example "Remote (0x01)" reply message to allow tiller to control motor speed
-    Byte        Field Definition    Example Value   Comments
-    ---------------------------------------------------------------------------------
-    byte 0      Header              0xAC
-    byte 1      TargetAddress       0x00            see MsgAddress enum
-    byte 2      Message ID          0x00            only master populates this. replies have this set to zero
-    byte 3      Flags               0x05            bit0=pin present, bit2=motor speed valid
-    byte 4      Status              0x00            0x20 if byte3=4, 0x0 is byte3=5
-    byte 5      Motor Speed MSB     ----            Motor Speed MSB (-1000 to +1000)
-    byte 6      Motor Speed LSB     ----            Motor Speed LSB (-1000 to +1000)
-    byte 7      CRC-Maxim           ----            CRC-Maxim value
-    byte 8      Footer              0xAD
-
-   More details of the TQ Bus protocol are available from Torqeedo after signing an NDA.
- */
+*/
 
 #pragma once
 
@@ -48,84 +23,30 @@
 
 #if HAL_TORQEEDO_ENABLED
 
-#include <AP_ESC_Telem/AP_ESC_Telem_Backend.h>
 #include <AP_Param/AP_Param.h>
+#include "AP_Torqeedo_Params.h"
 
-#define TORQEEDO_MESSAGE_LEN_MAX    35  // messages are no more than 35 bytes
+#define AP_TORQEEDO_MAX_INSTANCES   2   // maximum number of Torqeedo backends
+
+// declare backend classes
+class AP_Torqeedo_Backend;
+class AP_Torqeedo_TQBus;
+
+class AP_Torqeedo {
+
+    // declare backends as friends
+    friend class AP_Torqeedo_Backend;
+    friend class AP_Torqeedo_TQBus;
 
-class AP_Torqeedo : public AP_ESC_Telem_Backend {
 public:
     AP_Torqeedo();
 
+    /* Do not allow copies */
     CLASS_NO_COPY(AP_Torqeedo);
 
+    // get singleton instance
     static AP_Torqeedo* get_singleton();
 
-    // initialise driver
-    void init();
-
-    // consume incoming messages from motor, reply with latest motor speed
-    // runs in background thread
-    void thread_main();
-
-    // returns true if communicating with the motor
-    bool healthy();
-
-    // run pre-arm check.  returns false on failure and fills in failure_msg
-    // any failure_msg returned will not include a prefix
-    bool pre_arm_checks(char *failure_msg, uint8_t failure_msg_len);
-
-    // report changes in error codes to user
-    void report_error_codes();
-
-    // clear motor errors
-    void clear_motor_error() { _motor_clear_error = true; }
-
-    // get latest battery status info.  returns true on success and populates arguments
-    bool get_batt_info(float &voltage, float &current_amps, float &temp_C, uint8_t &pct_remaining) const WARN_IF_UNUSED;
-    bool get_batt_capacity_Ah(uint16_t &amp_hours) const;
-
-    static const struct AP_Param::GroupInfo var_info[];
-
-private:
-
-    // message addresses
-    enum class MsgAddress : uint8_t {
-        BUS_MASTER = 0x00,
-        REMOTE1 = 0x14,
-        DISPLAY = 0x20,
-        MOTOR = 0x30,
-        BATTERY = 0x80
-    };
-
-    // Remote specific message ids
-    enum class RemoteMsgId : uint8_t {
-        INFO = 0x00,
-        REMOTE = 0x01,
-        SETUP = 0x02
-    };
-
-    // Display specific message ids
-    enum class DisplayMsgId : uint8_t {
-        INFO = 0x00,
-        SYSTEM_STATE = 0x41,
-        SYSTEM_SETUP = 0x42
-    };
-
-    // Motor specific message ids
-    enum class MotorMsgId : uint8_t {
-        INFO = 0x00,
-        STATUS = 0x01,
-        PARAM = 0x03,
-        CONFIG = 0x04,
-        DRIVE = 0x82
-    };
-
-    enum class ParseState {
-        WAITING_FOR_HEADER = 0,
-        WAITING_FOR_FOOTER,
-    };
-
     // TYPE parameter values
     enum class ConnectionType : uint8_t {
         TYPE_DISABLED = 0,
@@ -134,232 +55,47 @@ private:
     };
 
     // OPTIONS parameter values
-    enum options {
+    enum class options {
         LOG             = 1<<0,
         DEBUG_TO_GCS    = 1<<1,
     };
 
-    // initialise serial port and gpio pins (run from background thread)
-    // returns true on success
-    bool init_internals();
+    // initialise driver
+    void init();
 
-    // returns true if the driver is enabled
+    // returns true if at least one backend has been configured (e.g. TYPE param has been set)
     bool enabled() const;
+    bool enabled(uint8_t instance) const;
 
-    // process a single byte received on serial port
-    // return true if a complete message has been received (the message will be held in _received_buff)
-    bool parse_byte(uint8_t b);
+    // returns true if all backends are communicating with the motor
+    bool healthy();
+    bool healthy(uint8_t instance);
 
-    // process message held in _received_buff
-    void parse_message();
+    // run pre-arm check.  returns false on failure and fills in failure_msg
+    // any failure_msg returned will not include a prefix
+    bool pre_arm_checks(char *failure_msg, uint8_t failure_msg_len);
 
-    // returns true if it is safe to send a message
-    bool safe_to_send() const { return ((_send_delay_us == 0) && (_reply_wait_start_ms == 0)); }
+    // clear motor errors
+    void clear_motor_error();
 
-    // set pin to enable sending a message
-    void send_start();
+    // get latest battery status info.  returns true on success and populates arguments
+    // instance is normally 0 or 1, if invalid instances are provided the first instance is used
+    bool get_batt_info(uint8_t instance, float &voltage, float &current_amps, float &temp_C, uint8_t &pct_remaining) const WARN_IF_UNUSED;
+    bool get_batt_capacity_Ah(uint8_t instance, uint16_t &amp_hours) const;
 
-    // check for timeout after sending a message and unset pin if required
-    void check_for_send_end();
+    // parameter var table
+    static const struct AP_Param::GroupInfo var_info[];
 
-    // calculate delay required to allow message to be completely sent
-    uint32_t calc_send_delay_us(uint8_t num_bytes);
+    // parameters for backends
+    AP_Torqeedo_Params _params[AP_TORQEEDO_MAX_INSTANCES];
 
-    // record msgid of message to wait for and set timer for reply timeout handling
-    void set_expected_reply_msgid(uint8_t msg_id);
+private:
 
-    // check for timeout waiting for reply
-    void check_for_reply_timeout();
+    // return pointer to backend given an instance number
+    AP_Torqeedo_Backend *get_instance(uint8_t instance) const;
 
-    // mark reply received. should be called whenever a message is received regardless of whether we are actually waiting for a reply
-    void set_reply_received();
-
-    // send a message to the motor with the specified message contents
-    // msg_contents should not include the header, footer or CRC
-    // returns true on success
-    bool send_message(const uint8_t msg_contents[], uint8_t num_bytes);
-
-    // add a byte to a message buffer including adding the escape character (0xAE) if necessary
-    // this should only be used when adding the contents to the buffer, not the header and footer
-    // num_bytes is updated to the next free byte
-    bool add_byte_to_message(uint8_t byte_to_add, uint8_t msg_buff[], uint8_t msg_buff_size, uint8_t &num_bytes) const;
-
-    // send a motor speed command as a value from -1000 to +1000
-    // value is taken directly from SRV_Channel
-    void send_motor_speed_cmd();
-
-    // send request to motor to reply with a particular message
-    // msg_id can be INFO, STATUS or PARAM
-    void send_motor_msg_request(MotorMsgId msg_id);
-
-    // calculate the limited motor speed that is sent to the motors
-    // desired_motor_speed argument and returned value are in the range -1000 to 1000
-    int16_t calc_motor_speed_limited(int16_t desired_motor_speed);
-    int16_t get_motor_speed_limited() const { return (int16_t)_motor_speed_limited; }
-
-    // log TRQD message which holds high level status and latest desired motors peed
-    // force_logging should be true to immediately write log bypassing timing check to avoid spamming
-    void log_TRQD(bool force_logging);
-
-    // send ESC telemetry
-    void update_esc_telem(float rpm, float voltage, float current_amps, float esc_tempC, float motor_tempC);
-
-    // parameters
-    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_Int16 _options;      // options bitmask
-    AP_Int8 _motor_power;   // motor power (0 ~ 100).  only applied when using motor connection
-    AP_Float _slew_time;    // slew rate specified as the minimum number of seconds required to increase the throttle from 0 to 100%.  A value of zero disables the limit
-    AP_Float _dir_delay;    // direction change delay.  output will remain at zero for this many seconds when transitioning between forward and backwards rotation
-
-    // 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;   // desired motor speed (set from within update method)
-    uint32_t _last_send_motor_ms;   // system time (in millis) last motor speed command was sent (used for health reporting)
-    bool _motor_clear_error;        // true if the motor error should be cleared (sent in "Drive" message)
-    uint32_t _send_start_us;        // system time (in micros) when last message started being sent (used for timing to unset DE pin)
-    uint32_t _send_delay_us;        // delay (in micros) to allow bytes to be sent after which pin can be unset.  0 if not delaying
-
-    // motor speed limit variables
-    float _motor_speed_limited;     // limited desired motor speed. this value is actually sent to the motor
-    uint32_t _motor_speed_limited_ms; // system time that _motor_speed_limited was last updated
-    int8_t _dir_limit;              // acceptable directions for output to motor (+1 = positive OK, -1 = negative OK, 0 = either positive or negative OK)
-    uint32_t _motor_speed_zero_ms;  // system time that _motor_speed_limited reached zero.  0 if currently not zero
-
-    // health reporting
-    HAL_Semaphore _last_healthy_sem;// semaphore protecting reading and updating of _last_send_motor_ms and _last_received_ms
-    uint32_t _last_log_TRQD_ms;     // system time (in millis) that TRQD was last logged
-
-    // message parsing members
-    ParseState _parse_state;        // current state of parsing
-    bool _parse_escape_received;    // true if the escape character has been received so we must XOR the next byte
-    uint32_t _parse_error_count;    // total number of parsing errors (for reporting)
-    uint32_t _parse_success_count;  // number of messages successfully parsed (for reporting)
-    uint8_t _received_buff[TORQEEDO_MESSAGE_LEN_MAX];   // characters received
-    uint8_t _received_buff_len;     // number of characters received
-    uint32_t _last_received_ms;     // system time (in millis) that a message was successfully parsed (for health reporting)
-
-    // reply message handling
-    uint8_t _reply_msgid;           // replies expected msgid (reply often does not specify the msgid so we must record it)
-    uint32_t _reply_wait_start_ms;  // system time that we started waiting for a reply message
-
-    // Display system state flags
-    typedef union PACKED {
-        struct {
-            uint8_t set_throttle_stop   : 1;    // 0, warning that user must set throttle to stop before motor can run
-            uint8_t setup_allowed       : 1;    // 1, remote is allowed to enter setup mode
-            uint8_t in_charge           : 1;    // 2, master is in charging state
-            uint8_t in_setup            : 1;    // 3, master is in setup state
-            uint8_t bank_available      : 1;    // 4
-            uint8_t no_menu             : 1;    // 5
-            uint8_t menu_off            : 1;    // 6
-            uint8_t reserved7           : 1;    // 7, unused
-            uint8_t temp_warning        : 1;    // 8, motor or battery temp warning
-            uint8_t batt_charge_valid   : 1;    // 9, battery charge valid
-            uint8_t batt_nearly_empty   : 1;    // 10, battery nearly empty
-            uint8_t batt_charging       : 1;    // 11, battery charging
-            uint8_t gps_searching       : 1;    // 12, gps searching for satellites
-            uint8_t gps_speed_valid     : 1;    // 13, gps speed is valid
-            uint8_t range_miles_valid   : 1;    // 14, range (in miles) is valid
-            uint8_t range_minutes_valid : 1;    // 15, range (in minutes) is valid
-        };
-        uint16_t value;
-    } DisplaySystemStateFlags;
-
-    // Display system state
-    struct DisplaySystemState {
-        DisplaySystemStateFlags flags;  // flags, see above for individual bit definitions
-        uint8_t master_state;           // deprecated
-        uint8_t master_error_code;      // error code (0=no error)
-        float motor_voltage;            // motor voltage in volts
-        float motor_current;            // motor current in amps
-        uint16_t motor_power;           // motor power in watts
-        int16_t motor_rpm;              // motor speed in rpm
-        uint8_t motor_pcb_temp;         // motor pcb temp in C
-        uint8_t motor_stator_temp;      // motor stator temp in C
-        uint8_t batt_charge_pct;        // battery state of charge (0 to 100%)
-        float batt_voltage;             // battery voltage in volts
-        float batt_current;             // battery current in amps
-        uint16_t gps_speed;             // gps speed in knots * 100
-        uint16_t range_miles;           // range in nautical miles * 10
-        uint16_t range_minutes;         // range in minutes (at current speed and current draw)
-        uint8_t temp_sw;                // master PCB temp in C (close to motor power switches)
-        uint8_t temp_rp;                // master PCB temp in C (close to reverse voltage protection)
-        uint32_t last_update_ms;        // system time that system state was last updated
-    } _display_system_state;
-
-    // Display system setup
-    struct DisplaySystemSetup {
-        uint8_t flags;              // 0 : battery config valid, all other bits unused
-        uint8_t motor_type;         // motor type (0 or 3:Unknown, 1:Ultralight, 2:Cruise2, 4:Cruise4, 5:Travel503, 6:Travel1003, 7:Cruise10kW)
-        uint16_t motor_sw_version;  // motor software version
-        uint16_t batt_capacity;     // battery capacity in amp hours
-        uint8_t batt_charge_pct;    // battery state of charge (0 to 100%)
-        uint8_t batt_type;          // battery type (0:lead acid, 1:Lithium)
-        uint16_t master_sw_version; // master software version
-    } _display_system_setup;
-
-    // Motor status
-    struct MotorStatus {
-        union PACKED {
-            struct {
-                uint8_t temp_limit_motor    : 1;    // 0, motor speed limited due to motor temp
-                uint8_t temp_limit_pcb      : 1;    // 1, motor speed limited tue to PCB temp
-                uint8_t emergency_stop      : 1;    // 2, motor in emergency stop (must be cleared by master)
-                uint8_t running             : 1;    // 3, motor running
-                uint8_t power_limit         : 1;    // 4, motor power limited
-                uint8_t low_voltage_limit   : 1;    // 5, motor speed limited because of low voltage
-                uint8_t tilt                : 1;    // 6, motor is tilted
-                uint8_t reserved7           : 1;    // 7, unused (always zero)
-            } status_flags;
-            uint8_t status_flags_value;
-        };
-        union PACKED {
-            struct {
-                uint8_t overcurrent         : 1;    // 0, motor stopped because of overcurrent
-                uint8_t blocked             : 1;    // 1, motor stopped because it is blocked
-                uint8_t overvoltage_static  : 1;    // 2, motor stopped because voltage too high
-                uint8_t undervoltage_static : 1;    // 3, motor stopped because voltage too low
-                uint8_t overvoltage_current : 1;    // 4, motor stopped because voltage spiked high
-                uint8_t undervoltage_current: 1;    // 5, motor stopped because voltage spiked low
-                uint8_t overtemp_motor      : 1;    // 6, motor stopped because stator temp too high
-                uint8_t overtemp_pcb        : 1;    // 7, motor stopped because pcb temp too high
-                uint8_t timeout_rs485       : 1;    // 8, motor stopped because Drive message not received for too long
-                uint8_t temp_sensor_error   : 1;    // 9, motor temp sensor is defective (motor will not stop)
-                uint8_t tilt                : 1;    // 10, motor stopped because it was tilted
-                uint8_t unused11to15        : 5;    // 11 ~ 15 (always zero)
-            } error_flags;
-            uint16_t error_flags_value;
-        };
-    } _motor_status;
-    uint32_t _last_send_motor_status_request_ms;    // system time (in milliseconds) that last motor status request was sent
-
-    // Motor params
-    struct MotorParam {
-        int16_t rpm;            // motor rpm
-        uint16_t power;         // motor power consumption in Watts
-        float voltage;          // motor voltage in volts
-        float current;          // motor current in amps
-        float pcb_temp;         // pcb temp in C
-        float stator_temp;      // stator temp in C
-        uint32_t last_update_ms;// system time that above values were updated
-    } _motor_param;
-    uint32_t _last_send_motor_param_request_ms;     // system time (in milliseconds) that last motor param request was sent
-
-    // error reporting
-    DisplaySystemStateFlags _display_system_state_flags_prev;   // backup of display system state flags
-    uint8_t _display_system_state_master_error_code_prev;       // backup of display system state master_error_code
-    uint32_t _last_error_report_ms;                             // system time that flag changes were last reported (used to prevent spamming user)
-    MotorStatus _motor_status_prev;                             // backup of motor status
     static AP_Torqeedo *_singleton;
-
-    // returns a human-readable string corresponding the passed-in
-    // master error code (see page 93 of https://media.torqeedo.com/downloads/manuals/torqeedo-Travel-manual-DE-EN.pdf)
-    // If no conversion is available then nullptr is returned
-    const char *map_master_error_code_to_string(uint8_t code) const;
+    AP_Torqeedo_Backend *_backends[AP_TORQEEDO_MAX_INSTANCES];  // pointers to instantiated backends
 };
 
 namespace AP {
diff --git a/libraries/AP_Torqeedo/AP_Torqeedo_Backend.cpp b/libraries/AP_Torqeedo/AP_Torqeedo_Backend.cpp
new file mode 100644
index 0000000000..243f7cab23
--- /dev/null
+++ b/libraries/AP_Torqeedo/AP_Torqeedo_Backend.cpp
@@ -0,0 +1,31 @@
+/*
+   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/>.
+ */
+
+#include "AP_Torqeedo_Backend.h"
+
+#if HAL_TORQEEDO_ENABLED
+
+#include <AP_Common/AP_Common.h>
+#include <AP_Math/AP_Math.h>
+
+extern const AP_HAL::HAL& hal;
+
+// constructor
+AP_Torqeedo_Backend::AP_Torqeedo_Backend(AP_Torqeedo_Params &params, uint8_t instance) :
+    _params(params),
+    _instance(instance)
+{}
+
+#endif // HAL_TORQEEDO_ENABLED
diff --git a/libraries/AP_Torqeedo/AP_Torqeedo_Backend.h b/libraries/AP_Torqeedo/AP_Torqeedo_Backend.h
new file mode 100644
index 0000000000..fd56d0612a
--- /dev/null
+++ b/libraries/AP_Torqeedo/AP_Torqeedo_Backend.h
@@ -0,0 +1,84 @@
+/*
+   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/>.
+ */
+ 
+/*
+   This driver supports communicating with Torqeedo motors that implement the "TQ Bus" protocol
+   which includes the Ultralight, Cruise 2.0 R, Cruise 4.0 R, Travel 503, Travel 1003 and Cruise 10kW
+
+   The autopilot should be connected either to the battery's tiller connector or directly to the motor
+   as described on the ArduPilot wiki. https://ardupilot.org/rover/docs/common-torqeedo.html
+   TQ Bus is a serial protocol over RS-485 meaning that a serial to RS-485 converter is required.
+
+       Tiller connection: Autopilot <-> Battery (master) <-> Motor
+       Motor connection:  Autopilot (master) <-> Motor
+
+    Communication between the components is always initiated by the master with replies sent within 25ms
+
+    Example "Remote (0x01)" reply message to allow tiller to control motor speed
+    Byte        Field Definition    Example Value   Comments
+    ---------------------------------------------------------------------------------
+    byte 0      Header              0xAC
+    byte 1      TargetAddress       0x00            see MsgAddress enum
+    byte 2      Message ID          0x00            only master populates this. replies have this set to zero
+    byte 3      Flags               0x05            bit0=pin present, bit2=motor speed valid
+    byte 4      Status              0x00            0x20 if byte3=4, 0x0 is byte3=5
+    byte 5      Motor Speed MSB     ----            Motor Speed MSB (-1000 to +1000)
+    byte 6      Motor Speed LSB     ----            Motor Speed LSB (-1000 to +1000)
+    byte 7      CRC-Maxim           ----            CRC-Maxim value
+    byte 8      Footer              0xAD
+
+   More details of the TQ Bus protocol are available from Torqeedo after signing an NDA.
+ */
+
+#pragma once
+
+#include "AP_Torqeedo_config.h"
+
+#if HAL_TORQEEDO_ENABLED
+
+#include "AP_Torqeedo.h"
+#include <AP_ESC_Telem/AP_ESC_Telem_Backend.h>
+
+class AP_Torqeedo_Backend : public AP_ESC_Telem_Backend {
+public:
+    AP_Torqeedo_Backend(AP_Torqeedo_Params &params, uint8_t instance);
+
+    // do not allow copies
+    CLASS_NO_COPY(AP_Torqeedo_Backend);
+
+    // initialise driver
+    virtual void init() = 0;
+
+    // returns true if communicating with the motor
+    virtual bool healthy() = 0;
+
+    // clear motor errors
+    virtual void clear_motor_error() = 0;
+
+    // get latest battery status info.  returns true on success and populates arguments
+    virtual bool get_batt_info(float &voltage, float &current_amps, float &temp_C, uint8_t &pct_remaining) const WARN_IF_UNUSED = 0;
+    virtual bool get_batt_capacity_Ah(uint16_t &amp_hours) const = 0;
+
+   protected:
+
+    // parameter helper functions
+    AP_Torqeedo::ConnectionType get_type() const { return (AP_Torqeedo::ConnectionType)_params.type.get(); }
+    bool option_enabled(AP_Torqeedo::options opt) const { return ((uint16_t)_params.options.get() & (uint16_t)opt) != 0; }
+
+    AP_Torqeedo_Params &_params;    // parameters for this backend
+    uint8_t _instance;              // this instance's number
+};
+
+#endif // HAL_TORQEEDO_ENABLED
diff --git a/libraries/AP_Torqeedo/AP_Torqeedo_Params.cpp b/libraries/AP_Torqeedo/AP_Torqeedo_Params.cpp
new file mode 100644
index 0000000000..2ded30ca84
--- /dev/null
+++ b/libraries/AP_Torqeedo/AP_Torqeedo_Params.cpp
@@ -0,0 +1,79 @@
+#include "AP_Torqeedo_Params.h"
+#include <SRV_Channel/SRV_Channel.h>
+
+// table of user settable parameters
+const AP_Param::GroupInfo AP_Torqeedo_Params::var_info[] = {
+
+    // @Param: TYPE
+    // @DisplayName: Torqeedo connection type
+    // @Description: Torqeedo connection type
+    // @Values: 0:Disabled, 1:Tiller, 2:Motor
+    // @User: Standard
+    // @RebootRequired: True
+    AP_GROUPINFO_FLAGS("TYPE", 1, AP_Torqeedo_Params, type, 0, AP_PARAM_FLAG_ENABLE),
+
+    // @Param: ONOFF_PIN
+    // @DisplayName: Torqeedo ON/Off pin
+    // @Description: Pin number connected to Torqeedo's on/off pin. -1 to use serial port's RTS pin if available
+    // @Values: -1:Disabled,50:AUX1,51:AUX2,52:AUX3,53:AUX4,54:AUX5,55:AUX6
+    // @User: Standard
+    // @RebootRequired: True
+    AP_GROUPINFO("ONOFF_PIN", 2, AP_Torqeedo_Params, pin_onoff, -1),
+
+    // @Param: DE_PIN
+    // @DisplayName: Torqeedo DE pin
+    // @Description: Pin number connected to RS485 to Serial converter's DE pin. -1 to use serial port's CTS pin if available
+    // @Values: -1:Disabled,50:AUX1,51:AUX2,52:AUX3,53:AUX4,54:AUX5,55:AUX6
+    // @User: Standard
+    // @RebootRequired: True
+    AP_GROUPINFO("DE_PIN", 3, AP_Torqeedo_Params, pin_de, -1),
+
+    // @Param: OPTIONS
+    // @DisplayName: Torqeedo Options
+    // @Description: Torqeedo Options Bitmask
+    // @Bitmask: 0:Log,1:Send debug to GCS
+    // @User: Advanced
+    AP_GROUPINFO("OPTIONS", 4, AP_Torqeedo_Params, options, 1),
+
+    // @Param: POWER
+    // @DisplayName: Torqeedo Motor Power
+    // @Description: Torqeedo motor power.  Only applied when using motor connection type (e.g. TRQD_TYPE=2)
+    // @Units: %
+    // @Range: 0 100
+    // @Increment: 1
+    // @User: Advanced
+    AP_GROUPINFO("POWER", 5, AP_Torqeedo_Params, motor_power, 100),
+
+    // @Param: SLEW_TIME
+    // @DisplayName: Torqeedo Throttle Slew Time
+    // @Description: Torqeedo slew rate specified as the minimum number of seconds required to increase the throttle from 0 to 100%.  Higher values cause a slower response, lower values cause a faster response.  A value of zero disables the limit
+    // @Units: s
+    // @Range: 0 5
+    // @Increment: 0.1
+    // @User: Advanced
+    AP_GROUPINFO("SLEW_TIME", 6, AP_Torqeedo_Params, slew_time, 2.0),
+
+    // @Param: DIR_DELAY
+    // @DisplayName: Torqeedo Direction Change Delay
+    // @Description: Torqeedo direction change delay.  Output will remain at zero for this many seconds when transitioning between forward and backwards rotation
+    // @Units: s
+    // @Range: 0 5
+    // @Increment: 0.1
+    // @User: Advanced
+    AP_GROUPINFO("DIR_DELAY", 7, AP_Torqeedo_Params, dir_delay, 1.0),
+
+    // @Param: SERVO_FN
+    // @DisplayName: Torqeedo Servo Output Function
+    // @Description: Torqeedo Servo Output Function
+    // @Values: 70:Throttle,73:ThrottleLeft,74:ThrottleRight
+    // @Increment: 0.1
+    // @User: Advanced
+    AP_GROUPINFO("SERVO_FN", 8, AP_Torqeedo_Params, servo_fn, (int16_t)SRV_Channel::k_throttle),
+
+    AP_GROUPEND
+};
+
+AP_Torqeedo_Params::AP_Torqeedo_Params(void)
+{
+    AP_Param::setup_object_defaults(this, var_info);
+}
diff --git a/libraries/AP_Torqeedo/AP_Torqeedo_Params.h b/libraries/AP_Torqeedo/AP_Torqeedo_Params.h
new file mode 100644
index 0000000000..21211de322
--- /dev/null
+++ b/libraries/AP_Torqeedo/AP_Torqeedo_Params.h
@@ -0,0 +1,24 @@
+#pragma once
+
+#include <AP_Param/AP_Param.h>
+#include <AP_Math/AP_Math.h>
+
+class AP_Torqeedo_Params {
+public:
+    static const struct AP_Param::GroupInfo var_info[];
+
+    AP_Torqeedo_Params(void);
+
+    /* Do not allow copies */
+    CLASS_NO_COPY(AP_Torqeedo_Params);
+
+    // parameters
+    AP_Int8 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_Int16 options;      // options bitmask
+    AP_Int8 motor_power;   // motor power (0 ~ 100).  only applied when using motor connection
+    AP_Float slew_time;    // slew rate specified as the minimum number of seconds required to increase the throttle from 0 to 100%.  A value of zero disables the limit
+    AP_Float dir_delay;    // direction change delay.  output will remain at zero for this many seconds when transitioning between forward and backwards rotation
+    AP_Int16 servo_fn;     // servo output function number
+};
diff --git a/libraries/AP_Torqeedo/AP_Torqeedo_TQBus.cpp b/libraries/AP_Torqeedo/AP_Torqeedo_TQBus.cpp
new file mode 100644
index 0000000000..d90749d4f8
--- /dev/null
+++ b/libraries/AP_Torqeedo/AP_Torqeedo_TQBus.cpp
@@ -0,0 +1,1077 @@
+/*
+   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/>.
+ */
+
+#include "AP_Torqeedo_TQBus.h"
+
+#if HAL_TORQEEDO_ENABLED
+
+#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>
+#include <AP_SerialManager/AP_SerialManager.h>
+#include <AP_ESC_Telem/AP_ESC_Telem_Backend.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 packet footer
+#define TORQEEDO_PACKET_ESCAPE      0xAE    // escape character for handling occurrences of header, footer and this escape bytes in original message
+#define TORQEEDO_PACKET_ESCAPE_MASK 0x80    // byte after ESCAPE character should be XOR'd with this value
+#define TORQEEDO_LOG_TRQD_INTERVAL_MS                   5000// log TRQD message at this interval in milliseconds
+#define TORQEEDO_SEND_MOTOR_SPEED_INTERVAL_MS           100 // motor speed sent at 10hz if connected to motor
+#define TORQEEDO_SEND_MOTOR_STATUS_REQUEST_INTERVAL_MS  400 // motor status requested every 0.4sec if connected to motor
+#define TORQEEDO_SEND_MOTOR_PARAM_REQUEST_INTERVAL_MS   400 // motor param requested every 0.4sec if connected to motor
+#define TORQEEDO_BATT_TIMEOUT_MS    5000    // battery info timeouts after 5 seconds
+#define TORQEEDO_REPLY_TIMEOUT_MS   25      // stop waiting for replies after 25ms
+#define TORQEEDO_ERROR_REPORT_INTERVAL_MAX_MS   10000   // errors reported to user at no less than once every 10 seconds
+
+extern const AP_HAL::HAL& hal;
+
+// initialise driver
+void AP_Torqeedo_TQBus::init()
+{
+    // only init once
+    // Note: a race condition exists here if init is called multiple times quickly before thread_main has a chance to set _initialise
+    if (_initialised) {
+        return;
+    }
+
+    // create background thread to process serial input and output
+    char thread_name[15];
+    hal.util->snprintf(thread_name, sizeof(thread_name), "torqeedo%u", (unsigned)_instance);
+    if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_Torqeedo_TQBus::thread_main, void), thread_name, 2048, AP_HAL::Scheduler::PRIORITY_RCOUT, 1)) {
+        return;
+    }
+}
+
+// returns true if communicating with the motor
+bool AP_Torqeedo_TQBus::healthy()
+{
+    if (!_initialised) {
+        return false;
+    }
+    {
+        // healthy if both receive and send have occurred in the last 3 seconds
+        WITH_SEMAPHORE(_last_healthy_sem);
+        const uint32_t now_ms = AP_HAL::millis();
+        return ((now_ms - _last_received_ms < 3000) && (now_ms - _last_send_motor_ms < 3000));
+    }
+}
+
+// initialise serial port and gpio pins (run from background thread)
+bool AP_Torqeedo_TQBus::init_internals()
+{
+    // find serial driver and initialise
+    const AP_SerialManager &serial_manager = AP::serialmanager();
+    _uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Torqeedo, _instance);
+    if (_uart == nullptr) {
+        return false;
+    }
+    _uart->begin(TORQEEDO_SERIAL_BAUD);
+    _uart->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE);
+    _uart->set_unbuffered_writes(true);
+
+    // if using tiller connection set on/off pin for 0.5 sec to turn on battery
+    if (get_type() == AP_Torqeedo::ConnectionType::TYPE_TILLER) {
+        if (_params.pin_onoff > -1) {
+            hal.gpio->pinMode(_params.pin_onoff, HAL_GPIO_OUTPUT);
+            hal.gpio->write(_params.pin_onoff, 1);
+            hal.scheduler->delay(500);
+            hal.gpio->write(_params.pin_onoff, 0);
+        } else {
+            // use serial port's RTS pin to turn on battery
+            _uart->set_RTS_pin(true);
+            hal.scheduler->delay(500);
+            _uart->set_RTS_pin(false);
+        }
+    }
+
+    // initialise RS485 DE pin (when high, allows send to motor)
+    if (_params.pin_de > -1) {
+        hal.gpio->pinMode(_params.pin_de, HAL_GPIO_OUTPUT);
+        hal.gpio->write(_params.pin_de, 0);
+    } else {
+        _uart->set_CTS_pin(false);
+    }
+
+    return true;
+}
+
+// consume incoming messages from motor, reply with latest motor speed
+// runs in background thread
+void AP_Torqeedo_TQBus::thread_main()
+{
+    // initialisation
+    if (!init_internals()) {
+        return;
+    }
+    _initialised = true;
+
+    while (true) {
+        // 1ms loop delay
+        hal.scheduler->delay_microseconds(1000);
+
+        // check if transmit pin should be unset
+        check_for_send_end();
+
+        // check for timeout waiting for reply
+        check_for_reply_timeout();
+
+        // parse incoming characters
+        uint32_t nbytes = MIN(_uart->available(), 1024U);
+        while (nbytes-- > 0) {
+            int16_t b = _uart->read();
+            if (b >= 0 ) {
+                if (parse_byte((uint8_t)b)) {
+                    // complete message received, parse it!
+                    parse_message();
+                    // clear wait-for-reply because if we are waiting for a reply, this message must be it
+                    set_reply_received();
+                }
+            }
+        }
+
+        // send motor speed
+        bool log_update = false;
+        if (safe_to_send()) {
+            uint32_t now_ms = AP_HAL::millis();
+
+            // if connected to motor
+            if (get_type() == AP_Torqeedo::ConnectionType::TYPE_MOTOR) {
+                if (now_ms - _last_send_motor_ms > TORQEEDO_SEND_MOTOR_SPEED_INTERVAL_MS) {
+                    // send motor speed every 0.1sec
+                    _send_motor_speed = true;
+                } else if (now_ms - _last_send_motor_status_request_ms > TORQEEDO_SEND_MOTOR_STATUS_REQUEST_INTERVAL_MS) {
+                    // send request for motor status
+                    send_motor_msg_request(MotorMsgId::STATUS);
+                    _last_send_motor_status_request_ms = now_ms;
+                } else if (now_ms - _last_send_motor_param_request_ms > TORQEEDO_SEND_MOTOR_PARAM_REQUEST_INTERVAL_MS) {
+                    // send request for motor params
+                    send_motor_msg_request(MotorMsgId::PARAM);
+                    _last_send_motor_param_request_ms = now_ms;
+                }
+            }
+
+            // send motor speed
+            if (_send_motor_speed) {
+                send_motor_speed_cmd();
+                _send_motor_speed = false;
+                log_update = true;
+            }
+        }
+
+        // log high level status and motor speed
+        log_TRQD(log_update);
+    }
+}
+
+// returns a human-readable string corresponding the passed-in
+// master error code (see page 93 of https://media.torqeedo.com/downloads/manuals/torqeedo-Travel-manual-DE-EN.pdf)
+// If no conversion is available then nullptr is returned
+const char * AP_Torqeedo_TQBus::map_master_error_code_to_string(uint8_t code) const
+{
+    switch (code) {
+    case 2:
+        return "stator high temp";
+    case 5:
+        return "propeller blocked";
+    case 6:
+        return "motor low voltage";
+    case 7:
+        return "motor high current";
+    case 8:
+        return "pcb temp high";
+    case 21:
+        return "tiller cal bad";
+    case 22:
+        return "mag bad";
+    case 23:
+        return "range incorrect";
+    case 30:
+        return "motor comm error";
+    case 32:
+        return "tiller comm error";
+    case 33:
+        return "general comm error";
+    case 41:
+    case 42:
+        return "charge voltage bad";
+    case 43:
+        return "battery flat";
+    case 45:
+        return "battery high current";
+    case 46:
+        return "battery temp error";
+    case 48:
+        return "charging temp error";
+    }
+
+    return nullptr;
+}
+
+// report changes in error codes to user
+void AP_Torqeedo_TQBus::report_error_codes()
+{
+    // skip reporting if we have already reported status very recently
+    const uint32_t now_ms = AP_HAL::millis();
+
+    // skip reporting if no changes in flags and already reported within 10 seconds
+    const bool flags_changed = (_display_system_state_flags_prev.value != _display_system_state.flags.value) ||
+                               (_display_system_state_master_error_code_prev != _display_system_state.master_error_code) ||
+                               (_motor_status_prev.status_flags_value != _motor_status.status_flags_value) ||
+                               (_motor_status_prev.error_flags_value != _motor_status.error_flags_value);
+    if (!flags_changed && ((now_ms - _last_error_report_ms) < TORQEEDO_ERROR_REPORT_INTERVAL_MAX_MS)) {
+        return;
+    }
+
+    // report display system errors
+    const char* msg_prefix = "Torqeedo:";
+    (void)msg_prefix;  // sometimes unused when HAL_GCS_ENABLED is false
+    if (_display_system_state.flags.set_throttle_stop) {
+        GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "%s zero throttle required", msg_prefix);
+    }
+    if (_display_system_state.flags.temp_warning) {
+        GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "%s high temp", msg_prefix);
+    }
+    if (_display_system_state.flags.batt_nearly_empty) {
+        GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "%s batt nearly empty", msg_prefix);
+    }
+    if (_display_system_state.master_error_code > 0) {
+        const char *error_string = map_master_error_code_to_string(_display_system_state.master_error_code);
+        if (error_string != nullptr) {
+            GCS_SEND_TEXT(
+                MAV_SEVERITY_CRITICAL, "%s err:%u %s",
+                msg_prefix,
+                _display_system_state.master_error_code,
+                error_string);
+        } else {
+            GCS_SEND_TEXT(
+                MAV_SEVERITY_CRITICAL, "%s err:%u",
+                msg_prefix,
+                _display_system_state.master_error_code);
+        }
+    }
+
+    // report motor status errors
+    if (_motor_status.error_flags.overcurrent) {
+        GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "%s overcurrent", msg_prefix);
+    }
+    if (_motor_status.error_flags.blocked) {
+        GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "%s prop blocked", msg_prefix);
+    }
+    if (_motor_status.error_flags.overvoltage_static || _motor_status.error_flags.overvoltage_current) {
+        GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "%s high voltage", msg_prefix);
+    }
+    if (_motor_status.error_flags.undervoltage_static || _motor_status.error_flags.undervoltage_current) {
+        GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "%s low voltage", msg_prefix);
+    }
+    if (_motor_status.error_flags.overtemp_motor || _motor_status.error_flags.overtemp_pcb) {
+        GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "%s high temp", msg_prefix);
+    }
+    if (_motor_status.error_flags.timeout_rs485) {
+        GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "%s comm timeout", msg_prefix);
+    }
+    if (_motor_status.error_flags.temp_sensor_error) {
+        GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "%s temp sensor err", msg_prefix);
+    }
+    if (_motor_status.error_flags.tilt) {
+        GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "%s tilted", msg_prefix);
+    }
+
+    // display OK if all errors cleared
+    const bool prev_errored = (_display_system_state_flags_prev.value != 0) ||
+                              (_display_system_state_master_error_code_prev != 0) ||
+                              (_motor_status_prev.error_flags_value != 0);
+
+    const bool now_errored = (_display_system_state.flags.value != 0) ||
+                             (_display_system_state.master_error_code != 0) ||
+                             (_motor_status.error_flags_value != 0);
+
+    if (!now_errored && prev_errored) {
+        GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s OK", msg_prefix);
+    }
+
+    // record change in state and reporting time
+    _display_system_state_flags_prev.value = _display_system_state.flags.value;
+    _display_system_state_master_error_code_prev = _display_system_state.master_error_code;
+    _motor_status_prev = _motor_status;
+    _last_error_report_ms = now_ms;
+}
+
+// get latest battery status info.  returns true on success and populates arguments
+bool AP_Torqeedo_TQBus::get_batt_info(float &voltage, float &current_amps, float &temp_C, uint8_t &pct_remaining) const
+{
+
+    // use battery info from display_system_state if available (tiller connection)
+    if ((AP_HAL::millis() - _display_system_state.last_update_ms) <= TORQEEDO_BATT_TIMEOUT_MS) {
+        voltage = _display_system_state.batt_voltage;
+        current_amps = _display_system_state.batt_current;
+        temp_C = MAX(_display_system_state.temp_sw, _display_system_state.temp_rp);
+        pct_remaining = _display_system_state.batt_charge_pct;
+        return true;
+    }
+
+    // use battery info from motor_param if available (motor connection)
+    if ((AP_HAL::millis() - _motor_param.last_update_ms) <= TORQEEDO_BATT_TIMEOUT_MS) {
+        voltage = _motor_param.voltage;
+        current_amps = _motor_param.current;
+        temp_C = MAX(_motor_param.pcb_temp, _motor_param.stator_temp);
+        pct_remaining = 0; // motor does not know percent remaining
+        return true;
+    }
+
+    return false;
+}
+
+// get battery capacity.  returns true on success and populates argument
+bool AP_Torqeedo_TQBus::get_batt_capacity_Ah(uint16_t &amp_hours) const
+{
+    if (_display_system_setup.batt_capacity == 0) {
+        return false;
+    }
+    amp_hours = _display_system_setup.batt_capacity;
+    return true;
+}
+
+// process a single byte received on serial port
+// return true if a complete message has been received (the message will be held in _received_buff)
+bool AP_Torqeedo_TQBus::parse_byte(uint8_t b)
+{
+    bool complete_msg_received = false;
+
+    switch (_parse_state) {
+    case ParseState::WAITING_FOR_HEADER:
+        if (b == TORQEEDO_PACKET_HEADER) {
+            _parse_state = ParseState::WAITING_FOR_FOOTER;
+        }
+        _received_buff_len = 0;
+        _parse_escape_received = false;
+        break;
+    case ParseState::WAITING_FOR_FOOTER:
+        if (b == TORQEEDO_PACKET_FOOTER) {
+            _parse_state = ParseState::WAITING_FOR_HEADER;
+
+            // check message length
+            if (_received_buff_len == 0) {
+                _parse_error_count++;
+                break;
+            }
+
+            // check crc
+            const uint8_t crc_expected = crc8_maxim(_received_buff, _received_buff_len-1);
+            if (_received_buff[_received_buff_len-1] != crc_expected) {
+                _parse_error_count++;
+                break;
+            }
+            _parse_success_count++;
+            {
+                // record time of successful receive for health reporting
+                WITH_SEMAPHORE(_last_healthy_sem);
+                _last_received_ms = AP_HAL::millis();
+            }
+            complete_msg_received = true;
+        } else {
+            // escape character handling
+            if (_parse_escape_received) {
+                b ^= TORQEEDO_PACKET_ESCAPE_MASK;
+                _parse_escape_received = false;
+            } else if (b == TORQEEDO_PACKET_ESCAPE) {
+                // escape character received, record this and ignore this byte
+                _parse_escape_received = true;
+                break;
+            }
+            // add to buffer
+            _received_buff[_received_buff_len] = b;
+            _received_buff_len++;
+            if (_received_buff_len > TORQEEDO_MESSAGE_LEN_MAX) {
+                // message too long
+                _parse_state = ParseState::WAITING_FOR_HEADER;
+                _parse_error_count++;
+            }
+        }
+        break;
+    }
+
+    return complete_msg_received;
+}
+
+// process message held in _received_buff
+void AP_Torqeedo_TQBus::parse_message()
+{
+    // message address (i.e. target of message)
+    const MsgAddress msg_addr = (MsgAddress)_received_buff[0];
+
+    // handle messages sent to "remote" (aka tiller)
+    if ((get_type() == AP_Torqeedo::ConnectionType::TYPE_TILLER) && (msg_addr == MsgAddress::REMOTE1)) {
+        RemoteMsgId msg_id = (RemoteMsgId)_received_buff[1];
+        if (msg_id == RemoteMsgId::REMOTE) {
+            // request received to send updated motor speed
+            _send_motor_speed = true;
+        }
+        return;
+    }
+
+    // handle messages sent to Display
+    if ((get_type() == AP_Torqeedo::ConnectionType::TYPE_TILLER) && (msg_addr == MsgAddress::DISPLAY)) {
+        DisplayMsgId msg_id = (DisplayMsgId)_received_buff[1];
+        switch (msg_id) {
+        case DisplayMsgId::SYSTEM_STATE :
+            if (_received_buff_len == 30) {
+                // fill in _display_system_state
+                _display_system_state.flags.value = UINT16_VALUE(_received_buff[2], _received_buff[3]);
+                _display_system_state.master_state = _received_buff[4]; // deprecated
+                _display_system_state.master_error_code = _received_buff[5];
+                _display_system_state.motor_voltage = UINT16_VALUE(_received_buff[6], _received_buff[7]) * 0.01;
+                _display_system_state.motor_current = UINT16_VALUE(_received_buff[8], _received_buff[9]) * 0.1;
+                _display_system_state.motor_power = UINT16_VALUE(_received_buff[10], _received_buff[11]);
+                _display_system_state.motor_rpm = (int16_t)UINT16_VALUE(_received_buff[12], _received_buff[13]);
+                _display_system_state.motor_pcb_temp = _received_buff[14];
+                _display_system_state.motor_stator_temp = _received_buff[15];
+                _display_system_state.batt_charge_pct = _received_buff[16];
+                _display_system_state.batt_voltage = UINT16_VALUE(_received_buff[17], _received_buff[18]) * 0.01;
+                _display_system_state.batt_current = UINT16_VALUE(_received_buff[19], _received_buff[20]) * 0.1;
+                _display_system_state.gps_speed = UINT16_VALUE(_received_buff[21], _received_buff[22]);
+                _display_system_state.range_miles = UINT16_VALUE(_received_buff[23], _received_buff[24]);
+                _display_system_state.range_minutes = UINT16_VALUE(_received_buff[25], _received_buff[26]);
+                _display_system_state.temp_sw = _received_buff[27];
+                _display_system_state.temp_rp = _received_buff[28];
+                _display_system_state.last_update_ms = AP_HAL::millis();
+
+                // update esc telem sent to ground station
+                const uint8_t esc_temp = MAX(_display_system_state.temp_sw, _display_system_state.temp_rp);
+                const uint8_t motor_temp = MAX(_display_system_state.motor_pcb_temp, _display_system_state.motor_stator_temp);
+                update_esc_telem(_display_system_state.motor_rpm,
+                        _display_system_state.motor_voltage,
+                        _display_system_state.motor_current,
+                        esc_temp,
+                        motor_temp);
+
+#if HAL_LOGGING_ENABLED
+                // log data
+                if (option_enabled(AP_Torqeedo::options::LOG)) {
+                    // @LoggerMessage: TRST
+                    // @Description: Torqeedo System State
+                    // @Field: TimeUS: Time since system startup
+                    // @Field: I: instance
+                    // @Field: F: Flags bitmask
+                    // @Field: Err: Master error code
+                    // @Field: MVolt: Motor voltage
+                    // @Field: MCur: Motor current
+                    // @Field: Pow: Motor power
+                    // @Field: RPM: Motor RPM
+                    // @Field: MTemp: Motor Temp (higher of pcb or stator)
+                    // @Field: BPct: Battery charge percentage
+                    // @Field: BVolt: Battery voltage
+                    // @Field: BCur: Battery current
+                    AP::logger().Write("TRST", "TimeUS,I,F,Err,MVolt,MCur,Pow,RPM,MTemp,BPct,BVolt,BCur",
+                                      "s#--vAWqO%vA",   // units
+                                      "F---00000000",   // multipliers
+                                      "QBHBffHhBBff",   // formats
+                                       AP_HAL::micros64(),
+                                       _instance,
+                                       _display_system_state.flags.value,
+                                       _display_system_state.master_error_code,
+                                       _display_system_state.motor_voltage,
+                                       _display_system_state.motor_current,
+                                       _display_system_state.motor_power,
+                                       _display_system_state.motor_rpm,
+                                       motor_temp,
+                                       _display_system_state.batt_charge_pct,
+                                      _display_system_state.batt_voltage,
+                                      _display_system_state.batt_current);
+                }
+#endif
+
+                // send to GCS
+                if (option_enabled(AP_Torqeedo::options::DEBUG_TO_GCS)) {
+                    GCS_SEND_TEXT(MAV_SEVERITY_INFO,"TRST i:%u, F:%u Err:%u MV:%4.1f MC:%4.1f P:%u MT:%d B%%:%d BV:%4.1f BC:%4.1f",
+                            (unsigned)_instance,
+                            (unsigned)_display_system_state.flags.value,
+                            (unsigned)_display_system_state.master_error_code,
+                            (double)_display_system_state.motor_voltage,
+                            (double)_display_system_state.motor_current,
+                            (unsigned)_display_system_state.motor_power,
+                            (int)motor_temp,
+                            (unsigned)_display_system_state.batt_charge_pct,
+                            (double)_display_system_state.batt_voltage,
+                            (double)_display_system_state.batt_current);
+                }
+
+                // report any errors
+                report_error_codes();
+            } else {
+                // unexpected length
+                _parse_error_count++;
+            }
+            break;
+        case DisplayMsgId::SYSTEM_SETUP:
+            if (_received_buff_len == 13) {
+                // fill in display system setup
+                _display_system_setup.flags = _received_buff[2];
+                _display_system_setup.motor_type = _received_buff[3];
+                _display_system_setup.motor_sw_version = UINT16_VALUE(_received_buff[4], _received_buff[5]);
+                _display_system_setup.batt_capacity = UINT16_VALUE(_received_buff[6], _received_buff[7]);
+                _display_system_setup.batt_charge_pct = _received_buff[8];
+                _display_system_setup.batt_type = _received_buff[9];
+                _display_system_setup.master_sw_version =  UINT16_VALUE(_received_buff[10], _received_buff[11]);
+
+#if HAL_LOGGING_ENABLED
+                // log data
+                if (option_enabled(AP_Torqeedo::options::LOG)) {
+                    // @LoggerMessage: TRSE
+                    // @Description: Torqeedo System Setup
+                    // @Field: TimeUS: Time since system startup
+                    // @Field: I: instance
+                    // @Field: Flag: Flags
+                    // @Field: MotType: Motor type
+                    // @Field: MotVer: Motor software version
+                    // @Field: BattCap: Battery capacity
+                    // @Field: BattPct: Battery charge percentage
+                    // @Field: BattType: Battery type
+                    // @Field: SwVer: Master software version
+                    AP::logger().Write("TRSE", "TimeUS,I,Flag,MotType,MotVer,BattCap,BattPct,BattType,SwVer",
+                                       "s#---a%--", // units
+                                       "F----00--", // multipliers
+                                       "QBBBHHBBH", // formats
+                                       AP_HAL::micros64(),
+                                       _instance,
+                                       _display_system_setup.flags,
+                                       _display_system_setup.motor_type,
+                                       _display_system_setup.motor_sw_version,
+                                       _display_system_setup.batt_capacity,
+                                       _display_system_setup.batt_charge_pct,
+                                       _display_system_setup.batt_type,
+                                       _display_system_setup.master_sw_version);
+                }
+#endif
+
+                // send to GCS
+                if (option_enabled(AP_Torqeedo::options::DEBUG_TO_GCS)) {
+                    GCS_SEND_TEXT(MAV_SEVERITY_INFO,"TRSE i:%u v:%u F:%u Mot:%u/%u Bat:%u/%u/%u%%",
+                            (unsigned)_instance,
+                            (unsigned)_display_system_setup.master_sw_version,
+                            (unsigned)_display_system_setup.flags,
+                            (unsigned)_display_system_setup.motor_type,
+                            (unsigned)_display_system_setup.motor_sw_version,
+                            (unsigned)_display_system_setup.batt_type,
+                            (unsigned)_display_system_setup.batt_capacity,
+                            (unsigned)_display_system_setup.batt_charge_pct);
+                }
+            } else {
+                // unexpected length
+                _parse_error_count++;
+            }
+            break;
+        default:
+            // ignore message
+            break;
+        }
+        return;
+    }
+
+    // handle reply from motor
+    if ((get_type() == AP_Torqeedo::ConnectionType::TYPE_MOTOR) && (msg_addr == MsgAddress::BUS_MASTER)) {
+        // replies strangely do not return the msgid so we must have stored it
+        MotorMsgId msg_id = (MotorMsgId)_reply_msgid;
+        switch (msg_id) {
+
+        case MotorMsgId::PARAM:
+            if (_received_buff_len == 15) {
+                _motor_param.rpm = (int16_t)UINT16_VALUE(_received_buff[2], _received_buff[3]);
+                _motor_param.power = UINT16_VALUE(_received_buff[4], _received_buff[5]);
+                _motor_param.voltage = UINT16_VALUE(_received_buff[6], _received_buff[7]) * 0.01;
+                _motor_param.current = UINT16_VALUE(_received_buff[8], _received_buff[9]) * 0.1;
+                _motor_param.pcb_temp = (int16_t)UINT16_VALUE(_received_buff[10], _received_buff[11]) * 0.1;
+                _motor_param.stator_temp = (int16_t)UINT16_VALUE(_received_buff[12], _received_buff[13]) * 0.1;
+                _motor_param.last_update_ms = AP_HAL::millis();
+
+                // update esc telem sent to ground station
+                update_esc_telem(_motor_param.rpm,
+                        _motor_param.voltage,
+                        _motor_param.current,
+                        _motor_param.pcb_temp,      // esc temp
+                        _motor_param.stator_temp);  // motor temp
+
+#if HAL_LOGGING_ENABLED
+                // log data
+                if (option_enabled(AP_Torqeedo::options::LOG)) {
+                    // @LoggerMessage: TRMP
+                    // @Description: Torqeedo Motor Param
+                    // @Field: TimeUS: Time since system startup
+                    // @Field: I: instance
+                    // @Field: RPM: Motor RPM
+                    // @Field: Pow: Motor power
+                    // @Field: Volt: Motor voltage
+                    // @Field: Cur: Motor current
+                    // @Field: ETemp: ESC Temp
+                    // @Field: MTemp: Motor Temp
+                    AP::logger().Write("TRMP", "TimeUS,I,RPM,Pow,Volt,Cur,ETemp,MTemp",
+                                       "s#qWvAOO",  // units
+                                       "F-000000",  // multipliers
+                                       "QBhHffff",  // formats
+                                       AP_HAL::micros64(),
+                                       _instance,
+                                       _motor_param.rpm,
+                                       _motor_param.power,
+                                       _motor_param.voltage,
+                                       _motor_param.current,
+                                       _motor_param.pcb_temp,
+                                       _motor_param.stator_temp);
+                }
+#endif
+
+                // send to GCS
+                if (option_enabled(AP_Torqeedo::options::DEBUG_TO_GCS)) {
+                    GCS_SEND_TEXT(MAV_SEVERITY_INFO, "TRMP i:%u rpm:%d p:%u V:%4.1f C:%4.1f PT:%4.1f MT:%4.1f",
+                                                       (unsigned)_instance,
+                                                       (int)_motor_param.rpm,
+                                                       (unsigned)_motor_param.power,
+                                                       (double)_motor_param.voltage,
+                                                       (double)_motor_param.current,
+                                                       (double)_motor_param.pcb_temp,
+                                                       (double)_motor_param.stator_temp);
+                }
+            } else {
+                // unexpected length
+                _parse_error_count++;
+            }
+            break;
+
+        case MotorMsgId::STATUS:
+            if (_received_buff_len == 6) {
+                _motor_status.status_flags_value = _received_buff[2];
+                _motor_status.error_flags_value = UINT16_VALUE(_received_buff[3], _received_buff[4]);
+
+#if HAL_LOGGING_ENABLED
+                // log data
+                if (option_enabled(AP_Torqeedo::options::LOG)) {
+                    // @LoggerMessage: TRMS
+                    // @Description: Torqeedo Motor Status
+                    // @Field: TimeUS: Time since system startup
+                    // @Field: I: instance
+                    // @Field: State: Motor status flags
+                    // @Field: Err: Motor error flags
+                    AP::logger().Write("TRMS", "TimeUS,I,State,Err",
+                                       "s#--",  // units
+                                       "F---",  // multipliers
+                                       "QBBH",  // formats
+                                       AP_HAL::micros64(),
+                                        _instance,
+                                       _motor_status.status_flags_value,
+                                       _motor_status.error_flags_value);
+                }
+#endif
+
+                // send to GCS
+                if (option_enabled(AP_Torqeedo::options::DEBUG_TO_GCS)) {
+                    GCS_SEND_TEXT(MAV_SEVERITY_INFO,"TRMS i:%u S:%d Err:%d",
+                                   (unsigned)_instance,
+                                   _motor_status.status_flags_value,
+                                   _motor_status.error_flags_value);
+                }
+
+                // report any errors
+                report_error_codes();
+            } else {
+                // unexpected length
+                _parse_error_count++;
+            }
+            break;
+
+        case MotorMsgId::INFO:
+        case MotorMsgId::DRIVE:
+        case MotorMsgId::CONFIG:
+            // we do not process these replies
+            break;
+
+        default:
+            // ignore unknown messages
+            break;
+        }
+    }
+}
+
+// set DE Serial CTS pin to enable sending commands to motor
+void AP_Torqeedo_TQBus::send_start()
+{
+    // set gpio pin or serial port's CTS pin
+    if (_params.pin_de > -1) {
+        hal.gpio->write(_params.pin_de, 1);
+    } else {
+        _uart->set_CTS_pin(true);
+    }
+}
+
+// check for timeout after sending and unset pin if required
+void AP_Torqeedo_TQBus::check_for_send_end()
+{
+    if (_send_delay_us == 0) {
+        // not sending
+        return;
+    }
+
+    if (AP_HAL::micros() - _send_start_us < _send_delay_us) {
+        // return if delay has not yet elapsed
+        return;
+    }
+    _send_delay_us = 0;
+
+    // unset gpio or serial port's CTS pin
+    if (_params.pin_de > -1) {
+        hal.gpio->write(_params.pin_de, 0);
+    } else {
+        _uart->set_CTS_pin(false);
+    }
+}
+
+// calculate delay require to allow bytes to be sent
+uint32_t AP_Torqeedo_TQBus::calc_send_delay_us(uint8_t num_bytes)
+{
+    // baud rate of 19200 bits/sec
+    // total number of bits = 10 x num_bytes
+    // convert from seconds to micros by multiplying by 1,000,000
+    // plus additional 300us safety margin
+    const uint32_t delay_us = 1e6 * num_bytes * 10 / TORQEEDO_SERIAL_BAUD + 300;
+    return delay_us;
+}
+
+// record msgid of message to wait for and set timer for timeout handling
+void AP_Torqeedo_TQBus::set_expected_reply_msgid(uint8_t msg_id)
+{
+    _reply_msgid = msg_id;
+    _reply_wait_start_ms = AP_HAL::millis();
+}
+
+// check for timeout waiting for reply message
+void AP_Torqeedo_TQBus::check_for_reply_timeout()
+{
+    // return immediately if not waiting for reply
+    if (_reply_wait_start_ms == 0) {
+        return;
+    }
+    if (AP_HAL::millis() - _reply_wait_start_ms > TORQEEDO_REPLY_TIMEOUT_MS) {
+        _reply_wait_start_ms = 0;
+        _parse_error_count++;
+    }
+}
+
+// mark reply received. should be called whenever a message is received regardless of whether we are actually waiting for a reply
+void AP_Torqeedo_TQBus::set_reply_received()
+{
+    _reply_wait_start_ms = 0;
+}
+
+// send a message to the motor with the specified message contents
+// msg_contents should not include the header, footer or CRC
+// returns true on success
+bool AP_Torqeedo_TQBus::send_message(const uint8_t msg_contents[], uint8_t num_bytes)
+{
+    // buffer for outgoing message
+    uint8_t send_buff[TORQEEDO_MESSAGE_LEN_MAX];
+    uint8_t send_buff_num_bytes = 0;
+
+    // calculate crc
+    const uint8_t crc = crc8_maxim(msg_contents, num_bytes);
+
+    // add header
+    send_buff[send_buff_num_bytes++] = TORQEEDO_PACKET_HEADER;
+
+    // add contents
+    for (uint8_t i=0; i<num_bytes; i++) {
+        if (!add_byte_to_message(msg_contents[i], send_buff, ARRAY_SIZE(send_buff), send_buff_num_bytes)) {
+            _parse_error_count++;
+            return false;
+        }
+    }
+
+    // add crc
+    if (!add_byte_to_message(crc, send_buff, ARRAY_SIZE(send_buff), send_buff_num_bytes)) {
+        _parse_error_count++;
+        return false;
+    }
+
+    // add footer
+    if (send_buff_num_bytes >= ARRAY_SIZE(send_buff)) {
+        _parse_error_count++;
+        return false;
+    }
+    send_buff[send_buff_num_bytes++] = TORQEEDO_PACKET_FOOTER;
+
+    // set send pin
+    send_start();
+
+    // write message
+    _uart->write(send_buff, send_buff_num_bytes);
+
+    // record start and expected delay to send message
+    _send_start_us = AP_HAL::micros();
+    _send_delay_us = calc_send_delay_us(send_buff_num_bytes);
+
+    return true;
+}
+
+// add a byte to a message buffer including adding the escape character (0xAE) if necessary
+// this should only be used when adding the contents to the buffer, not the header and footer
+// num_bytes is updated to the next free byte
+bool AP_Torqeedo_TQBus::add_byte_to_message(uint8_t byte_to_add, uint8_t msg_buff[], uint8_t msg_buff_size, uint8_t &num_bytes) const
+{
+    bool escape_required = (byte_to_add == TORQEEDO_PACKET_HEADER ||
+                            byte_to_add == TORQEEDO_PACKET_FOOTER ||
+                            byte_to_add == TORQEEDO_PACKET_ESCAPE);
+
+    // check if we have enough space
+    if (num_bytes + (escape_required ? 2 : 1) >= msg_buff_size) {
+        return false;
+    }
+
+    // add byte
+    if (escape_required) {
+        msg_buff[num_bytes++] = TORQEEDO_PACKET_ESCAPE;
+        msg_buff[num_bytes++] = byte_to_add ^ TORQEEDO_PACKET_ESCAPE_MASK;
+    } else {
+        msg_buff[num_bytes++] = byte_to_add;
+    }
+    return true;
+}
+
+// Example "Remote (0x01)" reply message to allow tiller to control motor speed
+// Byte     Field Definition    Example Value   Comments
+// ---------------------------------------------------------------------------------
+// byte 0   Header              0xAC
+// byte 1   TargetAddress       0x00            see MsgAddress enum
+// byte 2   Message ID          0x00            only master populates this. replies have this set to zero
+// byte 3   Flags               0x05            bit0=pin present, bit2=motor speed valid
+// byte 4   Status              0x00            0x20 if byte3=4, 0x0 is byte3=5
+// byte 5   Motor Speed MSB     ----            Motor Speed MSB (-1000 to +1000)
+// byte 6   Motor Speed LSB     ----            Motor Speed LSB (-1000 to +1000)
+// byte 7   CRC-Maxim           ----            CRC-Maxim value
+// byte 8   Footer              0xAD
+//
+// example message when rotating tiller handle forwards:  "AC 00 00 05 00 00 ED 95 AD"    (+237)
+// example message when rotating tiller handle backwards: "AC 00 00 05 00 FF AE 2C 0C AD" (-82)
+
+// send a motor speed command as a value from -1000 to +1000
+// value is taken directly from SRV_Channel
+// for tiller connection this sends the "Remote (0x01)" message
+// for motor connection this sends the "Motor Drive (0x82)" message
+void AP_Torqeedo_TQBus::send_motor_speed_cmd()
+{
+    // calculate desired motor speed
+    if (!hal.util->get_soft_armed()) {
+        _motor_speed_desired = 0;
+    } else {
+        // convert throttle output to motor output in range -1000 to +1000
+        // ToDo: convert PWM output to motor output so that SERVOx_MIN, MAX and TRIM take effect
+        _motor_speed_desired = constrain_int16(SRV_Channels::get_output_norm((SRV_Channel::Aux_servo_function_t)_params.servo_fn.get()) * 1000.0, -1000, 1000);
+    }
+
+    // updated limited motor speed
+    int16_t mot_speed_limited = calc_motor_speed_limited(_motor_speed_desired);
+
+    // by default use tiller connection command
+    uint8_t mot_speed_cmd_buff[] = {(uint8_t)MsgAddress::BUS_MASTER, 0x0, 0x5, 0x0, HIGHBYTE(mot_speed_limited), LOWBYTE(mot_speed_limited)};
+
+    // update message if using motor connection
+    if (get_type() == AP_Torqeedo::ConnectionType::TYPE_MOTOR) {
+        const uint8_t motor_power = (uint8_t)constrain_int16(_params.motor_power, 0, 100);
+        mot_speed_cmd_buff[0] = (uint8_t)MsgAddress::MOTOR;
+        mot_speed_cmd_buff[1] = (uint8_t)MotorMsgId::DRIVE;
+        mot_speed_cmd_buff[2] = (mot_speed_limited == 0 ? 0 : 0x01) | (_motor_clear_error ? 0x04 : 0);  // 1:enable motor, 2:fast off, 4:clear error
+        mot_speed_cmd_buff[3] = mot_speed_limited == 0 ? 0 : motor_power;   // motor power from 0 to 100
+
+        // set expected reply message id
+        set_expected_reply_msgid((uint8_t)MotorMsgId::DRIVE);
+
+        // reset motor clear error request
+        _motor_clear_error = false;
+    }
+
+    // send a message
+    if (send_message(mot_speed_cmd_buff, ARRAY_SIZE(mot_speed_cmd_buff))) {
+        // record time of send for health reporting
+        WITH_SEMAPHORE(_last_healthy_sem);
+        _last_send_motor_ms = AP_HAL::millis();
+    }
+}
+
+// send request to motor to reply with a particular message
+// msg_id can be INFO, STATUS or PARAM
+void AP_Torqeedo_TQBus::send_motor_msg_request(MotorMsgId msg_id)
+{
+    // prepare message
+    uint8_t mot_status_request_buff[] = {(uint8_t)MsgAddress::MOTOR, (uint8_t)msg_id};
+
+    // send a message
+    if (send_message(mot_status_request_buff, ARRAY_SIZE(mot_status_request_buff))) {
+        // record waiting for reply
+        set_expected_reply_msgid((uint8_t)msg_id);
+    }
+}
+
+// calculate the limited motor speed that is sent to the motors
+// desired_motor_speed argument and returned value are in the range -1000 to 1000
+int16_t AP_Torqeedo_TQBus::calc_motor_speed_limited(int16_t desired_motor_speed)
+{
+    const uint32_t now_ms = AP_HAL::millis();
+
+    // update dir_limit flag for forward-reverse transition delay
+    const bool dir_delay_active = is_positive(_params.dir_delay);
+    if (!dir_delay_active) {
+        // allow movement in either direction
+        _dir_limit = 0;
+    } else {
+        // by default limit motor direction to previous iteration's direction
+        if (is_positive(_motor_speed_limited)) {
+            _dir_limit = 1;
+        } else if (is_negative(_motor_speed_limited)) {
+            _dir_limit = -1;
+        } else {
+            // motor speed is zero
+            if ((_motor_speed_zero_ms != 0) && ((now_ms - _motor_speed_zero_ms) > (_params.dir_delay * 1000))) {
+                // delay has passed so allow movement in either direction
+                _dir_limit = 0;
+                _motor_speed_zero_ms = 0;
+            }
+        }
+    }
+
+    // calculate upper and lower limits for forward-reverse transition delay
+    int16_t lower_limit = -1000;
+    int16_t upper_limit = 1000;
+    if (_dir_limit < 0) {
+        upper_limit = 0;
+    }
+    if (_dir_limit > 0) {
+        lower_limit = 0;
+    }
+
+    // calculate dt since last update
+    float dt = (now_ms - _motor_speed_limited_ms) * 0.001f;
+    if (dt > 1.0) {
+        // after a long delay limit motor output to zero to avoid sudden starts
+        lower_limit = 0;
+        upper_limit = 0;
+    }
+    _motor_speed_limited_ms = now_ms;
+
+    // apply slew limit
+    if (_params.slew_time > 0) {
+       const float chg_max = 1000.0 * dt / _params.slew_time;
+       _motor_speed_limited = constrain_float(desired_motor_speed, _motor_speed_limited - chg_max, _motor_speed_limited + chg_max);
+    } else {
+        // no slew limit
+        _motor_speed_limited = desired_motor_speed;
+    }
+
+    // apply upper and lower limits
+    _motor_speed_limited = constrain_float(_motor_speed_limited, lower_limit, upper_limit);
+
+    // record time motor speed becomes zero
+    if (is_zero(_motor_speed_limited)) {
+        if (_motor_speed_zero_ms == 0) {
+            _motor_speed_zero_ms = now_ms;
+        }
+    } else {
+        // clear timer
+        _motor_speed_zero_ms = 0;
+    }
+
+    return (int16_t)_motor_speed_limited;
+}
+
+// output logging and debug messages (if required)
+// force_logging should be true if caller wants to ensure the latest status is logged
+void AP_Torqeedo_TQBus::log_TRQD(bool force_logging)
+{
+    // exit immediately if logging and debug options are not set
+    if (!option_enabled(AP_Torqeedo::options::LOG) && option_enabled(AP_Torqeedo::options::DEBUG_TO_GCS)) {
+        return;
+    }
+
+    // return if not enough time has passed since last output
+    const uint32_t now_ms = AP_HAL::millis();
+    if (!force_logging && (now_ms - _last_log_TRQD_ms < TORQEEDO_LOG_TRQD_INTERVAL_MS)) {
+        return;
+    }
+    _last_log_TRQD_ms = now_ms;
+
+#if HAL_LOGGING_ENABLED || HAL_GCS_ENABLED
+    const bool health = healthy();
+    int16_t actual_motor_speed = get_motor_speed_limited();
+
+#if HAL_LOGGING_ENABLED
+    if (option_enabled(AP_Torqeedo::options::LOG)) {
+        // @LoggerMessage: TRQD
+        // @Description: Torqeedo Status
+        // @Field: TimeUS: Time since system startup
+        // @Field: I: instance
+        // @Field: Health: Health
+        // @Field: DesMotSpeed: Desired Motor Speed (-1000 to 1000)
+        // @Field: MotSpeed: Motor Speed (-1000 to 1000)
+        // @Field: SuccCnt: Success Count
+        // @Field: ErrCnt: Error Count
+        AP::logger().Write("TRQD", "TimeUS,I,Health,DesMotSpeed,MotSpeed,SuccCnt,ErrCnt",
+                           "s#-----",   // units
+                           "F------",   // multipliers
+                           "QBBhhII",   // formats
+                           AP_HAL::micros64(),
+                           _instance,
+                           health,
+                           _motor_speed_desired,
+                           actual_motor_speed,
+                           _parse_success_count,
+                           _parse_error_count);
+    }
+#endif
+
+    if (option_enabled(AP_Torqeedo::options::DEBUG_TO_GCS)) {
+        GCS_SEND_TEXT(MAV_SEVERITY_INFO,"TRQD i:%u h:%u dspd:%d spd:%d succ:%ld err:%ld",
+                (unsigned)_instance,
+                (unsigned)health,
+                (int)_motor_speed_desired,
+                (int)actual_motor_speed,
+                (unsigned long)_parse_success_count,
+                (unsigned long)_parse_error_count);
+    }
+#endif  // HAL_LOGGING_ENABLED || HAL_GCS_ENABLED
+}
+
+// send ESC telemetry
+void AP_Torqeedo_TQBus::update_esc_telem(float rpm, float voltage, float current_amps, float esc_tempC, float motor_tempC)
+{
+#if HAL_WITH_ESC_TELEM
+    // find servo output channel
+    uint8_t telem_esc_index = 0;
+    IGNORE_RETURN(SRV_Channels::find_channel((SRV_Channel::Aux_servo_function_t)_params.servo_fn.get(), telem_esc_index));
+
+    // fill in telemetry data structure
+    AP_ESC_Telem_Backend::TelemetryData telem_dat {};
+    telem_dat.temperature_cdeg = esc_tempC * 100;   // temperature in centi-degrees
+    telem_dat.voltage = voltage;                    // voltage in volts
+    telem_dat.current = current_amps;               // current in amps
+    telem_dat.motor_temp_cdeg = motor_tempC * 100;  // motor temperature in centi-degrees
+
+    // send telem and rpm data
+    update_telem_data(telem_esc_index, telem_dat, AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE |
+                                                  AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE |
+                                                  AP_ESC_Telem_Backend::TelemetryType::CURRENT |
+                                                  AP_ESC_Telem_Backend::TelemetryType::VOLTAGE);
+
+    update_rpm(telem_esc_index, rpm);
+#endif
+}
+
+#endif // HAL_TORQEEDO_ENABLED
diff --git a/libraries/AP_Torqeedo/AP_Torqeedo_TQBus.h b/libraries/AP_Torqeedo/AP_Torqeedo_TQBus.h
new file mode 100644
index 0000000000..1b8dfc560a
--- /dev/null
+++ b/libraries/AP_Torqeedo/AP_Torqeedo_TQBus.h
@@ -0,0 +1,332 @@
+/*
+   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/>.
+ */
+ 
+/*
+   This driver supports communicating with Torqeedo motors that implement the "TQ Bus" protocol
+   which includes the Ultralight, Cruise 2.0 R, Cruise 4.0 R, Travel 503, Travel 1003 and Cruise 10kW
+
+   The autopilot should be connected either to the battery's tiller connector or directly to the motor
+   as described on the ArduPilot wiki. https://ardupilot.org/rover/docs/common-torqeedo.html
+   TQ Bus is a serial protocol over RS-485 meaning that a serial to RS-485 converter is required.
+
+       Tiller connection: Autopilot <-> Battery (master) <-> Motor
+       Motor connection:  Autopilot (master) <-> Motor
+
+    Communication between the components is always initiated by the master with replies sent within 25ms
+
+    Example "Remote (0x01)" reply message to allow tiller to control motor speed
+    Byte        Field Definition    Example Value   Comments
+    ---------------------------------------------------------------------------------
+    byte 0      Header              0xAC
+    byte 1      TargetAddress       0x00            see MsgAddress enum
+    byte 2      Message ID          0x00            only master populates this. replies have this set to zero
+    byte 3      Flags               0x05            bit0=pin present, bit2=motor speed valid
+    byte 4      Status              0x00            0x20 if byte3=4, 0x0 is byte3=5
+    byte 5      Motor Speed MSB     ----            Motor Speed MSB (-1000 to +1000)
+    byte 6      Motor Speed LSB     ----            Motor Speed LSB (-1000 to +1000)
+    byte 7      CRC-Maxim           ----            CRC-Maxim value
+    byte 8      Footer              0xAD
+
+   More details of the TQ Bus protocol are available from Torqeedo after signing an NDA.
+ */
+
+#pragma once
+
+#include "AP_Torqeedo_config.h"
+
+#if HAL_TORQEEDO_ENABLED
+
+#include "AP_Torqeedo_Backend.h"
+
+#define TORQEEDO_MESSAGE_LEN_MAX    35  // messages are no more than 35 bytes
+
+class AP_Torqeedo_TQBus : public AP_Torqeedo_Backend {
+public:
+
+    // constructor
+    using AP_Torqeedo_Backend::AP_Torqeedo_Backend;
+
+    CLASS_NO_COPY(AP_Torqeedo_TQBus);
+
+    // initialise driver
+    void init() override;
+
+    // returns true if communicating with the motor
+    bool healthy() override;
+
+    // clear motor errors
+    void clear_motor_error() override { _motor_clear_error = true; }
+
+    // get latest battery status info.  returns true on success and populates arguments
+    bool get_batt_info(float &voltage, float &current_amps, float &temp_C, uint8_t &pct_remaining) const override;
+    bool get_batt_capacity_Ah(uint16_t &amp_hours) const override;
+
+private:
+
+    // consume incoming messages from motor, reply with latest motor speed
+    // runs in background thread
+    void thread_main();
+
+    // report changes in error codes to user
+    void report_error_codes();
+
+    // message addresses
+    enum class MsgAddress : uint8_t {
+        BUS_MASTER = 0x00,
+        REMOTE1 = 0x14,
+        DISPLAY = 0x20,
+        MOTOR = 0x30,
+        BATTERY = 0x80
+    };
+
+    // Remote specific message ids
+    enum class RemoteMsgId : uint8_t {
+        INFO = 0x00,
+        REMOTE = 0x01,
+        SETUP = 0x02
+    };
+
+    // Display specific message ids
+    enum class DisplayMsgId : uint8_t {
+        INFO = 0x00,
+        SYSTEM_STATE = 0x41,
+        SYSTEM_SETUP = 0x42
+    };
+
+    // Motor specific message ids
+    enum class MotorMsgId : uint8_t {
+        INFO = 0x00,
+        STATUS = 0x01,
+        PARAM = 0x03,
+        CONFIG = 0x04,
+        DRIVE = 0x82
+    };
+
+    enum class ParseState {
+        WAITING_FOR_HEADER = 0,
+        WAITING_FOR_FOOTER,
+    };
+
+    // initialise serial port and gpio pins (run from background thread)
+    // returns true on success
+    bool init_internals();
+
+    // process a single byte received on serial port
+    // return true if a complete message has been received (the message will be held in _received_buff)
+    bool parse_byte(uint8_t b);
+
+    // process message held in _received_buff
+    void parse_message();
+
+    // returns true if it is safe to send a message
+    bool safe_to_send() const { return ((_send_delay_us == 0) && (_reply_wait_start_ms == 0)); }
+
+    // set pin to enable sending a message
+    void send_start();
+
+    // check for timeout after sending a message and unset pin if required
+    void check_for_send_end();
+
+    // calculate delay required to allow message to be completely sent
+    uint32_t calc_send_delay_us(uint8_t num_bytes);
+
+    // record msgid of message to wait for and set timer for reply timeout handling
+    void set_expected_reply_msgid(uint8_t msg_id);
+
+    // check for timeout waiting for reply
+    void check_for_reply_timeout();
+
+    // mark reply received. should be called whenever a message is received regardless of whether we are actually waiting for a reply
+    void set_reply_received();
+
+    // send a message to the motor with the specified message contents
+    // msg_contents should not include the header, footer or CRC
+    // returns true on success
+    bool send_message(const uint8_t msg_contents[], uint8_t num_bytes);
+
+    // add a byte to a message buffer including adding the escape character (0xAE) if necessary
+    // this should only be used when adding the contents to the buffer, not the header and footer
+    // num_bytes is updated to the next free byte
+    bool add_byte_to_message(uint8_t byte_to_add, uint8_t msg_buff[], uint8_t msg_buff_size, uint8_t &num_bytes) const;
+
+    // send a motor speed command as a value from -1000 to +1000
+    // value is taken directly from SRV_Channel
+    void send_motor_speed_cmd();
+
+    // send request to motor to reply with a particular message
+    // msg_id can be INFO, STATUS or PARAM
+    void send_motor_msg_request(MotorMsgId msg_id);
+
+    // calculate the limited motor speed that is sent to the motors
+    // desired_motor_speed argument and returned value are in the range -1000 to 1000
+    int16_t calc_motor_speed_limited(int16_t desired_motor_speed);
+    int16_t get_motor_speed_limited() const { return (int16_t)_motor_speed_limited; }
+
+    // log TRQD message which holds high level status and latest desired motors peed
+    // force_logging should be true to immediately write log bypassing timing check to avoid spamming
+    void log_TRQD(bool force_logging);
+
+    // send ESC telemetry
+    void update_esc_telem(float rpm, float voltage, float current_amps, float esc_tempC, float motor_tempC);
+
+    // 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;   // desired motor speed (set from within update method)
+    uint32_t _last_send_motor_ms;   // system time (in millis) last motor speed command was sent (used for health reporting)
+    bool _motor_clear_error;        // true if the motor error should be cleared (sent in "Drive" message)
+    uint32_t _send_start_us;        // system time (in micros) when last message started being sent (used for timing to unset DE pin)
+    uint32_t _send_delay_us;        // delay (in micros) to allow bytes to be sent after which pin can be unset.  0 if not delaying
+
+    // motor speed limit variables
+    float _motor_speed_limited;     // limited desired motor speed. this value is actually sent to the motor
+    uint32_t _motor_speed_limited_ms; // system time that _motor_speed_limited was last updated
+    int8_t _dir_limit;              // acceptable directions for output to motor (+1 = positive OK, -1 = negative OK, 0 = either positive or negative OK)
+    uint32_t _motor_speed_zero_ms;  // system time that _motor_speed_limited reached zero.  0 if currently not zero
+
+    // health reporting
+    HAL_Semaphore _last_healthy_sem;// semaphore protecting reading and updating of _last_send_motor_ms and _last_received_ms
+    uint32_t _last_log_TRQD_ms;     // system time (in millis) that TRQD was last logged
+
+    // message parsing members
+    ParseState _parse_state;        // current state of parsing
+    bool _parse_escape_received;    // true if the escape character has been received so we must XOR the next byte
+    uint32_t _parse_error_count;    // total number of parsing errors (for reporting)
+    uint32_t _parse_success_count;  // number of messages successfully parsed (for reporting)
+    uint8_t _received_buff[TORQEEDO_MESSAGE_LEN_MAX];   // characters received
+    uint8_t _received_buff_len;     // number of characters received
+    uint32_t _last_received_ms;     // system time (in millis) that a message was successfully parsed (for health reporting)
+
+    // reply message handling
+    uint8_t _reply_msgid;           // replies expected msgid (reply often does not specify the msgid so we must record it)
+    uint32_t _reply_wait_start_ms;  // system time that we started waiting for a reply message
+
+    // Display system state flags
+    typedef union PACKED {
+        struct {
+            uint8_t set_throttle_stop   : 1;    // 0, warning that user must set throttle to stop before motor can run
+            uint8_t setup_allowed       : 1;    // 1, remote is allowed to enter setup mode
+            uint8_t in_charge           : 1;    // 2, master is in charging state
+            uint8_t in_setup            : 1;    // 3, master is in setup state
+            uint8_t bank_available      : 1;    // 4
+            uint8_t no_menu             : 1;    // 5
+            uint8_t menu_off            : 1;    // 6
+            uint8_t reserved7           : 1;    // 7, unused
+            uint8_t temp_warning        : 1;    // 8, motor or battery temp warning
+            uint8_t batt_charge_valid   : 1;    // 9, battery charge valid
+            uint8_t batt_nearly_empty   : 1;    // 10, battery nearly empty
+            uint8_t batt_charging       : 1;    // 11, battery charging
+            uint8_t gps_searching       : 1;    // 12, gps searching for satellites
+            uint8_t gps_speed_valid     : 1;    // 13, gps speed is valid
+            uint8_t range_miles_valid   : 1;    // 14, range (in miles) is valid
+            uint8_t range_minutes_valid : 1;    // 15, range (in minutes) is valid
+        };
+        uint16_t value;
+    } DisplaySystemStateFlags;
+
+    // Display system state
+    struct DisplaySystemState {
+        DisplaySystemStateFlags flags;  // flags, see above for individual bit definitions
+        uint8_t master_state;           // deprecated
+        uint8_t master_error_code;      // error code (0=no error)
+        float motor_voltage;            // motor voltage in volts
+        float motor_current;            // motor current in amps
+        uint16_t motor_power;           // motor power in watts
+        int16_t motor_rpm;              // motor speed in rpm
+        uint8_t motor_pcb_temp;         // motor pcb temp in C
+        uint8_t motor_stator_temp;      // motor stator temp in C
+        uint8_t batt_charge_pct;        // battery state of charge (0 to 100%)
+        float batt_voltage;             // battery voltage in volts
+        float batt_current;             // battery current in amps
+        uint16_t gps_speed;             // gps speed in knots * 100
+        uint16_t range_miles;           // range in nautical miles * 10
+        uint16_t range_minutes;         // range in minutes (at current speed and current draw)
+        uint8_t temp_sw;                // master PCB temp in C (close to motor power switches)
+        uint8_t temp_rp;                // master PCB temp in C (close to reverse voltage protection)
+        uint32_t last_update_ms;        // system time that system state was last updated
+    } _display_system_state;
+
+    // Display system setup
+    struct DisplaySystemSetup {
+        uint8_t flags;              // 0 : battery config valid, all other bits unused
+        uint8_t motor_type;         // motor type (0 or 3:Unknown, 1:Ultralight, 2:Cruise2, 4:Cruise4, 5:Travel503, 6:Travel1003, 7:Cruise10kW)
+        uint16_t motor_sw_version;  // motor software version
+        uint16_t batt_capacity;     // battery capacity in amp hours
+        uint8_t batt_charge_pct;    // battery state of charge (0 to 100%)
+        uint8_t batt_type;          // battery type (0:lead acid, 1:Lithium)
+        uint16_t master_sw_version; // master software version
+    } _display_system_setup;
+
+    // Motor status
+    struct MotorStatus {
+        union PACKED {
+            struct {
+                uint8_t temp_limit_motor    : 1;    // 0, motor speed limited due to motor temp
+                uint8_t temp_limit_pcb      : 1;    // 1, motor speed limited tue to PCB temp
+                uint8_t emergency_stop      : 1;    // 2, motor in emergency stop (must be cleared by master)
+                uint8_t running             : 1;    // 3, motor running
+                uint8_t power_limit         : 1;    // 4, motor power limited
+                uint8_t low_voltage_limit   : 1;    // 5, motor speed limited because of low voltage
+                uint8_t tilt                : 1;    // 6, motor is tilted
+                uint8_t reserved7           : 1;    // 7, unused (always zero)
+            } status_flags;
+            uint8_t status_flags_value;
+        };
+        union PACKED {
+            struct {
+                uint8_t overcurrent         : 1;    // 0, motor stopped because of overcurrent
+                uint8_t blocked             : 1;    // 1, motor stopped because it is blocked
+                uint8_t overvoltage_static  : 1;    // 2, motor stopped because voltage too high
+                uint8_t undervoltage_static : 1;    // 3, motor stopped because voltage too low
+                uint8_t overvoltage_current : 1;    // 4, motor stopped because voltage spiked high
+                uint8_t undervoltage_current: 1;    // 5, motor stopped because voltage spiked low
+                uint8_t overtemp_motor      : 1;    // 6, motor stopped because stator temp too high
+                uint8_t overtemp_pcb        : 1;    // 7, motor stopped because pcb temp too high
+                uint8_t timeout_rs485       : 1;    // 8, motor stopped because Drive message not received for too long
+                uint8_t temp_sensor_error   : 1;    // 9, motor temp sensor is defective (motor will not stop)
+                uint8_t tilt                : 1;    // 10, motor stopped because it was tilted
+                uint8_t unused11to15        : 5;    // 11 ~ 15 (always zero)
+            } error_flags;
+            uint16_t error_flags_value;
+        };
+    } _motor_status;
+    uint32_t _last_send_motor_status_request_ms;    // system time (in milliseconds) that last motor status request was sent
+
+    // Motor params
+    struct MotorParam {
+        int16_t rpm;            // motor rpm
+        uint16_t power;         // motor power consumption in Watts
+        float voltage;          // motor voltage in volts
+        float current;          // motor current in amps
+        float pcb_temp;         // pcb temp in C
+        float stator_temp;      // stator temp in C
+        uint32_t last_update_ms;// system time that above values were updated
+    } _motor_param;
+    uint32_t _last_send_motor_param_request_ms;     // system time (in milliseconds) that last motor param request was sent
+
+    // error reporting
+    DisplaySystemStateFlags _display_system_state_flags_prev;   // backup of display system state flags
+    uint8_t _display_system_state_master_error_code_prev;       // backup of display system state master_error_code
+    uint32_t _last_error_report_ms;                             // system time that flag changes were last reported (used to prevent spamming user)
+    MotorStatus _motor_status_prev;                             // backup of motor status
+
+    // returns a human-readable string corresponding the passed-in
+    // master error code (see page 93 of https://media.torqeedo.com/downloads/manuals/torqeedo-Travel-manual-DE-EN.pdf)
+    // If no conversion is available then nullptr is returned
+    const char *map_master_error_code_to_string(uint8_t code) const;
+};
+
+#endif // HAL_TORQEEDO_ENABLED