mirror of https://github.com/ArduPilot/ardupilot
126 lines
3.0 KiB
C++
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);
|
|
|