fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-18 10:20:35 +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
src/modules/px4iofirmware/controls.c
+25 -2
View File
@@ -43,6 +43,7 @@
#include <drivers/drv_hrt.h>
#include <systemlib/perf_counter.h>
#include <systemlib/ppm_decode.h>
#include <rc/st24.h>
#include "px4io.h"
@@ -51,11 +52,21 @@
#define RC_CHANNEL_LOW_THRESH -8000 /* 10% threshold */
static bool ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len);
static bool dsm_port_input(void);
static perf_counter_t c_gather_dsm;
static perf_counter_t c_gather_sbus;
static perf_counter_t c_gather_ppm;
static int _dsm_fd;
bool dsm_port_input()
{
/* get data from FD and attempt to parse with DSM and ST24 libs */
return false;
}
void
controls_init(void)
{
@@ -65,7 +76,7 @@ controls_init(void)
system_state.rc_channels_timestamp_valid = 0;
/* DSM input (USART1) */
dsm_init("/dev/ttyS0");
_dsm_fd = dsm_init("/dev/ttyS0");
/* S.bus input (USART3) */
sbus_init("/dev/ttyS2");
@@ -175,6 +186,18 @@ controls_tick() {
}
perf_end(c_gather_ppm);
uint8_t st24_rssi, rx_count;
uint8_t st24_maxchans = 18;
bool st24_updated = st24_decode(0, &st24_rssi, &rx_count, r_raw_rc_values, st24_maxchans);
if (st24_updated) {
rssi = st24_rssi;
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM;
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
}
/* limit number of channels to allowable data size */
if (r_raw_rc_count > PX4IO_RC_INPUT_CHANNELS)
r_raw_rc_count = PX4IO_RC_INPUT_CHANNELS;
@@ -191,7 +214,7 @@ controls_tick() {
/*
* If we received a new frame from any of the RC sources, process it.
*/
if (dsm_updated || sbus_updated || ppm_updated) {
if (dsm_updated || sbus_updated || ppm_updated || st24_updated) {
/* record a bitmask of channels assigned */
unsigned assigned_channels = 0;
src/modules/px4iofirmware/module.mk
+2 -1
View File
@@ -14,7 +14,8 @@ SRCS = adc.c \
../systemlib/mixer/mixer_group.cpp \
../systemlib/mixer/mixer_multirotor.cpp \
../systemlib/mixer/mixer_simple.cpp \
../systemlib/pwm_limit/pwm_limit.c
../systemlib/pwm_limit/pwm_limit.c \
../../lib/rc/st24.c
ifeq ($(BOARD),px4io-v1)
SRCS += i2c.c