fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 14:43:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Compile and link ST24 parser in IO firmware

Browse Source
This commit is contained in:
Lorenz Meier
2014-09-01 00:45:41 +02:00
parent 5448396579
commit 69109b6227
4 changed files with 174 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/rc/st24.c
+118 -2
View File
@@ -39,6 +39,23 @@
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
#include <stdbool.h>
#include "st24.h"
enum ST24_DECODE_STATE {
ST24_DECODE_STATE_UNSYNCED,
ST24_DECODE_STATE_GOT_STX1,
ST24_DECODE_STATE_GOT_STX2,
ST24_DECODE_STATE_GOT_LEN,
ST24_DECODE_STATE_GOT_TYPE,
ST24_DECODE_STATE_GOT_DATA
};
static enum ST24_DECODE_STATE _decode_state = ST24_DECODE_STATE_UNSYNCED;
static unsigned _rxlen;
static ReceiverFcPacket _rxpacket;
uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len)
{
uint8_t i, crc ;
@@ -66,6 +83,105 @@ uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len)
}
uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *channels, uint16_t max_chan_count) {
uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *channels, uint16_t max_chan_count)
{
}
bool ret = false;
switch (_decode_state) {
case ST24_DECODE_STATE_UNSYNCED:
if (byte == ST24_STX1) {
_decode_state = ST24_DECODE_STATE_GOT_STX1;
}
break;
case ST24_DECODE_STATE_GOT_STX1:
if (byte == ST24_STX2) {
_decode_state = ST24_DECODE_STATE_GOT_STX2;
} else {
_decode_state = ST24_DECODE_STATE_UNSYNCED;
}
break;
case ST24_DECODE_STATE_GOT_STX2:
_rxpacket.length = byte;
_rxlen = 0;
_decode_state = ST24_DECODE_STATE_GOT_LEN;
break;
case ST24_DECODE_STATE_GOT_LEN:
_rxpacket.type = byte;
_rxlen++;
_decode_state = ST24_DECODE_STATE_GOT_TYPE;
break;
case ST24_DECODE_STATE_GOT_TYPE:
if (_rxlen < (_rxpacket.length - 1)) {
_rxpacket.st24_data[_rxlen] = byte;
_rxlen++;
} else {
_decode_state = ST24_DECODE_STATE_GOT_DATA;
}
break;
case ST24_DECODE_STATE_GOT_DATA:
_rxpacket.crc8 = byte;
_rxlen++;
if (st24_common_crc8((uint8_t*)&(_rxpacket.length), sizeof(_rxpacket.length) +
sizeof(_rxpacket.st24_data) + sizeof(_rxpacket.type)) == _rxpacket.crc8) {
ret = true;
/* decode the actual packet */
switch (_rxpacket.type) {
case ST24_PACKET_TYPE_CHANNELDATA12:
{
ChannelData12* d = (ChannelData12*)&_rxpacket;
*rssi = d->rssi;
*rx_count = d->packet_count;
for (unsigned i = 0; i < 1; i += 2) {
channels[i] = ((uint16_t)d->channel[i]) << 8;
channels[i] |= (0xF & d->channel[i+1]);
channels[i+1] = ((uint16_t)(0xF & d->channel[i+1])) << 4;
channels[i+1] |= d->channel[i+2];
}
}
break;
case ST24_PACKET_TYPE_CHANNELDATA24:
{
ChannelData24* d = (ChannelData12*)&_rxpacket;
*rssi = d->rssi;
*rx_count = d->packet_count;
for (unsigned i = 0; i < 1; i += 2) {
channels[i] = ((uint16_t)d->channel[i]) << 8;
channels[i] |= (0xF & d->channel[i+1]);
channels[i+1] = ((uint16_t)(0xF & d->channel[i+1])) << 4;
channels[i+1] |= d->channel[i+2];
}
}
break;
default:
ret = false;
break;
}
} else {
/* decoding failed */
_decode_state = ST24_DECODE_STATE_UNSYNCED;
}
break;
}
return ret;
}
src/lib/rc/st24.h
+29 -19
View File
@@ -39,22 +39,30 @@
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
#define ST24_DATA_LEN_MAX 64
#pragma once
enum {
#include <stdint.h>
__BEGIN_DECLS
#define ST24_DATA_LEN_MAX 64
#define ST24_STX1 0x55
#define ST24_STX2 0x55
enum ST24_PACKET_TYPE {
ST24_PACKET_TYPE_CHANNELDATA12 = 0,
ST24_PACKET_TYPE_CHANNELDATA24,
ST24_PACKET_TYPE_TRANSMITTERGPSDATA
} ST24_PACKET_TYPE;
};
#pragma pack(push, 1)
typedef struct {
uint8_t header1; ///< 0x55 for a valid packet
uint8_t header2; ///< 0x55 for a valid packet
uint8_t header1; ///< 0x55 for a valid packet
uint8_t header2; ///< 0x55 for a valid packet
uint8_t length; ///< length includes type, data, and crc = sizeof(type)+sizeof(data[payload_len])+sizeof(crc8)
uint8_t type; ///< from enum ST24_PACKET_TYPE
uint8_t type; ///< from enum ST24_PACKET_TYPE
uint8_t st24_data[ST24_DATA_LEN_MAX];
uint8_t crc8; ///< crc8 checksum, calculated by st24_common_crc8 and including fields length, type and st24_data
uint8_t crc8; ///< crc8 checksum, calculated by st24_common_crc8 and including fields length, type and st24_data
} ReceiverFcPacket;
/**
@@ -63,8 +71,8 @@ typedef struct {
* This is incoming from the ST24
*/
typedef struct {
uint16_t t; ///< packet counter or clock
uint8_t rssi ///< signal strength
uint16_t t; ///< packet counter or clock
uint8_t rssi; ///< signal strength
uint8_t packet_count; ///< Number of UART packets sent since reception of last RF frame (this tells something about age / rate)
uint8_t channel[18]; ///< channel data, 12 channels (12 bit numbers)
} ChannelData12;
@@ -74,8 +82,8 @@ typedef struct {
*
*/
typedef struct {
uint16_t t; ///< packet counter or clock
uint8_t rssi ///< signal strength
uint16_t t; ///< packet counter or clock
uint8_t rssi; ///< signal strength
uint8_t packet_count; ///< Number of UART packets sent since reception of last RF frame (this tells something about age / rate)
uint8_t channel[36]; ///< channel data, 24 channels (12 bit numbers)
} ChannelData24;
@@ -114,17 +122,17 @@ typedef struct {
*
*/
typedef struct {
uint16_t t; ///< packet counter or clock
int32_t lat; ///< lattitude (degrees) +/- 90 deg
int32_t lon; ///< longitude (degrees) +/- 180 deg
int32_t alt; ///< 0.01m resolution, altitude (meters)
uint16_t t; ///< packet counter or clock
int32_t lat; ///< lattitude (degrees) +/- 90 deg
int32_t lon; ///< longitude (degrees) +/- 180 deg
int32_t alt; ///< 0.01m resolution, altitude (meters)
int16_t vx, vy, vz; ///< velocity 0.01m res, +/-320.00 North-East- Down
uint8_t nsat; ///<number of satellites
uint8_t voltage; ///< 25.4V voltage = 5 + 255*0.1 = 30.5V, min=5V
uint8_t current; ///< 0.5A resolution
uint8_t nsat; ///<number of satellites
uint8_t voltage; ///< 25.4V voltage = 5 + 255*0.1 = 30.5V, min=5V
uint8_t current; ///< 0.5A resolution
int16_t roll, pitch, yaw; ///< 0.01 degree resolution
uint8_t motorStatus; ///< 1 bit per motor for status 1=good, 0= fail
uint8_t imuStatus; ///< inertial measurement unit status
uint8_t imuStatus; ///< inertial measurement unit status
uint8_t pressCompassStatus; ///< baro / compass status
} TelemetryData;
@@ -150,3 +158,5 @@ uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len);
* @return 0 for success (a decoded packet), 1 for no packet yet (accumulating), 3 for out of sync, 4 for checksum error
*/
__EXPORT uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *channels, uint16_t max_chan_count);
__END_DECLS