diff --git a/src/drivers/distance_sensor/cm8jl65/cm8jl65.cpp b/src/drivers/distance_sensor/cm8jl65/cm8jl65.cpp
index 15b8ec9faa601a3ce28e3d87fefc0cde71a858cb..611ece114ec3d5234fc14a1786f663924dce6855 100644
--- a/src/drivers/distance_sensor/cm8jl65/cm8jl65.cpp
+++ b/src/drivers/distance_sensor/cm8jl65/cm8jl65.cpp
@@ -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;
}
diff --git a/src/drivers/distance_sensor/cm8jl65/cm8jl65_parser.cpp b/src/drivers/distance_sensor/cm8jl65/cm8jl65_parser.cpp
index 46cf0b0ff591829818591d043fa8d40345a03696..6986d026bf8d94326fb4aa4a9d4d39bf17d6670d 100644
--- a/src/drivers/distance_sensor/cm8jl65/cm8jl65_parser.cpp
+++ b/src/drivers/distance_sensor/cm8jl65/cm8jl65_parser.cpp
@@ -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;
}
diff --git a/src/drivers/distance_sensor/cm8jl65/cm8jl65_parser.h b/src/drivers/distance_sensor/cm8jl65/cm8jl65_parser.h
index b7e54904e79902fcc1bb37d90c45b474418baa8e..979995e20609bd87ed67e3be8d7a896675d2eb56 100644
--- a/src/drivers/distance_sensor/cm8jl65/cm8jl65_parser.h
+++ b/src/drivers/distance_sensor/cm8jl65/cm8jl65_parser.h
@@ -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);
diff --git a/src/drivers/distance_sensor/cm8jl65/parameters.c b/src/drivers/distance_sensor/cm8jl65/parameters.c
deleted file mode 100644
index 7324b17031630d1f507a77b5f391193a1c795332..0000000000000000000000000000000000000000
--- a/src/drivers/distance_sensor/cm8jl65/parameters.c
+++ /dev/null
@@ -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);