Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 :
16 : /*
17 : This driver supports communicating with Torqeedo motors that implement the "TQ Bus" protocol
18 : which includes the Ultralight, Cruise 2.0 R, Cruise 4.0 R, Travel 503, Travel 1003 and Cruise 10kW
19 :
20 : The autopilot should be connected either to the battery's tiller connector or directly to the motor
21 : as described on the ArduPilot wiki. https://ardupilot.club/rover/docs/common-torqeedo.html
22 : TQ Bus is a serial protocol over RS-485 meaning that a serial to RS-485 converter is required.
23 :
24 : Tiller connection: Autopilot <-> Battery (master) <-> Motor
25 : Motor connection: Autopilot (master) <-> Motor
26 :
27 : Communication between the components is always initiated by the master with replies sent within 25ms
28 :
29 : Example "Remote (0x01)" reply message to allow tiller to control motor speed
30 : Byte Field Definition Example Value Comments
31 : ---------------------------------------------------------------------------------
32 : byte 0 Header 0xAC
33 : byte 1 TargetAddress 0x00 see MsgAddress enum
34 : byte 2 Message ID 0x00 only master populates this. replies have this set to zero
35 : byte 3 Flags 0x05 bit0=pin present, bit2=motor speed valid
36 : byte 4 Status 0x00 0x20 if byte3=4, 0x0 is byte3=5
37 : byte 5 Motor Speed MSB ---- Motor Speed MSB (-1000 to +1000)
38 : byte 6 Motor Speed LSB ---- Motor Speed LSB (-1000 to +1000)
39 : byte 7 CRC-Maxim ---- CRC-Maxim value
40 : byte 8 Footer 0xAD
41 :
42 : More details of the TQ Bus protocol are available from Torqeedo after signing an NDA.
43 : */
44 :
45 : #pragma once
46 :
47 : #include "AP_Torqeedo_config.h"
48 :
49 : #if HAL_TORQEEDO_ENABLED
50 :
51 : #include "AP_Torqeedo_Backend.h"
52 :
53 : #define TORQEEDO_MESSAGE_LEN_MAX 35 // messages are no more than 35 bytes
54 :
55 : class AP_Torqeedo_TQBus : public AP_Torqeedo_Backend {
56 : public:
57 :
58 : // constructor
59 : using AP_Torqeedo_Backend::AP_Torqeedo_Backend;
60 :
61 : CLASS_NO_COPY(AP_Torqeedo_TQBus);
62 :
63 : // initialise driver
64 : void init() override;
65 :
66 : // returns true if communicating with the motor
67 : bool healthy() override;
68 :
69 : // clear motor errors
70 0 : void clear_motor_error() override { _motor_clear_error = true; }
71 :
72 : // get latest battery status info. returns true on success and populates arguments
73 : bool get_batt_info(float &voltage, float ¤t_amps, float &temp_C, uint8_t &pct_remaining) const override;
74 : bool get_batt_capacity_Ah(uint16_t &_hours) const override;
75 :
76 : private:
77 :
78 : // consume incoming messages from motor, reply with latest motor speed
79 : // runs in background thread
80 : void thread_main();
81 :
82 : // report changes in error codes to user
83 : void report_error_codes();
84 :
85 : // message addresses
86 : enum class MsgAddress : uint8_t {
87 : BUS_MASTER = 0x00,
88 : REMOTE1 = 0x14,
89 : DISPLAY = 0x20,
90 : MOTOR = 0x30,
91 : BATTERY = 0x80
92 : };
93 :
94 : // Remote specific message ids
95 : enum class RemoteMsgId : uint8_t {
96 : INFO = 0x00,
97 : REMOTE = 0x01,
98 : SETUP = 0x02
99 : };
100 :
101 : // Display specific message ids
102 : enum class DisplayMsgId : uint8_t {
103 : INFO = 0x00,
104 : SYSTEM_STATE = 0x41,
105 : SYSTEM_SETUP = 0x42
106 : };
107 :
108 : // Motor specific message ids
109 : enum class MotorMsgId : uint8_t {
110 : INFO = 0x00,
111 : STATUS = 0x01,
112 : PARAM = 0x03,
113 : CONFIG = 0x04,
114 : DRIVE = 0x82
115 : };
116 :
117 : enum class ParseState {
118 : WAITING_FOR_HEADER = 0,
119 : WAITING_FOR_FOOTER,
120 : };
121 :
122 : // initialise serial port and gpio pins (run from background thread)
123 : // returns true on success
124 : bool init_internals();
125 :
126 : // process a single byte received on serial port
127 : // return true if a complete message has been received (the message will be held in _received_buff)
128 : bool parse_byte(uint8_t b);
129 :
130 : // process message held in _received_buff
131 : void parse_message();
132 :
133 : // returns true if it is safe to send a message
134 0 : bool safe_to_send() const { return ((_send_delay_us == 0) && (_reply_wait_start_ms == 0)); }
135 :
136 : // set pin to enable sending a message
137 : void send_start();
138 :
139 : // check for timeout after sending a message and unset pin if required
140 : void check_for_send_end();
141 :
142 : // calculate delay required to allow message to be completely sent
143 : uint32_t calc_send_delay_us(uint8_t num_bytes);
144 :
145 : // record msgid of message to wait for and set timer for reply timeout handling
146 : void set_expected_reply_msgid(uint8_t msg_id);
147 :
148 : // check for timeout waiting for reply
149 : void check_for_reply_timeout();
150 :
151 : // mark reply received. should be called whenever a message is received regardless of whether we are actually waiting for a reply
152 : void set_reply_received();
153 :
154 : // send a message to the motor with the specified message contents
155 : // msg_contents should not include the header, footer or CRC
156 : // returns true on success
157 : bool send_message(const uint8_t msg_contents[], uint8_t num_bytes);
158 :
159 : // add a byte to a message buffer including adding the escape character (0xAE) if necessary
160 : // this should only be used when adding the contents to the buffer, not the header and footer
161 : // num_bytes is updated to the next free byte
162 : bool add_byte_to_message(uint8_t byte_to_add, uint8_t msg_buff[], uint8_t msg_buff_size, uint8_t &num_bytes) const;
163 :
164 : // send a motor speed command as a value from -1000 to +1000
165 : // value is taken directly from SRV_Channel
166 : void send_motor_speed_cmd();
167 :
168 : // send request to motor to reply with a particular message
169 : // msg_id can be INFO, STATUS or PARAM
170 : void send_motor_msg_request(MotorMsgId msg_id);
171 :
172 : // calculate the limited motor speed that is sent to the motors
173 : // desired_motor_speed argument and returned value are in the range -1000 to 1000
174 : int16_t calc_motor_speed_limited(int16_t desired_motor_speed);
175 0 : int16_t get_motor_speed_limited() const { return (int16_t)_motor_speed_limited; }
176 :
177 : // log TRQD message which holds high level status and latest desired motors peed
178 : // force_logging should be true to immediately write log bypassing timing check to avoid spamming
179 : void log_TRQD(bool force_logging);
180 :
181 : // send ESC telemetry
182 : void update_esc_telem(float rpm, float voltage, float current_amps, float esc_tempC, float motor_tempC);
183 :
184 : // members
185 : AP_HAL::UARTDriver *_uart; // serial port to communicate with motor
186 : bool _initialised; // true once driver has been initialised
187 : bool _send_motor_speed; // true if motor speed should be sent at next opportunity
188 : int16_t _motor_speed_desired; // desired motor speed (set from within update method)
189 : uint32_t _last_send_motor_ms; // system time (in millis) last motor speed command was sent (used for health reporting)
190 : bool _motor_clear_error; // true if the motor error should be cleared (sent in "Drive" message)
191 : uint32_t _send_start_us; // system time (in micros) when last message started being sent (used for timing to unset DE pin)
192 : uint32_t _send_delay_us; // delay (in micros) to allow bytes to be sent after which pin can be unset. 0 if not delaying
193 :
194 : // motor speed limit variables
195 : float _motor_speed_limited; // limited desired motor speed. this value is actually sent to the motor
196 : uint32_t _motor_speed_limited_ms; // system time that _motor_speed_limited was last updated
197 : int8_t _dir_limit; // acceptable directions for output to motor (+1 = positive OK, -1 = negative OK, 0 = either positive or negative OK)
198 : uint32_t _motor_speed_zero_ms; // system time that _motor_speed_limited reached zero. 0 if currently not zero
199 :
200 : // health reporting
201 : HAL_Semaphore _last_healthy_sem;// semaphore protecting reading and updating of _last_send_motor_ms and _last_received_ms
202 : uint32_t _last_log_TRQD_ms; // system time (in millis) that TRQD was last logged
203 :
204 : // message parsing members
205 : ParseState _parse_state; // current state of parsing
206 : bool _parse_escape_received; // true if the escape character has been received so we must XOR the next byte
207 : uint32_t _parse_error_count; // total number of parsing errors (for reporting)
208 : uint32_t _parse_success_count; // number of messages successfully parsed (for reporting)
209 : uint8_t _received_buff[TORQEEDO_MESSAGE_LEN_MAX]; // characters received
210 : uint8_t _received_buff_len; // number of characters received
211 : uint32_t _last_received_ms; // system time (in millis) that a message was successfully parsed (for health reporting)
212 :
213 : // reply message handling
214 : uint8_t _reply_msgid; // replies expected msgid (reply often does not specify the msgid so we must record it)
215 : uint32_t _reply_wait_start_ms; // system time that we started waiting for a reply message
216 :
217 : // Display system state flags
218 : typedef union PACKED {
219 : struct {
220 : uint8_t set_throttle_stop : 1; // 0, warning that user must set throttle to stop before motor can run
221 : uint8_t setup_allowed : 1; // 1, remote is allowed to enter setup mode
222 : uint8_t in_charge : 1; // 2, master is in charging state
223 : uint8_t in_setup : 1; // 3, master is in setup state
224 : uint8_t bank_available : 1; // 4
225 : uint8_t no_menu : 1; // 5
226 : uint8_t menu_off : 1; // 6
227 : uint8_t reserved7 : 1; // 7, unused
228 : uint8_t temp_warning : 1; // 8, motor or battery temp warning
229 : uint8_t batt_charge_valid : 1; // 9, battery charge valid
230 : uint8_t batt_nearly_empty : 1; // 10, battery nearly empty
231 : uint8_t batt_charging : 1; // 11, battery charging
232 : uint8_t gps_searching : 1; // 12, gps searching for satellites
233 : uint8_t gps_speed_valid : 1; // 13, gps speed is valid
234 : uint8_t range_miles_valid : 1; // 14, range (in miles) is valid
235 : uint8_t range_minutes_valid : 1; // 15, range (in minutes) is valid
236 : };
237 : uint16_t value;
238 : } DisplaySystemStateFlags;
239 :
240 : // Display system state
241 : struct DisplaySystemState {
242 : DisplaySystemStateFlags flags; // flags, see above for individual bit definitions
243 : uint8_t master_state; // deprecated
244 : uint8_t master_error_code; // error code (0=no error)
245 : float motor_voltage; // motor voltage in volts
246 : float motor_current; // motor current in amps
247 : uint16_t motor_power; // motor power in watts
248 : int16_t motor_rpm; // motor speed in rpm
249 : uint8_t motor_pcb_temp; // motor pcb temp in C
250 : uint8_t motor_stator_temp; // motor stator temp in C
251 : uint8_t batt_charge_pct; // battery state of charge (0 to 100%)
252 : float batt_voltage; // battery voltage in volts
253 : float batt_current; // battery current in amps
254 : uint16_t gps_speed; // gps speed in knots * 100
255 : uint16_t range_miles; // range in nautical miles * 10
256 : uint16_t range_minutes; // range in minutes (at current speed and current draw)
257 : uint8_t temp_sw; // master PCB temp in C (close to motor power switches)
258 : uint8_t temp_rp; // master PCB temp in C (close to reverse voltage protection)
259 : uint32_t last_update_ms; // system time that system state was last updated
260 : } _display_system_state;
261 :
262 : // Display system setup
263 : struct DisplaySystemSetup {
264 : uint8_t flags; // 0 : battery config valid, all other bits unused
265 : uint8_t motor_type; // motor type (0 or 3:Unknown, 1:Ultralight, 2:Cruise2, 4:Cruise4, 5:Travel503, 6:Travel1003, 7:Cruise10kW)
266 : uint16_t motor_sw_version; // motor software version
267 : uint16_t batt_capacity; // battery capacity in amp hours
268 : uint8_t batt_charge_pct; // battery state of charge (0 to 100%)
269 : uint8_t batt_type; // battery type (0:lead acid, 1:Lithium)
270 : uint16_t master_sw_version; // master software version
271 : } _display_system_setup;
272 :
273 : // Motor status
274 : struct MotorStatus {
275 : union PACKED {
276 : struct {
277 : uint8_t temp_limit_motor : 1; // 0, motor speed limited due to motor temp
278 : uint8_t temp_limit_pcb : 1; // 1, motor speed limited tue to PCB temp
279 : uint8_t emergency_stop : 1; // 2, motor in emergency stop (must be cleared by master)
280 : uint8_t running : 1; // 3, motor running
281 : uint8_t power_limit : 1; // 4, motor power limited
282 : uint8_t low_voltage_limit : 1; // 5, motor speed limited because of low voltage
283 : uint8_t tilt : 1; // 6, motor is tilted
284 : uint8_t reserved7 : 1; // 7, unused (always zero)
285 : } status_flags;
286 : uint8_t status_flags_value;
287 : };
288 : union PACKED {
289 : struct {
290 : uint8_t overcurrent : 1; // 0, motor stopped because of overcurrent
291 : uint8_t blocked : 1; // 1, motor stopped because it is blocked
292 : uint8_t overvoltage_static : 1; // 2, motor stopped because voltage too high
293 : uint8_t undervoltage_static : 1; // 3, motor stopped because voltage too low
294 : uint8_t overvoltage_current : 1; // 4, motor stopped because voltage spiked high
295 : uint8_t undervoltage_current: 1; // 5, motor stopped because voltage spiked low
296 : uint8_t overtemp_motor : 1; // 6, motor stopped because stator temp too high
297 : uint8_t overtemp_pcb : 1; // 7, motor stopped because pcb temp too high
298 : uint8_t timeout_rs485 : 1; // 8, motor stopped because Drive message not received for too long
299 : uint8_t temp_sensor_error : 1; // 9, motor temp sensor is defective (motor will not stop)
300 : uint8_t tilt : 1; // 10, motor stopped because it was tilted
301 : uint8_t unused11to15 : 5; // 11 ~ 15 (always zero)
302 : } error_flags;
303 : uint16_t error_flags_value;
304 : };
305 : } _motor_status;
306 : uint32_t _last_send_motor_status_request_ms; // system time (in milliseconds) that last motor status request was sent
307 :
308 : // Motor params
309 : struct MotorParam {
310 : int16_t rpm; // motor rpm
311 : uint16_t power; // motor power consumption in Watts
312 : float voltage; // motor voltage in volts
313 : float current; // motor current in amps
314 : float pcb_temp; // pcb temp in C
315 : float stator_temp; // stator temp in C
316 : uint32_t last_update_ms;// system time that above values were updated
317 : } _motor_param;
318 : uint32_t _last_send_motor_param_request_ms; // system time (in milliseconds) that last motor param request was sent
319 :
320 : // error reporting
321 : DisplaySystemStateFlags _display_system_state_flags_prev; // backup of display system state flags
322 : uint8_t _display_system_state_master_error_code_prev; // backup of display system state master_error_code
323 : uint32_t _last_error_report_ms; // system time that flag changes were last reported (used to prevent spamming user)
324 : MotorStatus _motor_status_prev; // backup of motor status
325 :
326 : // returns a human-readable string corresponding the passed-in
327 : // master error code (see page 93 of https://media.torqeedo.com/downloads/manuals/torqeedo-Travel-manual-DE-EN.pdf)
328 : // If no conversion is available then nullptr is returned
329 : const char *map_master_error_code_to_string(uint8_t code) const;
330 : };
331 :
332 : #endif // HAL_TORQEEDO_ENABLED
|
