From c5fece6568cce7a2e36fc499ba94cf25c042279c Mon Sep 17 00:00:00 2001 From: Claudio Micheli Date: Wed, 19 Dec 2018 12:24:16 +0100 Subject: [PATCH] Redefined reading strategy from serial to get the latest available data. First "stable" version of the driver. TODO: - Cleanup the code - Bench test (compare data with IMU) - Flight Test Signed-off-by: Claudio Micheli --- .../distance_sensor/isl2950/isl2950.cpp | 107 ++++++------------ .../isl2950/isl2950_parser.cpp | 18 +-- 2 files changed, 42 insertions(+), 83 deletions(-) diff --git a/src/drivers/distance_sensor/isl2950/isl2950.cpp b/src/drivers/distance_sensor/isl2950/isl2950.cpp index b7939b8797e5..003c72579a8e 100644 --- a/src/drivers/distance_sensor/isl2950/isl2950.cpp +++ b/src/drivers/distance_sensor/isl2950/isl2950.cpp @@ -80,7 +80,7 @@ #define ISL2950_DEFAULT_PORT "/dev/ttyS1" // Its baudrate is 115200 // normal conversion wait time - #define ISL2950_CONVERSION_INTERVAL 100*1000UL/* 100ms */ + #define ISL2950_CONVERSION_INTERVAL 50*1000UL/* 100ms */ class ISL2950 : public cdev::CDev @@ -108,11 +108,9 @@ int _conversion_interval; work_s _work{}; ringbuffer::RingBuffer *_reports; - int _measure_ticks; bool _collect_phase; int _fd; - uint8_t _linebuf[20]; - uint8_t _residual_bytes; + uint8_t _linebuf[25]; uint8_t _cycle_counter; enum ISL2950_PARSE_STATE _parse_state; @@ -156,7 +154,6 @@ * and start a new one. */ void cycle(); - int measure(); int collect(); /** * Static trampoline from the workq context; because we don't have a @@ -188,10 +185,8 @@ _max_distance(40.0f), _conversion_interval(ISL2950_CONVERSION_INTERVAL), _reports(nullptr), - _measure_ticks(0), _collect_phase(false), _fd(-1), - _residual_bytes(0), _cycle_counter(0), _parse_state(TFS_NOT_STARTED), _frame_data{TOF_SFD1, TOF_SFD2, 0, 0}, @@ -311,26 +306,13 @@ ISL2950::ioctl(device::file_t *filp, int cmd, unsigned long arg) /* set default polling rate */ case SENSOR_POLLRATE_DEFAULT: { - /* do we need to start internal polling? */ - bool want_start = (_measure_ticks == 0); - - /* set interval for next measurement to minimum legal value */ - _measure_ticks = USEC2TICK(_conversion_interval); - - /* if we need to start the poll state machine, do it */ - if (want_start) { - start(); - } - + start(); return OK; } /* adjust to a legal polling interval in Hz */ default: { - /* do we need to start internal polling? */ - bool want_start = (_measure_ticks == 0); - /* convert hz to tick interval via microseconds */ int ticks = USEC2TICK(1000000 / arg); @@ -339,13 +321,8 @@ ISL2950::ioctl(device::file_t *filp, int cmd, unsigned long arg) return -EINVAL; } - /* update interval for next measurement */ - _measure_ticks = ticks; - - /* if we need to start the poll state machine, do it */ - if (want_start) { start(); - } + return OK; } @@ -366,28 +343,6 @@ ISL2950::read(device::file_t *filp, char *buffer, size_t buflen) }*/ -int -ISL2950::measure() -{ - int ret; - - /* - * Send the command to begin a measurement. - */ - char cmd = ISL2950_TAKE_RANGE_REG; - ret = ::write(_fd, &cmd, 1); - - if (ret != sizeof(cmd)) { - perf_count(_comms_errors); - printf("write fail %d", ret); - return ret; - } - - ret = OK; - - return ret; -} - int /* @@ -399,17 +354,16 @@ ISL2950::collect() { int bytes_read = 0; int bytes_processed = 0; - + int i = 0; bool crc_valid = false; perf_begin(_sample_perf); - printf("residual bytes %d \n",_residual_bytes); - /* read from the sensor (uart buffer) */ - bytes_read = ::read(_fd, &_linebuf[_residual_bytes], 20 - _residual_bytes); + bytes_read = ::read(_fd, &_linebuf[0], sizeof(_linebuf)); + if (bytes_read < 0) { PX4_DEBUG("read err: %d \n", bytes_read); @@ -417,30 +371,35 @@ ISL2950::collect() perf_end(_sample_perf); } else if (bytes_read > 0){ - 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++; + // printf("Bytes read: %d \n",bytes_read); + i = bytes_read - 6 ; + while ((i >=0) && (!crc_valid)) + { + if (_linebuf[i] == TOF_SFD1) { + bytes_processed = i; + while ((bytes_processed < bytes_read) && (!crc_valid)) + { + // printf("In the cycle, processing byte %d, 0x%02X \n",bytes_processed, _linebuf[bytes_processed]); + if (OK == isl2950_parser(_linebuf[bytes_processed],_frame_data, &_parse_state,&_crc16, &_distance_mm)){ + crc_valid = true; + } + bytes_processed++; + } + _parse_state = TFS_NOT_STARTED; + + } + // else {printf("Starting frame wrong. Index: %d value 0x%02X \n",i,_linebuf[i]);} + i--; } + } - _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, ((crc_valid) ? "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; @@ -506,7 +465,7 @@ ISL2950::cycle_trampoline(void *arg) void ISL2950::cycle() { - PX4_DEBUG("ISL2950::cycle() - in the cycle"); + //PX4_DEBUG("ISL2950::cycle() - in the cycle"); /* fds initialized? */ if (_fd < 0) { /* open fd */ @@ -552,13 +511,14 @@ ISL2950::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)&ISL2950::cycle_trampoline,this,USEC2TICK(1000LL)); - return; + // 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 - _cycle_counter * 1000LL)); + //work_queue(HPWORK,&_work,(worker_t)&ISL2950::cycle_trampoline,this,USEC2TICK(_conversion_interval - _cycle_counter * 1000LL)); + work_queue(HPWORK,&_work,(worker_t)&ISL2950::cycle_trampoline,this,USEC2TICK(_conversion_interval)); _cycle_counter = 0; } @@ -567,7 +527,6 @@ ISL2950::print_info() { perf_print_counter(_sample_perf); perf_print_counter(_comms_errors); - printf("poll interval: %d ticks\n", _measure_ticks); _reports->print_info("report queue"); } diff --git a/src/drivers/distance_sensor/isl2950/isl2950_parser.cpp b/src/drivers/distance_sensor/isl2950/isl2950_parser.cpp index 0c73ff7c170c..0c5e9de23335 100644 --- a/src/drivers/distance_sensor/isl2950/isl2950_parser.cpp +++ b/src/drivers/distance_sensor/isl2950/isl2950_parser.cpp @@ -169,30 +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; - //*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); + // 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"); + // printf("Checksum verified \n"); *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); } + /*else { + printf("Checksum not verified \n"); + //printf("Checksum invalidon low byte: 0x%02X, calculated: 0x%04X \n",c, *crc16); + }*/ break; default: