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 : Simulator for the FETtecOneWireESC
17 :
18 : TODO:
19 : - verify the assertion that DMA is required
20 : - stop ignoring REQ_TYPE while in bootloader?
21 : - correct visibility of members in simulation
22 : - half-duplex will require the use of a thread as every time we call update() we expect to send out a configuration message
23 : - tidy break vs return in AP_FETtec::handle_message
24 : - determine if we should have a "REQ_OK" as well as an "OK"
25 : - should rename simulated ESC "pwm" field to "value" or "fettec_value" or something
26 : - periodically log _unknown_esc_message, _message_invalid_in_state_count, _period_too_short, _receive_buf_used to dataflash using a low prio thread.
27 : - log type, version, subversion and sn to dataflash once.
28 :
29 :
30 :
31 : Protocol:
32 : - SET_FAST_COM_LENGTH could set a 32-bit bitmask that will be present rather than requring consecutive motors
33 : - Use two magic bytes in the header instead of just one
34 : - Use a 16bit CRC
35 : - the reply request needs to repeat the data that it replies to, to make sure the reply can be clearly assigned to a request
36 : - need to cope with reversals
37 : - in the case that we don't have ESC telemetry, consider probing ESCs periodically with an "OK"-request while disarmed
38 : */
39 :
40 : #include <AP_HAL/AP_HAL.h>
41 :
42 : extern const AP_HAL::HAL& hal;
43 :
44 : #include <AP_Math/AP_Math.h>
45 :
46 : #include "SIM_FETtecOneWireESC.h"
47 : #include "SITL.h"
48 : #include <AP_HAL/utility/sparse-endian.h>
49 :
50 : #include "SIM_Aircraft.h"
51 :
52 : #include <stdio.h>
53 : #include <errno.h>
54 :
55 : using namespace SITL;
56 :
57 : // table of user settable parameters
58 : const AP_Param::GroupInfo FETtecOneWireESC::var_info[] = {
59 :
60 : // @Param: ENA
61 : // @DisplayName: FETtec OneWire ESC simulator enable/disable
62 : // @Description: Allows you to enable (1) or disable (0) the FETtecOneWireESC simulator
63 : // @Values: 0:Disabled,1:Enabled
64 : // @User: Advanced
65 : AP_GROUPINFO("ENA", 1, FETtecOneWireESC, _enabled, 0),
66 :
67 : // @Param: POW
68 : // @DisplayName: Power off FETtec ESC mask
69 : // @Description: Allows you to turn power off to the simulated ESCs. Bits correspond to the ESC ID, *NOT* their servo channel.
70 : // @User: Advanced
71 : AP_GROUPINFO("POW", 2, FETtecOneWireESC, _powered_mask, 0xfff),
72 :
73 : AP_GROUPEND
74 : };
75 :
76 35632 : FETtecOneWireESC::FETtecOneWireESC() : SerialDevice::SerialDevice()
77 : {
78 2096 : AP_Param::setup_object_defaults(this, var_info);
79 :
80 : // initialise serial numbers and IDs
81 35632 : for (uint8_t n=0; n<ARRAY_SIZE(escs); n++) {
82 33536 : ESC &esc = escs[n];
83 33536 : esc.ofs = n; // so we can index for RPM, for example
84 33536 : esc.id = n+1; // really should parameterise this
85 435968 : for (uint8_t i=0; i<ARRAY_SIZE(esc.sn); i++) {
86 402432 : esc.sn[i] = n+1;
87 : }
88 : }
89 2096 : }
90 :
91 657015 : void FETtecOneWireESC::update_escs()
92 : {
93 : // process the power-off mask
94 11169255 : for (auto &esc : escs) {
95 10512240 : bool should_be_on = _powered_mask & (1U<<(esc.id-1));
96 10512240 : switch (esc.state) {
97 2662060 : case ESC::State::POWERED_OFF:
98 2662060 : if (should_be_on) {
99 208 : esc.state = ESC::State::IN_BOOTLOADER;
100 208 : esc.pwm = 0;
101 208 : esc.fast_com = {};
102 208 : esc.telem_request = false;
103 : }
104 2662060 : break;
105 7850180 : case ESC::State::IN_BOOTLOADER:
106 : case ESC::State::RUNNING:
107 : case ESC::State::RUNNING_START:
108 7850180 : if (!should_be_on) {
109 16 : esc.state = ESC::State::POWERED_OFF;
110 16 : break;
111 : }
112 : }
113 : }
114 :
115 11169255 : for (auto &esc : escs) {
116 10512240 : switch (esc.state) {
117 10512175 : case ESC::State::POWERED_OFF:
118 : case ESC::State::IN_BOOTLOADER:
119 : case ESC::State::RUNNING:
120 10512175 : continue;
121 65 : case ESC::State::RUNNING_START:
122 65 : esc.set_state(ESC::State::RUNNING);
123 65 : send_response(PackedMessage<OK> {
124 65 : esc.id,
125 : OK{}
126 : });
127 : }
128 : }
129 :
130 11169255 : for (auto &esc : escs) {
131 10512240 : if (esc.state != ESC::State::RUNNING) {
132 8580482 : continue;
133 : }
134 : // FIXME: this may not be an entirely accurate model of the
135 : // temperature profile of these ESCs.
136 1931758 : esc.temperature += esc.pwm/100000;
137 1931758 : esc.temperature *= 0.95;
138 : }
139 657015 : }
140 :
141 658525 : void FETtecOneWireESC::update(const class Aircraft &aircraft)
142 : {
143 658525 : if (!_enabled.get()) {
144 1510 : return;
145 : }
146 :
147 657015 : update_escs();
148 :
149 657015 : update_input();
150 657015 : update_send(aircraft);
151 : }
152 :
153 989 : void FETtecOneWireESC::handle_config_message()
154 : {
155 989 : ESC &esc = escs[u.config_message_header.target_id-1];
156 : simfet_debug("Config message type=%u esc=%u", (unsigned)u.config_message_header.request_type, (unsigned)u.config_message_header.target_id);
157 989 : if ((ResponseFrameHeaderID)u.config_message_header.header != ResponseFrameHeaderID::MASTER) {
158 0 : AP_HAL::panic("Unexpected header ID");
159 : }
160 989 : switch (esc.state) {
161 729 : case ESC::State::POWERED_OFF:
162 729 : return;
163 130 : case ESC::State::IN_BOOTLOADER:
164 130 : return bootloader_handle_config_message(esc);
165 0 : case ESC::State::RUNNING_START:
166 0 : return;
167 130 : case ESC::State::RUNNING:
168 130 : return running_handle_config_message(esc);
169 : }
170 0 : AP_HAL::panic("Unknown state");
171 : }
172 :
173 : template <typename T>
174 188422 : void FETtecOneWireESC::send_response(const T &r)
175 : {
176 : // simfet_debug("Sending response");
177 188422 : if (write_to_autopilot((char*)&r, sizeof(r)) != sizeof(r)) {
178 0 : AP_HAL::panic("short write");
179 : }
180 188422 : }
181 :
182 130 : void FETtecOneWireESC::bootloader_handle_config_message(FETtecOneWireESC::ESC &esc)
183 : {
184 130 : switch ((ConfigMessageType)u.config_message_header.request_type) {
185 65 : case ConfigMessageType::OK: {
186 : PackedMessage<OK> msg {
187 65 : esc.id,
188 : OK{},
189 65 : };
190 65 : msg.header = (uint8_t)ResponseFrameHeaderID::BOOTLOADER;
191 65 : msg.update_checksum();
192 65 : send_response(msg);
193 65 : return;
194 : }
195 0 : case ConfigMessageType::BL_PAGE_CORRECT: // BL only
196 : case ConfigMessageType::NOT_OK:
197 0 : break;
198 65 : case ConfigMessageType::BL_START_FW: // BL only
199 65 : esc.set_state(ESC::State::RUNNING_START);
200 : // the main firmware sends an OK
201 65 : return;
202 0 : case ConfigMessageType::BL_PAGES_TO_FLASH: // BL only
203 0 : break;
204 0 : case ConfigMessageType::REQ_TYPE:
205 : // ignore this for now
206 0 : return;
207 0 : case ConfigMessageType::REQ_SN:
208 : case ConfigMessageType::REQ_SW_VER:
209 : case ConfigMessageType::BEEP:
210 : case ConfigMessageType::SET_FAST_COM_LENGTH:
211 : case ConfigMessageType::SET_TLM_TYPE: //1 for alternative telemetry. ESC sends full telem per ESC: Temp, Volt, Current, ERPM, Consumption, CrcErrCount
212 : case ConfigMessageType::SET_LED_TMP_COLOR:
213 0 : break;
214 : }
215 0 : return;
216 : AP_HAL::panic("Unhandled config message in bootloader (%u)",
217 : (unsigned)u.config_message_header.request_type);
218 : }
219 :
220 130 : void FETtecOneWireESC::running_handle_config_message(FETtecOneWireESC::ESC &esc)
221 : {
222 130 : switch ((ConfigMessageType)u.config_message_header.request_type) {
223 :
224 0 : case ConfigMessageType::OK:
225 0 : return send_response(PackedMessage<OK> {
226 0 : esc.id,
227 : OK{}
228 0 : });
229 :
230 0 : case ConfigMessageType::BL_PAGE_CORRECT: // BL only
231 : case ConfigMessageType::NOT_OK:
232 0 : break;
233 0 : case ConfigMessageType::BL_START_FW: // BL only
234 0 : hal.console->printf("received unexpected BL_START_FW message\n");
235 0 : AP_HAL::panic("received unexpected BL_START_FW message");
236 : return;
237 0 : case ConfigMessageType::BL_PAGES_TO_FLASH: // BL only
238 0 : break;
239 :
240 0 : case ConfigMessageType::REQ_TYPE:
241 0 : return send_response(PackedMessage<ESC_TYPE> {
242 0 : esc.id,
243 0 : ESC_TYPE{esc.type}
244 0 : });
245 :
246 0 : case ConfigMessageType::REQ_SN:
247 0 : return send_response(PackedMessage<SN> {
248 0 : esc.id,
249 0 : SN{esc.sn, ARRAY_SIZE(esc.sn)}
250 0 : });
251 :
252 0 : case ConfigMessageType::REQ_SW_VER:
253 0 : return send_response(PackedMessage<SW_VER> {
254 0 : esc.id,
255 : SW_VER{esc.sw_version, esc.sw_subversion}
256 0 : });
257 :
258 0 : case ConfigMessageType::BEEP:
259 0 : break;
260 :
261 65 : case ConfigMessageType::SET_FAST_COM_LENGTH:
262 65 : esc.fast_com.length = u.packed_set_fast_com_length.msg.length;
263 65 : esc.fast_com.byte_count = u.packed_set_fast_com_length.msg.byte_count;
264 65 : esc.fast_com.min_esc_id = u.packed_set_fast_com_length.msg.min_esc_id;
265 65 : esc.fast_com.id_count = u.packed_set_fast_com_length.msg.id_count;
266 65 : return send_response(PackedMessage<OK> {
267 65 : esc.id,
268 : OK{}
269 65 : });
270 :
271 65 : case ConfigMessageType::SET_TLM_TYPE: //1 for alternative telemetry. ESC sends full telem per ESC: Temp, Volt, Current, ERPM, Consumption, CrcErrCount
272 65 : return handle_config_message_set_tlm_type(esc);
273 :
274 0 : case ConfigMessageType::SET_LED_TMP_COLOR:
275 0 : break;
276 : }
277 0 : AP_HAL::panic("Unknown config message (%u)", (unsigned)u.config_message_header.request_type);
278 : }
279 :
280 :
281 65 : void FETtecOneWireESC::handle_config_message_set_tlm_type(ESC &esc)
282 : {
283 65 : const TLMType type = (TLMType)u.packed_set_tlm_type.msg.type;
284 65 : switch (type) {
285 65 : case TLMType::NORMAL:
286 : case TLMType::ALTERNATIVE:
287 65 : esc.telem_type = type;
288 65 : send_response(PackedMessage<OK> {
289 65 : esc.id,
290 : OK{}
291 : });
292 65 : return;
293 : }
294 0 : AP_HAL::panic("unknown telem type=%u", (unsigned)type);
295 : }
296 :
297 188960 : void FETtecOneWireESC::handle_fast_esc_data()
298 : {
299 : // decode first byte - see driver for details
300 188960 : const uint8_t telem_request = u.buffer[0] >> 4;
301 :
302 : // offset into escs array for first esc involved in fast-throttle
303 : // command:
304 188960 : const uint8_t esc0_ofs = fast_com.min_esc_id - 1;
305 :
306 : // ::fprintf(stderr, "telem_request=%u\n", (unsigned)telem_request);
307 : uint16_t esc0_pwm;
308 188960 : esc0_pwm = ((u.buffer[0] >> 3) & 0x1) << 10;
309 188960 : if ((u.buffer[0] & 0b111) != 0x1) {
310 0 : AP_HAL::panic("expected fast-throttle command");
311 : }
312 :
313 : // decode second byte
314 188960 : esc0_pwm |= (u.buffer[1] >> 5) << 7;
315 188960 : if ((u.buffer[1] & 0b00011111) != 0x1f) {
316 0 : AP_HAL::panic("Unexpected 5-bit target id");
317 : }
318 :
319 : // decode enough of third byte to complete pwm[0]
320 188960 : esc0_pwm |= u.buffer[2] >> 1;
321 :
322 188960 : if (escs[esc0_ofs].state == ESC::State::RUNNING) {
323 188562 : ESC &esc { escs[esc0_ofs] };
324 188562 : esc.pwm = esc0_pwm;
325 :
326 188562 : if (telem_request == esc.id) {
327 68082 : esc.telem_request = true;
328 : }
329 : simfet_debug("esc=%u out: %u", esc.id, (unsigned)esc.pwm);
330 : }
331 :
332 : // decode remainder of ESC values
333 :
334 : // slides a window across the input buffer, extracting 11-bit ESC
335 : // values. The top 11 bits in "window" are the ESC value.
336 188960 : uint8_t byte_ofs = 2;
337 188960 : uint32_t window = u.buffer[byte_ofs++]<<24;
338 188960 : window <<= 7;
339 188960 : uint8_t bits_free = 32-1;
340 638593 : for (uint8_t i=esc0_ofs+1; i<esc0_ofs+fast_com.id_count; i++) {
341 1382393 : while (bits_free > 7) {
342 932760 : window |= u.buffer[byte_ofs++] << (bits_free-8);
343 932760 : bits_free -= 8;
344 : }
345 449633 : ESC &esc { escs[i] };
346 449633 : if (esc.state == ESC::State::RUNNING) {
347 446844 : if (telem_request == esc.id) {
348 120080 : esc.telem_request = true;
349 : }
350 446844 : esc.pwm = window >> 21;
351 : simfet_debug("esc=%u out: %u", esc.id, (unsigned)esc.pwm);
352 : }
353 449633 : window <<= 11;
354 449633 : bits_free += 11;
355 : }
356 :
357 3212320 : for (uint8_t i=0; i<ARRAY_SIZE(escs); i++) {
358 3023360 : const ESC &esc { escs[i] };
359 3023360 : if (esc.pwm == 0) {
360 2384767 : continue;
361 : }
362 : // this will need to adjust for reversals. We should also set
363 : // one of the simulated ESCs up to have a pair of motor wires
364 : // crossed i.e. spin backwards. Maybe a mask for it
365 638593 : if (esc.pwm >= 1000 && esc.pwm <= 2000) {
366 638593 : continue;
367 : }
368 0 : AP_HAL::panic("transmitted value out of range (%u)", esc.pwm);
369 : }
370 188960 : }
371 :
372 189949 : void FETtecOneWireESC::consume_bytes(uint8_t count)
373 : {
374 189949 : if (count > buflen) {
375 0 : AP_HAL::panic("Consuming more bytes than in buffer?");
376 : }
377 189949 : if (buflen == count) {
378 189949 : buflen = 0;
379 189949 : return;
380 : }
381 0 : memmove(&u.buffer[0], &u.buffer[count], buflen - count);
382 0 : buflen -= count;
383 : }
384 :
385 657015 : void FETtecOneWireESC::update_input()
386 : {
387 657015 : const ssize_t n = read_from_autopilot((char*)&u.buffer[buflen], ARRAY_SIZE(u.buffer) - buflen);
388 657015 : if (n < 0) {
389 : // TODO: do better here
390 0 : if (errno != EAGAIN && errno != EWOULDBLOCK && errno != 0) {
391 0 : AP_HAL::panic("Failed to read from autopilot");
392 : }
393 : } else {
394 657015 : buflen += n;
395 : }
396 :
397 : // bool config_message_checksum_fail = false;
398 :
399 657015 : if (buflen > offsetof(ConfigMessageHeader, header) &&
400 189949 : u.config_message_header.header == 0x01 &&
401 989 : buflen > offsetof(ConfigMessageHeader, frame_len) &&
402 989 : buflen >= u.config_message_header.frame_len) {
403 989 : const uint8_t calculated_checksum = crc8_dvb_update(0, u.buffer, u.config_message_header.frame_len-1);
404 989 : const uint8_t received_checksum = u.buffer[u.config_message_header.frame_len-1];
405 989 : if (calculated_checksum == received_checksum) {
406 989 : handle_config_message();
407 : // consume the message:
408 989 : consume_bytes(u.config_message_header.frame_len);
409 989 : return;
410 : } else {
411 : simfet_debug("Checksum mismatch");
412 0 : abort();
413 : // config_message_checksum_fail = true;
414 : }
415 : return; // 1 message/loop....
416 : }
417 :
418 : // no config message, so let's see if there's fast PWM input.
419 656026 : if (fast_com.id_count == 255) {
420 : // see if any ESC has been configured:
421 1381028 : for (uint8_t i=0; i<ARRAY_SIZE(escs); i++) {
422 1299792 : if (escs[i].fast_com.id_count == 255) {
423 1299776 : continue;
424 : }
425 16 : fast_com.id_count = escs[i].fast_com.id_count;
426 16 : fast_com.min_esc_id = escs[i].fast_com.min_esc_id;
427 16 : break;
428 : }
429 : }
430 656026 : if (fast_com.id_count == 255) {
431 : // no ESC is configured. Ummm.
432 81236 : buflen = 0;
433 81236 : return;
434 : }
435 :
436 574790 : const uint16_t total_bits_required = 4 + 1 + 7 + (fast_com.id_count*11);
437 574790 : const uint8_t bytes_required = (total_bits_required + 7) / 8 + 1;
438 :
439 574790 : if (buflen < bytes_required) {
440 385830 : return;
441 : }
442 :
443 188960 : if (buflen == bytes_required) {
444 188960 : const uint8_t calculated_checksum = crc8_dvb_update(0, u.buffer, buflen-1);
445 188960 : if (u.buffer[buflen-1] != calculated_checksum) {
446 0 : AP_HAL::panic("checksum failure");
447 : }
448 :
449 188960 : handle_fast_esc_data();
450 188960 : consume_bytes(bytes_required);
451 188960 : return;
452 : }
453 :
454 : // debug("Read (%d) bytes from autopilot (%u)", (signed)n, config_message_checksum_fail);
455 0 : if (n >= 0) {
456 0 : abort();
457 : }
458 0 : buflen = 0;
459 : }
460 :
461 657815 : void FETtecOneWireESC::update_sitl_input_pwm(struct sitl_input &input)
462 : {
463 : // overwrite the SITL input values passed through from
464 : // sitl_model->update_model with those we're receiving serially
465 11182855 : for (auto &esc : escs) {
466 10525040 : if (esc.id > ARRAY_SIZE(input.servos)) {
467 : // silently ignore; input.servos is 12-long, we are
468 : // usually 16-long
469 0 : continue;
470 : }
471 10525040 : input.servos[esc.id-1] = esc.pwm;
472 : }
473 657815 : }
474 :
475 657015 : void FETtecOneWireESC::send_esc_telemetry(const Aircraft &aircraft)
476 : {
477 11169255 : for (auto &esc : escs) {
478 10512240 : if (!esc.telem_request) {
479 10324078 : continue;
480 : }
481 188162 : esc.telem_request = false;
482 188162 : if (esc.state != ESC::State::RUNNING) {
483 0 : continue;
484 : }
485 188162 : if (esc.telem_type != TLMType::ALTERNATIVE) {
486 : // no idea what "normal" looks like
487 0 : abort();
488 : }
489 :
490 188162 : const int8_t temp_cdeg = esc.temperature * 100;
491 188162 : const uint16_t voltage = aircraft.get_battery_voltage() * 100;
492 188162 : const uint16_t current = (6 + esc.id * 100);
493 :
494 : // FIXME: the vehicle models should be supplying this RPM!
495 188162 : const uint16_t Kv = 1000;
496 188162 : const float p = (esc.pwm-1000)/1000.0;
497 188162 : int16_t rpm = aircraft.get_battery_voltage() * Kv * p;
498 :
499 188162 : const uint16_t consumption_mah = 0;
500 188162 : const uint16_t errcount = 17;
501 188162 : send_response(PackedMessage<ESCTelem> {
502 188162 : esc.id,
503 : ESCTelem{temp_cdeg, voltage, current, rpm, consumption_mah, errcount}
504 : });
505 : }
506 657015 : }
507 :
508 657015 : void FETtecOneWireESC::update_send(const Aircraft &aircraft)
509 : {
510 657015 : send_esc_telemetry(aircraft);
511 657015 : }
|
