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

串口驱动编译通过

tf_driver-1.12
zbr3550 3 years ago
parent
51c82baccf
commit
0c601dca67
4 changed files with 172 additions and 91 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 4
      src/drivers/distance_sensor/rds02uf/rds02uf.cpp
  2. 2
      src/drivers/distance_sensor/rds02uf/rds02uf.hpp
  3. 185
      src/drivers/distance_sensor/rds02uf/rds02uf_parser.cpp
  4. 72
      src/drivers/distance_sensor/rds02uf/rds02uf_parser.h

4
src/drivers/distance_sensor/rds02uf/rds02uf.cpp
Unescape View File

@ -230,7 +230,7 @@ void
Rds02uf::start() Rds02uf::start()
{ {
// schedule a cycle to start things (the sensor sends at 100Hz, but we run a bit faster to avoid missing data) // schedule a cycle to start things (the sensor sends at 100Hz, but we run a bit faster to avoid missing data)
ScheduleOnInterval(7_ms); ScheduleOnInterval(50_ms);
} }
void void
@ -252,7 +252,7 @@ Rds02uf::Run()
if (collect() == -EAGAIN) { if (collect() == -EAGAIN) {
// reschedule to grab the missing bits, time to transmit 9 bytes @ 115200 bps // reschedule to grab the missing bits, time to transmit 9 bytes @ 115200 bps
ScheduleClear(); ScheduleClear();
ScheduleOnInterval(7_ms, 87 * 9); ScheduleOnInterval(50_ms, 87 * 12);
return; return;
} }
} }

2
src/drivers/distance_sensor/rds02uf/rds02uf.hpp
Unescape View File

@ -77,7 +77,7 @@ private:
PX4Rangefinder _px4_rangefinder; PX4Rangefinder _px4_rangefinder;
RDS02UF_PARSE_STATE _parse_state {RDS02UF_PARSE_STATE::STATE0_UNSYNC}; RDS02UF_PARSE_STATE _parse_state {RDS02UF_PARSE_STATE::STATE1_SYNC_1};
char _linebuf[10] {}; char _linebuf[10] {};
char _port[20] {}; char _port[20] {};

185
src/drivers/distance_sensor/rds02uf/rds02uf_parser.cpp
Unescape View File

