mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
Solved frequency rate issue. Now driver publish at ~10Hz.
Signed-off-by: Claudio Micheli <claudio@auterion.com>
This commit is contained in:
Claudio Micheli
Beat Küng
parent
4b7be38e67
commit
dd300dca0a
@ -78,9 +78,9 @@
|
||||
|
||||
// designated serial port on Pixhawk
|
||||
#define ISL2950_DEFAULT_PORT "/dev/ttyS1" // Its baudrate is 115200
|
||||
// #define LANBAO_DEFAULT_BAUDRATE 115200
|
||||
|
||||
// normal conversion wait time
|
||||
#define ISL2950_CONVERSION_INTERVAL 50*1000UL /* 50ms */
|
||||
#define ISL2950_CONVERSION_INTERVAL 100*1000UL/* 100ms */
|
||||
|
||||
|
||||
class ISL2950 : public cdev::CDev
|
||||
@ -105,17 +105,20 @@
|
||||
uint8_t _rotation;
|
||||
float _min_distance;
|
||||
float _max_distance;
|
||||
int _conversion_interval;
|
||||
int _conversion_interval;
|
||||
work_s _work{};
|
||||
ringbuffer::RingBuffer *_reports;
|
||||
int _measure_ticks;
|
||||
bool _collect_phase;
|
||||
int _fd;
|
||||
char _linebuf[50];
|
||||
uint8_t _linebuf[20];
|
||||
unsigned _linebuf_index;
|
||||
enum ISL2950_PARSE_STATE _parse_state;
|
||||
hrt_abstime _last_read;
|
||||
|
||||
enum ISL2950_PARSE_STATE _parse_state;
|
||||
unsigned char _frame_data[4];
|
||||
uint16_t _crc16;
|
||||
|
||||
hrt_abstime _last_read;
|
||||
int _class_instance;
|
||||
int _orb_class_instance;
|
||||
|
||||
@ -190,7 +193,9 @@
|
||||
_collect_phase(false),
|
||||
_fd(-1),
|
||||
_linebuf_index(0),
|
||||
_parse_state(ISL2950_PARSE_STATE0_UNSYNC),
|
||||
_parse_state(TFS_NOT_STARTED),
|
||||
_frame_data{TOF_SFD1, TOF_SFD2, 0, 0},
|
||||
_crc16(0),
|
||||
_last_read(0),
|
||||
_class_instance(-1),
|
||||
_orb_class_instance(-1),
|
||||
@ -393,43 +398,111 @@ int
|
||||
*/
|
||||
ISL2950::collect()
|
||||
{
|
||||
int ret;
|
||||
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_elapsed_time(&_last_read);
|
||||
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);
|
||||
|
||||
if (bytes_read < 0) {
|
||||
stop_serial_read = true;
|
||||
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();
|
||||
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;
|
||||
}
|
||||
|
||||
printf("val (int): %d, raw: 0x%08X, valid: %s \n", distance_mm, _frame_data, ((full_frame) ? "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.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);
|
||||
|
||||
bytes_read = OK;
|
||||
|
||||
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;
|
||||
bool full_frame;
|
||||
int distance_mm = -1.0f;
|
||||
|
||||
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 */
|
||||
// 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 -EAGAIN; // SF0X drivers
|
||||
// LANBAO driver
|
||||
return OK; // If we dont read any bites we simply exit from collecting
|
||||
}
|
||||
|
||||
_last_read = hrt_absolute_time();
|
||||
@ -438,7 +511,9 @@ ISL2950::collect()
|
||||
|
||||
// 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
|
||||
@ -449,9 +524,8 @@ ISL2950::collect()
|
||||
printf("Measured Distance %d mm\n",distance_mm);
|
||||
}
|
||||
|
||||
else if (!full_frame) {
|
||||
return -EAGAIN;
|
||||
|
||||
else if (!full_frame) { // If the frame is not valid we avoid publishing it
|
||||
return OK;
|
||||
}
|
||||
|
||||
struct distance_sensor_s report;
|
||||
@ -459,26 +533,25 @@ ISL2950::collect()
|
||||
report.timestamp = hrt_absolute_time();
|
||||
report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
|
||||
report.orientation = _rotation;
|
||||
report.current_distance = distance_mm /1000.f; // To meters
|
||||
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 */
|
||||
// TODO: set proper ID
|
||||
report.id = 0;
|
||||
|
||||
/* publish it */
|
||||
// publish it
|
||||
orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report);
|
||||
|
||||
_reports->force(&report);
|
||||
|
||||
/* notify anyone waiting for data */
|
||||
// notify anyone waiting for data
|
||||
poll_notify(POLLIN);
|
||||
|
||||
ret = OK;
|
||||
|
||||
printf("tempo %d \n",tempo);
|
||||
perf_end(_sample_perf);
|
||||
return ret;
|
||||
return OK; */
|
||||
}
|
||||
|
||||
void
|
||||
@ -550,12 +623,7 @@ ISL2950::cycle()
|
||||
PX4_ERR("baud %d ATTR", termios_state);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
/* collection phase? */
|
||||
if (_collect_phase) {
|
||||
|
||||
//printf("COLLECT \n");
|
||||
/* perform collection */
|
||||
int collect_ret = collect();
|
||||
|
||||
@ -565,51 +633,27 @@ ISL2950::cycle()
|
||||
return;
|
||||
}
|
||||
|
||||
if (OK != collect_ret) {
|
||||
|
||||
// ------------------- DISABLED CHECKING OF CONSECUTIVE FAIL
|
||||
// if (OK != collect_ret) {
|
||||
|
||||
/* we know the sensor needs about four seconds to initialize */
|
||||
if (hrt_absolute_time() > 5 * 1000 * 1000LL && _consecutive_fail_count < 5) {
|
||||
PX4_ERR("collection error #%u", _consecutive_fail_count);
|
||||
}
|
||||
// if (hrt_absolute_time() > 1 * 1000 * 1000LL && _consecutive_fail_count < 5) {
|
||||
// PX4_ERR("collection error #%u", _consecutive_fail_count);
|
||||
// }
|
||||
|
||||
_consecutive_fail_count++;
|
||||
// _consecutive_fail_count++;
|
||||
|
||||
/* restart the measurement state machine */
|
||||
start();
|
||||
return;
|
||||
// start();
|
||||
// return;
|
||||
|
||||
} else {
|
||||
// } else {
|
||||
/* apparently success */
|
||||
_consecutive_fail_count = 0;
|
||||
}
|
||||
// _consecutive_fail_count = 0;
|
||||
// }
|
||||
// ------------------- DISABLED CHECKING OF CONSECUTIVE FAIL
|
||||
|
||||
/* next phase is measurement */
|
||||
_collect_phase = false;
|
||||
|
||||
/*
|
||||
* Is there a collect->measure gap?
|
||||
*/
|
||||
if (_measure_ticks > USEC2TICK(_conversion_interval)) {
|
||||
|
||||
/* schedule a fresh cycle call when we are ready to measure again */
|
||||
work_queue(HPWORK,
|
||||
&_work,
|
||||
(worker_t)&ISL2950::cycle_trampoline,
|
||||
this,
|
||||
_measure_ticks - USEC2TICK(_conversion_interval));
|
||||
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
/* measurement phase */
|
||||
/* if (OK != measure()) {
|
||||
PX4_DEBUG("measure error");
|
||||
}
|
||||
*/
|
||||
|
||||
/* next phase is collection */
|
||||
_collect_phase = true;
|
||||
|
||||
/* schedule a fresh cycle call when the measurement is done */
|
||||
work_queue(HPWORK,
|
||||
|
||||
@ -33,7 +33,8 @@
|
||||
|
||||
/**
|
||||
* @file isl2950_parser.cpp
|
||||
* @author Claudio Micheli claudio@auterion.com
|
||||
* @author Claudio Micheli
|
||||
* claudio@auterion.com
|
||||
*
|
||||
*/
|
||||
|
||||
@ -100,6 +101,7 @@ static const UCHAR aucCRCLo[] = {
|
||||
0x41, 0x81, 0x80, 0x40
|
||||
};
|
||||
|
||||
|
||||
// TOF frame format
|
||||
//
|
||||
// 1B 1B 1B 1B 2B
|
||||
@ -109,9 +111,7 @@ static const UCHAR aucCRCLo[] = {
|
||||
const static int TOF_DISTANCE_MSB_POS = 2;
|
||||
const static int TOF_DISTANCE_LSB_POS = 3;
|
||||
const static int TOF_CRC_CALC_DATA_LEN = 4;
|
||||
static unsigned char frame_data[TOF_CRC_CALC_DATA_LEN] = {
|
||||
TOF_SFD1, TOF_SFD2, 0, 0
|
||||
};
|
||||
|
||||
|
||||
USHORT usMBCRC16(UCHAR* pucFrame, USHORT usLen) {
|
||||
UCHAR ucCRCHi = 0xFF;
|
||||
@ -125,79 +125,81 @@ USHORT usMBCRC16(UCHAR* pucFrame, USHORT usLen) {
|
||||
return (USHORT)(ucCRCHi << 8 | ucCRCLo);
|
||||
}
|
||||
|
||||
int isl2950_parser(const uint8_t* buffer, int length, bool* full_frame, int* dist)
|
||||
int isl2950_parser(uint8_t c, uint8_t *parserbuf, ISL2950_PARSE_STATE *state, uint16_t *crc16, int *dist)
|
||||
{
|
||||
static TofFramingState state = TFS_NOT_STARTED;
|
||||
static uint16_t crc16 = 0;
|
||||
int bytes_processed = 0;
|
||||
int ret = -1;
|
||||
// int bytes_processed = 0;
|
||||
|
||||
while (bytes_processed < length) {
|
||||
uint8_t b = buffer[bytes_processed++];
|
||||
|
||||
// uint8_t b = buffer[bytes_processed++]; // Can be removed
|
||||
// printf("parse byte 0x%02X \n", b);
|
||||
|
||||
switch (state) {
|
||||
switch (*state) {
|
||||
case TFS_NOT_STARTED:
|
||||
if (b == TOF_SFD1) {
|
||||
state = TFS_GOT_SFD1;
|
||||
// printf("Got SFD1 \n");
|
||||
if (c == TOF_SFD1) {
|
||||
*state = TFS_GOT_SFD1;
|
||||
//printf("Got SFD1 \n");
|
||||
}
|
||||
break;
|
||||
|
||||
case TFS_GOT_SFD1:
|
||||
if (b == TOF_SFD2) {
|
||||
state = TFS_GOT_SFD2;
|
||||
// printf("Got SFD2 \n");
|
||||
} else if (b == TOF_SFD1) {
|
||||
state = TFS_GOT_SFD1;
|
||||
if (c == TOF_SFD2) {
|
||||
*state = TFS_GOT_SFD2;
|
||||
//printf("Got SFD2 \n");
|
||||
}
|
||||
// @NOTE (claudio@auterion.com): Strange thing, if second byte is wrong we skip all the frame !!
|
||||
else if (c == TOF_SFD1) {
|
||||
*state = TFS_GOT_SFD1;
|
||||
// printf("Discard previous SFD1, Got new SFD1 \n");
|
||||
} else {
|
||||
state = TFS_NOT_STARTED;
|
||||
*state = TFS_NOT_STARTED;
|
||||
}
|
||||
break;
|
||||
|
||||
case TFS_GOT_SFD2:
|
||||
frame_data[TOF_DISTANCE_MSB_POS] = b;
|
||||
state = TFS_GOT_DATA1;
|
||||
// printf("Got DATA1 0x%02X \n", b);
|
||||
*state = TFS_GOT_DATA1;
|
||||
parserbuf[TOF_DISTANCE_MSB_POS] = c; // MSB Data
|
||||
//printf("Got DATA1 0x%02X \n", c);
|
||||
break;
|
||||
|
||||
case TFS_GOT_DATA1:
|
||||
frame_data[TOF_DISTANCE_LSB_POS] = b;
|
||||
state = TFS_GOT_DATA2;
|
||||
// printf("Got DATA2 0x%02X \n", b);
|
||||
*state = TFS_GOT_DATA2;
|
||||
parserbuf[TOF_DISTANCE_LSB_POS] = c; // LSB Data
|
||||
//printf("Got DATA2 0x%02X \n", c);
|
||||
// do crc calculation
|
||||
crc16 = usMBCRC16(frame_data, TOF_CRC_CALC_DATA_LEN);
|
||||
*crc16 = usMBCRC16(parserbuf, TOF_CRC_CALC_DATA_LEN);
|
||||
// convert endian
|
||||
crc16 = (crc16 >> 8) | (crc16 << 8);
|
||||
*crc16 = (*crc16 >> 8) | (*crc16 << 8);
|
||||
break;
|
||||
|
||||
case TFS_GOT_DATA2:
|
||||
if (b == (crc16 >> 8)) {
|
||||
state = TFS_GOT_CHECKSUM1;
|
||||
if (c == (*crc16 >> 8)) {
|
||||
*state = TFS_GOT_CHECKSUM1;
|
||||
} else {
|
||||
// printf("Checksum invalid on high byte: 0x%02X, calculated: 0x%04X \n",b, crc16);
|
||||
state = TFS_NOT_STARTED;
|
||||
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:
|
||||
// Here, reset state to `NOT-STARTED` no matter crc ok or not
|
||||
state = TFS_NOT_STARTED;
|
||||
if (b == (crc16 & 0xFF)) {
|
||||
*state = TFS_NOT_STARTED;
|
||||
/*if (c == (*crc16 & 0xFF)) {
|
||||
//printf("Checksum verified \n");
|
||||
*dist = (frame_data[TOF_DISTANCE_MSB_POS] << 8) | frame_data[TOF_DISTANCE_LSB_POS];
|
||||
*full_frame = true;
|
||||
return bytes_processed;
|
||||
} else {
|
||||
printf("Checksum invalidon low byte: 0x%02X, calculated: 0x%04X \n",b, crc16);
|
||||
}
|
||||
*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;
|
||||
|
||||
break;
|
||||
|
||||
default:
|
||||
printf("This should never happen. \n");
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// SOME STUFFS
|
||||
return bytes_processed;
|
||||
return ret;
|
||||
}
|
||||
|
||||
@ -40,11 +40,12 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <stdint.h>
|
||||
// frame start delimiter
|
||||
#define TOF_SFD1 0xA5
|
||||
#define TOF_SFD2 0x5A
|
||||
|
||||
typedef enum {
|
||||
/*typedef enum {
|
||||
TFS_NOT_STARTED = 0,
|
||||
TFS_GOT_SFD1,
|
||||
TFS_GOT_SFD2,
|
||||
@ -52,7 +53,17 @@ typedef enum {
|
||||
TFS_GOT_DATA2,
|
||||
TFS_GOT_CHECKSUM1,
|
||||
TFS_GOT_CHECKSUM2,
|
||||
} TofFramingState;
|
||||
} TofFramingState;*/
|
||||
|
||||
enum ISL2950_PARSE_STATE {
|
||||
TFS_NOT_STARTED = 0,
|
||||
TFS_GOT_SFD1,
|
||||
TFS_GOT_SFD2,
|
||||
TFS_GOT_DATA1,
|
||||
TFS_GOT_DATA2,
|
||||
TFS_GOT_CHECKSUM1,
|
||||
TFS_GOT_CHECKSUM2,
|
||||
};
|
||||
|
||||
enum IslWorkingMode {
|
||||
KEEP_HEIGHT = 0,
|
||||
@ -61,7 +72,7 @@ enum IslWorkingMode {
|
||||
|
||||
|
||||
// SF0X STYLE
|
||||
enum ISL2950_PARSE_STATE {
|
||||
/*enum ISL2950_PARSE_STATE {
|
||||
ISL2950_PARSE_STATE0_UNSYNC = 0,
|
||||
ISL2950_PARSE_STATE1_SYNC,
|
||||
ISL2950_PARSE_STATE2_GOT_DIGIT0,
|
||||
@ -69,7 +80,7 @@ enum ISL2950_PARSE_STATE {
|
||||
ISL2950_PARSE_STATE4_GOT_DIGIT1,
|
||||
ISL2950_PARSE_STATE5_GOT_DIGIT2,
|
||||
ISL2950_PARSE_STATE6_GOT_CARRIAGE_RETURN
|
||||
};
|
||||
};*/
|
||||
|
||||
|
||||
int isl2950_parser(const uint8_t* buffer, int length, bool* full_frame, int* dist);
|
||||
int isl2950_parser(uint8_t c, uint8_t *parserbuf,enum ISL2950_PARSE_STATE *state,uint16_t *crc16, int *dist);
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user