Skip to content

Commit

Permalink
Redefined reading strategy from serial to get the latest available data.
Browse files Browse the repository at this point in the history 
First "stable" version of the driver.

TODO: - Cleanup the code
      - Bench test (compare data with IMU)
      - Flight Test

Signed-off-by: Claudio Micheli <[email protected]>
  • Loading branch information
@cmic0 @bkueng
cmic0 authored and bkueng committed Jan 15, 2019
1 parent 39ed84b commit c5fece6
Show file tree
Hide file tree
Showing 2 changed files with 42 additions and 83 deletions.
107 changes: 33 additions & 74 deletions src/drivers/distance_sensor/isl2950/isl2950.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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},
Expand Down Expand Up @@ -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);

Expand All @@ -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;
}
Expand All @@ -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

/*
Expand All @@ -399,48 +354,52 @@ 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);
perf_count(_comms_errors);
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;

Expand Down Expand Up @@ -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 */
Expand Down Expand Up @@ -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;
}

Expand All @@ -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");
}

Expand Down
18 changes: 9 additions & 9 deletions src/drivers/distance_sensor/isl2950/isl2950_parser.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 ([email protected]) : 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:
Expand Down

0 comments on commit c5fece6

Please sign in to comment.