APM:Libraries
RCOutput_Tap.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2016 PX4 Development Team. All rights reserved.
3  * Copyright (C) 2017 Intel Corporation. All rights reserved.
4  *
5  * This file is free software: you can redistribute it and/or modify it
6  * under the terms of the GNU General Public License as published by the
7  * Free Software Foundation, either version 3 of the License, or
8  * (at your option) any later version.
9  *
10  * This file is distributed in the hope that it will be useful, but
11  * WITHOUT ANY WARRANTY; without even the implied warranty of
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
13  * See the GNU General Public License for more details.
14  *
15  * You should have received a copy of the GNU General Public License along
16  * with this program. If not, see <http://www.gnu.org/licenses/>.
17  */
18 
19 /*
20  * Implementation of TAP UART ESCs. Used the implementation from PX4 as a base
21  * which is BSD-licensed:
22  *
23  * Redistribution and use in source and binary forms, with or without
24  * modification, are permitted provided that the following conditions
25  * are met:
26  *
27  * 1. Redistributions of source code must retain the above copyright
28  * notice, this list of conditions and the following disclaimer.
29  * 2. Redistributions in binary form must reproduce the above copyright
30  * notice, this list of conditions and the following disclaimer in
31  * the documentation and/or other materials provided with the
32  * distribution.
33  * 3. Neither the name PX4 nor the names of its contributors may be
34  * used to endorse or promote products derived from this software
35  * without specific prior written permission.
36  *
37  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
38  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
39  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
40  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
41  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
42  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
43  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
44  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
45  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
46  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
47  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
48  * POSSIBILITY OF SUCH DAMAGE.
49  */
50 
51 #include <AP_HAL/AP_HAL.h>
52 
53 #ifdef HAL_RCOUTPUT_TAP_DEVICE
54 
55 #include "RCOutput_Tap.h"
56 
57 #include <errno.h>
58 #include <fcntl.h>
59 #include <stdio.h>
60 #include <sys/stat.h>
61 #include <sys/types.h>
62 #include <termios.h>
63 #include <unistd.h>
64 
65 #include <AP_Math/AP_Math.h>
66 
67 #define DEBUG 0
68 #if DEBUG
69 #define debug(fmt, args...) ::printf(fmt "\n", ##args)
70 #else
71 #define debug(fmt, args...)
72 #endif
73 
74 extern const AP_HAL::HAL &hal;
75 
76 /****** ESC data types ******/
77 
78 #define ESC_HAVE_CURRENT_SENSOR
79 
80 static const uint8_t crcTable[256] = {
81  0x00, 0xE7, 0x29, 0xCE, 0x52, 0xB5, 0x7B, 0x9C, 0xA4, 0x43, 0x8D, 0x6A,
82  0xF6, 0x11, 0xDF, 0x38, 0xAF, 0x48, 0x86, 0x61, 0xFD, 0x1A, 0xD4, 0x33,
83  0x0B, 0xEC, 0x22, 0xC5, 0x59, 0xBE, 0x70, 0x97, 0xB9, 0x5E, 0x90, 0x77,
84  0xEB, 0x0C, 0xC2, 0x25, 0x1D, 0xFA, 0x34, 0xD3, 0x4F, 0xA8, 0x66, 0x81,
85  0x16, 0xF1, 0x3F, 0xD8, 0x44, 0xA3, 0x6D, 0x8A, 0xB2, 0x55, 0x9B, 0x7C,
86  0xE0, 0x07, 0xC9, 0x2E, 0x95, 0x72, 0xBC, 0x5B, 0xC7, 0x20, 0xEE, 0x09,
87  0x31, 0xD6, 0x18, 0xFF, 0x63, 0x84, 0x4A, 0xAD, 0x3A, 0xDD, 0x13, 0xF4,
88  0x68, 0x8F, 0x41, 0xA6, 0x9E, 0x79, 0xB7, 0x50, 0xCC, 0x2B, 0xE5, 0x02,
89  0x2C, 0xCB, 0x05, 0xE2, 0x7E, 0x99, 0x57, 0xB0, 0x88, 0x6F, 0xA1, 0x46,
90  0xDA, 0x3D, 0xF3, 0x14, 0x83, 0x64, 0xAA, 0x4D, 0xD1, 0x36, 0xF8, 0x1F,
91  0x27, 0xC0, 0x0E, 0xE9, 0x75, 0x92, 0x5C, 0xBB, 0xCD, 0x2A, 0xE4, 0x03,
92  0x9F, 0x78, 0xB6, 0x51, 0x69, 0x8E, 0x40, 0xA7, 0x3B, 0xDC, 0x12, 0xF5,
93  0x62, 0x85, 0x4B, 0xAC, 0x30, 0xD7, 0x19, 0xFE, 0xC6, 0x21, 0xEF, 0x08,
94  0x94, 0x73, 0xBD, 0x5A, 0x74, 0x93, 0x5D, 0xBA, 0x26, 0xC1, 0x0F, 0xE8,
95  0xD0, 0x37, 0xF9, 0x1E, 0x82, 0x65, 0xAB, 0x4C, 0xDB, 0x3C, 0xF2, 0x15,
96  0x89, 0x6E, 0xA0, 0x47, 0x7F, 0x98, 0x56, 0xB1, 0x2D, 0xCA, 0x04, 0xE3,
97  0x58, 0xBF, 0x71, 0x96, 0x0A, 0xED, 0x23, 0xC4, 0xFC, 0x1B, 0xD5, 0x32,
98  0xAE, 0x49, 0x87, 0x60, 0xF7, 0x10, 0xDE, 0x39, 0xA5, 0x42, 0x8C, 0x6B,
99  0x53, 0xB4, 0x7A, 0x9D, 0x01, 0xE6, 0x28, 0xCF, 0xE1, 0x06, 0xC8, 0x2F,
100  0xB3, 0x54, 0x9A, 0x7D, 0x45, 0xA2, 0x6C, 0x8B, 0x17, 0xF0, 0x3E, 0xD9,
101  0x4E, 0xA9, 0x67, 0x80, 0x1C, 0xFB, 0x35, 0xD2, 0xEA, 0x0D, 0xC3, 0x24,
102  0xB8, 0x5F, 0x91, 0x76
103 };
104 
105 // Circular from back right in CCW direction
106 static const uint8_t device_mux_map[] = {0, 1, 4, 3, 2, 5, 7, 8};
107 // 0 is CW, 1 is CCW
108 static const uint8_t device_dir_map[] = {0, 1, 0, 1, 0, 1, 0, 1};
109 
110 #define TAP_ESC_MAX_PACKET_LEN 20
111 #define TAP_ESC_MAX_MOTOR_NUM 8
112 
113 /*
114  * ESC_POS maps the values stored in the channelMapTable to reorder the ESC's
115  * id so that that match the mux setting, so that the ressonder's data
116  * will be read.
117  * The index on channelMapTable[p] p is the physical ESC
118  * The value it is set to is the logical value from ESC_POS[p]
119  * Phy Log
120  * 0 0
121  * 1 1
122  * 2 4
123  * 3 3
124  * 4 2
125  * 5 5
126  * ....
127  *
128  */
129 
130 #define RPMMAX 1900
131 #define RPMMIN 1200
132 #define RPMSTOPPED (RPMMIN - 10)
133 
134 #define MIN_BOOT_TIME_MSEC (550) // Minimum time to wait after Power on before sending commands
135 
136 namespace ap {
137 
138 /****** Run ***********/
139 
140 #define RUN_CHANNEL_VALUE_MASK (uint16_t)0x07ff
141 #define RUN_RED_LED_ON_MASK (uint16_t)0x0800
142 #define RUN_GREEN_LED_ON_MASK (uint16_t)0x1000
143 #define RUN_BLUE_LED_ON_MASK (uint16_t)0x2000
144 #define RUN_LED_ON_MASK (uint16_t)0x3800
145 #define RUN_FEEDBACK_ENABLE_MASK (uint16_t)0x4000
146 #define RUN_REVERSE_MASK (uint16_t)0x8000
147 
148 struct PACKED RunReq {
149  uint16_t value[TAP_ESC_MAX_MOTOR_NUM];
150 };
151 
152 struct PACKED RunInfoRepsonse {
153  uint8_t channelID;
154  uint8_t ESCStatus;
155  int16_t speed; // -32767 - 32768
156 #if defined(ESC_HAVE_VOLTAGE_SENSOR)
157  uint16_t voltage; // 0.00 - 100.00 V
158 #endif
159 #if defined(ESC_HAVE_CURRENT_SENSOR)
160  uint16_t current; // 0.0 - 200.0 A
161 #endif
162 #if defined(ESC_HAVE_TEMPERATURE_SENSOR)
163  uint8_t temperature; // 0 - 256 degree celsius
164 #endif
165 };
166 /****** Run ***********/
167 
168 /****** ConFigInfoBasic ***********/
169 struct PACKED ConfigInfoBasicRequest {
170  uint8_t maxChannelInUse;
171  uint8_t channelMapTable[TAP_ESC_MAX_MOTOR_NUM];
172  uint8_t monitorMsgType;
173  uint8_t controlMode;
174  uint16_t minChannelValue;
175  uint16_t maxChannelValue;
176 };
177 
178 struct PACKED ConfigInfoBasicResponse {
179  uint8_t channelID;
180  ConfigInfoBasicRequest resp;
181 };
182 
183 #define ESC_CHANNEL_MAP_CHANNEL 0x0f
184 #define ESC_CHANNEL_MAP_RUNNING_DIRECTION 0xf0
185 /****** ConFigInfoBasicResponse ***********/
186 
187 /****** InfoRequest ***********/
188 enum InfoTypes {
189  REQUEST_INFO_BASIC = 0,
190  REQUEST_INFO_FUll,
191  REQUEST_INFO_RUN,
192  REQUEST_INFO_STUDY,
193  REQUEST_INFO_COMM,
194  REQUEST_INFO_DEVICE,
195 };
196 
197 struct PACKED InfoRequest {
198  uint8_t channelID;
199  uint8_t requestInfoType;
200 };
201 
202 /****** InfoRequest ***********/
203 
204 struct PACKED EscPacket {
205  uint8_t head;
206  uint8_t len;
207  uint8_t msg_id;
208  union {
209  InfoRequest reqInfo;
210  ConfigInfoBasicRequest reqConfigInfoBasic;
211  RunReq reqRun;
212 
213  ConfigInfoBasicResponse rspConfigInfoBasic;
214  RunInfoRepsonse rspRunInfo;
215  uint8_t bytes[100];
216  } d;
217  uint8_t crc_data;
218 };
219 
220 //static const unsigned ESC_PACKET_DATA_OFFSET = offsetof(EscPacket, d);
221 static const unsigned ESC_PACKET_DATA_OFFSET = 3;
222 
223 /******************************************************************************************
224  * ESCBUS_MSG_ID_RUN_INFO packet
225  *
226  * Monitor message of ESCs while motor is running
227  *
228  * channelID: assigned channel number
229  *
230  * ESCStatus: status of ESC
231  * Num Health status
232  * 0 HEALTHY
233  * 1 WARNING_LOW_VOLTAGE
234  * 2 WARNING_OVER_CURRENT
235  * 3 WARNING_OVER_HEAT
236  * 4 ERROR_MOTOR_LOW_SPEED_LOSE_STEP
237  * 5 ERROR_MOTOR_STALL
238  * 6 ERROR_HARDWARE
239  * 7 ERROR_LOSE_PROPELLER
240  * 8 ERROR_OVER_CURRENT
241  *
242  * speed: -32767 - 32767 rpm
243  *
244  * temperature: 0 - 256 celsius degree (if available)
245  * voltage: 0.00 - 100.00 V (if available)
246  * current: 0.0 - 200.0 A (if available)
247  */
248 
249 enum ESCBUS_ENUM_ESC_STATUS {
250  ESC_STATUS_HEALTHY,
251  ESC_STATUS_WARNING_LOW_VOLTAGE,
252  ESC_STATUS_WARNING_OVER_HEAT,
253  ESC_STATUS_ERROR_MOTOR_LOW_SPEED_LOSE_STEP,
254  ESC_STATUS_ERROR_MOTOR_STALL,
255  ESC_STATUS_ERROR_HARDWARE,
256  ESC_STATUS_ERROR_LOSE_PROPELLER,
257  ESC_STATUS_ERROR_OVER_CURRENT,
258  ESC_STATUS_ERROR_MOTOR_HIGH_SPEED_LOSE_STEP,
259  ESC_STATUS_ERROR_LOSE_CMD,
260 };
261 
262 enum ESCBUS_ENUM_MESSAGE_ID {
263  // messages or command to ESC
264  ESCBUS_MSG_ID_CONFIG_BASIC = 0,
265  ESCBUS_MSG_ID_CONFIG_FULL,
266  ESCBUS_MSG_ID_RUN,
267  ESCBUS_MSG_ID_TUNE,
268  ESCBUS_MSG_ID_DO_CMD,
269  // messages from ESC
270  ESCBUS_MSG_ID_REQUEST_INFO,
271  ESCBUS_MSG_ID_CONFIG_INFO_BASIC, // simple configuration info for request from flight controller
272  ESCBUS_MSG_ID_CONFIG_INFO_FULL, // full configuration info for request from host such as computer
273  ESCBUS_MSG_ID_RUN_INFO, // feedback message in RUN mode
274  ESCBUS_MSG_ID_STUDY_INFO, // studied parameters in STUDY mode
275  ESCBUS_MSG_ID_COMM_INFO, // communication method info
276  ESCBUS_MSG_ID_DEVICE_INFO, // ESC device info
277  ESCBUS_MSG_ID_ASSIGNED_ID, // never touch ESCBUS_MSG_ID_MAX_NUM
278  //boot loader used
279  PROTO_OK = 0x10, // INSYNC/OK - 'ok' response
280  PROTO_FAILED = 0x11, // INSYNC/FAILED - 'fail' response
281 
282  ESCBUS_MSG_ID_BOOT_SYNC = 0x21, // boot loader used
283  PROTO_GET_DEVICE = 0x22, // get device ID bytes
284  PROTO_CHIP_ERASE = 0x23, // erase program area and reset program address
285  PROTO_PROG_MULTI = 0x27, // write bytes at program address and increment
286  PROTO_GET_CRC = 0x29, // compute & return a CRC
287  PROTO_BOOT = 0x30, // boot the application
288  PROTO_GET_SOFTWARE_VERSION = 0x40,
289  ESCBUS_MSG_ID_MAX_NUM,
290 };
291 
292 enum PARSR_ESC_STATE {
293  HEAD,
294  LEN,
295  ID,
296  DATA,
297  CRC,
298 };
299 
300 /****************************/
301 }
302 
303 using namespace ap;
304 
306 {
307  if (_uart_fd < 0) {
308  return;
309  }
310  ::close(_uart_fd);
311  _uart_fd = -1;
312 }
313 
315 {
316  // open uart
317  _uart_fd = open(HAL_RCOUTPUT_TAP_DEVICE, O_RDWR | O_NOCTTY | O_NONBLOCK);
318  int termios_state = -1;
319 
320  if (_uart_fd < 0) {
321  ::fprintf(stderr, "failed to open uart device! %s\n", HAL_RCOUTPUT_TAP_DEVICE);
322  return -1;
323  }
324 
325  struct termios uart_config;
326  memset(&uart_config, 0, sizeof(uart_config));
327  tcgetattr(_uart_fd, &uart_config);
328 
329  // clear ONLCR flag (which appends a CR for every LF)
330  uart_config.c_oflag &= ~ONLCR;
331 
332  if ((termios_state = tcsetattr(_uart_fd, TCSANOW, &uart_config)) < 0) {
333  ::fprintf(stderr, "tcsetattr failed for %s\n", HAL_RCOUTPUT_TAP_DEVICE);
334  _uart_close();
335  return false;
336  }
337 
338  if (!_uart_set_speed(250000)) {
339  ::fprintf(stderr, "failed to set baudrate for %s: %m\n",
340  HAL_RCOUTPUT_TAP_DEVICE);
341  _uart_close();
342  return false;
343  }
344 
345  return true;
346 }
347 
348 void RCOutput_Tap::init()
349 {
350  _perf_rcout = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "APM_rcout");
351 
352  if (!_uart_open()) {
353  AP_HAL::panic("Unable to open " HAL_RCOUTPUT_TAP_DEVICE);
354  return;
355  }
356 
357  uint32_t now = AP_HAL::millis();
358  if (now < MIN_BOOT_TIME_MSEC) {
359  hal.scheduler->delay(MIN_BOOT_TIME_MSEC - now);
360  }
361 
362  /* Issue Basic Config */
363 
364  EscPacket packet = {0xfe, sizeof(ConfigInfoBasicRequest), ESCBUS_MSG_ID_CONFIG_BASIC};
365  ConfigInfoBasicRequest &config = packet.d.reqConfigInfoBasic;
366  memset(&config, 0, sizeof(ConfigInfoBasicRequest));
367  config.maxChannelInUse = _channels_count;
368  config.controlMode = 1;
369 
370  /* Assign the id's to the ESCs to match the mux */
371  for (uint8_t phy_chan_index = 0; phy_chan_index < _channels_count; phy_chan_index++) {
372  config.channelMapTable[phy_chan_index] = device_mux_map[phy_chan_index] & ESC_CHANNEL_MAP_CHANNEL;
373  config.channelMapTable[phy_chan_index] |= (device_dir_map[phy_chan_index] << 4) & ESC_CHANNEL_MAP_RUNNING_DIRECTION;
374  }
375 
376  config.maxChannelValue = RPMMAX;
377  config.minChannelValue = RPMMIN;
378 
379  int ret = _send_packet(packet);
380  if (ret < 0) {
381  _uart_close();
382  AP_HAL::panic("Unable to send configuration to " HAL_RCOUTPUT_TAP_DEVICE);
383  return;
384  }
385 
386  /*
387  * To Unlock the ESC from the Power up state we need to issue 10
388  * ESCBUS_MSG_ID_RUN request with all the values 0;
389  */
390  EscPacket unlock_packet = {0xfe, _channels_count, ESCBUS_MSG_ID_RUN};
391  unlock_packet.len *= sizeof(unlock_packet.d.reqRun.value[0]);
392  memset(unlock_packet.d.bytes, 0, sizeof(packet.d.bytes));
393 
394  for (uint8_t i = 0; i < 10; i++) {
395  _send_packet(unlock_packet);
396  /* Min Packet to Packet time is 1 Ms so use 2 */
397  hal.scheduler->delay(2);
398  }
399 }
400 
401 int RCOutput_Tap::_send_packet(EscPacket &packet)
402 {
403  int packet_len = _crc_packet(packet);
404  int ret = ::write(_uart_fd, &packet.head, packet_len);
405 
406  if (ret != packet_len) {
407  debug("TX ERROR: ret: %d, errno: %d", ret, errno);
408  }
409 
410  return ret;
411 }
412 
413 uint8_t RCOutput_Tap::_crc8_esc(uint8_t *p, uint8_t len)
414 {
415  uint8_t crc = 0;
416 
417  for (uint8_t i = 0; i < len; i++) {
418  crc = crcTable[crc ^ *p++];
419  }
420 
421  return crc;
422 }
423 
424 uint8_t RCOutput_Tap::_crc_packet(EscPacket &p)
425 {
426  /* Calculate the crc over Len,ID,data */
427  p.d.bytes[p.len] = _crc8_esc(&p.len, p.len + 2);
428  return p.len + ESC_PACKET_DATA_OFFSET + 1;
429 }
430 
431 /*
432  set output frequency
433  */
434 void RCOutput_Tap::set_freq(uint32_t chmask, uint16_t freq_hz)
435 {
436 }
437 
438 uint16_t RCOutput_Tap::get_freq(uint8_t ch)
439 {
440  return 400;
441 }
442 
443 void RCOutput_Tap::enable_ch(uint8_t ch)
444 {
445  if (ch >= MAX_MOTORS) {
446  return;
447  }
448  _enabled_channels |= (1U << ch);
449 }
450 
451 void RCOutput_Tap::disable_ch(uint8_t ch)
452 {
453  if (ch >= MAX_MOTORS) {
454  return;
455  }
456 
457  _enabled_channels &= ~(1U << ch);
458 }
459 
460 void RCOutput_Tap::write(uint8_t ch, uint16_t period_us)
461 {
462  if (ch >= MAX_MOTORS) {
463  return;
464  }
465  if (!(_enabled_channels & (1U << ch))) {
466  // not enabled
467  return;
468  }
469 
470  _period[ch] = period_us;
471 
472  if (!_corking) {
473  push();
474  }
475 }
476 
477 uint16_t RCOutput_Tap::read(uint8_t ch)
478 {
479  if (ch >= MAX_MOTORS) {
480  return 0;
481  }
482  return _period[ch];
483 }
484 
485 void RCOutput_Tap::read(uint16_t *period_us, uint8_t len)
486 {
487  for (uint8_t i = 0; i < len; i++) {
488  period_us[i] = read(i);
489  }
490 }
491 
492 void RCOutput_Tap::cork()
493 {
494  _corking = true;
495 }
496 
497 void RCOutput_Tap::push()
498 {
499  _corking = false;
500 
501  hal.util->perf_begin(_perf_rcout);
502 
503  uint16_t out[TAP_ESC_MAX_MOTOR_NUM];
504  uint8_t motor_cnt = _channels_count;
505 
506  uint8_t motor_mapping[] = {
507  [0] = 2,
508  [1] = 1,
509  [2] = 0,
510  [3] = 3,
511  };
512 
513  // map from the RPM range to 0 - 100% duty cycle for the ESCs
514  for (uint8_t i = 0; i < motor_cnt; i++) {
515  uint16_t *val = &out[motor_mapping[i]];
516 
517  if (!(_enabled_channels & (1U << i))) {
518  *val = RPMSTOPPED;
519  } else if (_period[i] < _esc_pwm_min) {
520  *val = RPMSTOPPED;
521  } else if (_period[i] >= _esc_pwm_max) {
522  *val = RPMMAX;
523  } else {
524  float period_us = constrain_int16(_period[i], _esc_pwm_min, _esc_pwm_max);
525 
526  /*
527  * Map to [ RPMSTOPPED, RPMMAX ] range rather than
528  * [ RPMMIN, RPMMAX ] because AP_Motors will send us _esc_pwm_min
529  * when it's disarmed
530  */
531  float rpm = (period_us - _esc_pwm_min)/(_esc_pwm_max - _esc_pwm_min)
532  * (RPMMAX - RPMSTOPPED) + RPMSTOPPED;
533 
534  *val = (uint16_t) rpm;
535  }
536  }
537  for (uint8_t i = motor_cnt; i < TAP_ESC_MAX_MOTOR_NUM; i++) {
538  out[i] = RPMSTOPPED;
539  }
540 
541  /*
542  * Value packet format, little endian
543  *
544  * | 15 | 14 | 13 | 12 | 11 | 10 ...... 0 |
545  * ------------------------------------------
546  * | REV | FEN | BL | GL | RL | RPM value |
547  *
548  * RPM value: [ RPMMIN, RPMMAX ]
549  * RL: LED1
550  * GL: LED2 (ESC may have only one LED that works, independent of the color)
551  * BL: LED3
552  * FEN: Feedback enable
553  * REV: Reverse direction
554  */
555 
556  EscPacket packet = {0xfe, _channels_count, ESCBUS_MSG_ID_RUN};
557  packet.len *= sizeof(packet.d.reqRun.value[0]);
558 
559  uint32_t tnow = AP_HAL::millis();
560  if (tnow - _last_led_update_msec > 250) {
561  _led_on = !_led_on;
562  _last_led_update_msec = tnow;
563  }
564 
565  for (uint8_t i = 0; i < _channels_count; i++) {
566  packet.d.reqRun.value[i] = out[i] & RUN_CHANNEL_VALUE_MASK;
567  if (_led_on) {
568  packet.d.reqRun.value[i] |= RUN_LED_ON_MASK;
569  }
570  }
571 
572  int ret = _send_packet(packet);
573  if (ret < 1) {
574  debug("TX ERROR: ret: %d, errno: %d", ret, errno);
575  }
576 
577  hal.util->perf_end(_perf_rcout);
578 }
579 
580 #endif
static uint8_t _crc8_esc(uint8_t *p, uint8_t len)
virtual perf_counter_t perf_alloc(perf_counter_type t, const char *name)
Definition: Util.h:102
int16_t constrain_int16(const int16_t amt, const int16_t low, const int16_t high)
Definition: AP_Math.h:147
int open(const char *pathname, int flags)
POSIX Open a file with integer mode flags.
Definition: posix.c:885
uint16_t read(uint8_t ch) override
ssize_t write(int fd, const void *buf, size_t count)
POSIX Write count bytes from *buf to fileno fd.
Definition: posix.c:1169
void cork() override
AP_HAL::Util * util
Definition: HAL.h:115
const AP_HAL::HAL & hal
Definition: UARTDriver.cpp:37
void push() override
void init() override
int _send_packet(EscPacket &p)
#define debug(fmt, args ...)
float temperature
Definition: Airspeed.cpp:32
virtual void delay(uint16_t ms)=0
void set_freq(uint32_t chmask, uint16_t freq_hz) override
void write(uint8_t ch, uint16_t period_us) override
ssize_t read(int fd, void *buf, size_t count)
POSIX read count bytes from *buf to fileno fd.
Definition: posix.c:995
uint32_t millis()
Definition: system.cpp:157
int close(int fileno)
POSIX Close a file with fileno handel.
Definition: posix.c:675
virtual void perf_end(perf_counter_t h)
Definition: Util.h:104
virtual void perf_begin(perf_counter_t h)
Definition: Util.h:103
uint16_t get_freq(uint8_t ch) override
int errno
Note: fdevopen assigns stdin,stdout,stderr.
Definition: posix.c:118
float voltage
int fprintf(FILE *fp, const char *fmt,...)
fprintf character write function
Definition: posix.c:2539
#define PACKED
Definition: AP_Common.h:28
static uint8_t _crc_packet(EscPacket &p)
float value
void disable_ch(uint8_t ch) override
void panic(const char *errormsg,...) FMT_PRINTF(1
Definition: system.cpp:140
void enable_ch(uint8_t ch) override
AP_HAL::Scheduler * scheduler
Definition: HAL.h:114