@ -42,112 +42,124 @@
#include <string.h> #include <string.h>
#include <stdlib.h> #include <stdlib.h>
uint16_t data_len;
uint16_t data_index;
uint8_t crc_data;
uint16_t fc_code;
uint8_t err_code;
int rds02uf_parse(char c, char *parserbuf, unsigned *parserbuf_index, RDS02UF_PARSE_STATE *state, float *dist) int rds02uf_parse(char c, char *parserbuf, unsigned *parserbuf_index, RDS02UF_PARSE_STATE *state, float *dist)
{ {
int ret = -1; int ret = -1;
char data = c;
//char *end; //char *end;
switch (*state) { switch (*state) {
case RDS02UF_PARSE_STATE::STATE6_GOT_CHECKSUM: case RDS02UF_PARSE_STATE::STATE1_SYNC_1:
if (c == 'Y') { if (data == RDS02_HEAD1)
*state = RDS02UF_PARSE_STATE::STATE1_SYNC_1; {
parserbuf[*parserbuf_index] = c; parserbuf[*parserbuf_index] = data;
(*parserbuf_index)++; (*parserbuf_index)++;
*state = RDS02UF_PARSE_STATE::STATE2_SYNC_2;
} else {
*state = RDS02UF_PARSE_STATE::STATE0_UNSYNC;
} }
break; break;
case RDS02UF_PARSE_STATE::STATE2_SYNC_2:
case RDS02UF_PARSE_STATE::STATE0_UNSYNC: if (data == RDS02_HEAD2)
if (c == 'Y') { {
*state = RDS02UF_PARSE_STATE::STATE1_SYNC_1; parserbuf[*parserbuf_index] = data;
parserbuf[*parserbuf_index] = c;
(*parserbuf_index)++; (*parserbuf_index)++;
*state = RDS02UF_PARSE_STATE::STATE3_ADDRESS;
} }
else
break; {
ParseDataFailed(parserbuf, sizeof(parserbuf));
case RDS02UF_PARSE_STATE::STATE1_SYNC_1:
if (c == 'Y') {
*state = RDS02UF_PARSE_STATE::STATE1_SYNC_2;
parserbuf[*parserbuf_index] = c;
(*parserbuf_index)++;
} else {
*state = RDS02UF_PARSE_STATE::STATE0_UNSYNC;
*parserbuf_index = 0;
} }
break; break;
case RDS02UF_PARSE_STATE::STATE3_ADDRESS: // address
case RDS02UF_PARSE_STATE::STATE1_SYNC_2: parserbuf[*parserbuf_index] = data;
*state = RDS02UF_PARSE_STATE::STATE2_GOT_DIST_L;
parserbuf[*parserbuf_index] = c;
(*parserbuf_index)++; (*parserbuf_index)++;
*state = RDS02UF_PARSE_STATE::STATE4_ERROR_CODE;
break; break;
case RDS02UF_PARSE_STATE::STATE4_ERROR_CODE: // error_code
case RDS02UF_PARSE_STATE::STATE2_GOT_DIST_L: parserbuf[*parserbuf_index] = data;
*state = RDS02UF_PARSE_STATE::STATE2_GOT_DIST_H; err_code = data;
parserbuf[*parserbuf_index] = c;
(*parserbuf_index)++; (*parserbuf_index)++;
*state = RDS02UF_PARSE_STATE::STATE5_FC_CODE_L;
break; break;
case RDS02UF_PARSE_STATE::STATE5_FC_CODE_L: // fc_code low
case RDS02UF_PARSE_STATE::STATE2_GOT_DIST_H: parserbuf[*parserbuf_index] = data;
*state = RDS02UF_PARSE_STATE::STATE3_GOT_STRENGTH_L; fc_code = data;
parserbuf[*parserbuf_index] = c;
(*parserbuf_index)++; (*parserbuf_index)++;
*state = RDS02UF_PARSE_STATE::STATE6_FC_CODE_H;
break; break;
case RDS02UF_PARSE_STATE::STATE6_FC_CODE_H: // fc_code high
case RDS02UF_PARSE_STATE::STATE3_GOT_STRENGTH_L: parserbuf[*parserbuf_index] = data;
*state = RDS02UF_PARSE_STATE::STATE3_GOT_STRENGTH_H; fc_code = fc_code | (data << 8);
parserbuf[*parserbuf_index] = c;
(*parserbuf_index)++; (*parserbuf_index)++;
*state = RDS02UF_PARSE_STATE::STATE7_LENGTH_L;
break; break;
case RDS02UF_PARSE_STATE::STATE7_LENGTH_L: // lengh_low
case RDS02UF_PARSE_STATE::STATE3_GOT_STRENGTH_H: data_len = data;
*state = RDS02UF_PARSE_STATE::STATE4_GOT_RESERVED; parserbuf[*parserbuf_index] = data;
parserbuf[*parserbuf_index] = c;
(*parserbuf_index)++; (*parserbuf_index)++;
*state = RDS02UF_PARSE_STATE::STATE8_LENGTH_H;
break; break;
case RDS02UF_PARSE_STATE::STATE8_LENGTH_H: // lengh_high
case RDS02UF_PARSE_STATE::STATE4_GOT_RESERVED: data_len = data << 8 | data_len;
*state = RDS02UF_PARSE_STATE::STATE5_GOT_QUALITY; parserbuf[*parserbuf_index] = data;
parserbuf[*parserbuf_index] = c;
(*parserbuf_index)++; (*parserbuf_index)++;
*state = RDS02UF_PARSE_STATE::STATE9_REAL_DATA;
break; break;
case RDS02UF_PARSE_STATE::STATE9_REAL_DATA: // real_data
parserbuf[*parserbuf_index + data_index] = data;
case RDS02UF_PARSE_STATE::STATE5_GOT_QUALITY: data_index++;
// Find the checksum if (data_index == data_len)
unsigned char cksm = 0; {
(*parserbuf_index)++;
for (int i = 0; i < 8; i++) { *state = RDS02UF_PARSE_STATE::STATE10_CRC;
cksm += parserbuf[i];
} }
break;
if (c == cksm) { case RDS02UF_PARSE_STATE::STATE10_CRC: // crc
parserbuf[*parserbuf_index] = '\0'; crc_data = crc8(&parserbuf[2], 6 + data_len);
unsigned int t1 = parserbuf[2]; parserbuf[*parserbuf_index + data_index] = data;
unsigned int t2 = parserbuf[3]; if (crc_data == data || data == 0xff)
t2 <<= 8; {
t2 += t1; (*parserbuf_index)++;
*dist = ((float)t2) / 100; *state = RDS02UF_PARSE_STATE::STATE11_END_1;
*state = RDS02UF_PARSE_STATE::STATE6_GOT_CHECKSUM; }
*parserbuf_index = 0; else
ret = 0; {
ParseDataFailed(parserbuf, sizeof(parserbuf));
} else { }
*state = RDS02UF_PARSE_STATE::STATE0_UNSYNC; break;
*parserbuf_index = 0; case RDS02UF_PARSE_STATE::STATE11_END_1: //
if (data == RDS02_END)
{
parserbuf[*parserbuf_index + data_index] = data;
(*parserbuf_index)++;
*state = RDS02UF_PARSE_STATE::STATE12_END_2;
}
else
{
ParseDataFailed(parserbuf, sizeof(parserbuf));
} }
break;
case RDS02UF_PARSE_STATE::STATE12_END_2: //
if (data == RDS02_END)
{
if (fc_code == 0x03ff && err_code == 0)
{
if(parserbuf[RDS02_DATA_START_INDEX] == RDS02_TARGET_INFO)
{
float distance = (parserbuf[RDS02_DATA_Y_INDEX + 1] * 256 + parserbuf[RDS02_DATA_Y_INDEX]) / 100.0f;
*dist = distance;
ret = true;
}
}
ParseDataFailed(parserbuf, sizeof(parserbuf));
}
break; break;
} }
@ -157,3 +169,22 @@ int rds02uf_parse(char c, char *parserbuf, unsigned *parserbuf_index, RDS02UF_PA
return ret; return ret;
} }
void ParseDataFailed(char* data_in, uint8_t datalengh)
{
memset(data_in, 0, datalengh);
data_len = 0;
data_index = 0;
crc_data = 0;
fc_code = 0;
err_code = 0;
}
uint8_t crc8(char* pbuf, int32_t len)
{
char* data = pbuf;
uint8_t crc = 0;
while ( len-- )
crc = crc8_table[crc^*(data++)];
return crc;
}

72
src/drivers/distance_sensor/rds02uf/rds02uf_parser.h
Unescape View File

@ -42,6 +42,17 @@
#pragma once #pragma once
#define RDS02_HEAD1 0x55
#define RDS02_HEAD2 0x55
#define RDS02_END 0xAA
#define RDS02_BUFFER_SIZE 50
#define RDS02_DATA_START_INDEX 8
#define RDS02_DATA_Y_INDEX 13
#define RDS02_DATA_FC_INDEX RDS02_DATA_START_INDEX
#define RDS02_TARGET_INFO 0x0C
// Data Format for Benewake TFmini // Data Format for Benewake TFmini
// =============================== // ===============================
// 9 bytes total per message: // 9 bytes total per message:
@ -56,17 +67,56 @@
// 9) Checksum parity bit (low 8bit), Checksum = Byte1 + Byte2 +...+Byte8. This is only a low 8bit though // 9) Checksum parity bit (low 8bit), Checksum = Byte1 + Byte2 +...+Byte8. This is only a low 8bit though
enum class RDS02UF_PARSE_STATE { enum RDS02UF_PARSE_STATE {
STATE0_UNSYNC = 0, STATE1_SYNC_1 = 0,
STATE1_SYNC_1, STATE2_SYNC_2,
STATE1_SYNC_2, STATE3_ADDRESS,
STATE2_GOT_DIST_L, STATE4_ERROR_CODE,
STATE2_GOT_DIST_H, STATE5_FC_CODE_L,
STATE3_GOT_STRENGTH_L, STATE6_FC_CODE_H,
STATE3_GOT_STRENGTH_H, STATE7_LENGTH_L,
STATE4_GOT_RESERVED, STATE8_LENGTH_H,
STATE5_GOT_QUALITY, STATE9_REAL_DATA,
STATE6_GOT_CHECKSUM STATE10_CRC,
STATE11_END_1,
STATE12_END_2
}; };
int rds02uf_parse(char c, char *parserbuf, unsigned *parserbuf_index, RDS02UF_PARSE_STATE *state, float *dist); int rds02uf_parse(char c, char *parserbuf, unsigned *parserbuf_index, RDS02UF_PARSE_STATE *state, float *dist);
uint8_t crc8(char* pbuf, int32_t len);
void ParseDataFailed(char* data_in, uint8_t datalengh);
const uint8_t crc8_table[256] = {
0x93,0x98,0xE4,0x46,0xEB,0xBA,0x04,0x4C,
0xFA,0x40,0xB8,0x96,0x0E,0xB2,0xB7,0xC0,
0x0C,0x32,0x9B,0x80,0xFF,0x30,0x7F,0x9D,
0xB3,0x81,0x58,0xE7,0xF1,0x19,0x7E,0xB6,
0xCD,0xF7,0xB4,0xCB,0xBC,0x5C,0xD6,0x09,
0x20,0x0A,0xE0,0x37,0x51,0x67,0x24,0x95,
0xE1,0x62,0xF8,0x5E,0x38,0x15,0x54,0x77,
0x63,0x57,0x6D,0xE9,0x89,0x76,0xBE,0x41,
0x5D,0xF9,0xB1,0x4D,0x6C,0x53,0x9C,0xA2,
0x23,0xC4,0x8E,0xC8,0x05,0x42,0x61,0x71,
0xC5,0x00,0x18,0x6F,0x5F,0xFB,0x7B,0x11,
0x65,0x2D,0x8C,0xED,0x14,0xAB,0x88,0xD5,
0xD9,0xC2,0x36,0x34,0x7C,0x5B,0x3C,0xF6,
0x48,0x0B,0xEE,0x02,0x83,0x79,0x17,0xE6,
0xA8,0x78,0xF5,0xD3,0x4E,0x50,0x52,0x91,
0xD8,0xC6,0x22,0xEC,0x3B,0xE5,0x3F,0x86,
0x06,0xCF,0x2B,0x2F,0x3D,0x59,0x1C,0x87,
0xEF,0x4F,0x10,0xD2,0x7D,0xDA,0x72,0xA0,
0x9F,0xDE,0x6B,0x75,0x56,0xBD,0xC7,0xC1,
0x70,0x1D,0x25,0x92,0xA5,0x31,0xE2,0xD7,
0xD0,0x9A,0xAF,0xA9,0xC9,0x97,0x08,0x33,
0x5A,0x99,0xC3,0x16,0x84,0x82,0x8A,0xF3,
0x4A,0xCE,0xDB,0x29,0x0F,0xAE,0x6E,0xE3,
0x8B,0x07,0x3A,0x74,0x47,0xB0,0xBB,0xB5,
0x7A,0xAA,0x2C,0xD4,0x03,0x3E,0x1A,0xA7,
0x27,0x64,0x06,0xBF,0x55,0x73,0x1E,0xFE,
0x49,0x01,0x39,0x28,0xF4,0x26,0xDF,0xDD,
0x44,0x0D,0x21,0xF2,0x85,0xB9,0xEA,0x4B,
0xDC,0x6A,0xCA,0xAC,0x12,0xFC,0x2E,0x2A,
0xA3,0xF0,0x66,0xE8,0x60,0x45,0xA1,0x8D,
0x68,0x35,0xFD,0x8F,0x9E,0x1F,0x13,0xD1,
0xAD,0x69,0xCC,0xA4,0x94,0x90,0x1B,0x43,
};

Write Preview
Loading…
Cancel
Save