Solved frequency rate issue. Now driver publish at ~10Hz.

Signed-off-by: Claudio Micheli <claudio@auterion.com>

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;
 
+   enum ISL2950_PARSE_STATE		_parse_state;
+   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 ---------------------------
-
+/*
   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
+  	return OK;                         // If we dont read any bites we simply exit from collecting
-                                       // LANBAO driver
   	}
 
   _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) {
+  else if (!full_frame) {     // If the frame is not valid we avoid publishing it
-    return -EAGAIN;
+    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);
 		}
 	}
-
+    //printf("COLLECT \n");
-
-
-	/* collection phase? */
-	if (_collect_phase) {
-
 		/* 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) {
+	//		if (hrt_absolute_time() > 1 * 1000 * 1000LL && _consecutive_fail_count < 5) {
-				PX4_ERR("collection error #%u", _consecutive_fail_count);
+	//			PX4_ERR("collection error #%u", _consecutive_fail_count);
-			}
+	//		}
 
-			_consecutive_fail_count++;
+	//		_consecutive_fail_count++;
 
 			/* restart the measurement state machine */
-			start();
+	//		start();
-			return;
+	//		return;
 
-		} else {
+	//	} else {
 			/* apparently success */
-			_consecutive_fail_count = 0;
+	//		_consecutive_fail_count = 0;
-		}
+	//	}
+  // ------------------- DISABLED CHECKING OF CONSECUTIVE FAIL
 
-		/* 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");
-	}
-*/
-
-	/* next phase is collection */
-	_collect_phase = true;
 
 	/* schedule a fresh cycle call when the measurement is done */
 	work_queue(HPWORK,

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;
+  int ret = -1;
-  static uint16_t crc16 = 0;
+//  int bytes_processed = 0;
-  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) {
+      if (c == TOF_SFD1) {
-        state = TFS_GOT_SFD1;
+        *state = TFS_GOT_SFD1;
-  //      printf("Got SFD1 \n");
+        //printf("Got SFD1 \n");
       }
       break;
 
     case TFS_GOT_SFD1:
-      if (b == TOF_SFD2) {
+      if (c == TOF_SFD2) {
-        state = TFS_GOT_SFD2;
+          *state = TFS_GOT_SFD2;
-//        printf("Got SFD2 \n");
+         //printf("Got SFD2 \n");
-      } else if (b == TOF_SFD1) {
+      }
-        state = TFS_GOT_SFD1;
+      // @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;
-      state = TFS_GOT_DATA1;
+      parserbuf[TOF_DISTANCE_MSB_POS] = c;                  // MSB Data
-//      printf("Got DATA1 0x%02X \n", b);
+      //printf("Got DATA1 0x%02X \n", c);
       break;
 
     case TFS_GOT_DATA1:
-      frame_data[TOF_DISTANCE_LSB_POS] = b;
+      *state = TFS_GOT_DATA2;
-      state = TFS_GOT_DATA2;
+      parserbuf[TOF_DISTANCE_LSB_POS] = c;                  // LSB Data
-//      printf("Got DATA2 0x%02X \n", b);
+      //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)) {
+      if (c == (*crc16 >> 8)) {
-        state = TFS_GOT_CHECKSUM1;
+        *state = TFS_GOT_CHECKSUM1;
       } else {
-//        printf("Checksum invalid on high byte: 0x%02X, calculated: 0x%04X \n",b, crc16);
+        printf("Checksum invalid on high byte: 0x%02X, calculated: 0x%04X \n",c, *crc16);
-        state = TFS_NOT_STARTED;
+        //*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;
+      *state = TFS_NOT_STARTED;
-      if (b == (crc16 & 0xFF)) {
+      /*if (c == (*crc16 & 0xFF)) {
       //printf("Checksum verified \n");
-        *dist = (frame_data[TOF_DISTANCE_MSB_POS] << 8) | frame_data[TOF_DISTANCE_LSB_POS];
+        *dist = (parserbuf[TOF_DISTANCE_MSB_POS] << 8) | parserbuf[TOF_DISTANCE_LSB_POS];
-        *full_frame = true;
+        return OK;
-        return bytes_processed;
+      }*/
-      } else {
+      *dist = (parserbuf[TOF_DISTANCE_MSB_POS] << 8) | parserbuf[TOF_DISTANCE_LSB_POS];
-        printf("Checksum invalidon low byte: 0x%02X, calculated: 0x%04X \n",b, crc16);
+      return OK;
-      }
+
       break;
 
     default:
       printf("This should never happen. \n");
       break;
     }
-  }
+
   // SOME STUFFS
-  return bytes_processed;
+  return ret;
 }

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);