Skip to content

Commit

Permalink
Debugging printf.
Browse files Browse the repository at this point in the history 
Signed-off-by: Claudio Micheli <[email protected]>
  • Loading branch information
@cmic0 @bkueng
cmic0 authored and bkueng committed Jan 15, 2019
1 parent 5d70ac7 commit 39ed84b
Show file tree
Hide file tree
Showing 2 changed files with 57 additions and 48 deletions.
72 changes: 41 additions & 31 deletions src/drivers/distance_sensor/isl2950/isl2950.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down Expand Up @@ -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"))
{
Expand Down Expand Up @@ -399,52 +398,56 @@ 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);
perf_count(_comms_errors);
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;
Expand All @@ -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;

}

Expand Down Expand Up @@ -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
Expand Down
33 changes: 16 additions & 17 deletions src/drivers/distance_sensor/isl2950/isl2950_parser.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Expand All @@ -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 ([email protected]): Strange thing, if second byte is wrong we skip all the frame !!
// @NOTE: ([email protected]): 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;
}
Expand All @@ -172,34 +169,36 @@ 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;
} 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:
printf("This should never happen. \n");
break;
}

// SOME STUFFS
return ret;
}

0 comments on commit 39ed84b

Please sign in to comment.