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 : #include "AP_Torqeedo_TQBus.h"
17 :
18 : #if HAL_TORQEEDO_ENABLED
19 :
20 : #include <AP_Common/AP_Common.h>
21 : #include <AP_Math/AP_Math.h>
22 : #include <SRV_Channel/SRV_Channel.h>
23 : #include <AP_Logger/AP_Logger.h>
24 : #include <GCS_MAVLink/GCS.h>
25 : #include <AP_SerialManager/AP_SerialManager.h>
26 : #include <AP_ESC_Telem/AP_ESC_Telem_Backend.h>
27 :
28 : #define TORQEEDO_SERIAL_BAUD 19200 // communication is always at 19200
29 : #define TORQEEDO_PACKET_HEADER 0xAC // communication packet header
30 : #define TORQEEDO_PACKET_FOOTER 0xAD // communication packet footer
31 : #define TORQEEDO_PACKET_ESCAPE 0xAE // escape character for handling occurrences of header, footer and this escape bytes in original message
32 : #define TORQEEDO_PACKET_ESCAPE_MASK 0x80 // byte after ESCAPE character should be XOR'd with this value
33 : #define TORQEEDO_LOG_TRQD_INTERVAL_MS 5000// log TRQD message at this interval in milliseconds
34 : #define TORQEEDO_SEND_MOTOR_SPEED_INTERVAL_MS 100 // motor speed sent at 10hz if connected to motor
35 : #define TORQEEDO_SEND_MOTOR_STATUS_REQUEST_INTERVAL_MS 400 // motor status requested every 0.4sec if connected to motor
36 : #define TORQEEDO_SEND_MOTOR_PARAM_REQUEST_INTERVAL_MS 400 // motor param requested every 0.4sec if connected to motor
37 : #define TORQEEDO_BATT_TIMEOUT_MS 5000 // battery info timeouts after 5 seconds
38 : #define TORQEEDO_REPLY_TIMEOUT_MS 25 // stop waiting for replies after 25ms
39 : #define TORQEEDO_ERROR_REPORT_INTERVAL_MAX_MS 10000 // errors reported to user at no less than once every 10 seconds
40 :
41 : extern const AP_HAL::HAL& hal;
42 :
43 : // initialise driver
44 0 : void AP_Torqeedo_TQBus::init()
45 : {
46 : // only init once
47 : // Note: a race condition exists here if init is called multiple times quickly before thread_main has a chance to set _initialise
48 0 : if (_initialised) {
49 0 : return;
50 : }
51 :
52 : // create background thread to process serial input and output
53 : char thread_name[15];
54 0 : hal.util->snprintf(thread_name, sizeof(thread_name), "torqeedo%u", (unsigned)_instance);
55 0 : if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_Torqeedo_TQBus::thread_main, void), thread_name, 2048, AP_HAL::Scheduler::PRIORITY_RCOUT, 1)) {
56 0 : return;
57 : }
58 : }
59 :
60 : // returns true if communicating with the motor
61 0 : bool AP_Torqeedo_TQBus::healthy()
62 : {
63 0 : if (!_initialised) {
64 0 : return false;
65 : }
66 : {
67 : // healthy if both receive and send have occurred in the last 3 seconds
68 0 : WITH_SEMAPHORE(_last_healthy_sem);
69 0 : const uint32_t now_ms = AP_HAL::millis();
70 0 : return ((now_ms - _last_received_ms < 3000) && (now_ms - _last_send_motor_ms < 3000));
71 0 : }
72 : }
73 :
74 : // initialise serial port and gpio pins (run from background thread)
75 0 : bool AP_Torqeedo_TQBus::init_internals()
76 : {
77 : // find serial driver and initialise
78 0 : const AP_SerialManager &serial_manager = AP::serialmanager();
79 0 : _uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Torqeedo, _instance);
80 0 : if (_uart == nullptr) {
81 0 : return false;
82 : }
83 0 : _uart->begin(TORQEEDO_SERIAL_BAUD);
84 0 : _uart->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE);
85 0 : _uart->set_unbuffered_writes(true);
86 :
87 : // if using tiller connection set on/off pin for 0.5 sec to turn on battery
88 0 : if (get_type() == AP_Torqeedo::ConnectionType::TYPE_TILLER) {
89 0 : if (_params.pin_onoff > -1) {
90 0 : hal.gpio->pinMode(_params.pin_onoff, HAL_GPIO_OUTPUT);
91 0 : hal.gpio->write(_params.pin_onoff, 1);
92 0 : hal.scheduler->delay(500);
93 0 : hal.gpio->write(_params.pin_onoff, 0);
94 : } else {
95 : // use serial port's RTS pin to turn on battery
96 0 : _uart->set_RTS_pin(true);
97 0 : hal.scheduler->delay(500);
98 0 : _uart->set_RTS_pin(false);
99 : }
100 : }
101 :
102 : // initialise RS485 DE pin (when high, allows send to motor)
103 0 : if (_params.pin_de > -1) {
104 0 : hal.gpio->pinMode(_params.pin_de, HAL_GPIO_OUTPUT);
105 0 : hal.gpio->write(_params.pin_de, 0);
106 : } else {
107 0 : _uart->set_CTS_pin(false);
108 : }
109 :
110 0 : return true;
111 : }
112 :
113 : // consume incoming messages from motor, reply with latest motor speed
114 : // runs in background thread
115 0 : void AP_Torqeedo_TQBus::thread_main()
116 : {
117 : // initialisation
118 0 : if (!init_internals()) {
119 0 : return;
120 : }
121 0 : _initialised = true;
122 :
123 : while (true) {
124 : // 1ms loop delay
125 0 : hal.scheduler->delay_microseconds(1000);
126 :
127 : // check if transmit pin should be unset
128 0 : check_for_send_end();
129 :
130 : // check for timeout waiting for reply
131 0 : check_for_reply_timeout();
132 :
133 : // parse incoming characters
134 0 : uint32_t nbytes = MIN(_uart->available(), 1024U);
135 0 : while (nbytes-- > 0) {
136 0 : int16_t b = _uart->read();
137 0 : if (b >= 0 ) {
138 0 : if (parse_byte((uint8_t)b)) {
139 : // complete message received, parse it!
140 0 : parse_message();
141 : // clear wait-for-reply because if we are waiting for a reply, this message must be it
142 0 : set_reply_received();
143 : }
144 : }
145 : }
146 :
147 : // send motor speed
148 0 : bool log_update = false;
149 0 : if (safe_to_send()) {
150 0 : uint32_t now_ms = AP_HAL::millis();
151 :
152 : // if connected to motor
153 0 : if (get_type() == AP_Torqeedo::ConnectionType::TYPE_MOTOR) {
154 0 : if (now_ms - _last_send_motor_ms > TORQEEDO_SEND_MOTOR_SPEED_INTERVAL_MS) {
155 : // send motor speed every 0.1sec
156 0 : _send_motor_speed = true;
157 0 : } else if (now_ms - _last_send_motor_status_request_ms > TORQEEDO_SEND_MOTOR_STATUS_REQUEST_INTERVAL_MS) {
158 : // send request for motor status
159 0 : send_motor_msg_request(MotorMsgId::STATUS);
160 0 : _last_send_motor_status_request_ms = now_ms;
161 0 : } else if (now_ms - _last_send_motor_param_request_ms > TORQEEDO_SEND_MOTOR_PARAM_REQUEST_INTERVAL_MS) {
162 : // send request for motor params
163 0 : send_motor_msg_request(MotorMsgId::PARAM);
164 0 : _last_send_motor_param_request_ms = now_ms;
165 : }
166 : }
167 :
168 : // send motor speed
169 0 : if (_send_motor_speed) {
170 0 : send_motor_speed_cmd();
171 0 : _send_motor_speed = false;
172 0 : log_update = true;
173 : }
174 : }
175 :
176 : // log high level status and motor speed
177 0 : log_TRQD(log_update);
178 0 : }
179 : }
180 :
181 : // returns a human-readable string corresponding the passed-in
182 : // master error code (see page 93 of https://media.torqeedo.com/downloads/manuals/torqeedo-Travel-manual-DE-EN.pdf)
183 : // If no conversion is available then nullptr is returned
184 0 : const char * AP_Torqeedo_TQBus::map_master_error_code_to_string(uint8_t code) const
185 : {
186 0 : switch (code) {
187 0 : case 2:
188 0 : return "stator high temp";
189 0 : case 5:
190 0 : return "propeller blocked";
191 0 : case 6:
192 0 : return "motor low voltage";
193 0 : case 7:
194 0 : return "motor high current";
195 0 : case 8:
196 0 : return "pcb temp high";
197 0 : case 21:
198 0 : return "tiller cal bad";
199 0 : case 22:
200 0 : return "mag bad";
201 0 : case 23:
202 0 : return "range incorrect";
203 0 : case 30:
204 0 : return "motor comm error";
205 0 : case 32:
206 0 : return "tiller comm error";
207 0 : case 33:
208 0 : return "general comm error";
209 0 : case 41:
210 : case 42:
211 0 : return "charge voltage bad";
212 0 : case 43:
213 0 : return "battery flat";
214 0 : case 45:
215 0 : return "battery high current";
216 0 : case 46:
217 0 : return "battery temp error";
218 0 : case 48:
219 0 : return "charging temp error";
220 : }
221 :
222 0 : return nullptr;
223 : }
224 :
225 : // report changes in error codes to user
226 0 : void AP_Torqeedo_TQBus::report_error_codes()
227 : {
228 : // skip reporting if we have already reported status very recently
229 0 : const uint32_t now_ms = AP_HAL::millis();
230 :
231 : // skip reporting if no changes in flags and already reported within 10 seconds
232 0 : const bool flags_changed = (_display_system_state_flags_prev.value != _display_system_state.flags.value) ||
233 0 : (_display_system_state_master_error_code_prev != _display_system_state.master_error_code) ||
234 0 : (_motor_status_prev.status_flags_value != _motor_status.status_flags_value) ||
235 0 : (_motor_status_prev.error_flags_value != _motor_status.error_flags_value);
236 0 : if (!flags_changed && ((now_ms - _last_error_report_ms) < TORQEEDO_ERROR_REPORT_INTERVAL_MAX_MS)) {
237 0 : return;
238 : }
239 :
240 : // report display system errors
241 0 : const char* msg_prefix = "Torqeedo:";
242 : (void)msg_prefix; // sometimes unused when HAL_GCS_ENABLED is false
243 0 : if (_display_system_state.flags.set_throttle_stop) {
244 0 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "%s zero throttle required", msg_prefix);
245 : }
246 0 : if (_display_system_state.flags.temp_warning) {
247 0 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "%s high temp", msg_prefix);
248 : }
249 0 : if (_display_system_state.flags.batt_nearly_empty) {
250 0 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "%s batt nearly empty", msg_prefix);
251 : }
252 0 : if (_display_system_state.master_error_code > 0) {
253 0 : const char *error_string = map_master_error_code_to_string(_display_system_state.master_error_code);
254 0 : if (error_string != nullptr) {
255 0 : GCS_SEND_TEXT(
256 : MAV_SEVERITY_CRITICAL, "%s err:%u %s",
257 : msg_prefix,
258 : _display_system_state.master_error_code,
259 : error_string);
260 : } else {
261 0 : GCS_SEND_TEXT(
262 : MAV_SEVERITY_CRITICAL, "%s err:%u",
263 : msg_prefix,
264 : _display_system_state.master_error_code);
265 : }
266 : }
267 :
268 : // report motor status errors
269 0 : if (_motor_status.error_flags.overcurrent) {
270 0 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "%s overcurrent", msg_prefix);
271 : }
272 0 : if (_motor_status.error_flags.blocked) {
273 0 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "%s prop blocked", msg_prefix);
274 : }
275 0 : if (_motor_status.error_flags.overvoltage_static || _motor_status.error_flags.overvoltage_current) {
276 0 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "%s high voltage", msg_prefix);
277 : }
278 0 : if (_motor_status.error_flags.undervoltage_static || _motor_status.error_flags.undervoltage_current) {
279 0 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "%s low voltage", msg_prefix);
280 : }
281 0 : if (_motor_status.error_flags.overtemp_motor || _motor_status.error_flags.overtemp_pcb) {
282 0 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "%s high temp", msg_prefix);
283 : }
284 0 : if (_motor_status.error_flags.timeout_rs485) {
285 0 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "%s comm timeout", msg_prefix);
286 : }
287 0 : if (_motor_status.error_flags.temp_sensor_error) {
288 0 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "%s temp sensor err", msg_prefix);
289 : }
290 0 : if (_motor_status.error_flags.tilt) {
291 0 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "%s tilted", msg_prefix);
292 : }
293 :
294 : // display OK if all errors cleared
295 0 : const bool prev_errored = (_display_system_state_flags_prev.value != 0) ||
296 0 : (_display_system_state_master_error_code_prev != 0) ||
297 0 : (_motor_status_prev.error_flags_value != 0);
298 :
299 0 : const bool now_errored = (_display_system_state.flags.value != 0) ||
300 0 : (_display_system_state.master_error_code != 0) ||
301 0 : (_motor_status.error_flags_value != 0);
302 :
303 0 : if (!now_errored && prev_errored) {
304 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s OK", msg_prefix);
305 : }
306 :
307 : // record change in state and reporting time
308 0 : _display_system_state_flags_prev.value = _display_system_state.flags.value;
309 0 : _display_system_state_master_error_code_prev = _display_system_state.master_error_code;
310 0 : _motor_status_prev = _motor_status;
311 0 : _last_error_report_ms = now_ms;
312 : }
313 :
314 : // get latest battery status info. returns true on success and populates arguments
315 0 : bool AP_Torqeedo_TQBus::get_batt_info(float &voltage, float ¤t_amps, float &temp_C, uint8_t &pct_remaining) const
316 : {
317 :
318 : // use battery info from display_system_state if available (tiller connection)
319 0 : if ((AP_HAL::millis() - _display_system_state.last_update_ms) <= TORQEEDO_BATT_TIMEOUT_MS) {
320 0 : voltage = _display_system_state.batt_voltage;
321 0 : current_amps = _display_system_state.batt_current;
322 0 : temp_C = MAX(_display_system_state.temp_sw, _display_system_state.temp_rp);
323 0 : pct_remaining = _display_system_state.batt_charge_pct;
324 0 : return true;
325 : }
326 :
327 : // use battery info from motor_param if available (motor connection)
328 0 : if ((AP_HAL::millis() - _motor_param.last_update_ms) <= TORQEEDO_BATT_TIMEOUT_MS) {
329 0 : voltage = _motor_param.voltage;
330 0 : current_amps = _motor_param.current;
331 0 : temp_C = MAX(_motor_param.pcb_temp, _motor_param.stator_temp);
332 0 : pct_remaining = 0; // motor does not know percent remaining
333 0 : return true;
334 : }
335 :
336 0 : return false;
337 : }
338 :
339 : // get battery capacity. returns true on success and populates argument
340 0 : bool AP_Torqeedo_TQBus::get_batt_capacity_Ah(uint16_t &_hours) const
341 : {
342 0 : if (_display_system_setup.batt_capacity == 0) {
343 0 : return false;
344 : }
345 0 : amp_hours = _display_system_setup.batt_capacity;
346 0 : return true;
347 : }
348 :
349 : // process a single byte received on serial port
350 : // return true if a complete message has been received (the message will be held in _received_buff)
351 0 : bool AP_Torqeedo_TQBus::parse_byte(uint8_t b)
352 : {
353 0 : bool complete_msg_received = false;
354 :
355 0 : switch (_parse_state) {
356 0 : case ParseState::WAITING_FOR_HEADER:
357 0 : if (b == TORQEEDO_PACKET_HEADER) {
358 0 : _parse_state = ParseState::WAITING_FOR_FOOTER;
359 : }
360 0 : _received_buff_len = 0;
361 0 : _parse_escape_received = false;
362 0 : break;
363 0 : case ParseState::WAITING_FOR_FOOTER:
364 0 : if (b == TORQEEDO_PACKET_FOOTER) {
365 0 : _parse_state = ParseState::WAITING_FOR_HEADER;
366 :
367 : // check message length
368 0 : if (_received_buff_len == 0) {
369 0 : _parse_error_count++;
370 0 : break;
371 : }
372 :
373 : // check crc
374 0 : const uint8_t crc_expected = crc8_maxim(_received_buff, _received_buff_len-1);
375 0 : if (_received_buff[_received_buff_len-1] != crc_expected) {
376 0 : _parse_error_count++;
377 0 : break;
378 : }
379 0 : _parse_success_count++;
380 : {
381 : // record time of successful receive for health reporting
382 0 : WITH_SEMAPHORE(_last_healthy_sem);
383 0 : _last_received_ms = AP_HAL::millis();
384 0 : }
385 0 : complete_msg_received = true;
386 : } else {
387 : // escape character handling
388 0 : if (_parse_escape_received) {
389 0 : b ^= TORQEEDO_PACKET_ESCAPE_MASK;
390 0 : _parse_escape_received = false;
391 0 : } else if (b == TORQEEDO_PACKET_ESCAPE) {
392 : // escape character received, record this and ignore this byte
393 0 : _parse_escape_received = true;
394 0 : break;
395 : }
396 : // add to buffer
397 0 : _received_buff[_received_buff_len] = b;
398 0 : _received_buff_len++;
399 0 : if (_received_buff_len > TORQEEDO_MESSAGE_LEN_MAX) {
400 : // message too long
401 0 : _parse_state = ParseState::WAITING_FOR_HEADER;
402 0 : _parse_error_count++;
403 : }
404 : }
405 0 : break;
406 : }
407 :
408 0 : return complete_msg_received;
409 : }
410 :
411 : // process message held in _received_buff
412 0 : void AP_Torqeedo_TQBus::parse_message()
413 : {
414 : // message address (i.e. target of message)
415 0 : const MsgAddress msg_addr = (MsgAddress)_received_buff[0];
416 :
417 : // handle messages sent to "remote" (aka tiller)
418 0 : if ((get_type() == AP_Torqeedo::ConnectionType::TYPE_TILLER) && (msg_addr == MsgAddress::REMOTE1)) {
419 0 : RemoteMsgId msg_id = (RemoteMsgId)_received_buff[1];
420 0 : if (msg_id == RemoteMsgId::REMOTE) {
421 : // request received to send updated motor speed
422 0 : _send_motor_speed = true;
423 : }
424 0 : return;
425 : }
426 :
427 : // handle messages sent to Display
428 0 : if ((get_type() == AP_Torqeedo::ConnectionType::TYPE_TILLER) && (msg_addr == MsgAddress::DISPLAY)) {
429 0 : DisplayMsgId msg_id = (DisplayMsgId)_received_buff[1];
430 0 : switch (msg_id) {
431 0 : case DisplayMsgId::SYSTEM_STATE :
432 0 : if (_received_buff_len == 30) {
433 : // fill in _display_system_state
434 0 : _display_system_state.flags.value = UINT16_VALUE(_received_buff[2], _received_buff[3]);
435 0 : _display_system_state.master_state = _received_buff[4]; // deprecated
436 0 : _display_system_state.master_error_code = _received_buff[5];
437 0 : _display_system_state.motor_voltage = UINT16_VALUE(_received_buff[6], _received_buff[7]) * 0.01;
438 0 : _display_system_state.motor_current = UINT16_VALUE(_received_buff[8], _received_buff[9]) * 0.1;
439 0 : _display_system_state.motor_power = UINT16_VALUE(_received_buff[10], _received_buff[11]);
440 0 : _display_system_state.motor_rpm = (int16_t)UINT16_VALUE(_received_buff[12], _received_buff[13]);
441 0 : _display_system_state.motor_pcb_temp = _received_buff[14];
442 0 : _display_system_state.motor_stator_temp = _received_buff[15];
443 0 : _display_system_state.batt_charge_pct = _received_buff[16];
444 0 : _display_system_state.batt_voltage = UINT16_VALUE(_received_buff[17], _received_buff[18]) * 0.01;
445 0 : _display_system_state.batt_current = UINT16_VALUE(_received_buff[19], _received_buff[20]) * 0.1;
446 0 : _display_system_state.gps_speed = UINT16_VALUE(_received_buff[21], _received_buff[22]);
447 0 : _display_system_state.range_miles = UINT16_VALUE(_received_buff[23], _received_buff[24]);
448 0 : _display_system_state.range_minutes = UINT16_VALUE(_received_buff[25], _received_buff[26]);
449 0 : _display_system_state.temp_sw = _received_buff[27];
450 0 : _display_system_state.temp_rp = _received_buff[28];
451 0 : _display_system_state.last_update_ms = AP_HAL::millis();
452 :
453 : // update esc telem sent to ground station
454 0 : const uint8_t esc_temp = MAX(_display_system_state.temp_sw, _display_system_state.temp_rp);
455 0 : const uint8_t motor_temp = MAX(_display_system_state.motor_pcb_temp, _display_system_state.motor_stator_temp);
456 0 : update_esc_telem(_display_system_state.motor_rpm,
457 : _display_system_state.motor_voltage,
458 : _display_system_state.motor_current,
459 : esc_temp,
460 : motor_temp);
461 :
462 : #if HAL_LOGGING_ENABLED
463 : // log data
464 0 : if (option_enabled(AP_Torqeedo::options::LOG)) {
465 : // @LoggerMessage: TRST
466 : // @Description: Torqeedo System State
467 : // @Field: TimeUS: Time since system startup
468 : // @Field: I: instance
469 : // @Field: F: Flags bitmask
470 : // @Field: Err: Master error code
471 : // @Field: MVolt: Motor voltage
472 : // @Field: MCur: Motor current
473 : // @Field: Pow: Motor power
474 : // @Field: RPM: Motor RPM
475 : // @Field: MTemp: Motor Temp (higher of pcb or stator)
476 : // @Field: BPct: Battery charge percentage
477 : // @Field: BVolt: Battery voltage
478 : // @Field: BCur: Battery current
479 0 : AP::logger().Write("TRST", "TimeUS,I,F,Err,MVolt,MCur,Pow,RPM,MTemp,BPct,BVolt,BCur",
480 : "s#--vAWqO%vA", // units
481 : "F---00000000", // multipliers
482 : "QBHBffHhBBff", // formats
483 : AP_HAL::micros64(),
484 0 : _instance,
485 0 : _display_system_state.flags.value,
486 0 : _display_system_state.master_error_code,
487 0 : _display_system_state.motor_voltage,
488 0 : _display_system_state.motor_current,
489 0 : _display_system_state.motor_power,
490 0 : _display_system_state.motor_rpm,
491 : motor_temp,
492 0 : _display_system_state.batt_charge_pct,
493 0 : _display_system_state.batt_voltage,
494 0 : _display_system_state.batt_current);
495 : }
496 : #endif
497 :
498 : // send to GCS
499 0 : if (option_enabled(AP_Torqeedo::options::DEBUG_TO_GCS)) {
500 0 : 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",
501 : (unsigned)_instance,
502 : (unsigned)_display_system_state.flags.value,
503 : (unsigned)_display_system_state.master_error_code,
504 : (double)_display_system_state.motor_voltage,
505 : (double)_display_system_state.motor_current,
506 : (unsigned)_display_system_state.motor_power,
507 : (int)motor_temp,
508 : (unsigned)_display_system_state.batt_charge_pct,
509 : (double)_display_system_state.batt_voltage,
510 : (double)_display_system_state.batt_current);
511 : }
512 :
513 : // report any errors
514 0 : report_error_codes();
515 : } else {
516 : // unexpected length
517 0 : _parse_error_count++;
518 : }
519 0 : break;
520 0 : case DisplayMsgId::SYSTEM_SETUP:
521 0 : if (_received_buff_len == 13) {
522 : // fill in display system setup
523 0 : _display_system_setup.flags = _received_buff[2];
524 0 : _display_system_setup.motor_type = _received_buff[3];
525 0 : _display_system_setup.motor_sw_version = UINT16_VALUE(_received_buff[4], _received_buff[5]);
526 0 : _display_system_setup.batt_capacity = UINT16_VALUE(_received_buff[6], _received_buff[7]);
527 0 : _display_system_setup.batt_charge_pct = _received_buff[8];
528 0 : _display_system_setup.batt_type = _received_buff[9];
529 0 : _display_system_setup.master_sw_version = UINT16_VALUE(_received_buff[10], _received_buff[11]);
530 :
531 : #if HAL_LOGGING_ENABLED
532 : // log data
533 0 : if (option_enabled(AP_Torqeedo::options::LOG)) {
534 : // @LoggerMessage: TRSE
535 : // @Description: Torqeedo System Setup
536 : // @Field: TimeUS: Time since system startup
537 : // @Field: I: instance
538 : // @Field: Flag: Flags
539 : // @Field: MotType: Motor type
540 : // @Field: MotVer: Motor software version
541 : // @Field: BattCap: Battery capacity
542 : // @Field: BattPct: Battery charge percentage
543 : // @Field: BattType: Battery type
544 : // @Field: SwVer: Master software version
545 0 : AP::logger().Write("TRSE", "TimeUS,I,Flag,MotType,MotVer,BattCap,BattPct,BattType,SwVer",
546 : "s#---a%--", // units
547 : "F----00--", // multipliers
548 : "QBBBHHBBH", // formats
549 : AP_HAL::micros64(),
550 0 : _instance,
551 0 : _display_system_setup.flags,
552 0 : _display_system_setup.motor_type,
553 0 : _display_system_setup.motor_sw_version,
554 0 : _display_system_setup.batt_capacity,
555 0 : _display_system_setup.batt_charge_pct,
556 0 : _display_system_setup.batt_type,
557 0 : _display_system_setup.master_sw_version);
558 : }
559 : #endif
560 :
561 : // send to GCS
562 0 : if (option_enabled(AP_Torqeedo::options::DEBUG_TO_GCS)) {
563 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO,"TRSE i:%u v:%u F:%u Mot:%u/%u Bat:%u/%u/%u%%",
564 : (unsigned)_instance,
565 : (unsigned)_display_system_setup.master_sw_version,
566 : (unsigned)_display_system_setup.flags,
567 : (unsigned)_display_system_setup.motor_type,
568 : (unsigned)_display_system_setup.motor_sw_version,
569 : (unsigned)_display_system_setup.batt_type,
570 : (unsigned)_display_system_setup.batt_capacity,
571 : (unsigned)_display_system_setup.batt_charge_pct);
572 : }
573 : } else {
574 : // unexpected length
575 0 : _parse_error_count++;
576 : }
577 0 : break;
578 0 : default:
579 : // ignore message
580 0 : break;
581 : }
582 0 : return;
583 : }
584 :
585 : // handle reply from motor
586 0 : if ((get_type() == AP_Torqeedo::ConnectionType::TYPE_MOTOR) && (msg_addr == MsgAddress::BUS_MASTER)) {
587 : // replies strangely do not return the msgid so we must have stored it
588 0 : MotorMsgId msg_id = (MotorMsgId)_reply_msgid;
589 0 : switch (msg_id) {
590 :
591 0 : case MotorMsgId::PARAM:
592 0 : if (_received_buff_len == 15) {
593 0 : _motor_param.rpm = (int16_t)UINT16_VALUE(_received_buff[2], _received_buff[3]);
594 0 : _motor_param.power = UINT16_VALUE(_received_buff[4], _received_buff[5]);
595 0 : _motor_param.voltage = UINT16_VALUE(_received_buff[6], _received_buff[7]) * 0.01;
596 0 : _motor_param.current = UINT16_VALUE(_received_buff[8], _received_buff[9]) * 0.1;
597 0 : _motor_param.pcb_temp = (int16_t)UINT16_VALUE(_received_buff[10], _received_buff[11]) * 0.1;
598 0 : _motor_param.stator_temp = (int16_t)UINT16_VALUE(_received_buff[12], _received_buff[13]) * 0.1;
599 0 : _motor_param.last_update_ms = AP_HAL::millis();
600 :
601 : // update esc telem sent to ground station
602 0 : update_esc_telem(_motor_param.rpm,
603 : _motor_param.voltage,
604 : _motor_param.current,
605 : _motor_param.pcb_temp, // esc temp
606 : _motor_param.stator_temp); // motor temp
607 :
608 : #if HAL_LOGGING_ENABLED
609 : // log data
610 0 : if (option_enabled(AP_Torqeedo::options::LOG)) {
611 : // @LoggerMessage: TRMP
612 : // @Description: Torqeedo Motor Param
613 : // @Field: TimeUS: Time since system startup
614 : // @Field: I: instance
615 : // @Field: RPM: Motor RPM
616 : // @Field: Pow: Motor power
617 : // @Field: Volt: Motor voltage
618 : // @Field: Cur: Motor current
619 : // @Field: ETemp: ESC Temp
620 : // @Field: MTemp: Motor Temp
621 0 : AP::logger().Write("TRMP", "TimeUS,I,RPM,Pow,Volt,Cur,ETemp,MTemp",
622 : "s#qWvAOO", // units
623 : "F-000000", // multipliers
624 : "QBhHffff", // formats
625 : AP_HAL::micros64(),
626 0 : _instance,
627 0 : _motor_param.rpm,
628 0 : _motor_param.power,
629 0 : _motor_param.voltage,
630 0 : _motor_param.current,
631 0 : _motor_param.pcb_temp,
632 0 : _motor_param.stator_temp);
633 : }
634 : #endif
635 :
636 : // send to GCS
637 0 : if (option_enabled(AP_Torqeedo::options::DEBUG_TO_GCS)) {
638 0 : 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",
639 : (unsigned)_instance,
640 : (int)_motor_param.rpm,
641 : (unsigned)_motor_param.power,
642 : (double)_motor_param.voltage,
643 : (double)_motor_param.current,
644 : (double)_motor_param.pcb_temp,
645 : (double)_motor_param.stator_temp);
646 : }
647 : } else {
648 : // unexpected length
649 0 : _parse_error_count++;
650 : }
651 0 : break;
652 :
653 0 : case MotorMsgId::STATUS:
654 0 : if (_received_buff_len == 6) {
655 0 : _motor_status.status_flags_value = _received_buff[2];
656 0 : _motor_status.error_flags_value = UINT16_VALUE(_received_buff[3], _received_buff[4]);
657 :
658 : #if HAL_LOGGING_ENABLED
659 : // log data
660 0 : if (option_enabled(AP_Torqeedo::options::LOG)) {
661 : // @LoggerMessage: TRMS
662 : // @Description: Torqeedo Motor Status
663 : // @Field: TimeUS: Time since system startup
664 : // @Field: I: instance
665 : // @Field: State: Motor status flags
666 : // @Field: Err: Motor error flags
667 0 : AP::logger().Write("TRMS", "TimeUS,I,State,Err",
668 : "s#--", // units
669 : "F---", // multipliers
670 : "QBBH", // formats
671 : AP_HAL::micros64(),
672 0 : _instance,
673 0 : _motor_status.status_flags_value,
674 0 : _motor_status.error_flags_value);
675 : }
676 : #endif
677 :
678 : // send to GCS
679 0 : if (option_enabled(AP_Torqeedo::options::DEBUG_TO_GCS)) {
680 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO,"TRMS i:%u S:%d Err:%d",
681 : (unsigned)_instance,
682 : _motor_status.status_flags_value,
683 : _motor_status.error_flags_value);
684 : }
685 :
686 : // report any errors
687 0 : report_error_codes();
688 : } else {
689 : // unexpected length
690 0 : _parse_error_count++;
691 : }
692 0 : break;
693 :
694 0 : case MotorMsgId::INFO:
695 : case MotorMsgId::DRIVE:
696 : case MotorMsgId::CONFIG:
697 : // we do not process these replies
698 0 : break;
699 :
700 0 : default:
701 : // ignore unknown messages
702 0 : break;
703 : }
704 : }
705 : }
706 :
707 : // set DE Serial CTS pin to enable sending commands to motor
708 0 : void AP_Torqeedo_TQBus::send_start()
709 : {
710 : // set gpio pin or serial port's CTS pin
711 0 : if (_params.pin_de > -1) {
712 0 : hal.gpio->write(_params.pin_de, 1);
713 : } else {
714 0 : _uart->set_CTS_pin(true);
715 : }
716 0 : }
717 :
718 : // check for timeout after sending and unset pin if required
719 0 : void AP_Torqeedo_TQBus::check_for_send_end()
720 : {
721 0 : if (_send_delay_us == 0) {
722 : // not sending
723 0 : return;
724 : }
725 :
726 0 : if (AP_HAL::micros() - _send_start_us < _send_delay_us) {
727 : // return if delay has not yet elapsed
728 0 : return;
729 : }
730 0 : _send_delay_us = 0;
731 :
732 : // unset gpio or serial port's CTS pin
733 0 : if (_params.pin_de > -1) {
734 0 : hal.gpio->write(_params.pin_de, 0);
735 : } else {
736 0 : _uart->set_CTS_pin(false);
737 : }
738 : }
739 :
740 : // calculate delay require to allow bytes to be sent
741 0 : uint32_t AP_Torqeedo_TQBus::calc_send_delay_us(uint8_t num_bytes)
742 : {
743 : // baud rate of 19200 bits/sec
744 : // total number of bits = 10 x num_bytes
745 : // convert from seconds to micros by multiplying by 1,000,000
746 : // plus additional 300us safety margin
747 0 : const uint32_t delay_us = 1e6 * num_bytes * 10 / TORQEEDO_SERIAL_BAUD + 300;
748 0 : return delay_us;
749 : }
750 :
751 : // record msgid of message to wait for and set timer for timeout handling
752 0 : void AP_Torqeedo_TQBus::set_expected_reply_msgid(uint8_t msg_id)
753 : {
754 0 : _reply_msgid = msg_id;
755 0 : _reply_wait_start_ms = AP_HAL::millis();
756 0 : }
757 :
758 : // check for timeout waiting for reply message
759 0 : void AP_Torqeedo_TQBus::check_for_reply_timeout()
760 : {
761 : // return immediately if not waiting for reply
762 0 : if (_reply_wait_start_ms == 0) {
763 0 : return;
764 : }
765 0 : if (AP_HAL::millis() - _reply_wait_start_ms > TORQEEDO_REPLY_TIMEOUT_MS) {
766 0 : _reply_wait_start_ms = 0;
767 0 : _parse_error_count++;
768 : }
769 : }
770 :
771 : // mark reply received. should be called whenever a message is received regardless of whether we are actually waiting for a reply
772 0 : void AP_Torqeedo_TQBus::set_reply_received()
773 : {
774 0 : _reply_wait_start_ms = 0;
775 0 : }
776 :
777 : // send a message to the motor with the specified message contents
778 : // msg_contents should not include the header, footer or CRC
779 : // returns true on success
780 0 : bool AP_Torqeedo_TQBus::send_message(const uint8_t msg_contents[], uint8_t num_bytes)
781 : {
782 : // buffer for outgoing message
783 : uint8_t send_buff[TORQEEDO_MESSAGE_LEN_MAX];
784 0 : uint8_t send_buff_num_bytes = 0;
785 :
786 : // calculate crc
787 0 : const uint8_t crc = crc8_maxim(msg_contents, num_bytes);
788 :
789 : // add header
790 0 : send_buff[send_buff_num_bytes++] = TORQEEDO_PACKET_HEADER;
791 :
792 : // add contents
793 0 : for (uint8_t i=0; i<num_bytes; i++) {
794 0 : if (!add_byte_to_message(msg_contents[i], send_buff, ARRAY_SIZE(send_buff), send_buff_num_bytes)) {
795 0 : _parse_error_count++;
796 0 : return false;
797 : }
798 : }
799 :
800 : // add crc
801 0 : if (!add_byte_to_message(crc, send_buff, ARRAY_SIZE(send_buff), send_buff_num_bytes)) {
802 0 : _parse_error_count++;
803 0 : return false;
804 : }
805 :
806 : // add footer
807 0 : if (send_buff_num_bytes >= ARRAY_SIZE(send_buff)) {
808 0 : _parse_error_count++;
809 0 : return false;
810 : }
811 0 : send_buff[send_buff_num_bytes++] = TORQEEDO_PACKET_FOOTER;
812 :
813 : // set send pin
814 0 : send_start();
815 :
816 : // write message
817 0 : _uart->write(send_buff, send_buff_num_bytes);
818 :
819 : // record start and expected delay to send message
820 0 : _send_start_us = AP_HAL::micros();
821 0 : _send_delay_us = calc_send_delay_us(send_buff_num_bytes);
822 :
823 0 : return true;
824 : }
825 :
826 : // add a byte to a message buffer including adding the escape character (0xAE) if necessary
827 : // this should only be used when adding the contents to the buffer, not the header and footer
828 : // num_bytes is updated to the next free byte
829 0 : 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
830 : {
831 0 : bool escape_required = (byte_to_add == TORQEEDO_PACKET_HEADER ||
832 0 : byte_to_add == TORQEEDO_PACKET_FOOTER ||
833 : byte_to_add == TORQEEDO_PACKET_ESCAPE);
834 :
835 : // check if we have enough space
836 0 : if (num_bytes + (escape_required ? 2 : 1) >= msg_buff_size) {
837 0 : return false;
838 : }
839 :
840 : // add byte
841 0 : if (escape_required) {
842 0 : msg_buff[num_bytes++] = TORQEEDO_PACKET_ESCAPE;
843 0 : msg_buff[num_bytes++] = byte_to_add ^ TORQEEDO_PACKET_ESCAPE_MASK;
844 : } else {
845 0 : msg_buff[num_bytes++] = byte_to_add;
846 : }
847 0 : return true;
848 : }
849 :
850 : // Example "Remote (0x01)" reply message to allow tiller to control motor speed
851 : // Byte Field Definition Example Value Comments
852 : // ---------------------------------------------------------------------------------
853 : // byte 0 Header 0xAC
854 : // byte 1 TargetAddress 0x00 see MsgAddress enum
855 : // byte 2 Message ID 0x00 only master populates this. replies have this set to zero
856 : // byte 3 Flags 0x05 bit0=pin present, bit2=motor speed valid
857 : // byte 4 Status 0x00 0x20 if byte3=4, 0x0 is byte3=5
858 : // byte 5 Motor Speed MSB ---- Motor Speed MSB (-1000 to +1000)
859 : // byte 6 Motor Speed LSB ---- Motor Speed LSB (-1000 to +1000)
860 : // byte 7 CRC-Maxim ---- CRC-Maxim value
861 : // byte 8 Footer 0xAD
862 : //
863 : // example message when rotating tiller handle forwards: "AC 00 00 05 00 00 ED 95 AD" (+237)
864 : // example message when rotating tiller handle backwards: "AC 00 00 05 00 FF AE 2C 0C AD" (-82)
865 :
866 : // send a motor speed command as a value from -1000 to +1000
867 : // value is taken directly from SRV_Channel
868 : // for tiller connection this sends the "Remote (0x01)" message
869 : // for motor connection this sends the "Motor Drive (0x82)" message
870 0 : void AP_Torqeedo_TQBus::send_motor_speed_cmd()
871 : {
872 : // calculate desired motor speed
873 0 : if (!hal.util->get_soft_armed()) {
874 0 : _motor_speed_desired = 0;
875 : } else {
876 : // convert throttle output to motor output in range -1000 to +1000
877 : // ToDo: convert PWM output to motor output so that SERVOx_MIN, MAX and TRIM take effect
878 0 : _motor_speed_desired = constrain_int16(SRV_Channels::get_output_norm((SRV_Channel::Function)_params.servo_fn.get()) * 1000.0, -1000, 1000);
879 : }
880 :
881 : // updated limited motor speed
882 0 : int16_t mot_speed_limited = calc_motor_speed_limited(_motor_speed_desired);
883 :
884 : // by default use tiller connection command
885 0 : uint8_t mot_speed_cmd_buff[] = {(uint8_t)MsgAddress::BUS_MASTER, 0x0, 0x5, 0x0, HIGHBYTE(mot_speed_limited), LOWBYTE(mot_speed_limited)};
886 :
887 : // update message if using motor connection
888 0 : if (get_type() == AP_Torqeedo::ConnectionType::TYPE_MOTOR) {
889 0 : const uint8_t motor_power = (uint8_t)constrain_int16(_params.motor_power, 0, 100);
890 0 : mot_speed_cmd_buff[0] = (uint8_t)MsgAddress::MOTOR;
891 0 : mot_speed_cmd_buff[1] = (uint8_t)MotorMsgId::DRIVE;
892 0 : 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
893 0 : mot_speed_cmd_buff[3] = mot_speed_limited == 0 ? 0 : motor_power; // motor power from 0 to 100
894 :
895 : // set expected reply message id
896 0 : set_expected_reply_msgid((uint8_t)MotorMsgId::DRIVE);
897 :
898 : // reset motor clear error request
899 0 : _motor_clear_error = false;
900 : }
901 :
902 : // send a message
903 0 : if (send_message(mot_speed_cmd_buff, ARRAY_SIZE(mot_speed_cmd_buff))) {
904 : // record time of send for health reporting
905 0 : WITH_SEMAPHORE(_last_healthy_sem);
906 0 : _last_send_motor_ms = AP_HAL::millis();
907 0 : }
908 0 : }
909 :
910 : // send request to motor to reply with a particular message
911 : // msg_id can be INFO, STATUS or PARAM
912 0 : void AP_Torqeedo_TQBus::send_motor_msg_request(MotorMsgId msg_id)
913 : {
914 : // prepare message
915 0 : uint8_t mot_status_request_buff[] = {(uint8_t)MsgAddress::MOTOR, (uint8_t)msg_id};
916 :
917 : // send a message
918 0 : if (send_message(mot_status_request_buff, ARRAY_SIZE(mot_status_request_buff))) {
919 : // record waiting for reply
920 0 : set_expected_reply_msgid((uint8_t)msg_id);
921 : }
922 0 : }
923 :
924 : // calculate the limited motor speed that is sent to the motors
925 : // desired_motor_speed argument and returned value are in the range -1000 to 1000
926 0 : int16_t AP_Torqeedo_TQBus::calc_motor_speed_limited(int16_t desired_motor_speed)
927 : {
928 0 : const uint32_t now_ms = AP_HAL::millis();
929 :
930 : // update dir_limit flag for forward-reverse transition delay
931 0 : const bool dir_delay_active = is_positive(_params.dir_delay);
932 0 : if (!dir_delay_active) {
933 : // allow movement in either direction
934 0 : _dir_limit = 0;
935 : } else {
936 : // by default limit motor direction to previous iteration's direction
937 0 : if (is_positive(_motor_speed_limited)) {
938 0 : _dir_limit = 1;
939 0 : } else if (is_negative(_motor_speed_limited)) {
940 0 : _dir_limit = -1;
941 : } else {
942 : // motor speed is zero
943 0 : if ((_motor_speed_zero_ms != 0) && ((now_ms - _motor_speed_zero_ms) > (_params.dir_delay * 1000))) {
944 : // delay has passed so allow movement in either direction
945 0 : _dir_limit = 0;
946 0 : _motor_speed_zero_ms = 0;
947 : }
948 : }
949 : }
950 :
951 : // calculate upper and lower limits for forward-reverse transition delay
952 0 : int16_t lower_limit = -1000;
953 0 : int16_t upper_limit = 1000;
954 0 : if (_dir_limit < 0) {
955 0 : upper_limit = 0;
956 : }
957 0 : if (_dir_limit > 0) {
958 0 : lower_limit = 0;
959 : }
960 :
961 : // calculate dt since last update
962 0 : float dt = (now_ms - _motor_speed_limited_ms) * 0.001f;
963 0 : if (dt > 1.0) {
964 : // after a long delay limit motor output to zero to avoid sudden starts
965 0 : lower_limit = 0;
966 0 : upper_limit = 0;
967 : }
968 0 : _motor_speed_limited_ms = now_ms;
969 :
970 : // apply slew limit
971 0 : if (_params.slew_time > 0) {
972 0 : const float chg_max = 1000.0 * dt / _params.slew_time;
973 0 : _motor_speed_limited = constrain_float(desired_motor_speed, _motor_speed_limited - chg_max, _motor_speed_limited + chg_max);
974 : } else {
975 : // no slew limit
976 0 : _motor_speed_limited = desired_motor_speed;
977 : }
978 :
979 : // apply upper and lower limits
980 0 : _motor_speed_limited = constrain_float(_motor_speed_limited, lower_limit, upper_limit);
981 :
982 : // record time motor speed becomes zero
983 0 : if (is_zero(_motor_speed_limited)) {
984 0 : if (_motor_speed_zero_ms == 0) {
985 0 : _motor_speed_zero_ms = now_ms;
986 : }
987 : } else {
988 : // clear timer
989 0 : _motor_speed_zero_ms = 0;
990 : }
991 :
992 0 : return (int16_t)_motor_speed_limited;
993 : }
994 :
995 : // output logging and debug messages (if required)
996 : // force_logging should be true if caller wants to ensure the latest status is logged
997 0 : void AP_Torqeedo_TQBus::log_TRQD(bool force_logging)
998 : {
999 : // exit immediately if logging and debug options are not set
1000 0 : if (!option_enabled(AP_Torqeedo::options::LOG) && option_enabled(AP_Torqeedo::options::DEBUG_TO_GCS)) {
1001 0 : return;
1002 : }
1003 :
1004 : // return if not enough time has passed since last output
1005 0 : const uint32_t now_ms = AP_HAL::millis();
1006 0 : if (!force_logging && (now_ms - _last_log_TRQD_ms < TORQEEDO_LOG_TRQD_INTERVAL_MS)) {
1007 0 : return;
1008 : }
1009 0 : _last_log_TRQD_ms = now_ms;
1010 :
1011 : #if HAL_LOGGING_ENABLED || HAL_GCS_ENABLED
1012 0 : const bool health = healthy();
1013 0 : int16_t actual_motor_speed = get_motor_speed_limited();
1014 :
1015 : #if HAL_LOGGING_ENABLED
1016 0 : if (option_enabled(AP_Torqeedo::options::LOG)) {
1017 : // @LoggerMessage: TRQD
1018 : // @Description: Torqeedo Status
1019 : // @Field: TimeUS: Time since system startup
1020 : // @Field: I: instance
1021 : // @Field: Health: Health
1022 : // @Field: DesMotSpeed: Desired Motor Speed (-1000 to 1000)
1023 : // @Field: MotSpeed: Motor Speed (-1000 to 1000)
1024 : // @Field: SuccCnt: Success Count
1025 : // @Field: ErrCnt: Error Count
1026 0 : AP::logger().Write("TRQD", "TimeUS,I,Health,DesMotSpeed,MotSpeed,SuccCnt,ErrCnt",
1027 : "s#-----", // units
1028 : "F------", // multipliers
1029 : "QBBhhII", // formats
1030 : AP_HAL::micros64(),
1031 0 : _instance,
1032 : health,
1033 0 : _motor_speed_desired,
1034 : actual_motor_speed,
1035 : _parse_success_count,
1036 : _parse_error_count);
1037 : }
1038 : #endif
1039 :
1040 0 : if (option_enabled(AP_Torqeedo::options::DEBUG_TO_GCS)) {
1041 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO,"TRQD i:%u h:%u dspd:%d spd:%d succ:%ld err:%ld",
1042 : (unsigned)_instance,
1043 : (unsigned)health,
1044 : (int)_motor_speed_desired,
1045 : (int)actual_motor_speed,
1046 : (unsigned long)_parse_success_count,
1047 : (unsigned long)_parse_error_count);
1048 : }
1049 : #endif // HAL_LOGGING_ENABLED || HAL_GCS_ENABLED
1050 : }
1051 :
1052 : // send ESC telemetry
1053 0 : void AP_Torqeedo_TQBus::update_esc_telem(float rpm, float voltage, float current_amps, float esc_tempC, float motor_tempC)
1054 : {
1055 : #if HAL_WITH_ESC_TELEM
1056 : // find servo output channel
1057 0 : uint8_t telem_esc_index = 0;
1058 0 : IGNORE_RETURN(SRV_Channels::find_channel((SRV_Channel::Function)_params.servo_fn.get(), telem_esc_index));
1059 :
1060 : // fill in telemetry data structure
1061 0 : AP_ESC_Telem_Backend::TelemetryData telem_dat {};
1062 0 : telem_dat.temperature_cdeg = esc_tempC * 100; // temperature in centi-degrees
1063 0 : telem_dat.voltage = voltage; // voltage in volts
1064 0 : telem_dat.current = current_amps; // current in amps
1065 0 : telem_dat.motor_temp_cdeg = motor_tempC * 100; // motor temperature in centi-degrees
1066 :
1067 : // send telem and rpm data
1068 0 : update_telem_data(telem_esc_index, telem_dat, AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE |
1069 : AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE |
1070 : AP_ESC_Telem_Backend::TelemetryType::CURRENT |
1071 : AP_ESC_Telem_Backend::TelemetryType::VOLTAGE);
1072 :
1073 0 : update_rpm(telem_esc_index, rpm);
1074 : #endif
1075 0 : }
1076 :
1077 : #endif // HAL_TORQEEDO_ENABLED
|
