From b0ac8fe9635ea57750ecc0a7714f32d62f99de77 Mon Sep 17 00:00:00 2001 From: Claudio Micheli Date: Mon, 17 Dec 2018 17:39:08 +0100 Subject: [PATCH] Removed blocking "while" cycle to access serial port. Serial is now cycled with work_queue rescheduling if some bytes are missing. TODO: - Fix occasional sensor spikes (can be identified with crc) - Clean up the code - disable debug printf Signed-off-by: Claudio Micheli --- .../distance_sensor/airlango/isl2950.cpp | 130 ++---------------- 1 file changed, 10 insertions(+), 120 deletions(-) diff --git a/src/drivers/distance_sensor/airlango/isl2950.cpp b/src/drivers/distance_sensor/airlango/isl2950.cpp index 30dbe88308e8..559a33f07273 100644 --- a/src/drivers/distance_sensor/airlango/isl2950.cpp +++ b/src/drivers/distance_sensor/airlango/isl2950.cpp @@ -399,23 +399,19 @@ int ISL2950::collect() { 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_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); @@ -427,18 +423,16 @@ ISL2950::collect() } 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; @@ -472,86 +466,6 @@ ISL2950::collect() 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; - - 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 - 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 OK; // If we dont read any bites we simply exit from collecting - } - - _last_read = hrt_absolute_time(); - - bytes_available += bytes_read; - - // 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 - bytes_available -= bytes_processed; - memcpy(buffer, buffer + bytes_processed, bytes_available); - - if (full_frame) { - printf("Measured Distance %d mm\n",distance_mm); - } - - else if (!full_frame) { // If the frame is not valid we avoid publishing it - return OK; - } - - 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; // 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 - 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); - - printf("tempo %d \n",tempo); - perf_end(_sample_perf); - return OK; */ } void @@ -623,44 +537,20 @@ ISL2950::cycle() PX4_ERR("baud %d ATTR", termios_state); } } - //printf("COLLECT \n"); + /* perform collection */ int collect_ret = collect(); if (collect_ret == -EAGAIN) { - /* reschedule to grab the missing bits, time to transmit 8 bytes @ 9600 bps */ - work_queue(HPWORK,&_work,(worker_t)&ISL2950::cycle_trampoline,this,USEC2TICK(1042 * 8)); + + /* 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; } - // ------------------- DISABLED CHECKING OF CONSECUTIVE FAIL - // if (OK != collect_ret) { - - /* we know the sensor needs about four seconds to initialize */ - // if (hrt_absolute_time() > 1 * 1000 * 1000LL && _consecutive_fail_count < 5) { - // PX4_ERR("collection error #%u", _consecutive_fail_count); - // } - - // _consecutive_fail_count++; - - /* restart the measurement state machine */ - // start(); - // return; - - // } else { - /* apparently success */ - // _consecutive_fail_count = 0; - // } - // ------------------- DISABLED CHECKING OF CONSECUTIVE FAIL - - - /* schedule a fresh cycle call when the measurement is done */ - work_queue(HPWORK, - &_work, - (worker_t)&ISL2950::cycle_trampoline, - this, - USEC2TICK(_conversion_interval)); + /* 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)); } void