Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_RCProtocol: added FPort2 protocol test

Browse Source
This commit is contained in:
Andrew Tridgell 2020-10-26 17:33:39 +11:00
parent a8770a5a82
commit df5105e20b
1 changed files with 29 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
libraries/AP_RCProtocol/examples/RCProtocolTest/RCProtocolTest.cpp
View File

@ -93,11 +93,11 @@ static bool test_byte_protocol(const char *name, uint32_t baudrate,
return ret; return ret;
} }
static void send_bit(uint8_t bit, uint32_t baudrate) static void send_bit(uint8_t bit, uint32_t baudrate, bool inverted)
{ {
static uint16_t bits_0, bits_1; static uint16_t bits_0, bits_1;
if (baudrate == 115200) { if (!inverted) {
// yes, this is backwards ... // inverted serial idles low
bit = !bit; bit = !bit;
} }
if (bit == 0) { if (bit == 0) {
@ -119,35 +119,35 @@ static void send_bit(uint8_t bit, uint32_t baudrate)
/* /*
call process_pulse() for a byte of input call process_pulse() for a byte of input
*/ */
static void send_byte(uint8_t b, uint32_t baudrate) static void send_byte(uint8_t b, uint32_t baudrate, bool inverted)
{ {
send_bit(0, baudrate); // start bit send_bit(0, baudrate, inverted); // start bit
uint8_t parity = 0; uint8_t parity = 0;
for (uint8_t i=0; i<8; i++) { for (uint8_t i=0; i<8; i++) {
uint8_t bit = (b & (1U<<i))?1:0; uint8_t bit = (b & (1U<<i))?1:0;
send_bit(bit, baudrate); send_bit(bit, baudrate, inverted);
if (bit) { if (bit) {
parity = !parity; parity = !parity;
} }
} }
if (baudrate == 100000) { if (baudrate == 100000) {
// assume SBUS, send parity // assume SBUS, send parity
send_bit(parity, baudrate); send_bit(parity, baudrate, inverted);
} }
send_bit(1, baudrate); // stop bit send_bit(1, baudrate, inverted); // stop bit
if (baudrate == 100000) { if (baudrate == 100000) {
send_bit(1, baudrate); // 2nd stop bit send_bit(1, baudrate, inverted); // 2nd stop bit
} }
} }
/* /*
add a gap in bits add a gap in bits
*/ */
static void send_pause(uint8_t b, uint32_t baudrate, uint32_t pause_us) static void send_pause(uint8_t b, uint32_t baudrate, uint32_t pause_us, bool inverted)
{ {
uint32_t nbits = pause_us * 1e6 / baudrate; uint32_t nbits = pause_us * 1e6 / baudrate;
for (uint32_t i=0; i<nbits; i++) { for (uint32_t i=0; i<nbits; i++) {
send_bit(b, baudrate); send_bit(b, baudrate, inverted);
} }
} }
@ -157,18 +157,19 @@ static void send_pause(uint8_t b, uint32_t baudrate, uint32_t pause_us)
static bool test_pulse_protocol(const char *name, uint32_t baudrate, static bool test_pulse_protocol(const char *name, uint32_t baudrate,
const uint8_t *bytes, uint8_t nbytes, const uint8_t *bytes, uint8_t nbytes,
const uint16_t *values, uint8_t nvalues, const uint16_t *values, uint8_t nvalues,
uint8_t repeats, uint8_t pause_at) uint8_t repeats, uint8_t pause_at,
bool inverted)
{ {
bool ret = true; bool ret = true;
for (uint8_t repeat=0; repeat<repeats+4; repeat++) { for (uint8_t repeat=0; repeat<repeats+4; repeat++) {
send_pause(1, baudrate, 6000); send_pause(1, baudrate, 6000, inverted);
for (uint8_t i=0; i<nbytes; i++) { for (uint8_t i=0; i<nbytes; i++) {
if (pause_at > 0 && i > 0 && ((i % pause_at) == 0)) { if (pause_at > 0 && i > 0 && ((i % pause_at) == 0)) {
send_pause(1, baudrate, 10000); send_pause(1, baudrate, 10000, inverted);
} }
send_byte(bytes[i], baudrate); send_byte(bytes[i], baudrate, inverted);
} }
send_pause(1, baudrate, 6000); send_pause(1, baudrate, 6000, inverted);
if (repeat > repeats) { if (repeat > repeats) {
ret &= check_result(name, false, values, nvalues); ret &= check_result(name, false, values, nvalues);
} }
@ -183,7 +184,8 @@ static bool test_protocol(const char *name, uint32_t baudrate,
const uint8_t *bytes, uint8_t nbytes, const uint8_t *bytes, uint8_t nbytes,
const uint16_t *values, uint8_t nvalues, const uint16_t *values, uint8_t nvalues,
uint8_t repeats=1, uint8_t repeats=1,
int8_t pause_at=0) int8_t pause_at=0,
bool inverted=false)
{ {
bool ret = true; bool ret = true;
rcprot = new AP_RCProtocol(); rcprot = new AP_RCProtocol();
@ -194,7 +196,7 @@ static bool test_protocol(const char *name, uint32_t baudrate,
rcprot = new AP_RCProtocol(); rcprot = new AP_RCProtocol();
rcprot->init(); rcprot->init();
ret &= test_pulse_protocol(name, baudrate, bytes, nbytes, values, nvalues, repeats, pause_at); ret &= test_pulse_protocol(name, baudrate, bytes, nbytes, values, nvalues, repeats, pause_at, inverted);
delete rcprot; delete rcprot;
return ret; return ret;
@ -316,6 +318,12 @@ void loop()
const uint8_t ibus_bytes[] = {0x20, 0x40, 0xdc, 0x05, 0xdc, 0x05, 0xe8, 0x03, 0xdc, 0x05, 0xdc, 0x05, 0xdc, 0x05, 0xdc, 0x05, 0xdc, 0x05, 0xdc, 0x05, 0xdc, 0x05, 0xdc, 0x05, 0xdc, 0x05, 0xdc, 0x05, 0xdc, 0x05, 0x47, 0xf3}; const uint8_t ibus_bytes[] = {0x20, 0x40, 0xdc, 0x05, 0xdc, 0x05, 0xe8, 0x03, 0xdc, 0x05, 0xdc, 0x05, 0xdc, 0x05, 0xdc, 0x05, 0xdc, 0x05, 0xdc, 0x05, 0xdc, 0x05, 0xdc, 0x05, 0xdc, 0x05, 0xdc, 0x05, 0xdc, 0x05, 0x47, 0xf3};
const uint16_t ibus_output[] = {1500, 1500, 1000, 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500}; const uint16_t ibus_output[] = {1500, 1500, 1000, 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500};
const uint8_t fport2_bytes[] = {0x18, 0xff, 0xac, 0x00, 0x5f, 0xf8, 0xc0, 0x07,
0x3e, 0xf0, 0x81, 0x0f, 0x7c, 0xe0, 0x03, 0x1f,
0xf8, 0xc0, 0x07, 0x3e, 0xf0, 0x81, 0x0f, 0x7c,
0x00, 0x5e, 0x98};
const uint16_t fport2_output[] = {982, 1495, 1495, 1495, 1495, 1495, 1495, 1495, 1495, 1495, 1495, 1495, 1495, 1495, 1495, 1495};
test_protocol("SRXL", 115200, srxl_bytes, sizeof(srxl_bytes), srxl_output, ARRAY_SIZE(srxl_output), 1); test_protocol("SRXL", 115200, srxl_bytes, sizeof(srxl_bytes), srxl_output, ARRAY_SIZE(srxl_output), 1);
test_protocol("SUMD", 115200, sumd_bytes, sizeof(sumd_bytes), sumd_output, ARRAY_SIZE(sumd_output), 1); test_protocol("SUMD", 115200, sumd_bytes, sizeof(sumd_bytes), sumd_output, ARRAY_SIZE(sumd_output), 1);
test_protocol("SUMD2", 115200, sumd_bytes2, sizeof(sumd_bytes2), sumd_output2, ARRAY_SIZE(sumd_output2), 1); test_protocol("SUMD2", 115200, sumd_bytes2, sizeof(sumd_bytes2), sumd_output2, ARRAY_SIZE(sumd_output2), 1);
@ -323,7 +331,7 @@ void loop()
test_protocol("IBUS", 115200, ibus_bytes, sizeof(ibus_bytes), ibus_output, ARRAY_SIZE(ibus_output), 1); test_protocol("IBUS", 115200, ibus_bytes, sizeof(ibus_bytes), ibus_output, ARRAY_SIZE(ibus_output), 1);
// SBUS needs 3 repeats to pass the RCProtocol 3 frames test // SBUS needs 3 repeats to pass the RCProtocol 3 frames test
test_protocol("SBUS", 100000, sbus_bytes, sizeof(sbus_bytes), sbus_output, ARRAY_SIZE(sbus_output), 3); test_protocol("SBUS", 100000, sbus_bytes, sizeof(sbus_bytes), sbus_output, ARRAY_SIZE(sbus_output), 3, 0, true);
// DSM needs 8 repeats, 5 to guess the format, then 3 to pass the RCProtocol 3 frames test // DSM needs 8 repeats, 5 to guess the format, then 3 to pass the RCProtocol 3 frames test
test_protocol("DSM1", 115200, dsm_bytes, sizeof(dsm_bytes), dsm_output, ARRAY_SIZE(dsm_output), 9); test_protocol("DSM1", 115200, dsm_bytes, sizeof(dsm_bytes), dsm_output, ARRAY_SIZE(dsm_output), 9);
@ -335,6 +343,8 @@ void loop()
test_protocol("DSMX22_VTX", 115200, dsmx22ms_vtx_bytes, sizeof(dsmx22ms_vtx_bytes), dsmx22ms_vtx_output, ARRAY_SIZE(dsmx22ms_vtx_output), 9, 16); test_protocol("DSMX22_VTX", 115200, dsmx22ms_vtx_bytes, sizeof(dsmx22ms_vtx_bytes), dsmx22ms_vtx_output, ARRAY_SIZE(dsmx22ms_vtx_output), 9, 16);
test_protocol("DSMX11", 115200, dsmx11ms_bytes, sizeof(dsmx11ms_bytes), dsmx11ms_output, ARRAY_SIZE(dsmx11ms_output), 9, 16); test_protocol("DSMX11", 115200, dsmx11ms_bytes, sizeof(dsmx11ms_bytes), dsmx11ms_output, ARRAY_SIZE(dsmx11ms_output), 9, 16);
test_protocol("DSMX11_VTX", 115200, dsmx11ms_vtx_bytes, sizeof(dsmx11ms_vtx_bytes), dsmx11ms_vtx_output, ARRAY_SIZE(dsmx11ms_vtx_output), 9, 16); test_protocol("DSMX11_VTX", 115200, dsmx11ms_vtx_bytes, sizeof(dsmx11ms_vtx_bytes), dsmx11ms_vtx_output, ARRAY_SIZE(dsmx11ms_vtx_output), 9, 16);
test_protocol("FPORT2", 115200, fport2_bytes, sizeof(fport2_bytes), fport2_output, ARRAY_SIZE(fport2_output), 3, 0, true);
} }
AP_HAL_MAIN(); AP_HAL_MAIN();