ardupilot/libraries/RC_Channel/examples/RC_UART/RC_UART.cpp

126 lines
3.0 KiB
C++

/*
take RC channels in from UART and put out as PWM
*/
#include <AP_HAL/AP_HAL.h>
#include <GCS_MAVLink/include/mavlink/v2.0/checksum.h>
const AP_HAL::HAL& hal = AP_HAL::get_HAL();
#define NUM_CHANNELS 4
#define ESC_MAGIC 0xF7
#define RC_SPEED 490
#define UART serial(4)
class RC_UART : public AP_HAL::HAL::Callbacks {
public:
// HAL::Callbacks implementation.
void setup() override;
void loop() override;
private:
uint8_t read_wait(void);
uint8_t enable_mask;
const uint32_t baudrate = 115200;
uint32_t counter;
};
void RC_UART::setup()
{
hal.scheduler->delay(1000);
hal.console->printf("RC_UART starting\n");
hal.UART->begin(baudrate, 512, 512);
hal.rcout->set_freq(0xFF, RC_SPEED);
}
uint8_t RC_UART::read_wait(void)
{
while (true) {
int16_t c = hal.UART->read();
if (c != -1) {
// hal.console->printf("c=0x%02x\n", (unsigned)c);
return c;
}
hal.scheduler->delay_microseconds(100);
}
}
void RC_UART::loop()
{
union {
uint16_t period[NUM_CHANNELS];
uint8_t bytes[NUM_CHANNELS*2];
} u;
// wait for magic
while (true) {
uint8_t c = read_wait();
if (c == ESC_MAGIC) break;
// hal.console->printf("c=0x%02x\n", (unsigned)c);
}
uint8_t nbytes=0;
// wait for periods
while (nbytes < NUM_CHANNELS*2) {
u.bytes[nbytes++] = read_wait();
}
// and CRC
union {
uint8_t crc[2];
uint16_t crc16;
} u2;
u2.crc[0] = read_wait();
u2.crc[1] = read_wait();
uint16_t crc2 = crc_calculate(u.bytes, NUM_CHANNELS*2);
if (crc2 != u2.crc16) {
hal.console->printf("bad CRC 0x%04x should be 0x%04x\n", (unsigned)crc2, (unsigned)u2.crc16);
return;
}
// and output
for (uint8_t i=0; i<NUM_CHANNELS; i++) {
if (u.period[i] == 0) {
continue;
}
if (!(enable_mask & 1U<<i)) {
if (enable_mask == 0) {
hal.rcout->force_safety_off();
}
hal.rcout->enable_ch(i);
enable_mask |= 1U<<i;
}
hal.rcout->write(i, u.period[i]);
}
// report periods to console for debug
counter++;
if (counter % 100 == 0) {
hal.console->printf("%4u %4u %4u %4u\n",
(unsigned)u.period[0],
(unsigned)u.period[1],
(unsigned)u.period[2],
(unsigned)u.period[3]);
}
// every 10th frame give an RCInput frame if possible
if (counter % 10 == 0) {
struct PACKED {
uint8_t magic = 0xf6;
uint16_t rcin[8];
uint16_t crc;
} rcin;
uint16_t rcval[8];
if (hal.rcin->new_input() && hal.rcin->read(rcval, 8) == 8) {
memcpy(rcin.rcin, rcval, sizeof(rcval));
rcin.crc = crc_calculate((uint8_t*)&rcin.rcin[0], 16);
hal.UART->write((uint8_t*)&rcin, sizeof(rcin));
}
}
}
RC_UART rc_uart;
AP_HAL_MAIN_CALLBACKS(&rc_uart);