diff --git a/libraries/AP_Torqeedo/AP_Torqeedo.cpp b/libraries/AP_Torqeedo/AP_Torqeedo.cpp
new file mode 100644
index 0000000000..9a8ecc55f0
--- /dev/null
+++ b/libraries/AP_Torqeedo/AP_Torqeedo.cpp
@@ -0,0 +1,309 @@
+/*
+ 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 .
+ */
+
+#include "AP_Torqeedo.h"
+
+#if HAL_TORQEEDO_ENABLED
+
+#include
+#include
+#include
+
+#define TORQEEDO_SERIAL_BAUD 19200 // communication is always at 19200
+#define TORQEEDO_PACKET_HEADER 0xAC // communication packet header
+#define TORQEEDO_PACKET_FOOTER 0xAD // communication packer footer
+
+extern const AP_HAL::HAL& hal;
+
+// parameters
+const AP_Param::GroupInfo AP_Torqeedo::var_info[] = {
+
+ // @Param: ENABLE
+ // @DisplayName: Torqeedo enable
+ // @Description: Torqeedo enable
+ // @Values: 0:Disabled, 1:Enabled
+ // @User: Standard
+ // @RebootRequired: True
+ AP_GROUPINFO_FLAGS("ENABLE", 1, AP_Torqeedo, _enable, 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 disable turning motor on/off from autopilot
+ // @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),
+
+ // @Param: DE_PIN
+ // @DisplayName: Torqeedo DE pin
+ // @Description: Pin number connected to RS485 to Serial converter's DE pin. -1 to disable sending commands to motor
+ // @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),
+
+ AP_GROUPEND
+};
+
+AP_Torqeedo::AP_Torqeedo()
+{
+ _singleton = this;
+ AP_Param::setup_object_defaults(this, var_info);
+}
+
+// initialise driver
+void AP_Torqeedo::init()
+{
+ // only init once
+ 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);
+
+ // initialise onoff pin and set for 0.5 sec to turn on motor
+ if (_pin_onoff > -1) {
+ hal.gpio->pinMode(_pin_onoff, HAL_GPIO_OUTPUT);
+ hal.gpio->write(_pin_onoff, 1);
+
+ // delay 0.5 sec and then unset pin
+ hal.scheduler->delay(500);
+ hal.gpio->write(_pin_onoff, 0);
+ }
+
+ // 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);
+ }
+
+ return true;
+}
+
+// consume incoming messages from motor, reply with latest motor speed
+// runs in background thread
+void AP_Torqeedo::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();
+
+ // parse incoming characters
+ uint32_t nbytes = MIN(_uart->available(), 1024U);
+ bool motor_speed_request_received = false;
+ while (nbytes-- > 0) {
+ int16_t b = _uart->read();
+ if (b >= 0 ) {
+ if (parse_byte((uint8_t)b)) {
+ // request received to send updated motor speed
+ motor_speed_request_received = true;
+ }
+ }
+ }
+
+ // send motor speed
+ if (motor_speed_request_received) {
+ send_motor_speed_cmd();
+ }
+ }
+}
+
+// process a single byte received on serial port
+// return true if a this driver should send a set-motor-speed message
+bool AP_Torqeedo::parse_byte(uint8_t b)
+{
+ bool motor_speed_requested = false;
+
+ switch (_parse_state) {
+ case ParseState::WAITING_FOR_HEADER:
+ if (b == TORQEEDO_PACKET_HEADER) {
+ _parse_state = ParseState::WAITING_FOR_FOOTER;
+ }
+ _received_buff_len = 0;
+ 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++;
+
+ // check message id
+ MsgId msg_id = (MsgId)_received_buff[0];
+ if (msg_id == MsgId::REQUEST_MOTOR_SPEED) {
+ motor_speed_requested = true;
+ }
+
+ } else {
+ // 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 motor_speed_requested;
+}
+
+// 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() - _last_send_motor_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;
+}
+
+// format of motor speed command packet
+//
+// Data Byte Field Definition Example Value Comments
+// ---------------------------------------------------------------------------------
+// byte 0 Header 0xAC
+// byte 1 SourceId? 0x00 0 = tiller?
+// byte 2 Destination ID? 0x00 0 = all?
+// byte 3 Command Id? 0x05 0=Stop? 4=Don'tTurn? 5=Turn?
+// byte 4 Command Id? 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 forwards: "AC 00 00 05 00 00 ED 95 AD" (+237)
+// example message when rotating 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
+void AP_Torqeedo::send_motor_speed_cmd()
+{
+ // calculate desired motor speed
+ if (!hal.util->get_soft_armed()) {
+ _motor_speed = 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 = constrain_int16(SRV_Channels::get_output_norm(SRV_Channel::Aux_servo_function_t::k_throttle) * 1000.0, -1000, 1000);
+ }
+
+ // set send pin
+ send_start();
+
+ uint8_t mot_speed_cmd_buff[] = {TORQEEDO_PACKET_HEADER, 0x0, 0x0, 0x5, 0x0, HIGHBYTE(_motor_speed), LOWBYTE(_motor_speed), 0x0, TORQEEDO_PACKET_FOOTER};
+ uint8_t buff_size = ARRAY_SIZE(mot_speed_cmd_buff);
+
+ // calculate crc and add to buffer
+ const uint8_t crc = crc8_maxim(&mot_speed_cmd_buff[1], buff_size-3);
+ mot_speed_cmd_buff[buff_size-2] = crc;
+
+ // write message
+ _uart->write(mot_speed_cmd_buff, buff_size);
+
+ _last_send_motor_us = AP_HAL::micros();
+ _send_delay_us = calc_send_delay_us(buff_size);
+}
+
+// get the AP_Torqeedo singleton
+AP_Torqeedo *AP_Torqeedo::get_singleton()
+{
+ return _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
new file mode 100644
index 0000000000..094b713ec5
--- /dev/null
+++ b/libraries/AP_Torqeedo/AP_Torqeedo.h
@@ -0,0 +1,110 @@
+/*
+ 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 .
+ */
+
+#pragma once
+
+#include
+
+#ifndef HAL_TORQEEDO_ENABLED
+#define HAL_TORQEEDO_ENABLED (BOARD_FLASH_SIZE > 1024)
+#endif
+
+#if HAL_TORQEEDO_ENABLED
+
+#include
+
+#define TORQEEDO_MESSAGE_LEN_MAX 30 // messages are no more than 30 bytes
+
+class AP_Torqeedo {
+public:
+ AP_Torqeedo();
+
+ CLASS_NO_COPY(AP_Torqeedo);
+
+ 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();
+
+ static const struct AP_Param::GroupInfo var_info[];
+
+private:
+
+ // message ids
+ enum class MsgId : uint8_t {
+ SET_MOTOR_SPEED = 0x0,
+ UNKNOWN_0x01 = 0x1,
+ REQUEST_MOTOR_SPEED = 0x14,
+ UNKNOWN_0x20 = 0x20,
+ UNKNOWN_0x30 = 0x30,
+ };
+
+ 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 this driver should send a set-motor-speed message
+ bool parse_byte(uint8_t b);
+
+ // set pin to enable sending commands to motor
+ void send_start();
+
+ // check for timeout after sending and unset pin if required
+ void check_for_send_end();
+
+ // calculate delay require to allow bytes to be sent
+ uint32_t calc_send_delay_us(uint8_t num_bytes);
+
+ // send a motor speed command as a value from -1000 to +1000
+ // value is taken directly from SRV_Channel
+ void send_motor_speed_cmd();
+
+ // parameters
+ AP_Int8 _enable; // 1 if torqeedo feature is enabled
+ AP_Int8 _pin_onoff; // Pin number connected to Torqeedo's on/off pin. -1 to disable turning motor on/off from autopilot
+ AP_Int8 _pin_de; // Pin number connected to RS485 to Serial converter's DE pin. -1 to disable sending commands to motor
+
+ // members
+ AP_HAL::UARTDriver *_uart; // serial port to communicate with motor
+ bool _initialised; // true once driver has been initialised
+ int16_t _motor_speed; // desired motor speed (set from within update method)
+ uint32_t _last_send_motor_us; // system time (in micros) last motor speed command was sent
+ uint32_t _send_delay_us; // delay (in micros) to allow bytes to be sent after which pin can be unset. 0 if not delaying
+
+ // message parsing members
+ ParseState _parse_state; // current state of parsing
+ 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
+
+ static AP_Torqeedo *_singleton;
+};
+
+namespace AP {
+ AP_Torqeedo *torqeedo();
+};
+
+#endif // HAL_TORQEEDO_ENABLED