ardupilot/libraries/AP_Torqeedo/AP_Torqeedo.cpp

/*
   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.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>

#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;

// 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),

    // @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),

    // @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),

    AP_GROUPEND
};

AP_Torqeedo::AP_Torqeedo()
{
    _singleton = this;
    AP_Param::setup_object_defaults(this, var_info);
}

// 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);
        }
    }

    // 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);
    }

    return true;
}

// returns true if the driver is enabled
bool AP_Torqeedo::enabled() const
{
    switch ((ConnectionType)_type) {
    case ConnectionType::TYPE_DISABLED:
        return false;
    case ConnectionType::TYPE_TILLER:
    case ConnectionType::TYPE_MOTOR:
        return true;
    }

    return false;
}

// consume incoming messages from motor, reply with latest motor speed
// runs in background thread
void AP_Torqeedo::thread_main()
{
    // 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 (_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);
    }
}

// returns true if communicating with the motor
bool AP_Torqeedo::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));
    }
}

// run pre-arm check.  returns false on failure and fills in failure_msg
// any failure_msg returned will not include a prefix
bool AP_Torqeedo::pre_arm_checks(char *failure_msg, uint8_t failure_msg_len)
{
    // exit immediately if not enabled
    if (!enabled()) {
        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;
    }
    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
{
    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);
        }
    }

    // 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
{

    // 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::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::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::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;

    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);
    }
}

// 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
}

// 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