/* 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 . */ /* 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 #include #define TORQEEDO_MESSAGE_LEN_MAX 35 // messages are no more than 35 bytes class AP_Torqeedo : public AP_ESC_Telem_Backend { 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(); // 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 ¤t_amps, float &temp_C, uint8_t &pct_remaining) const WARN_IF_UNUSED; bool get_batt_capacity_Ah(uint16_t &_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, TYPE_TILLER = 1, TYPE_MOTOR = 2 }; // OPTIONS parameter values enum 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(); // returns true if the driver is enabled bool enabled() 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); // 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); // parameters AP_Enum _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; }; namespace AP { AP_Torqeedo *torqeedo(); }; #endif // HAL_TORQEEDO_ENABLED