APM:Libraries
RC_UART.cpp
Go to the documentation of this file.
1 /*
2  take RC channels in from UART and put out as PWM
3  */
4 
5 #include <AP_HAL/AP_HAL.h>
6 #include <GCS_MAVLink/include/mavlink/v2.0/checksum.h>
7 
9 
10 #define NUM_CHANNELS 4
11 #define ESC_MAGIC 0xF7
12 #define RC_SPEED 490
13 #define UART uartE
14 
16 public:
17  // HAL::Callbacks implementation.
18  void setup() override;
19  void loop() override;
20 
21 private:
22  uint8_t read_wait(void);
23  uint8_t enable_mask;
24  const uint32_t baudrate = 115200;
25  uint32_t counter;
26 };
27 
29 {
30  hal.scheduler->delay(1000);
31  hal.console->printf("RC_UART starting\n");
32  hal.UART->begin(baudrate, 512, 512);
33  hal.rcout->set_freq(0xFF, RC_SPEED);
34 }
35 
36 uint8_t RC_UART::read_wait(void)
37 {
38  while (true) {
39  int16_t c = hal.UART->read();
40  if (c != -1) {
41  // hal.console->printf("c=0x%02x\n", (unsigned)c);
42  return c;
43  }
44  hal.scheduler->delay_microseconds(100);
45  }
46 }
47 
49 {
50  union {
51  uint16_t period[NUM_CHANNELS];
52  uint8_t bytes[NUM_CHANNELS*2];
53  } u;
54 
55  // wait for magic
56  while (true) {
57  uint8_t c = read_wait();
58  if (c == ESC_MAGIC) break;
59  // hal.console->printf("c=0x%02x\n", (unsigned)c);
60  }
61 
62  uint8_t nbytes=0;
63  // wait for periods
64  while (nbytes < NUM_CHANNELS*2) {
65  u.bytes[nbytes++] = read_wait();
66  }
67 
68  // and CRC
69  union {
70  uint8_t crc[2];
71  uint16_t crc16;
72  } u2;
73  u2.crc[0] = read_wait();
74  u2.crc[1] = read_wait();
75  uint16_t crc2 = crc_calculate(u.bytes, NUM_CHANNELS*2);
76  if (crc2 != u2.crc16) {
77  hal.console->printf("bad CRC 0x%04x should be 0x%04x\n", (unsigned)crc2, (unsigned)u2.crc16);
78  return;
79  }
80 
81  // and output
82  for (uint8_t i=0; i<NUM_CHANNELS; i++) {
83  if (u.period[i] == 0) {
84  continue;
85  }
86  if (!(enable_mask & 1U<<i)) {
87  if (enable_mask == 0) {
88  hal.rcout->force_safety_off();
89  }
90  hal.rcout->enable_ch(i);
91  enable_mask |= 1U<<i;
92  }
93  hal.rcout->write(i, u.period[i]);
94  }
95 
96  // report periods to console for debug
97  counter++;
98  if (counter % 100 == 0) {
99  hal.console->printf("%4u %4u %4u %4u\n",
100  (unsigned)u.period[0],
101  (unsigned)u.period[1],
102  (unsigned)u.period[2],
103  (unsigned)u.period[3]);
104  }
105 
106  // every 10th frame give an RCInput frame if possible
107  if (counter % 10 == 0) {
108  struct PACKED {
109  uint8_t magic = 0xf6;
110  uint16_t rcin[8];
111  uint16_t crc;
112  } rcin;
113  if (hal.rcin->new_input() && hal.rcin->read(rcin.rcin, 8) == 8) {
114  rcin.crc = crc_calculate((uint8_t*)&rcin.rcin[0], 16);
115  hal.UART->write((uint8_t*)&rcin, sizeof(rcin));
116  }
117  }
118 }
119 
121 
122 AP_HAL_MAIN_CALLBACKS(&rc_uart);
123 
virtual void force_safety_off(void)
Definition: RCOutput.h:98
AP_HAL::UARTDriver * console
Definition: HAL.h:110
virtual bool new_input(void)=0
uint32_t counter
Definition: RC_UART.cpp:25
#define NUM_CHANNELS
Definition: RC_UART.cpp:10
virtual void set_freq(uint32_t chmask, uint16_t freq_hz)=0
#define AP_HAL_MAIN_CALLBACKS(CALLBACKS)
Definition: AP_HAL_Main.h:39
RC_UART rc_uart
Definition: RC_UART.cpp:120
virtual uint16_t read(uint8_t ch)=0
const AP_HAL::HAL & hal
-*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
Definition: RC_UART.cpp:8
virtual void delay(uint16_t ms)=0
virtual void enable_ch(uint8_t ch)=0
virtual void printf(const char *,...) FMT_PRINTF(2
Definition: BetterStream.cpp:5
uint8_t read_wait(void)
Definition: RC_UART.cpp:36
void loop() override
Definition: RC_UART.cpp:48
virtual void write(uint8_t ch, uint16_t period_us)=0
const HAL & get_HAL()
#define RC_SPEED
Definition: RC_UART.cpp:12
virtual void delay_microseconds(uint16_t us)=0
const uint32_t baudrate
Definition: RC_UART.cpp:24
AP_HAL::RCOutput * rcout
Definition: HAL.h:113
#define PACKED
Definition: AP_Common.h:28
#define ESC_MAGIC
Definition: RC_UART.cpp:11
void setup() override
Definition: RC_UART.cpp:28
AP_HAL::RCInput * rcin
Definition: HAL.h:112
uint8_t enable_mask
Definition: RC_UART.cpp:23
AP_HAL::Scheduler * scheduler
Definition: HAL.h:114