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

AP_RCProtocol: put back DSM VTX control and logging, add tests

Browse Source
This commit is contained in:
Andy Piper 2020-08-24 21:56:49 +01:00 committed by Andrew Tridgell
parent c6e45dd536
commit 1f3c24d436
3 changed files with 118 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

72
libraries/AP_RCProtocol/AP_RCProtocol_DSM.cpp
View File

@ -17,12 +17,17 @@
/*
with thanks to PX4 dsm.c for DSM decoding approach
*/
#include <AP_Vehicle/AP_Vehicle_Type.h>
#include "AP_RCProtocol_DSM.h"
#if !APM_BUILD_TYPE(APM_BUILD_iofirmware)
#include "AP_RCProtocol_SRXL2.h"
#endif
extern const AP_HAL::HAL& hal;
// #define DSM_DEBUG
#ifdef DSM_DEBUG
#include <stdio.h>
# define debug(fmt, args...) printf(fmt "\n", ##args)
#else
# define debug(fmt, args...) do {} while(0)
@ -31,6 +36,18 @@ extern const AP_HAL::HAL& hal;
#define DSM_FRAME_SIZE 16 /**<DSM frame size in bytes*/
#define DSM_FRAME_CHANNELS 7 /**<Max supported DSM channels*/
#define SPEKTRUM_VTX_CONTROL_FRAME_MASK 0xf000f000
#define SPEKTRUM_VTX_CONTROL_FRAME 0xe000e000
#define SPEKTRUM_VTX_BAND_MASK 0x00e00000
#define SPEKTRUM_VTX_CHANNEL_MASK 0x000f0000
#define SPEKTRUM_VTX_PIT_MODE_MASK 0x00000010
#define SPEKTRUM_VTX_POWER_MASK 0x00000007
#define SPEKTRUM_VTX_BAND_SHIFT 21
#define SPEKTRUM_VTX_CHANNEL_SHIFT 16
#define SPEKTRUM_VTX_PIT_MODE_SHIFT 4
#define SPEKTRUM_VTX_POWER_SHIFT 0
void AP_RCProtocol_DSM::process_pulse(uint32_t width_s0, uint32_t width_s1)
{
@ -87,7 +104,7 @@ bool AP_RCProtocol_DSM::dsm_decode_channel(uint16_t raw, unsigned shift, unsigne
*
* @param[in] reset true=reset the 10/11 bit state to unknown
*/
void AP_RCProtocol_DSM::dsm_guess_format(bool reset, const uint8_t dsm_frame[16])
void AP_RCProtocol_DSM::dsm_guess_format(bool reset, const uint8_t dsm_frame[16], unsigned frame_channels)
{
/* reset the 10/11 bit sniffed channel masks */
if (reset) {
@ -99,18 +116,18 @@ void AP_RCProtocol_DSM::dsm_guess_format(bool reset, const uint8_t dsm_frame[16]
}
/* scan the channels in the current dsm_frame in both 10- and 11-bit mode */
for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) {
for (unsigned i = 0; i < frame_channels; i++) {
const uint8_t *dp = &dsm_frame[2 + (2 * i)];
uint16_t raw = (dp[0] << 8) | dp[1];
unsigned channel, value;
/* if the channel decodes, remember the assigned number */
if (dsm_decode_channel(raw, 10, &channel, &value) && (channel < 31)) {
if (dsm_decode_channel(raw, 10, &channel, &value) && (channel < 16)) {
cs10 |= (1 << channel);
}
if (dsm_decode_channel(raw, 11, &channel, &value) && (channel < 31)) {
if (dsm_decode_channel(raw, 11, &channel, &value) && (channel < 16)) {
cs11 |= (1 << channel);
}
@ -134,15 +151,18 @@ void AP_RCProtocol_DSM::dsm_guess_format(bool reset, const uint8_t dsm_frame[16]
* of this issue.
*/
static const uint32_t masks[] = {
0x1f, /* 5 channels (DX6 VTX frame) */
0x3f, /* 6 channels (DX6) */
0x7f, /* 7 channels (DX7) */
0xff, /* 8 channels (DX8) */
0x1ff, /* 9 channels (DX9, etc.) */
0x3ff, /* 10 channels (DX10) */
0x7ff, /* 11 channels DX8 22ms */
0xfff, /* 12 channels DX8 22ms */
0x1fff, /* 13 channels (DX10t) */
0x3fff /* 18 channels (DX10) */
0x7ff, /* 11 channels */
0xfff, /* 12 channels */
0x1fff, /* 13 channels */
0x3fff, /* 14 channels */
0x7fff, /* 15 channels */
0xffff /* 16 channels */ // the remote receiver protocol supports max 16 channels
};
unsigned votes10 = 0;
unsigned votes11 = 0;
@ -172,7 +192,7 @@ void AP_RCProtocol_DSM::dsm_guess_format(bool reset, const uint8_t dsm_frame[16]
/* call ourselves to reset our state ... we have to try again */
debug("DSM: format detect fail, 10: 0x%08x %u 11: 0x%08x %u", cs10, votes10, cs11, votes11);
dsm_guess_format(true, dsm_frame);
dsm_guess_format(true, dsm_frame, frame_channels);
}
/**
@ -187,18 +207,44 @@ bool AP_RCProtocol_DSM::dsm_decode(uint32_t frame_time_ms, const uint8_t dsm_fra
* format guessing heuristic.
*/
if (((frame_time_ms - last_frame_time_ms) > 200U) && (channel_shift != 0)) {
dsm_guess_format(true, dsm_frame);
dsm_guess_format(true, dsm_frame, DSM_FRAME_CHANNELS);
}
/* we have received something we think is a dsm_frame */
last_frame_time_ms = frame_time_ms;
// Get the VTX control bytes in a frame
uint32_t vtxControl = ((dsm_frame[DSM_FRAME_SIZE-4] << 24)
| (dsm_frame[DSM_FRAME_SIZE-3] << 16)
| (dsm_frame[DSM_FRAME_SIZE-2] << 8)
| (dsm_frame[DSM_FRAME_SIZE-1] << 0));
const bool haveVtxControl =
((vtxControl & SPEKTRUM_VTX_CONTROL_FRAME_MASK) == SPEKTRUM_VTX_CONTROL_FRAME &&
(dsm_frame[2] & 0x80) == 0);
unsigned frame_channels = DSM_FRAME_CHANNELS;
// Handle VTX control frame.
if (haveVtxControl) {
frame_channels = DSM_FRAME_CHANNELS - 2;
}
/* if we don't know the dsm_frame format, update the guessing state machine */
if (channel_shift == 0) {
dsm_guess_format(false, dsm_frame);
dsm_guess_format(false, dsm_frame, frame_channels);
return false;
}
// Handle VTX control frame.
if (haveVtxControl) {
#if !APM_BUILD_TYPE(APM_BUILD_iofirmware)
AP_RCProtocol_SRXL2::configure_vtx(
(vtxControl & SPEKTRUM_VTX_BAND_MASK) >> SPEKTRUM_VTX_BAND_SHIFT,
(vtxControl & SPEKTRUM_VTX_CHANNEL_MASK) >> SPEKTRUM_VTX_CHANNEL_SHIFT,
(vtxControl & SPEKTRUM_VTX_POWER_MASK) >> SPEKTRUM_VTX_POWER_SHIFT,
(vtxControl & SPEKTRUM_VTX_PIT_MODE_MASK) >> SPEKTRUM_VTX_PIT_MODE_SHIFT);
#endif
}
/*
* The encoding of the first two bytes is uncertain, so we're
* going to ignore them for now.
@ -210,7 +256,7 @@ bool AP_RCProtocol_DSM::dsm_decode(uint32_t frame_time_ms, const uint8_t dsm_fra
* seven channels are being transmitted.
*/
for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) {
for (unsigned i = 0; i < frame_channels; i++) {
const uint8_t *dp = &dsm_frame[2 + (2 * i)];
uint16_t raw = (dp[0] << 8) | dp[1];
@ -431,6 +477,8 @@ bool AP_RCProtocol_DSM::dsm_parse_byte(uint32_t frame_time_ms, uint8_t b, uint16
* Great, it looks like we might have a frame. Go ahead and
* decode it.
*/
log_data(AP_RCProtocol::DSM, frame_time_ms * 1000, byte_input.buf, byte_input.ofs);
decode_ret = dsm_decode(frame_time_ms, byte_input.buf, values, &chan_count, max_channels);
/* we consumed the partial frame, reset */

2
libraries/AP_RCProtocol/AP_RCProtocol_DSM.h
View File

@ -34,7 +34,7 @@ private:
void _process_byte(uint32_t timestamp_ms, uint8_t byte);
void dsm_decode();
bool dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value);
void dsm_guess_format(bool reset, const uint8_t dsm_frame[16]);
void dsm_guess_format(bool reset, const uint8_t dsm_frame[16], unsigned frame_channels);
bool dsm_parse_byte(uint32_t frame_time_ms, uint8_t b, uint16_t *values,
uint16_t *num_values, uint16_t max_channels);
bool dsm_decode(uint32_t frame_time_ms, const uint8_t dsm_frame[16],

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

@ -75,12 +75,12 @@ static bool test_byte_protocol(const char *name, uint32_t baudrate,
const uint8_t *bytes, uint8_t nbytes,
const uint16_t *values, uint8_t nvalues,
uint8_t repeats,
int8_t pause_at)
uint8_t pause_at)
{
bool ret = true;
for (uint8_t repeat=0; repeat<repeats+4; repeat++) {
for (uint8_t i=0; i<nbytes; i++) {
if (pause_at >= 0 && i == pause_at) {
if (pause_at > 0 && i > 0 && ((i % pause_at) == 0)) {
hal.scheduler->delay(10);
}
rcprot->process_byte(bytes[i], baudrate);
@ -157,13 +157,13 @@ 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,
const uint8_t *bytes, uint8_t nbytes,
const uint16_t *values, uint8_t nvalues,
uint8_t repeats, int8_t pause_at)
uint8_t repeats, uint8_t pause_at)
{
bool ret = true;
for (uint8_t repeat=0; repeat<repeats+4; repeat++) {
send_pause(1, baudrate, 6000);
for (uint8_t i=0; i<nbytes; i++) {
if (pause_at >= 0 && i == pause_at) {
if (pause_at > 0 && i > 0 && ((i % pause_at) == 0)) {
send_pause(1, baudrate, 10000);
}
send_byte(bytes[i], baudrate);
@ -248,6 +248,55 @@ void loop()
const uint16_t dsm_output5[] = {1498, 1496, 999, 1497, 1901, 1901, 1099};
// DSMX 22ms D6G3 and SPM4648 autobound
const uint8_t dsmx22ms_bytes[] = {
0x00, 0xB2, 0x0C, 0x00, 0x29, 0x56, 0x14, 0x00,
0x25, 0xF8, 0x34, 0x00, 0x54, 0x00, 0xFF, 0xFF,
0x00, 0xB2, 0x81, 0x50, 0x3C, 0x00, 0x1B, 0xFD,
0x44, 0x00, 0x4C, 0x00, 0x5C, 0x00, 0xFF, 0xFF
};
const uint16_t dsmx22ms_output[] = {
1500, 1500, 1096, 1499, 1796, 1099, 1500, 1500, 1500, 1500, 1500, 1500
};
// DSMX 22ms D6G3 and SPM4648 autobound VTX frame Ch1, B1, Pw25, Race
const uint8_t dsmx22ms_vtx_bytes[] = {
// two normal frames to satisfy the format guesser
0x00, 0xB2, 0x0C, 0x00, 0x29, 0x56, 0x14, 0x00,
0x25, 0xF8, 0x34, 0x00, 0x54, 0x00, 0xFF, 0xFF,
0x00, 0xB2, 0x81, 0x50, 0x3C, 0x00, 0x1B, 0xFD,
0x44, 0x00, 0x4C, 0x00, 0x5C, 0x00, 0xFF, 0xFF,
// This is channels 1, 5, 2, 4, 6
0x00, 0xB2, 0x0C, 0x00, 0x29, 0x56, 0x14, 0x00,
0x25, 0xF8, 0x34, 0x00, 0xE0, 0x00, 0xE0, 0x0A
};
const uint16_t dsmx22ms_vtx_output[] = {
1500, 1500, 1096, 1499, 1796, 1099, 1500, 1500, 1500, 1500, 1500, 1500
};
// DSMX 11ms D6G3 and SPM4648 autobound
const uint8_t dsmx11ms_bytes[] = {
0x01, 0xB2, 0x0C, 0x00, 0x29, 0x56, 0x14, 0x00,
0x1B, 0xFC, 0x25, 0xF8, 0x44, 0x00, 0x4C, 0x00,
0x01, 0xB2, 0x8C, 0x00, 0x29, 0x56, 0x14, 0x00,
0x1B, 0xFC, 0x01, 0x50, 0x3C, 0x00, 0x34, 0x00
};
const uint16_t dsmx11ms_output[] = {
1500, 1500, 1096, 1498, 1796, 1099, 1500, 1500, 1500, 1500
};
// DSMX 11ms D6G3 and SPM4648 autobound VTX frame Ch1, B1, Pw25, Race
const uint8_t dsmx11ms_vtx_bytes[] = {
0x01, 0xB2, 0x0C, 0x00, 0x29, 0x56, 0x14, 0x00,
0x1B, 0xFD, 0x25, 0xF8, 0x44, 0x00, 0x4C, 0x00,
0x01, 0xB2, 0x8C, 0x00, 0x29, 0x56, 0x14, 0x00,
0x1B, 0xFD, 0x01, 0x50, 0x3C, 0x00, 0x34, 0x00,
0x00, 0xB2, 0x0C, 0x00, 0x29, 0x56, 0x14, 0x00,
0x1B, 0xFD, 0x25, 0xF8, 0xE0, 0x00, 0xE0, 0x0A
};
const uint16_t dsmx11ms_vtx_output[] = {
1500, 1500, 1096, 1499, 1796, 1099, 1500, 1500, 1500, 1500
};
const uint8_t sumd_bytes[] = {0xA8, 0x01, 0x08, 0x2F, 0x50, 0x31, 0xE8, 0x21, 0xA0,
0x2F, 0x50, 0x22, 0x60, 0x22, 0x60, 0x2E, 0xE0, 0x2E,
0xE0, 0x87, 0xC6};
@ -282,6 +331,10 @@ void loop()
test_protocol("DSM3", 115200, dsm_bytes3, sizeof(dsm_bytes3), dsm_output3, ARRAY_SIZE(dsm_output3), 9, 16);
test_protocol("DSM4", 115200, dsm_bytes4, sizeof(dsm_bytes4), dsm_output4, ARRAY_SIZE(dsm_output4), 9, 16);
test_protocol("DSM5", 115200, dsm_bytes5, sizeof(dsm_bytes5), dsm_output5, ARRAY_SIZE(dsm_output5), 9);
test_protocol("DSMX22", 115200, dsmx22ms_bytes, sizeof(dsmx22ms_bytes), dsmx22ms_output, ARRAY_SIZE(dsmx22ms_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_VTX", 115200, dsmx11ms_vtx_bytes, sizeof(dsmx11ms_vtx_bytes), dsmx11ms_vtx_output, ARRAY_SIZE(dsmx11ms_vtx_output), 9, 16);
}
AP_HAL_MAIN();