fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Removed blocking "while" cycle to access serial port.

Browse Source 
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 <claudio@auterion.com>
This commit is contained in:
Claudio Micheli 2018-12-17 17:39:08 +01:00 committed by Beat Küng
parent 93b3cf241b
commit b0ac8fe963
1 changed files with 10 additions and 120 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

130
src/drivers/distance_sensor/airlango/isl2950.cpp
View File

@ -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