diff --git a/src/drivers/distance_sensor/airlango/isl2950.cpp b/src/drivers/distance_sensor/airlango/isl2950.cpp
index 6659b1347c89d4c7d65d51689f3e93df64fab989..30dbe88308e80c04f9e5fa93d32766ee39a9f933 100644
--- a/src/drivers/distance_sensor/airlango/isl2950.cpp
+++ b/src/drivers/distance_sensor/airlango/isl2950.cpp
@@ -78,9 +78,9 @@
// designated serial port on Pixhawk
#define ISL2950_DEFAULT_PORT "/dev/ttyS1" // Its baudrate is 115200
- // #define LANBAO_DEFAULT_BAUDRATE 115200
+
// normal conversion wait time
- #define ISL2950_CONVERSION_INTERVAL 50*1000UL /* 50ms */
+ #define ISL2950_CONVERSION_INTERVAL 100*1000UL/* 100ms */
class ISL2950 : public cdev::CDev
@@ -105,17 +105,20 @@
uint8_t _rotation;
float _min_distance;
float _max_distance;
- int _conversion_interval;
+ int _conversion_interval;
work_s _work{};
ringbuffer::RingBuffer *_reports;
int _measure_ticks;
bool _collect_phase;
int _fd;
- char _linebuf[50];
+ uint8_t _linebuf[20];
unsigned _linebuf_index;
+
enum ISL2950_PARSE_STATE _parse_state;
- hrt_abstime _last_read;
+ unsigned char _frame_data[4];
+ uint16_t _crc16;
+ hrt_abstime _last_read;
int _class_instance;
int _orb_class_instance;
@@ -190,7 +193,9 @@
_collect_phase(false),
_fd(-1),
_linebuf_index(0),
- _parse_state(ISL2950_PARSE_STATE0_UNSYNC),
+ _parse_state(TFS_NOT_STARTED),
+ _frame_data{TOF_SFD1, TOF_SFD2, 0, 0},
+ _crc16(0),
_last_read(0),
_class_instance(-1),
_orb_class_instance(-1),
@@ -393,43 +398,111 @@ int
*/
ISL2950::collect()
{
- int ret;
+ int bytes_read = 0;
+ int bytes_available = 0;
+
+ int distance_mm = -1.0f;
+ bool full_frame = false;
+ bool stop_serial_read = false;
perf_begin(_sample_perf);
/* clear buffer if last read was too long ago */
- int64_t read_elapsed = hrt_elapsed_time(&_last_read);
+ int64_t read_elapsed = hrt_absolute_time();
+ read_elapsed = read_elapsed - _last_read;
+
+ /* the buffer for read chars is buflen minus null termination */
+ uint8_t readbuf[sizeof(_linebuf)];
+ unsigned readlen = sizeof(readbuf);
+
+ while ((!stop_serial_read)) {
+ /* read from the sensor (uart buffer) */
+ bytes_read = ::read(_fd, &readbuf[0], readlen);
+
+ if (bytes_read < 0) {
+ stop_serial_read = true;
+ PX4_DEBUG("read err: %d \n", bytes_read);
+ perf_count(_comms_errors);
+ perf_end(_sample_perf);
+
+ } else if (bytes_read > 0){
+ _last_read = hrt_absolute_time();
+ bytes_available += bytes_read;
+
+ //printf("Got a buffer with %d bytes,read %d \n", bytes_available,bytes_read);
+ for (int i = 0; i < bytes_read; i++) {
+ if (OK == isl2950_parser(readbuf[i],_frame_data, &_parse_state,&_crc16, &distance_mm)){
+ stop_serial_read = true;
+ full_frame = true;
+ }
+ }
+
+ }
+ }
+
+ if (!full_frame) {
+ return -EAGAIN;
+ }
+
+ printf("val (int): %d, raw: 0x%08X, valid: %s \n", distance_mm, _frame_data, ((full_frame) ? "OK" : "NO"));
+
+ struct distance_sensor_s report;
+
+ report.timestamp = hrt_absolute_time();
+ report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
+ report.orientation = _rotation;
+ report.current_distance = distance_mm/1000.0f;
+ report.min_distance = get_minimum_distance();
+ report.max_distance = get_maximum_distance();
+ report.covariance = 0.0f;
+ report.signal_quality = -1;
+ /* TODO: set proper ID */
+ report.id = 0;
+
+ /* publish it */
+ orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report);
+
+ _reports->force(&report);
+
+ /* notify anyone waiting for data */
+ poll_notify(POLLIN);
+
+ bytes_read = OK;
+
+ perf_end(_sample_perf);
+ return bytes_read;
- // ----------------------- LANBAO SPECIFIC ---------------------------
+
+ // ----------------------- LANBAO SPECIFIC ---------------------------
+/*
uint8_t buffer[50];
int bytes_available = 0;
int bytes_processed = 0;
int bytes_read = 0;
- bool full_frame;
- int distance_mm = -1.0f;
+ int distance_mm = -1.0f;
bytes_read = ::read(_fd, buffer + bytes_available, 50 - bytes_available);
//printf("read() returns %02X %02X %02X %02X \n", buffer[0], buffer[1],buffer[2],buffer[3] );
-
//--------------------------------------------------------------------
if (bytes_read < 0) {
PX4_ERR("isl2950 - read() error: %d \n", bytes_read);
perf_count(_comms_errors);
perf_end(_sample_perf);
- /* only throw an error if we time out */
+ // only throw an error if we time out
if (read_elapsed > (_conversion_interval * 2)) {
+ printf("read elapsed %d , conversion interval %d",read_elapsed,_conversion_interval * 2);
return bytes_read;
} else {
+ printf("EAGAIN",read_elapsed,_conversion_interval * 2);
return -EAGAIN;
}
} else if (bytes_read == 0) {
- return -EAGAIN; // SF0X drivers
- // LANBAO driver
+ return OK; // If we dont read any bites we simply exit from collecting
}
_last_read = hrt_absolute_time();
@@ -438,7 +511,9 @@ ISL2950::collect()
// parse the buffer data
full_frame = false;
+
bytes_processed = isl2950_parser(buffer, bytes_available, &full_frame,&distance_mm);
+ tempo = tempo - hrt_absolute_time();
//printf("isl2950_parser() processed %d bytes, full_frame %d \n", bytes_processed, full_frame);
// discard the processed bytes and move the buffer content to the head
@@ -449,9 +524,8 @@ ISL2950::collect()
printf("Measured Distance %d mm\n",distance_mm);
}
- else if (!full_frame) {
- return -EAGAIN;
-
+ else if (!full_frame) { // If the frame is not valid we avoid publishing it
+ return OK;
}
struct distance_sensor_s report;
@@ -459,26 +533,25 @@ ISL2950::collect()
report.timestamp = hrt_absolute_time();
report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
report.orientation = _rotation;
- report.current_distance = distance_mm /1000.f; // To meters
+ report.current_distance = distance_mm/1000; // To meters
report.min_distance = get_minimum_distance();
report.max_distance = get_maximum_distance();
report.covariance = 0.0f;
report.signal_quality = -1;
- /* TODO: set proper ID */
+ // TODO: set proper ID
report.id = 0;
- /* publish it */
+ // publish it
orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report);
_reports->force(&report);
- /* notify anyone waiting for data */
+ // notify anyone waiting for data
poll_notify(POLLIN);
- ret = OK;
-
+ printf("tempo %d \n",tempo);
perf_end(_sample_perf);
- return ret;
+ return OK; */
}
void
@@ -550,12 +623,7 @@ ISL2950::cycle()
PX4_ERR("baud %d ATTR", termios_state);
}
}
-
-
-
- /* collection phase? */
- if (_collect_phase) {
-
+ //printf("COLLECT \n");
/* perform collection */
int collect_ret = collect();
@@ -565,51 +633,27 @@ ISL2950::cycle()
return;
}
- if (OK != collect_ret) {
+
+ // ------------------- DISABLED CHECKING OF CONSECUTIVE FAIL
+ // if (OK != collect_ret) {
/* we know the sensor needs about four seconds to initialize */
- if (hrt_absolute_time() > 5 * 1000 * 1000LL && _consecutive_fail_count < 5) {
- PX4_ERR("collection error #%u", _consecutive_fail_count);
- }
+ // if (hrt_absolute_time() > 1 * 1000 * 1000LL && _consecutive_fail_count < 5) {
+ // PX4_ERR("collection error #%u", _consecutive_fail_count);
+ // }
- _consecutive_fail_count++;
+ // _consecutive_fail_count++;
/* restart the measurement state machine */
- start();
- return;
+ // start();
+ // return;
- } else {
+ // } else {
/* apparently success */
- _consecutive_fail_count = 0;
- }
-
- /* next phase is measurement */
- _collect_phase = false;
-
- /*
- * Is there a collect->measure gap?
- */
- if (_measure_ticks > USEC2TICK(_conversion_interval)) {
-
- /* schedule a fresh cycle call when we are ready to measure again */
- work_queue(HPWORK,
- &_work,
- (worker_t)&ISL2950::cycle_trampoline,
- this,
- _measure_ticks - USEC2TICK(_conversion_interval));
-
- return;
- }
- }
-
- /* measurement phase */
-/* if (OK != measure()) {
- PX4_DEBUG("measure error");
- }
-*/
+ // _consecutive_fail_count = 0;
+ // }
+ // ------------------- DISABLED CHECKING OF CONSECUTIVE FAIL
- /* next phase is collection */
- _collect_phase = true;
/* schedule a fresh cycle call when the measurement is done */
work_queue(HPWORK,
diff --git a/src/drivers/distance_sensor/airlango/isl2950_parser.cpp b/src/drivers/distance_sensor/airlango/isl2950_parser.cpp
index ea4c5b11e86d8a095c8fa139b6c05bcc8593982f..468e02299ad01d601e1c868c5d183753ebe765df 100644
--- a/src/drivers/distance_sensor/airlango/isl2950_parser.cpp
+++ b/src/drivers/distance_sensor/airlango/isl2950_parser.cpp
@@ -33,7 +33,8 @@
/**
* @file isl2950_parser.cpp
- * @author Claudio Micheli claudio@auterion.com
+ * @author Claudio Micheli
+ * claudio@auterion.com
*
*/
@@ -100,6 +101,7 @@ static const UCHAR aucCRCLo[] = {
0x41, 0x81, 0x80, 0x40
};
+
// TOF frame format
//
// 1B 1B 1B 1B 2B
@@ -109,9 +111,7 @@ static const UCHAR aucCRCLo[] = {
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;
-static unsigned char frame_data[TOF_CRC_CALC_DATA_LEN] = {
- TOF_SFD1, TOF_SFD2, 0, 0
-};
+
USHORT usMBCRC16(UCHAR* pucFrame, USHORT usLen) {
UCHAR ucCRCHi = 0xFF;
@@ -125,79 +125,81 @@ USHORT usMBCRC16(UCHAR* pucFrame, USHORT usLen) {
return (USHORT)(ucCRCHi << 8 | ucCRCLo);
}
-int isl2950_parser(const uint8_t* buffer, int length, bool* full_frame, int* dist)
+int isl2950_parser(uint8_t c, uint8_t *parserbuf, ISL2950_PARSE_STATE *state, uint16_t *crc16, int *dist)
{
- static TofFramingState state = TFS_NOT_STARTED;
- static uint16_t crc16 = 0;
- int bytes_processed = 0;
+ int ret = -1;
+// int bytes_processed = 0;
- while (bytes_processed < length) {
- uint8_t b = buffer[bytes_processed++];
+
+// uint8_t b = buffer[bytes_processed++]; // Can be removed
// printf("parse byte 0x%02X \n", b);
- switch (state) {
+ switch (*state) {
case TFS_NOT_STARTED:
- if (b == TOF_SFD1) {
- state = TFS_GOT_SFD1;
- // printf("Got SFD1 \n");
+ if (c == TOF_SFD1) {
+ *state = TFS_GOT_SFD1;
+ //printf("Got SFD1 \n");
}
break;
case TFS_GOT_SFD1:
- if (b == TOF_SFD2) {
- state = TFS_GOT_SFD2;
-// printf("Got SFD2 \n");
- } else if (b == TOF_SFD1) {
- state = TFS_GOT_SFD1;
+ if (c == TOF_SFD2) {
+ *state = TFS_GOT_SFD2;
+ //printf("Got SFD2 \n");
+ }
+ // @NOTE (claudio@auterion.com): Strange thing, if second byte is wrong we skip all the frame !!
+ else if (c == TOF_SFD1) {
+ *state = TFS_GOT_SFD1;
// printf("Discard previous SFD1, Got new SFD1 \n");
} else {
- state = TFS_NOT_STARTED;
+ *state = TFS_NOT_STARTED;
}
break;
case TFS_GOT_SFD2:
- frame_data[TOF_DISTANCE_MSB_POS] = b;
- state = TFS_GOT_DATA1;
-// printf("Got DATA1 0x%02X \n", b);
+ *state = TFS_GOT_DATA1;
+ parserbuf[TOF_DISTANCE_MSB_POS] = c; // MSB Data
+ //printf("Got DATA1 0x%02X \n", c);
break;
case TFS_GOT_DATA1:
- frame_data[TOF_DISTANCE_LSB_POS] = b;
- state = TFS_GOT_DATA2;
-// printf("Got DATA2 0x%02X \n", b);
+ *state = TFS_GOT_DATA2;
+ parserbuf[TOF_DISTANCE_LSB_POS] = c; // LSB Data
+ //printf("Got DATA2 0x%02X \n", c);
// do crc calculation
- crc16 = usMBCRC16(frame_data, TOF_CRC_CALC_DATA_LEN);
+ *crc16 = usMBCRC16(parserbuf, TOF_CRC_CALC_DATA_LEN);
// convert endian
- crc16 = (crc16 >> 8) | (crc16 << 8);
+ *crc16 = (*crc16 >> 8) | (*crc16 << 8);
break;
case TFS_GOT_DATA2:
- if (b == (crc16 >> 8)) {
- state = TFS_GOT_CHECKSUM1;
+ if (c == (*crc16 >> 8)) {
+ *state = TFS_GOT_CHECKSUM1;
} else {
-// printf("Checksum invalid on high byte: 0x%02X, calculated: 0x%04X \n",b, crc16);
- state = TFS_NOT_STARTED;
+ printf("Checksum invalid on high byte: 0x%02X, calculated: 0x%04X \n",c, *crc16);
+ //*state = TFS_NOT_STARTED;
+ *state = TFS_GOT_CHECKSUM1; // Forcing to print the value anyway
}
break;
case TFS_GOT_CHECKSUM1:
// Here, reset state to `NOT-STARTED` no matter crc ok or not
- state = TFS_NOT_STARTED;
- if (b == (crc16 & 0xFF)) {
+ *state = TFS_NOT_STARTED;
+ /*if (c == (*crc16 & 0xFF)) {
//printf("Checksum verified \n");
- *dist = (frame_data[TOF_DISTANCE_MSB_POS] << 8) | frame_data[TOF_DISTANCE_LSB_POS];
- *full_frame = true;
- return bytes_processed;
- } else {
- printf("Checksum invalidon low byte: 0x%02X, calculated: 0x%04X \n",b, crc16);
- }
+ *dist = (parserbuf[TOF_DISTANCE_MSB_POS] << 8) | parserbuf[TOF_DISTANCE_LSB_POS];
+ return OK;
+ }*/
+ *dist = (parserbuf[TOF_DISTANCE_MSB_POS] << 8) | parserbuf[TOF_DISTANCE_LSB_POS];
+ return OK;
+
break;
default:
printf("This should never happen. \n");
break;
}
- }
+
// SOME STUFFS
- return bytes_processed;
+ return ret;
}
diff --git a/src/drivers/distance_sensor/airlango/isl2950_parser.h b/src/drivers/distance_sensor/airlango/isl2950_parser.h
index b12bc53008cbc4dc2e9934666c887c561c589efc..4729c44fb3a1be6f6275778e36c7efc0d2d6f840 100644
--- a/src/drivers/distance_sensor/airlango/isl2950_parser.h
+++ b/src/drivers/distance_sensor/airlango/isl2950_parser.h
@@ -40,11 +40,12 @@
#pragma once
+#include <stdint.h>
// frame start delimiter
#define TOF_SFD1 0xA5
#define TOF_SFD2 0x5A
-typedef enum {
+/*typedef enum {
TFS_NOT_STARTED = 0,
TFS_GOT_SFD1,
TFS_GOT_SFD2,
@@ -52,7 +53,17 @@ typedef enum {
TFS_GOT_DATA2,
TFS_GOT_CHECKSUM1,
TFS_GOT_CHECKSUM2,
-} TofFramingState;
+} 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,
+};
enum IslWorkingMode {
KEEP_HEIGHT = 0,
@@ -61,7 +72,7 @@ enum IslWorkingMode {
// SF0X STYLE
-enum ISL2950_PARSE_STATE {
+/*enum ISL2950_PARSE_STATE {
ISL2950_PARSE_STATE0_UNSYNC = 0,
ISL2950_PARSE_STATE1_SYNC,
ISL2950_PARSE_STATE2_GOT_DIGIT0,
@@ -69,7 +80,7 @@ enum ISL2950_PARSE_STATE {
ISL2950_PARSE_STATE4_GOT_DIGIT1,
ISL2950_PARSE_STATE5_GOT_DIGIT2,
ISL2950_PARSE_STATE6_GOT_CARRIAGE_RETURN
-};
+};*/
-int isl2950_parser(const uint8_t* buffer, int length, bool* full_frame, int* dist);
+int isl2950_parser(uint8_t c, uint8_t *parserbuf,enum ISL2950_PARSE_STATE *state,uint16_t *crc16, int *dist);