zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Move the common definitions for the PX4IO serial protocol into the shared header.

sbg
px4dev 12 years ago
parent
19b2e1de85
commit
87a4f1507a
3 changed files with 82 additions and 156 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 81
      src/drivers/px4io/interface_serial.cpp
  2. 78
      src/modules/px4iofirmware/protocol.h
  3. 79
      src/modules/px4iofirmware/serial.c

81
src/drivers/px4io/interface_serial.cpp
Unescape View File

@ -69,18 +69,6 @@ @@ -69,18 +69,6 @@
#include "interface.h"
#define PKT_MAX_REGS 32 // by agreement w/IO
#pragma pack(push, 1)
struct IOPacket {
uint8_t count_code;
uint8_t crc;
uint8_t page;
uint8_t offset;
uint16_t regs[PKT_MAX_REGS];
};
#pragma pack(pop)
/* serial register accessors */
#define REG(_x) (*(volatile uint32_t *)(PX4IO_SERIAL_BASE + _x))
#define rSR REG(STM32_USART_SR_OFFSET)
@ -91,22 +79,6 @@ struct IOPacket { @@ -91,22 +79,6 @@ struct IOPacket {
#define rCR3 REG(STM32_USART_CR3_OFFSET)
#define rGTPR REG(STM32_USART_GTPR_OFFSET)
#define PKT_CODE_READ 0x00 /* FMU->IO read transaction */
#define PKT_CODE_WRITE 0x40 /* FMU->IO write transaction */
#define PKT_CODE_SUCCESS 0x00 /* IO->FMU success reply */
#define PKT_CODE_CORRUPT 0x40 /* IO->FMU bad packet reply */
#define PKT_CODE_ERROR 0x80 /* IO->FMU register op error reply */
#define PKT_CODE_MASK 0xc0
#define PKT_COUNT_MASK 0x3f
#define PKT_COUNT(_p) ((_p).count_code & PKT_COUNT_MASK)
#define PKT_CODE(_p) ((_p).count_code & PKT_CODE_MASK)
#define PKT_SIZE(_p) ((uint8_t *)&((_p).regs[PKT_COUNT(_p)]) - ((uint8_t *)&(_p)))
static uint8_t crc_packet(IOPacket &pkt);
class PX4IO_serial : public PX4IO_interface
{
public:
@ -496,7 +468,7 @@ PX4IO_serial::_wait_complete() @@ -496,7 +468,7 @@ PX4IO_serial::_wait_complete()
/* start TX DMA - no callback if we also expect a reply */
/* DMA setup time ~3µs */
_dma_buffer.crc = 0;
_dma_buffer.crc = crc_packet(_dma_buffer);
_dma_buffer.crc = crc_packet(&_dma_buffer);
stm32_dmasetup(
_tx_dma,
PX4IO_SERIAL_BASE + STM32_USART_DR_OFFSET,
@ -543,7 +515,7 @@ PX4IO_serial::_wait_complete() @@ -543,7 +515,7 @@ PX4IO_serial::_wait_complete()
/* check packet CRC - corrupt packet errors mean IO receive CRC error */
uint8_t crc = _dma_buffer.crc;
_dma_buffer.crc = 0;
if ((crc != crc_packet(_dma_buffer)) | (PKT_CODE(_dma_buffer) == PKT_CODE_CORRUPT)) {
if ((crc != crc_packet(&_dma_buffer)) | (PKT_CODE(_dma_buffer) == PKT_CODE_CORRUPT)) {
perf_count(_pc_crcerrs);
ret = -1;
errno = EIO;
@ -683,52 +655,3 @@ PX4IO_serial::_abort_dma() @@ -683,52 +655,3 @@ PX4IO_serial::_abort_dma()
stm32_dmastop(_tx_dma);
stm32_dmastop(_rx_dma);
}
static const uint8_t crc8_tab[256] =
{
0x00, 0x07, 0x0E, 0x09, 0x1C, 0x1B, 0x12, 0x15,
0x38, 0x3F, 0x36, 0x31, 0x24, 0x23, 0x2A, 0x2D,
0x70, 0x77, 0x7E, 0x79, 0x6C, 0x6B, 0x62, 0x65,
0x48, 0x4F, 0x46, 0x41, 0x54, 0x53, 0x5A, 0x5D,
0xE0, 0xE7, 0xEE, 0xE9, 0xFC, 0xFB, 0xF2, 0xF5,
0xD8, 0xDF, 0xD6, 0xD1, 0xC4, 0xC3, 0xCA, 0xCD,
0x90, 0x97, 0x9E, 0x99, 0x8C, 0x8B, 0x82, 0x85,
0xA8, 0xAF, 0xA6, 0xA1, 0xB4, 0xB3, 0xBA, 0xBD,
0xC7, 0xC0, 0xC9, 0xCE, 0xDB, 0xDC, 0xD5, 0xD2,
0xFF, 0xF8, 0xF1, 0xF6, 0xE3, 0xE4, 0xED, 0xEA,
0xB7, 0xB0, 0xB9, 0xBE, 0xAB, 0xAC, 0xA5, 0xA2,
0x8F, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9D, 0x9A,
0x27, 0x20, 0x29, 0x2E, 0x3B, 0x3C, 0x35, 0x32,
0x1F, 0x18, 0x11, 0x16, 0x03, 0x04, 0x0D, 0x0A,
0x57, 0x50, 0x59, 0x5E, 0x4B, 0x4C, 0x45, 0x42,
0x6F, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7D, 0x7A,
0x89, 0x8E, 0x87, 0x80, 0x95, 0x92, 0x9B, 0x9C,
0xB1, 0xB6, 0xBF, 0xB8, 0xAD, 0xAA, 0xA3, 0xA4,
0xF9, 0xFE, 0xF7, 0xF0, 0xE5, 0xE2, 0xEB, 0xEC,
0xC1, 0xC6, 0xCF, 0xC8, 0xDD, 0xDA, 0xD3, 0xD4,
0x69, 0x6E, 0x67, 0x60, 0x75, 0x72, 0x7B, 0x7C,
0x51, 0x56, 0x5F, 0x58, 0x4D, 0x4A, 0x43, 0x44,
0x19, 0x1E, 0x17, 0x10, 0x05, 0x02, 0x0B, 0x0C,
0x21, 0x26, 0x2F, 0x28, 0x3D, 0x3A, 0x33, 0x34,
0x4E, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5C, 0x5B,
0x76, 0x71, 0x78, 0x7F, 0x6A, 0x6D, 0x64, 0x63,
0x3E, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2C, 0x2B,
0x06, 0x01, 0x08, 0x0F, 0x1A, 0x1D, 0x14, 0x13,
0xAE, 0xA9, 0xA0, 0xA7, 0xB2, 0xB5, 0xBC, 0xBB,
0x96, 0x91, 0x98, 0x9F, 0x8A, 0x8D, 0x84, 0x83,
0xDE, 0xD9, 0xD0, 0xD7, 0xC2, 0xC5, 0xCC, 0xCB,
0xE6, 0xE1, 0xE8, 0xEF, 0xFA, 0xFD, 0xF4, 0xF3
};
static uint8_t
crc_packet(IOPacket &pkt)
{
uint8_t *end = (uint8_t *)(&pkt.regs[PKT_COUNT(pkt)]);
uint8_t *p = (uint8_t *)&pkt;
uint8_t c = 0;
while (p < end)
c = crc8_tab[c ^ *(p++)];
return c;
}

78
src/modules/px4iofirmware/protocol.h
Unescape View File

@ -206,3 +206,81 @@ struct px4io_mixdata { @@ -206,3 +206,81 @@ struct px4io_mixdata {
};
#pragma pack(pop)
/**
* Serial protocol encapsulation.
*/
#define PKT_MAX_REGS 32 // by agreement w/FMU
#pragma pack(push, 1)
struct IOPacket {
uint8_t count_code;
uint8_t crc;
uint8_t page;
uint8_t offset;
uint16_t regs[PKT_MAX_REGS];
};
#pragma pack(pop)
#define PKT_CODE_READ 0x00 /* FMU->IO read transaction */
#define PKT_CODE_WRITE 0x40 /* FMU->IO write transaction */
#define PKT_CODE_SUCCESS 0x00 /* IO->FMU success reply */
#define PKT_CODE_CORRUPT 0x40 /* IO->FMU bad packet reply */
#define PKT_CODE_ERROR 0x80 /* IO->FMU register op error reply */
#define PKT_CODE_MASK 0xc0
#define PKT_COUNT_MASK 0x3f
#define PKT_COUNT(_p) ((_p).count_code & PKT_COUNT_MASK)
#define PKT_CODE(_p) ((_p).count_code & PKT_CODE_MASK)
#define PKT_SIZE(_p) ((uint8_t *)&((_p).regs[PKT_COUNT(_p)]) - ((uint8_t *)&(_p)))
static const uint8_t crc8_tab[256] __attribute__((unused)) =
{
0x00, 0x07, 0x0E, 0x09, 0x1C, 0x1B, 0x12, 0x15,
0x38, 0x3F, 0x36, 0x31, 0x24, 0x23, 0x2A, 0x2D,
0x70, 0x77, 0x7E, 0x79, 0x6C, 0x6B, 0x62, 0x65,
0x48, 0x4F, 0x46, 0x41, 0x54, 0x53, 0x5A, 0x5D,
0xE0, 0xE7, 0xEE, 0xE9, 0xFC, 0xFB, 0xF2, 0xF5,
0xD8, 0xDF, 0xD6, 0xD1, 0xC4, 0xC3, 0xCA, 0xCD,
0x90, 0x97, 0x9E, 0x99, 0x8C, 0x8B, 0x82, 0x85,
0xA8, 0xAF, 0xA6, 0xA1, 0xB4, 0xB3, 0xBA, 0xBD,
0xC7, 0xC0, 0xC9, 0xCE, 0xDB, 0xDC, 0xD5, 0xD2,
0xFF, 0xF8, 0xF1, 0xF6, 0xE3, 0xE4, 0xED, 0xEA,
0xB7, 0xB0, 0xB9, 0xBE, 0xAB, 0xAC, 0xA5, 0xA2,
0x8F, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9D, 0x9A,
0x27, 0x20, 0x29, 0x2E, 0x3B, 0x3C, 0x35, 0x32,
0x1F, 0x18, 0x11, 0x16, 0x03, 0x04, 0x0D, 0x0A,
0x57, 0x50, 0x59, 0x5E, 0x4B, 0x4C, 0x45, 0x42,
0x6F, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7D, 0x7A,
0x89, 0x8E, 0x87, 0x80, 0x95, 0x92, 0x9B, 0x9C,
0xB1, 0xB6, 0xBF, 0xB8, 0xAD, 0xAA, 0xA3, 0xA4,
0xF9, 0xFE, 0xF7, 0xF0, 0xE5, 0xE2, 0xEB, 0xEC,
0xC1, 0xC6, 0xCF, 0xC8, 0xDD, 0xDA, 0xD3, 0xD4,
0x69, 0x6E, 0x67, 0x60, 0x75, 0x72, 0x7B, 0x7C,
0x51, 0x56, 0x5F, 0x58, 0x4D, 0x4A, 0x43, 0x44,
0x19, 0x1E, 0x17, 0x10, 0x05, 0x02, 0x0B, 0x0C,
0x21, 0x26, 0x2F, 0x28, 0x3D, 0x3A, 0x33, 0x34,
0x4E, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5C, 0x5B,
0x76, 0x71, 0x78, 0x7F, 0x6A, 0x6D, 0x64, 0x63,
0x3E, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2C, 0x2B,
0x06, 0x01, 0x08, 0x0F, 0x1A, 0x1D, 0x14, 0x13,
0xAE, 0xA9, 0xA0, 0xA7, 0xB2, 0xB5, 0xBC, 0xBB,
0x96, 0x91, 0x98, 0x9F, 0x8A, 0x8D, 0x84, 0x83,
0xDE, 0xD9, 0xD0, 0xD7, 0xC2, 0xC5, 0xCC, 0xCB,
0xE6, 0xE1, 0xE8, 0xEF, 0xFA, 0xFD, 0xF4, 0xF3
};
static uint8_t crc_packet(struct IOPacket *pkt) __attribute__((unused));
static uint8_t
crc_packet(struct IOPacket *pkt)
{
uint8_t *end = (uint8_t *)(&pkt->regs[PKT_COUNT(*pkt)]);
uint8_t *p = (uint8_t *)pkt;
uint8_t c = 0;
while (p < end)
c = crc8_tab[c ^ *(p++)];
return c;
}

79
src/modules/px4iofirmware/serial.c
Unescape View File

@ -77,33 +77,6 @@ static void dma_reset(void); @@ -77,33 +77,6 @@ static void dma_reset(void);
/* if we spend this many ticks idle, reset the DMA */
static unsigned idle_ticks;
#define PKT_MAX_REGS 32 // by agreement w/FMU
#pragma pack(push, 1)
struct IOPacket {
uint8_t count_code;
uint8_t crc;
uint8_t page;
uint8_t offset;
uint16_t regs[PKT_MAX_REGS];
};
#pragma pack(pop)
#define PKT_CODE_READ 0x00 /* FMU->IO read transaction */
#define PKT_CODE_WRITE 0x40 /* FMU->IO write transaction */
#define PKT_CODE_SUCCESS 0x00 /* IO->FMU success reply */
#define PKT_CODE_CORRUPT 0x40 /* IO->FMU bad packet reply */
#define PKT_CODE_ERROR 0x80 /* IO->FMU register op error reply */
#define PKT_CODE_MASK 0xc0
#define PKT_COUNT_MASK 0x3f
#define PKT_COUNT(_p) ((_p).count_code & PKT_COUNT_MASK)
#define PKT_CODE(_p) ((_p).count_code & PKT_CODE_MASK)
#define PKT_SIZE(_p) ((uint8_t *)&((_p).regs[PKT_COUNT(_p)]) - ((uint8_t *)&(_p)))
static uint8_t crc_packet(void);
static struct IOPacket dma_packet;
/* serial register accessors */
@ -193,7 +166,7 @@ rx_handle_packet(void) @@ -193,7 +166,7 @@ rx_handle_packet(void)
/* check packet CRC */
uint8_t crc = dma_packet.crc;
dma_packet.crc = 0;
if (crc != crc_packet()) {
if (crc != crc_packet(&dma_packet)) {
perf_count(pc_crcerr);
/* send a CRC error reply */
@ -264,7 +237,7 @@ rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) @@ -264,7 +237,7 @@ rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg)
/* send the reply to the previous request */
dma_packet.crc = 0;
dma_packet.crc = crc_packet();
dma_packet.crc = crc_packet(&dma_packet);
stm32_dmasetup(
tx_dma,
(uint32_t)&rDR,
@ -354,51 +327,3 @@ dma_reset(void) @@ -354,51 +327,3 @@ dma_reset(void)
rCR3 |= USART_CR3_DMAR;
}
static const uint8_t crc8_tab[256] =
{
0x00, 0x07, 0x0E, 0x09, 0x1C, 0x1B, 0x12, 0x15,
0x38, 0x3F, 0x36, 0x31, 0x24, 0x23, 0x2A, 0x2D,
0x70, 0x77, 0x7E, 0x79, 0x6C, 0x6B, 0x62, 0x65,
0x48, 0x4F, 0x46, 0x41, 0x54, 0x53, 0x5A, 0x5D,
0xE0, 0xE7, 0xEE, 0xE9, 0xFC, 0xFB, 0xF2, 0xF5,
0xD8, 0xDF, 0xD6, 0xD1, 0xC4, 0xC3, 0xCA, 0xCD,
0x90, 0x97, 0x9E, 0x99, 0x8C, 0x8B, 0x82, 0x85,
0xA8, 0xAF, 0xA6, 0xA1, 0xB4, 0xB3, 0xBA, 0xBD,
0xC7, 0xC0, 0xC9, 0xCE, 0xDB, 0xDC, 0xD5, 0xD2,
0xFF, 0xF8, 0xF1, 0xF6, 0xE3, 0xE4, 0xED, 0xEA,
0xB7, 0xB0, 0xB9, 0xBE, 0xAB, 0xAC, 0xA5, 0xA2,
0x8F, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9D, 0x9A,
0x27, 0x20, 0x29, 0x2E, 0x3B, 0x3C, 0x35, 0x32,
0x1F, 0x18, 0x11, 0x16, 0x03, 0x04, 0x0D, 0x0A,
0x57, 0x50, 0x59, 0x5E, 0x4B, 0x4C, 0x45, 0x42,
0x6F, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7D, 0x7A,
0x89, 0x8E, 0x87, 0x80, 0x95, 0x92, 0x9B, 0x9C,
0xB1, 0xB6, 0xBF, 0xB8, 0xAD, 0xAA, 0xA3, 0xA4,
0xF9, 0xFE, 0xF7, 0xF0, 0xE5, 0xE2, 0xEB, 0xEC,
0xC1, 0xC6, 0xCF, 0xC8, 0xDD, 0xDA, 0xD3, 0xD4,
0x69, 0x6E, 0x67, 0x60, 0x75, 0x72, 0x7B, 0x7C,
0x51, 0x56, 0x5F, 0x58, 0x4D, 0x4A, 0x43, 0x44,
0x19, 0x1E, 0x17, 0x10, 0x05, 0x02, 0x0B, 0x0C,
0x21, 0x26, 0x2F, 0x28, 0x3D, 0x3A, 0x33, 0x34,
0x4E, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5C, 0x5B,
0x76, 0x71, 0x78, 0x7F, 0x6A, 0x6D, 0x64, 0x63,
0x3E, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2C, 0x2B,
0x06, 0x01, 0x08, 0x0F, 0x1A, 0x1D, 0x14, 0x13,
0xAE, 0xA9, 0xA0, 0xA7, 0xB2, 0xB5, 0xBC, 0xBB,
0x96, 0x91, 0x98, 0x9F, 0x8A, 0x8D, 0x84, 0x83,
0xDE, 0xD9, 0xD0, 0xD7, 0xC2, 0xC5, 0xCC, 0xCB,
0xE6, 0xE1, 0xE8, 0xEF, 0xFA, 0xFD, 0xF4, 0xF3
};
static uint8_t
crc_packet()
{
uint8_t *end = (uint8_t *)(&dma_packet.regs[PKT_COUNT(dma_packet)]);
uint8_t *p = (uint8_t *)&dma_packet;
uint8_t c = 0;
while (p < end)
c = crc8_tab[c ^ *(p++)];
return c;
}

Write Preview
Loading…
Cancel
Save