zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Debugging printf. 

Signed-off-by: Claudio Micheli <claudio@auterion.com>
sbg
Claudio Micheli 6 years ago committed by Beat Küng
parent
5d70ac7fd9
commit
39ed84b5cd
2 changed files with 57 additions and 48 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 72
      src/drivers/distance_sensor/isl2950/isl2950.cpp
  2. 33
      src/drivers/distance_sensor/isl2950/isl2950_parser.cpp

72
src/drivers/distance_sensor/isl2950/isl2950.cpp
Unescape View File

@ -112,20 +112,19 @@ @@ -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 @@ @@ -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 @@ -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() @@ -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() @@ -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() @@ -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

33
src/drivers/distance_sensor/isl2950/isl2950_parser.cpp
Unescape View File

@ -128,10 +128,7 @@ USHORT usMBCRC16(UCHAR* pucFrame, USHORT usLen) { @@ -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 @@ -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 @@ -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 @@ -200,6 +200,5 @@ int isl2950_parser(uint8_t c, uint8_t *parserbuf, ISL2950_PARSE_STATE *state, ui
break;
}
// SOME STUFFS
return ret;
}

Write Preview
Loading…
Cancel
Save