diff --git a/src/drivers/distance_sensor/isl2950/isl2950.cpp b/src/drivers/distance_sensor/isl2950/isl2950.cpp index fed521bf8791..b7939b8797e5 100644 --- a/src/drivers/distance_sensor/isl2950/isl2950.cpp +++ b/src/drivers/distance_sensor/isl2950/isl2950.cpp @@ -112,20 +112,19 @@ bool _collect_phase; int _fd; uint8_t _linebuf[20]; - unsigned _linebuf_index; + uint8_t _residual_bytes; + uint8_t _cycle_counter; enum ISL2950_PARSE_STATE _parse_state; unsigned char _frame_data[4]; uint16_t _crc16; + int _distance_mm; - hrt_abstime _last_read; int _class_instance; int _orb_class_instance; orb_advert_t _distance_sensor_topic; - unsigned _consecutive_fail_count; - perf_counter_t _sample_perf; perf_counter_t _comms_errors; @@ -185,22 +184,22 @@ ISL2950::ISL2950(const char *port, uint8_t rotation) : CDev(RANGE_FINDER0_DEVICE_PATH), _rotation(rotation), - _min_distance(0.10f), + _min_distance(0.14f), _max_distance(40.0f), _conversion_interval(ISL2950_CONVERSION_INTERVAL), _reports(nullptr), _measure_ticks(0), _collect_phase(false), _fd(-1), - _linebuf_index(0), + _residual_bytes(0), + _cycle_counter(0), _parse_state(TFS_NOT_STARTED), _frame_data{TOF_SFD1, TOF_SFD2, 0, 0}, _crc16(0), - _last_read(0), + _distance_mm(-1), _class_instance(-1), _orb_class_instance(-1), _distance_sensor_topic(nullptr), - _consecutive_fail_count(0), _sample_perf(perf_alloc(PC_ELAPSED, "isl2950_read")), _comms_errors(perf_alloc(PC_COUNT, "isl2950_com_err")) { @@ -399,21 +398,18 @@ int ISL2950::collect() { int bytes_read = 0; + int bytes_processed = 0; - int distance_mm = -1.0f; - bool full_frame = false; + bool crc_valid = false; perf_begin(_sample_perf); - /* the buffer for read chars is buflen minus null termination */ - uint8_t readbuf[sizeof(_linebuf)]; - unsigned readlen = sizeof(readbuf); - + printf("residual bytes %d \n",_residual_bytes); /* read from the sensor (uart buffer) */ - bytes_read = ::read(_fd, &readbuf[0], readlen); + bytes_read = ::read(_fd, &_linebuf[_residual_bytes], 20 - _residual_bytes); if (bytes_read < 0) { PX4_DEBUG("read err: %d \n", bytes_read); @@ -421,30 +417,37 @@ ISL2950::collect() perf_end(_sample_perf); } else if (bytes_read > 0){ - _last_read = hrt_absolute_time(); - - for (int i = 0; i < bytes_read; i++) { - if (OK == isl2950_parser(readbuf[i],_frame_data, &_parse_state,&_crc16, &distance_mm)){ - - full_frame = true; + printf("Bytes read: %d \n",bytes_read); + _residual_bytes += bytes_read; + while ((bytes_processed < _residual_bytes) && (!crc_valid)) + { + printf("In the cycle, processing byte %d \n",bytes_processed); + if (OK == isl2950_parser(_linebuf[bytes_processed],_frame_data, &_parse_state,&_crc16, &_distance_mm)){ + crc_valid = true; } + bytes_processed++; } - } - - - if (!full_frame) { + _residual_bytes -= bytes_processed; + printf("Bytes read: %d Bytes Processed: %d Residual: %d \n",bytes_read,bytes_processed,_residual_bytes); + if (_residual_bytes > 0) { + printf("copy from %d to %d \n",bytes_processed,bytes_read); + memmove(&_linebuf[0], &_linebuf[bytes_processed], bytes_read); + } + + if (!crc_valid) { return -EAGAIN; } - printf("val (int): %d, raw: 0x%08X, valid: %s \n", distance_mm, _frame_data, ((full_frame) ? "OK" : "NO")); + + printf("val (int): %d, raw: 0x%08X, valid: %s \n", _distance_mm, _frame_data, ((crc_valid) ? "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.current_distance = _distance_mm/1000.0f; report.min_distance = get_minimum_distance(); report.max_distance = get_maximum_distance(); report.covariance = 0.0f; @@ -463,7 +466,13 @@ ISL2950::collect() bytes_read = OK; perf_end(_sample_perf); - return bytes_read; + + /* ENABLE THIS IF YOU WANT TO PRINT OLD VALUES WHILE CRC CHECK IS WRONG + if (!crc_valid) { + return -EAGAIN; + } + else return OK; */ + return OK; } @@ -541,15 +550,16 @@ ISL2950::cycle() int collect_ret = collect(); if (collect_ret == -EAGAIN) { - + _cycle_counter++; /* We are missing bytes to complete the packet, re-cycle at 1ms */ - work_queue(HPWORK,&_work,(worker_t)&ISL2950::cycle_trampoline,this,USEC2TICK(500LL)); + work_queue(HPWORK,&_work,(worker_t)&ISL2950::cycle_trampoline,this,USEC2TICK(1000LL)); return; } /* schedule a fresh cycle call when a complete packet has been received */ - work_queue(HPWORK,&_work,(worker_t)&ISL2950::cycle_trampoline,this,USEC2TICK(_conversion_interval)); + work_queue(HPWORK,&_work,(worker_t)&ISL2950::cycle_trampoline,this,USEC2TICK(_conversion_interval - _cycle_counter * 1000LL)); + _cycle_counter = 0; } void diff --git a/src/drivers/distance_sensor/isl2950/isl2950_parser.cpp b/src/drivers/distance_sensor/isl2950/isl2950_parser.cpp index 468e02299ad0..0c73ff7c170c 100644 --- a/src/drivers/distance_sensor/isl2950/isl2950_parser.cpp +++ b/src/drivers/distance_sensor/isl2950/isl2950_parser.cpp @@ -128,10 +128,7 @@ USHORT usMBCRC16(UCHAR* pucFrame, USHORT usLen) { int isl2950_parser(uint8_t c, uint8_t *parserbuf, ISL2950_PARSE_STATE *state, uint16_t *crc16, int *dist) { int ret = -1; -// int bytes_processed = 0; - -// uint8_t b = buffer[bytes_processed++]; // Can be removed // printf("parse byte 0x%02X \n", b); switch (*state) { @@ -147,10 +144,10 @@ int isl2950_parser(uint8_t c, uint8_t *parserbuf, ISL2950_PARSE_STATE *state, ui *state = TFS_GOT_SFD2; //printf("Got SFD2 \n"); } - // @NOTE (claudio@auterion.com): Strange thing, if second byte is wrong we skip all the frame !! + // @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; -// printf("Discard previous SFD1, Got new SFD1 \n"); + //printf("Discard previous SFD1, Got new SFD1 \n"); } else { *state = TFS_NOT_STARTED; } @@ -172,27 +169,30 @@ int isl2950_parser(uint8_t c, uint8_t *parserbuf, ISL2950_PARSE_STATE *state, ui *crc16 = (*crc16 >> 8) | (*crc16 << 8); break; + /* @NOTE (claudio@auterion.com) : Since data that not pass this crc check seems to be valid anyway, it will be published discard the bad high CRC*/ case TFS_GOT_DATA2: if (c == (*crc16 >> 8)) { *state = TFS_GOT_CHECKSUM1; - } else { - 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 + //*dist = (parserbuf[TOF_DISTANCE_MSB_POS] << 8) | parserbuf[TOF_DISTANCE_LSB_POS]; + //return OK; + } + else { + 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: + case TFS_GOT_CHECKSUM1: // Here, reset state to `NOT-STARTED` no matter crc ok or not *state = TFS_NOT_STARTED; - /*if (c == (*crc16 & 0xFF)) { - //printf("Checksum verified \n"); + if (c == (*crc16 & 0xFF)) { + printf("Checksum verified \n"); *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; - + } else { + //printf("Checksum invalidon low byte: 0x%02X, calculated: 0x%04X \n",c, *crc16); + } break; default: @@ -200,6 +200,5 @@ int isl2950_parser(uint8_t c, uint8_t *parserbuf, ISL2950_PARSE_STATE *state, ui break; } - // SOME STUFFS return ret; }