mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
Applied code-review suggestions.
- Removed blocking serial. - Added passing variables by reference to parser. - Removed commented code. Signed-off-by: Claudio Micheli <claudio@auterion.com>
This commit is contained in:
Claudio Micheli
Beat Küng
parent
d1da46ef46
commit
8a313ffc26
@ -114,7 +114,7 @@ private:
|
||||
uint8_t _cycle_counter;
|
||||
|
||||
enum CM8JL65_PARSE_STATE _parse_state;
|
||||
unsigned char _frame_data[4];
|
||||
unsigned char _frame_data[PARSER_BUF_LENGTH];
|
||||
uint16_t _crc16;
|
||||
int _distance_mm;
|
||||
|
||||
@ -368,7 +368,7 @@ CM8JL65::collect()
|
||||
|
||||
while ((bytes_processed < bytes_read) && (!crc_valid)) {
|
||||
// printf("In the cycle, processing byte %d, 0x%02X \n",bytes_processed, _linebuf[bytes_processed]);
|
||||
if (OK == cm8jl65_parser(_linebuf[bytes_processed], _frame_data, &_parse_state, &_crc16, &_distance_mm)) {
|
||||
if (OK == cm8jl65_parser(_linebuf[bytes_processed], _frame_data, _parse_state, _crc16, _distance_mm)) {
|
||||
crc_valid = true;
|
||||
}
|
||||
|
||||
@ -379,7 +379,6 @@ CM8JL65::collect()
|
||||
|
||||
}
|
||||
|
||||
// else {printf("Starting frame wrong. Index: %d value 0x%02X \n",i,_linebuf[i]);}
|
||||
i--;
|
||||
}
|
||||
|
||||
@ -417,11 +416,6 @@ CM8JL65::collect()
|
||||
|
||||
perf_end(_sample_perf);
|
||||
|
||||
/* ENABLE THIS IF YOU WANT TO PRINT OLD VALUES WHILE CRC CHECK IS WRONG
|
||||
if (!crc_valid) {
|
||||
return -EAGAIN;
|
||||
}
|
||||
else return OK; */
|
||||
return OK;
|
||||
|
||||
}
|
||||
@ -455,11 +449,10 @@ CM8JL65::cycle_trampoline(void *arg)
|
||||
void
|
||||
CM8JL65::cycle()
|
||||
{
|
||||
//PX4_DEBUG("CM8JL65::cycle() - in the cycle");
|
||||
/* fds initialized? */
|
||||
if (_fd < 0) {
|
||||
/* open fd */
|
||||
_fd = ::open(_port, O_RDWR);
|
||||
_fd = ::open(_port, O_RDWR | O_NOCTTY | O_NONBLOCK);
|
||||
|
||||
if (_fd < 0) {
|
||||
PX4_ERR("open failed (%i)", errno);
|
||||
@ -500,14 +493,10 @@ CM8JL65::cycle()
|
||||
|
||||
if (collect_ret == -EAGAIN) {
|
||||
_cycle_counter++;
|
||||
/* We are missing bytes to complete the packet, re-cycle at 1ms */
|
||||
// work_queue(HPWORK,&_work,(worker_t)&CM8JL65::cycle_trampoline,this,USEC2TICK(1000LL));
|
||||
// return;
|
||||
}
|
||||
|
||||
|
||||
/* schedule a fresh cycle call when a complete packet has been received */
|
||||
//work_queue(HPWORK,&_work,(worker_t)&CM8JL65::cycle_trampoline,this,USEC2TICK(_conversion_interval - _cycle_counter * 1000LL));
|
||||
work_queue(HPWORK, &_work, (worker_t)&CM8JL65::cycle_trampoline, this, USEC2TICK(_conversion_interval));
|
||||
_cycle_counter = 0;
|
||||
}
|
||||
|
||||
@ -48,7 +48,7 @@
|
||||
|
||||
|
||||
|
||||
// Note : No clue what those static variables are
|
||||
|
||||
static const unsigned char crc_msb_vector[] = {
|
||||
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
|
||||
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
|
||||
@ -103,14 +103,14 @@ static const unsigned char crc_lsb_vector[] = {
|
||||
|
||||
|
||||
|
||||
unsigned short crc16_calc(unsigned char *dataFrame, uint8_t crc16_length)
|
||||
unsigned short crc16_calc(unsigned char *data_frame, uint8_t crc16_length)
|
||||
{
|
||||
unsigned char crc_high_byte = 0xFF;
|
||||
unsigned char crc_low_byte = 0xFF;
|
||||
int i;
|
||||
|
||||
while (crc16_length--) {
|
||||
i = crc_low_byte ^ *(dataFrame++);
|
||||
i = crc_low_byte ^ *(data_frame++);
|
||||
crc_low_byte = (unsigned char)(crc_high_byte ^ crc_msb_vector[i]);
|
||||
crc_high_byte = crc_lsb_vector[i];
|
||||
}
|
||||
@ -118,62 +118,56 @@ unsigned short crc16_calc(unsigned char *dataFrame, uint8_t crc16_length)
|
||||
return (unsigned short)(crc_high_byte << 8 | crc_low_byte);
|
||||
}
|
||||
|
||||
int cm8jl65_parser(uint8_t c, uint8_t *parserbuf, CM8JL65_PARSE_STATE *state, uint16_t *crc16, int *dist)
|
||||
int cm8jl65_parser(uint8_t c, uint8_t parserbuf[PARSER_BUF_LENGTH], CM8JL65_PARSE_STATE &state, uint16_t &crc16,
|
||||
int &dist)
|
||||
{
|
||||
int ret = -1;
|
||||
|
||||
// printf("parse byte 0x%02X \n", b);
|
||||
|
||||
switch (*state) {
|
||||
switch (state) {
|
||||
case STATE0_WAITING_FRAME:
|
||||
if (c == START_FRAME_DIGIT1) {
|
||||
*state = STATE1_GOT_DIGIT1;
|
||||
//printf("Got SFD1 \n");
|
||||
state = STATE1_GOT_DIGIT1;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case STATE1_GOT_DIGIT1:
|
||||
if (c == START_FRAME_DIGIT2) {
|
||||
*state = STATE2_GOT_DIGIT2;
|
||||
//printf("Got SFD2 \n");
|
||||
state = STATE2_GOT_DIGIT2;
|
||||
}
|
||||
|
||||
// @NOTE: (claudio@auterion.com): if second byte is wrong we skip all the frame and restart parsing !!
|
||||
else if (c == START_FRAME_DIGIT1) {
|
||||
*state = STATE1_GOT_DIGIT1;
|
||||
//printf("Discard previous SFD1, Got new SFD1 \n");
|
||||
state = STATE1_GOT_DIGIT1;
|
||||
|
||||
} else {
|
||||
*state = STATE0_WAITING_FRAME;
|
||||
state = STATE0_WAITING_FRAME;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case STATE2_GOT_DIGIT2:
|
||||
*state = STATE3_GOT_MSB_DATA;
|
||||
state = STATE3_GOT_MSB_DATA;
|
||||
parserbuf[DISTANCE_MSB_POS] = c; // MSB Data
|
||||
//printf("Got DATA1 0x%02X \n", c);
|
||||
break;
|
||||
|
||||
case STATE3_GOT_MSB_DATA:
|
||||
*state = STATE4_GOT_LSB_DATA;
|
||||
state = STATE4_GOT_LSB_DATA;
|
||||
parserbuf[DISTANCE_LSB_POS] = c; // LSB Data
|
||||
//printf("Got DATA2 0x%02X \n", c);
|
||||
|
||||
// do crc calculation
|
||||
*crc16 = crc16_calc(parserbuf, CHECKSUM_LENGTH);
|
||||
crc16 = crc16_calc(parserbuf, PARSER_BUF_LENGTH);
|
||||
// convert endian
|
||||
*crc16 = (*crc16 >> 8) | (*crc16 << 8);
|
||||
crc16 = (crc16 >> 8) | (crc16 << 8);
|
||||
break;
|
||||
|
||||
|
||||
case STATE4_GOT_LSB_DATA:
|
||||
if (c == (*crc16 >> 8)) {
|
||||
*state = STATE5_GOT_CHKSUM1;
|
||||
if (c == (crc16 >> 8)) {
|
||||
state = STATE5_GOT_CHKSUM1;
|
||||
|
||||
} else {
|
||||
// printf("Checksum invalid on high byte: 0x%02X, calculated: 0x%04X \n",c, *crc16);
|
||||
*state = STATE0_WAITING_FRAME;
|
||||
state = STATE0_WAITING_FRAME;
|
||||
|
||||
}
|
||||
|
||||
@ -181,21 +175,18 @@ int cm8jl65_parser(uint8_t c, uint8_t *parserbuf, CM8JL65_PARSE_STATE *state, ui
|
||||
|
||||
case STATE5_GOT_CHKSUM1:
|
||||
// Here, reset state to `NOT-STARTED` no matter crc ok or not
|
||||
*state = STATE0_WAITING_FRAME;
|
||||
state = STATE0_WAITING_FRAME;
|
||||
|
||||
if (c == (*crc16 & 0xFF)) {
|
||||
if (c == (crc16 & 0xFF)) {
|
||||
// printf("Checksum verified \n");
|
||||
*dist = (parserbuf[DISTANCE_MSB_POS] << 8) | parserbuf[DISTANCE_LSB_POS];
|
||||
dist = (parserbuf[DISTANCE_MSB_POS] << 8) | parserbuf[DISTANCE_LSB_POS];
|
||||
return 0;
|
||||
}
|
||||
|
||||
/*else {
|
||||
//printf("Checksum invalidon low byte: 0x%02X, calculated: 0x%04X \n",c, *crc16);
|
||||
}*/
|
||||
break;
|
||||
|
||||
default:
|
||||
printf("This should never happen. \n");
|
||||
// Nothing todo
|
||||
break;
|
||||
}
|
||||
|
||||
|
||||
@ -54,7 +54,7 @@
|
||||
// Frame data saved for CRC calculation
|
||||
#define DISTANCE_MSB_POS 2
|
||||
#define DISTANCE_LSB_POS 3
|
||||
#define CHECKSUM_LENGTH 4
|
||||
#define PARSER_BUF_LENGTH 4
|
||||
|
||||
|
||||
enum CM8JL65_PARSE_STATE {
|
||||
@ -69,4 +69,5 @@ enum CM8JL65_PARSE_STATE {
|
||||
|
||||
|
||||
|
||||
int cm8jl65_parser(uint8_t c, uint8_t *parserbuf, enum CM8JL65_PARSE_STATE *state, uint16_t *crc16, int *dist);
|
||||
int cm8jl65_parser(uint8_t c, uint8_t parserbuf[PARSER_BUF_LENGTH], enum CM8JL65_PARSE_STATE &state, uint16_t &crc16,
|
||||
int &dist);
|
||||
|
||||
@ -1,45 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2018 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* Lanbao PSK-CM8JL65-CC5 distance sensor
|
||||
*
|
||||
* @reboot_required true
|
||||
* @min 0
|
||||
* @max 2
|
||||
* @group Sensors
|
||||
* @value 0 Disabled
|
||||
* @value 1 Enabled on TELEM2
|
||||
* @value 2 Enabled on TELEM1
|
||||
*/
|
||||
PARAM_DEFINE_INT32(SENS_EN_CM8JL65, 0);
|
||||
Loading…
x
Reference in New Issue
Block a user