From aee6a83fe002c86517f4526d623607ac4397c0fb Mon Sep 17 00:00:00 2001
From: Claudio Micheli <claudio@auterion.com>
Date: Fri, 21 Dec 2018 11:25:03 +0100
Subject: [PATCH] Code Cleanup. Added configuration notes.
Signed-off-by: Claudio Micheli <claudio@auterion.com>
---
.../distance_sensor/isl2950/isl2950.cpp | 168 ++++++++----------
.../isl2950/isl2950_parser.cpp | 91 +++++-----
.../distance_sensor/isl2950/isl2950_parser.h | 68 +++----
3 files changed, 144 insertions(+), 183 deletions(-)
diff --git a/src/drivers/distance_sensor/isl2950/isl2950.cpp b/src/drivers/distance_sensor/isl2950/isl2950.cpp
index 003c72579a..b986ef6c56 100644
--- a/src/drivers/distance_sensor/isl2950/isl2950.cpp
+++ b/src/drivers/distance_sensor/isl2950/isl2950.cpp
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (c) 2014-2015 PX4 Development Team. All rights reserved.
+ * 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
@@ -32,10 +32,13 @@
****************************************************************************/
/**
- * @file landbao15L2950.cpp
+ * @file isl2950.cpp
* @author Claudio Micheli <claudio@auterion.com>
*
- * Driver for the ISL2950
+ * Driver for the Lanbao PSK-CM8JL65-CC5 distance sensor.
+ * Make sure to disable MAVLINK messages (MAV_0_CONFIG PARAMETER)
+ * on the serial port you connect the sensor,i.e TELEM1.
+ *
*/
#include <px4_config.h>
@@ -46,13 +49,9 @@
#include <stdint.h>
#include <stdlib.h>
#include <stdbool.h>
- #include <semaphore.h>
#include <string.h>
- #include <fcntl.h>
#include <poll.h>
- #include <errno.h>
#include <stdio.h>
- #include <math.h>
#include <unistd.h>
#include <termios.h>
@@ -76,24 +75,25 @@
#define ISL2950_TAKE_RANGE_REG 'd'
-// designated serial port on Pixhawk
- #define ISL2950_DEFAULT_PORT "/dev/ttyS1" // Its baudrate is 115200
+// designated serial port on Pixhawk (TELEM1)
+#define ISL2950_DEFAULT_PORT "/dev/ttyS1" // Its baudrate is 115200
- // normal conversion wait time
- #define ISL2950_CONVERSION_INTERVAL 50*1000UL/* 100ms */
+// normal conversion wait time
+#define ISL2950_CONVERSION_INTERVAL 50*1000UL/* 50ms */
class ISL2950 : public cdev::CDev
{
public:
- // Constructor
- ISL2950(const char *port = ISL2950_DEFAULT_PORT, uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
- // Virtual destructor
- virtual ~ISL2950();
- virtual int init();
- //virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen);
- virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg);
+ // Constructor
+ ISL2950(const char *port = ISL2950_DEFAULT_PORT, uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
+
+ // Virtual destructor
+ virtual ~ISL2950();
+
+ virtual int init();
+ virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg);
/**
* Diagnostics - print some basic information about the driver.
@@ -101,30 +101,30 @@
void print_info();
private:
- char _port[20];
- uint8_t _rotation;
- float _min_distance;
- float _max_distance;
- int _conversion_interval;
- work_s _work{};
- ringbuffer::RingBuffer *_reports;
- bool _collect_phase;
- int _fd;
- uint8_t _linebuf[25];
- uint8_t _cycle_counter;
-
- enum ISL2950_PARSE_STATE _parse_state;
- unsigned char _frame_data[4];
- uint16_t _crc16;
- int _distance_mm;
-
- int _class_instance;
- int _orb_class_instance;
-
- orb_advert_t _distance_sensor_topic;
-
- perf_counter_t _sample_perf;
- perf_counter_t _comms_errors;
+
+ char _port[20];
+ uint8_t _rotation;
+ float _min_distance;
+ float _max_distance;
+ int _conversion_interval;
+ work_s _work{};
+ ringbuffer::RingBuffer *_reports;
+ int _fd;
+ uint8_t _linebuf[25];
+ uint8_t _cycle_counter;
+
+ enum ISL2950_PARSE_STATE _parse_state;
+ unsigned char _frame_data[4];
+ uint16_t _crc16;
+ int _distance_mm;
+
+ int _class_instance;
+ int _orb_class_instance;
+
+ orb_advert_t _distance_sensor_topic;
+
+ perf_counter_t _sample_perf;
+ perf_counter_t _comms_errors;
/**
* Initialise the automatic measurement state machine and start it.
@@ -132,41 +132,37 @@
* @note This function is called at open and error time. It might make sense
* to make it more aggressive about resetting the bus in case of errors.
*/
- void start();
+ void start();
- /**
- * Stop the automatic measurement state machine.
- */
- void stop();
+ /**
+ * Stop the automatic measurement state machine.
+ */
+ void stop();
- /**
- * Set the min and max distance thresholds if you want the end points of the sensors
- * range to be brought in at all, otherwise it will use the defaults SF0X_MIN_DISTANCE
- * and SF0X_MAX_DISTANCE
- */
- void set_minimum_distance(float min);
- void set_maximum_distance(float max);
- float get_minimum_distance();
- float get_maximum_distance();
-
- /**
- * Perform a poll cycle; collect from the previous measurement
- * and start a new one.
- */
- void cycle();
- int collect();
- /**
- * Static trampoline from the workq context; because we don't have a
- * generic workq wrapper yet.
- *
- * @param arg Instance pointer for the driver that is polling.
- */
- static void cycle_trampoline(void *arg);
+ /**
+ * Set the min and max distance thresholds.
+ */
+ void set_minimum_distance(float min);
+ void set_maximum_distance(float max);
+ float get_minimum_distance();
+ float get_maximum_distance();
+ /**
+ * Perform a reading cycle; collect from the previous measurement
+ * and start a new one.
+ */
+ void cycle();
+ int collect();
+ /**
+ * Static trampoline from the workq context; because we don't have a
+ * generic workq wrapper yet.
+ *
+ * @param arg Instance pointer for the driver that is polling.
+ */
+ static void cycle_trampoline(void *arg);
};
-
/*
* Driver 'main' command.
*/
@@ -181,15 +177,14 @@
ISL2950::ISL2950(const char *port, uint8_t rotation) :
CDev(RANGE_FINDER0_DEVICE_PATH),
_rotation(rotation),
- _min_distance(0.14f),
- _max_distance(40.0f),
+ _min_distance(0.10f),
+ _max_distance(9.0f),
_conversion_interval(ISL2950_CONVERSION_INTERVAL),
_reports(nullptr),
- _collect_phase(false),
_fd(-1),
_cycle_counter(0),
- _parse_state(TFS_NOT_STARTED),
- _frame_data{TOF_SFD1, TOF_SFD2, 0, 0},
+ _parse_state(STATE0_WAITING_FRAME),
+ _frame_data{START_FRAME_DIGIT1, START_FRAME_DIGIT2, 0, 0},
_crc16(0),
_distance_mm(-1),
_class_instance(-1),
@@ -335,20 +330,12 @@ ISL2950::ioctl(device::file_t *filp, int cmd, unsigned long arg)
}
}
-/*
-ssize_t
-ISL2950::read(device::file_t *filp, char *buffer, size_t buflen)
-{
- // SOME STUFFS
-
-}*/
-
int
/*
* Method: collect()
*
- * This method reads data from serial UART and places it into a buffer
+ * This method reads data from serial UART and places it into a buffer
*/
ISL2950::collect()
{
@@ -375,7 +362,7 @@ ISL2950::collect()
i = bytes_read - 6 ;
while ((i >=0) && (!crc_valid))
{
- if (_linebuf[i] == TOF_SFD1) {
+ if (_linebuf[i] == START_FRAME_DIGIT1) {
bytes_processed = i;
while ((bytes_processed < bytes_read) && (!crc_valid))
{
@@ -385,7 +372,7 @@ ISL2950::collect()
}
bytes_processed++;
}
- _parse_state = TFS_NOT_STARTED;
+ _parse_state = STATE0_WAITING_FRAME;
}
// else {printf("Starting frame wrong. Index: %d value 0x%02X \n",i,_linebuf[i]);}
@@ -438,9 +425,8 @@ ISL2950::collect()
void
ISL2950::start()
{
- PX4_INFO("ISL2950::start() - launch the work queue");
- /* reset the report ring and state machine */
- _collect_phase = false;
+ PX4_INFO("driver started");
+
_reports->flush();
/* schedule a cycle to start things */
@@ -472,7 +458,7 @@ ISL2950::cycle()
_fd = ::open(_port,O_RDWR);
if (_fd < 0) {
- PX4_ERR("ISL2950::cycle() - open failed (%i)", errno);
+ PX4_ERR("open failed (%i)", errno);
return;
}
diff --git a/src/drivers/distance_sensor/isl2950/isl2950_parser.cpp b/src/drivers/distance_sensor/isl2950/isl2950_parser.cpp
index 0c5e9de233..ee9c783d18 100644
--- a/src/drivers/distance_sensor/isl2950/isl2950_parser.cpp
+++ b/src/drivers/distance_sensor/isl2950/isl2950_parser.cpp
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ * 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
@@ -33,8 +33,7 @@
/**
* @file isl2950_parser.cpp
- * @author Claudio Micheli
- * claudio@auterion.com
+ * @author Claudio Micheli <claudio@auterion.com>
*
*/
@@ -47,11 +46,10 @@
#include <string.h>
#include <stdlib.h>
-typedef unsigned char UCHAR;
-typedef unsigned short USHORT;
+
// Note : No clue what those static variables are
-static const UCHAR aucCRCHi[] = {
+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,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
@@ -76,7 +74,7 @@ static const UCHAR aucCRCHi[] = {
0x00, 0xC1, 0x81, 0x40
};
-static const UCHAR aucCRCLo[] = {
+static const unsigned char crc_lsb_vector[] = {
0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06, 0x07, 0xC7,
0x05, 0xC5, 0xC4, 0x04, 0xCC, 0x0C, 0x0D, 0xCD, 0x0F, 0xCF, 0xCE, 0x0E,
0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 0x09, 0x08, 0xC8, 0xD8, 0x18, 0x19, 0xD9,
@@ -102,27 +100,19 @@ static const UCHAR aucCRCLo[] = {
};
-// TOF frame format
-//
-// 1B 1B 1B 1B 2B
-// | 0xA5 | 0x5A | distance-MSB | distance-LSB | crc-16 |
-//
-// Frame data saved for CRC calculation
-const static int TOF_DISTANCE_MSB_POS = 2;
-const static int TOF_DISTANCE_LSB_POS = 3;
-const static int TOF_CRC_CALC_DATA_LEN = 4;
-
-
-USHORT usMBCRC16(UCHAR* pucFrame, USHORT usLen) {
- UCHAR ucCRCHi = 0xFF;
- UCHAR ucCRCLo = 0xFF;
- int iIndex;
- while (usLen--) {
- iIndex = ucCRCLo ^ *(pucFrame++);
- ucCRCLo = (UCHAR)(ucCRCHi ^ aucCRCHi[iIndex]);
- ucCRCHi = aucCRCLo[iIndex];
+
+
+
+unsigned short crc16_calc(unsigned char *dataFrame,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++);
+ crc_low_byte = (unsigned char)(crc_high_byte ^ crc_msb_vector[i]);
+ crc_high_byte = crc_lsb_vector[i];
}
- return (USHORT)(ucCRCHi << 8 | ucCRCLo);
+ return (unsigned short)(crc_high_byte << 8 | crc_low_byte);
}
int isl2950_parser(uint8_t c, uint8_t *parserbuf, ISL2950_PARSE_STATE *state, uint16_t *crc16, int *dist)
@@ -132,65 +122,64 @@ int isl2950_parser(uint8_t c, uint8_t *parserbuf, ISL2950_PARSE_STATE *state, ui
// printf("parse byte 0x%02X \n", b);
switch (*state) {
- case TFS_NOT_STARTED:
- if (c == TOF_SFD1) {
- *state = TFS_GOT_SFD1;
+ case STATE0_WAITING_FRAME:
+ if (c == START_FRAME_DIGIT1) {
+ *state = STATE1_GOT_DIGIT1;
//printf("Got SFD1 \n");
}
break;
- case TFS_GOT_SFD1:
- if (c == TOF_SFD2) {
- *state = TFS_GOT_SFD2;
+ case STATE1_GOT_DIGIT1:
+ if (c == START_FRAME_DIGIT2) {
+ *state = STATE2_GOT_DIGIT2;
//printf("Got SFD2 \n");
}
- // @NOTE: (claudio@auterion.com): Strange thing, if second byte is wrong we skip all the frame and restart parsing !!
- else if (c == TOF_SFD1) {
- *state = TFS_GOT_SFD1;
+ // @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");
} else {
- *state = TFS_NOT_STARTED;
+ *state = STATE0_WAITING_FRAME;
}
break;
- case TFS_GOT_SFD2:
- *state = TFS_GOT_DATA1;
- parserbuf[TOF_DISTANCE_MSB_POS] = c; // MSB Data
+ case STATE2_GOT_DIGIT2:
+ *state = STATE3_GOT_MSB_DATA;
+ parserbuf[DISTANCE_MSB_POS] = c; // MSB Data
//printf("Got DATA1 0x%02X \n", c);
break;
- case TFS_GOT_DATA1:
- *state = TFS_GOT_DATA2;
- parserbuf[TOF_DISTANCE_LSB_POS] = c; // LSB Data
+ case STATE3_GOT_MSB_DATA:
+ *state = STATE4_GOT_LSB_DATA;
+ parserbuf[DISTANCE_LSB_POS] = c; // LSB Data
//printf("Got DATA2 0x%02X \n", c);
// do crc calculation
- *crc16 = usMBCRC16(parserbuf, TOF_CRC_CALC_DATA_LEN);
+ *crc16 = crc16_calc(parserbuf, CHECKSUM_LENGTH);
// convert endian
*crc16 = (*crc16 >> 8) | (*crc16 << 8);
break;
- case TFS_GOT_DATA2:
+ case STATE4_GOT_LSB_DATA:
if (c == (*crc16 >> 8)) {
- *state = TFS_GOT_CHECKSUM1;
+ *state = STATE5_GOT_CHKSUM1;
}
else {
// printf("Checksum invalid on high byte: 0x%02X, calculated: 0x%04X \n",c, *crc16);
- *state = TFS_NOT_STARTED;
+ *state = STATE0_WAITING_FRAME;
}
break;
- case TFS_GOT_CHECKSUM1:
+ case STATE5_GOT_CHKSUM1:
// Here, reset state to `NOT-STARTED` no matter crc ok or not
- *state = TFS_NOT_STARTED;
+ *state = STATE0_WAITING_FRAME;
if (c == (*crc16 & 0xFF)) {
// printf("Checksum verified \n");
- *dist = (parserbuf[TOF_DISTANCE_MSB_POS] << 8) | parserbuf[TOF_DISTANCE_LSB_POS];
+ *dist = (parserbuf[DISTANCE_MSB_POS] << 8) | parserbuf[DISTANCE_LSB_POS];
return OK;
}
/*else {
- printf("Checksum not verified \n");
//printf("Checksum invalidon low byte: 0x%02X, calculated: 0x%04X \n",c, *crc16);
}*/
break;
diff --git a/src/drivers/distance_sensor/isl2950/isl2950_parser.h b/src/drivers/distance_sensor/isl2950/isl2950_parser.h
index 4729c44fb3..5d9a808af1 100644
--- a/src/drivers/distance_sensor/isl2950/isl2950_parser.h
+++ b/src/drivers/distance_sensor/isl2950/isl2950_parser.h
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ * 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
@@ -31,56 +31,42 @@
*
****************************************************************************/
-/**
- * @file sf0x_parser.cpp
- * @author Lorenz Meier <lm@inf.ethz.ch>
- *
- * Declarations of parser for the Lightware SF0x laser rangefinder series
- */
+ /**
+ * @file isl2950.cpp
+ * @author Claudio Micheli <claudio@auterion.com>
+ *
+ * Parser declarations for Lanbao PSK-CM8JL65-CC5 distance sensor
+ */
#pragma once
#include <stdint.h>
// frame start delimiter
-#define TOF_SFD1 0xA5
-#define TOF_SFD2 0x5A
+#define START_FRAME_DIGIT1 0xA5
+#define START_FRAME_DIGIT2 0x5A
-/*typedef enum {
- TFS_NOT_STARTED = 0,
- TFS_GOT_SFD1,
- TFS_GOT_SFD2,
- TFS_GOT_DATA1,
- TFS_GOT_DATA2,
- TFS_GOT_CHECKSUM1,
- TFS_GOT_CHECKSUM2,
-} TofFramingState;*/
-enum ISL2950_PARSE_STATE {
- TFS_NOT_STARTED = 0,
- TFS_GOT_SFD1,
- TFS_GOT_SFD2,
- TFS_GOT_DATA1,
- TFS_GOT_DATA2,
- TFS_GOT_CHECKSUM1,
- TFS_GOT_CHECKSUM2,
-};
+// Frame format definition
+//
+// 1B 1B 1B 1B 2B
+// | 0xA5 | 0x5A | distance-MSB | distance-LSB | crc-16 |
+//
+// Frame data saved for CRC calculation
+#define DISTANCE_MSB_POS 2
+#define DISTANCE_LSB_POS 3
+#define CHECKSUM_LENGTH 4
-enum IslWorkingMode {
- KEEP_HEIGHT = 0,
- NUM_WORKING_MODE
-};
+enum ISL2950_PARSE_STATE {
+ STATE0_WAITING_FRAME = 0,
+ STATE1_GOT_DIGIT1,
+ STATE2_GOT_DIGIT2,
+ STATE3_GOT_MSB_DATA,
+ STATE4_GOT_LSB_DATA,
+ STATE5_GOT_CHKSUM1,
+ STATE6_GOT_CHKSUM2,
+};
-// SF0X STYLE
-/*enum ISL2950_PARSE_STATE {
- ISL2950_PARSE_STATE0_UNSYNC = 0,
- ISL2950_PARSE_STATE1_SYNC,
- ISL2950_PARSE_STATE2_GOT_DIGIT0,
- ISL2950_PARSE_STATE3_GOT_DOT,
- ISL2950_PARSE_STATE4_GOT_DIGIT1,
- ISL2950_PARSE_STATE5_GOT_DIGIT2,
- ISL2950_PARSE_STATE6_GOT_CARRIAGE_RETURN
-};*/
int isl2950_parser(uint8_t c, uint8_t *parserbuf,enum ISL2950_PARSE_STATE *state,uint16_t *crc16, int *dist);
--
GitLab