dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Code Cleanup. Added configuration notes. 

Signed-off-by: Claudio Micheli <claudio@auterion.com>
This commit is contained in:
Claudio Micheli 2018-12-21 11:25:03 +01:00 committed by Beat Küng
parent c5fece6568
commit aee6a83fe0
3 changed files with 138 additions and 177 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

160
src/drivers/distance_sensor/isl2950/isl2950.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2014-2015 PX4 Development Team. All rights reserved.
* Copyright (c) 2018 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -32,10 +32,13 @@
****************************************************************************/
/**
* @file landbao15L2950.cpp
* @file isl2950.cpp
* @author Claudio Micheli <claudio@auterion.com>
*
* Driver for the ISL2950
* Driver for the Lanbao PSK-CM8JL65-CC5 distance sensor.
* Make sure to disable MAVLINK messages (MAV_0_CONFIG PARAMETER)
* on the serial port you connect the sensor,i.e TELEM1.
*
*/
#include <px4_config.h>
@ -46,13 +49,9 @@
#include <stdint.h>
#include <stdlib.h>
#include <stdbool.h>
#include <semaphore.h>
#include <string.h>
#include <fcntl.h>
#include <poll.h>
#include <errno.h>
#include <stdio.h>
#include <math.h>
#include <unistd.h>
#include <termios.h>
@ -76,24 +75,25 @@
#define ISL2950_TAKE_RANGE_REG 'd'
// designated serial port on Pixhawk
#define ISL2950_DEFAULT_PORT "/dev/ttyS1" // Its baudrate is 115200
// designated serial port on Pixhawk (TELEM1)
#define ISL2950_DEFAULT_PORT "/dev/ttyS1" // Its baudrate is 115200
// normal conversion wait time
#define ISL2950_CONVERSION_INTERVAL 50*1000UL/* 100ms */
// normal conversion wait time
#define ISL2950_CONVERSION_INTERVAL 50*1000UL/* 50ms */
class ISL2950 : public cdev::CDev
{
public:
// Constructor
ISL2950(const char *port = ISL2950_DEFAULT_PORT, uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
// Virtual destructor
virtual ~ISL2950();
virtual int init();
//virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen);
virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg);
// Constructor
ISL2950(const char *port = ISL2950_DEFAULT_PORT, uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
// Virtual destructor
virtual ~ISL2950();
virtual int init();
virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg);
/**
* Diagnostics - print some basic information about the driver.
@ -101,30 +101,30 @@
void print_info();
private:
char _port[20];
uint8_t _rotation;
float _min_distance;
float _max_distance;
int _conversion_interval;
work_s _work{};
ringbuffer::RingBuffer *_reports;
bool _collect_phase;
int _fd;
uint8_t _linebuf[25];
uint8_t _cycle_counter;
enum ISL2950_PARSE_STATE _parse_state;
unsigned char _frame_data[4];
uint16_t _crc16;
int _distance_mm;
char _port[20];
uint8_t _rotation;
float _min_distance;
float _max_distance;
int _conversion_interval;
work_s _work{};
ringbuffer::RingBuffer *_reports;
int _fd;
uint8_t _linebuf[25];
uint8_t _cycle_counter;
int _class_instance;
int _orb_class_instance;
enum ISL2950_PARSE_STATE _parse_state;
unsigned char _frame_data[4];
uint16_t _crc16;
int _distance_mm;
orb_advert_t _distance_sensor_topic;
int _class_instance;
int _orb_class_instance;
perf_counter_t _sample_perf;
perf_counter_t _comms_errors;
orb_advert_t _distance_sensor_topic;
perf_counter_t _sample_perf;
perf_counter_t _comms_errors;
/**
* Initialise the automatic measurement state machine and start it.
@ -132,41 +132,37 @@
* @note This function is called at open and error time. It might make sense
* to make it more aggressive about resetting the bus in case of errors.
*/
void start();
void start();
/**
* Stop the automatic measurement state machine.
*/
void stop();
/**
* Stop the automatic measurement state machine.
*/
void stop();
/**
* Set the min and max distance thresholds if you want the end points of the sensors
* range to be brought in at all, otherwise it will use the defaults SF0X_MIN_DISTANCE
* and SF0X_MAX_DISTANCE
*/
void set_minimum_distance(float min);
void set_maximum_distance(float max);
float get_minimum_distance();
float get_maximum_distance();
/**
* Perform a poll cycle; collect from the previous measurement
* and start a new one.
*/
void cycle();
int collect();
/**
* Static trampoline from the workq context; because we don't have a
* generic workq wrapper yet.
*
* @param arg Instance pointer for the driver that is polling.
*/
static void cycle_trampoline(void *arg);
/**
* Set the min and max distance thresholds.
*/
void set_minimum_distance(float min);
void set_maximum_distance(float max);
float get_minimum_distance();
float get_maximum_distance();
/**
* Perform a reading cycle; collect from the previous measurement
* and start a new one.
*/
void cycle();
int collect();
/**
* Static trampoline from the workq context; because we don't have a
* generic workq wrapper yet.
*
* @param arg Instance pointer for the driver that is polling.
*/
static void cycle_trampoline(void *arg);
};
/*
* Driver 'main' command.
*/
@ -181,15 +177,14 @@
ISL2950::ISL2950(const char *port, uint8_t rotation) :
CDev(RANGE_FINDER0_DEVICE_PATH),
_rotation(rotation),
_min_distance(0.14f),
_max_distance(40.0f),
_min_distance(0.10f),
_max_distance(9.0f),
_conversion_interval(ISL2950_CONVERSION_INTERVAL),
_reports(nullptr),
_collect_phase(false),
_fd(-1),
_cycle_counter(0),
_parse_state(TFS_NOT_STARTED),
_frame_data{TOF_SFD1, TOF_SFD2, 0, 0},
_parse_state(STATE0_WAITING_FRAME),
_frame_data{START_FRAME_DIGIT1, START_FRAME_DIGIT2, 0, 0},
_crc16(0),
_distance_mm(-1),
_class_instance(-1),
@ -335,20 +330,12 @@ ISL2950::ioctl(device::file_t *filp, int cmd, unsigned long arg)
}
}
/*
ssize_t
ISL2950::read(device::file_t *filp, char *buffer, size_t buflen)
{
// SOME STUFFS
}*/
int
/*
* Method: collect()
*
* This method reads data from serial UART and places it into a buffer
* This method reads data from serial UART and places it into a buffer
*/
ISL2950::collect()
{
@ -375,7 +362,7 @@ ISL2950::collect()
i = bytes_read - 6 ;
while ((i >=0) && (!crc_valid))
{
if (_linebuf[i] == TOF_SFD1) {
if (_linebuf[i] == START_FRAME_DIGIT1) {
bytes_processed = i;
while ((bytes_processed < bytes_read) && (!crc_valid))
{
@ -385,7 +372,7 @@ ISL2950::collect()
}
bytes_processed++;
}
_parse_state = TFS_NOT_STARTED;
_parse_state = STATE0_WAITING_FRAME;
}
// else {printf("Starting frame wrong. Index: %d value 0x%02X \n",i,_linebuf[i]);}
@ -438,9 +425,8 @@ ISL2950::collect()
void
ISL2950::start()
{
PX4_INFO("ISL2950::start() - launch the work queue");
/* reset the report ring and state machine */
_collect_phase = false;
PX4_INFO("driver started");
_reports->flush();
/* schedule a cycle to start things */
@ -472,7 +458,7 @@ ISL2950::cycle()
_fd = ::open(_port,O_RDWR);
if (_fd < 0) {
PX4_ERR("ISL2950::cycle() - open failed (%i)", errno);
PX4_ERR("open failed (%i)", errno);
return;
}

87
src/drivers/distance_sensor/isl2950/isl2950_parser.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
* Copyright (c) 2018 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -33,8 +33,7 @@
/**
* @file isl2950_parser.cpp
* @author Claudio Micheli
* claudio@auterion.com
* @author Claudio Micheli <claudio@auterion.com>
*
*/
@ -47,11 +46,10 @@
#include <string.h>
#include <stdlib.h>
typedef unsigned char UCHAR;
typedef unsigned short USHORT;
// Note : No clue what those static variables are
static const UCHAR aucCRCHi[] = {
static const unsigned char crc_msb_vector[] = {
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
@ -76,7 +74,7 @@ static const UCHAR aucCRCHi[] = {
0x00, 0xC1, 0x81, 0x40
};
static const UCHAR aucCRCLo[] = {
static const unsigned char crc_lsb_vector[] = {
0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06, 0x07, 0xC7,
0x05, 0xC5, 0xC4, 0x04, 0xCC, 0x0C, 0x0D, 0xCD, 0x0F, 0xCF, 0xCE, 0x0E,
0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 0x09, 0x08, 0xC8, 0xD8, 0x18, 0x19, 0xD9,
@ -102,27 +100,19 @@ static const UCHAR aucCRCLo[] = {
};
// TOF frame format
//
// 1B 1B 1B 1B 2B
// | 0xA5 | 0x5A | distance-MSB | distance-LSB | crc-16 |
//
// Frame data saved for CRC calculation
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;
USHORT usMBCRC16(UCHAR* pucFrame, USHORT usLen) {
UCHAR ucCRCHi = 0xFF;
UCHAR ucCRCLo = 0xFF;
int iIndex;
while (usLen--) {
iIndex = ucCRCLo ^ *(pucFrame++);
ucCRCLo = (UCHAR)(ucCRCHi ^ aucCRCHi[iIndex]);
ucCRCHi = aucCRCLo[iIndex];
unsigned short crc16_calc(unsigned char *dataFrame,uint8_t crc16_length) {
unsigned char crc_high_byte = 0xFF;
unsigned char crc_low_byte = 0xFF;
int i;
while (crc16_length--) {
i = crc_low_byte ^ *(dataFrame++);
crc_low_byte = (unsigned char)(crc_high_byte ^ crc_msb_vector[i]);
crc_high_byte = crc_lsb_vector[i];
}
return (USHORT)(ucCRCHi << 8 | ucCRCLo);
return (unsigned short)(crc_high_byte << 8 | crc_low_byte);
}
int isl2950_parser(uint8_t c, uint8_t *parserbuf, ISL2950_PARSE_STATE *state, uint16_t *crc16, int *dist)
@ -132,65 +122,64 @@ int isl2950_parser(uint8_t c, uint8_t *parserbuf, ISL2950_PARSE_STATE *state, ui
// printf("parse byte 0x%02X \n", b);
switch (*state) {
case TFS_NOT_STARTED:
if (c == TOF_SFD1) {
*state = TFS_GOT_SFD1;
case STATE0_WAITING_FRAME:
if (c == START_FRAME_DIGIT1) {
*state = STATE1_GOT_DIGIT1;
//printf("Got SFD1 \n");
}
break;
case TFS_GOT_SFD1:
if (c == TOF_SFD2) {
*state = TFS_GOT_SFD2;
case STATE1_GOT_DIGIT1:
if (c == START_FRAME_DIGIT2) {
*state = STATE2_GOT_DIGIT2;
//printf("Got SFD2 \n");
}
// @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;
// @NOTE: (claudio@auterion.com): if second byte is wrong we skip all the frame and restart parsing !!
else if (c == START_FRAME_DIGIT1) {
*state = STATE1_GOT_DIGIT1;
//printf("Discard previous SFD1, Got new SFD1 \n");
} else {
*state = TFS_NOT_STARTED;
*state = STATE0_WAITING_FRAME;
}
break;
case TFS_GOT_SFD2:
*state = TFS_GOT_DATA1;
parserbuf[TOF_DISTANCE_MSB_POS] = c; // MSB Data
case STATE2_GOT_DIGIT2:
*state = STATE3_GOT_MSB_DATA;
parserbuf[DISTANCE_MSB_POS] = c; // MSB Data
//printf("Got DATA1 0x%02X \n", c);
break;
case TFS_GOT_DATA1:
*state = TFS_GOT_DATA2;
parserbuf[TOF_DISTANCE_LSB_POS] = c; // LSB Data
case STATE3_GOT_MSB_DATA:
*state = STATE4_GOT_LSB_DATA;
parserbuf[DISTANCE_LSB_POS] = c; // LSB Data
//printf("Got DATA2 0x%02X \n", c);
// do crc calculation
*crc16 = usMBCRC16(parserbuf, TOF_CRC_CALC_DATA_LEN);
*crc16 = crc16_calc(parserbuf, CHECKSUM_LENGTH);
// convert endian
*crc16 = (*crc16 >> 8) | (*crc16 << 8);
break;
case TFS_GOT_DATA2:
case STATE4_GOT_LSB_DATA:
if (c == (*crc16 >> 8)) {
*state = TFS_GOT_CHECKSUM1;
*state = STATE5_GOT_CHKSUM1;
}
else {
// printf("Checksum invalid on high byte: 0x%02X, calculated: 0x%04X \n",c, *crc16);
*state = TFS_NOT_STARTED;
*state = STATE0_WAITING_FRAME;
}
break;
case TFS_GOT_CHECKSUM1:
case STATE5_GOT_CHKSUM1:
// Here, reset state to `NOT-STARTED` no matter crc ok or not
*state = TFS_NOT_STARTED;
*state = STATE0_WAITING_FRAME;
if (c == (*crc16 & 0xFF)) {
// printf("Checksum verified \n");
*dist = (parserbuf[TOF_DISTANCE_MSB_POS] << 8) | parserbuf[TOF_DISTANCE_LSB_POS];
*dist = (parserbuf[DISTANCE_MSB_POS] << 8) | parserbuf[DISTANCE_LSB_POS];
return OK;
}
/*else {
printf("Checksum not verified \n");
//printf("Checksum invalidon low byte: 0x%02X, calculated: 0x%04X \n",c, *crc16);
}*/
break;

68
src/drivers/distance_sensor/isl2950/isl2950_parser.h
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
* Copyright (c) 2018 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -31,56 +31,42 @@
*
****************************************************************************/
/**
* @file sf0x_parser.cpp
* @author Lorenz Meier <lm@inf.ethz.ch>
*
* Declarations of parser for the Lightware SF0x laser rangefinder series
*/
/**
* @file isl2950.cpp
* @author Claudio Micheli <claudio@auterion.com>
*
* Parser declarations for Lanbao PSK-CM8JL65-CC5 distance sensor
*/
#pragma once
#include <stdint.h>
// frame start delimiter
#define TOF_SFD1 0xA5
#define TOF_SFD2 0x5A
#define START_FRAME_DIGIT1 0xA5
#define START_FRAME_DIGIT2 0x5A
// Frame format definition
//
// 1B 1B 1B 1B 2B
// | 0xA5 | 0x5A | distance-MSB | distance-LSB | crc-16 |
//
// Frame data saved for CRC calculation
#define DISTANCE_MSB_POS 2
#define DISTANCE_LSB_POS 3
#define CHECKSUM_LENGTH 4
/*typedef enum {
TFS_NOT_STARTED = 0,
TFS_GOT_SFD1,
TFS_GOT_SFD2,
TFS_GOT_DATA1,
TFS_GOT_DATA2,
TFS_GOT_CHECKSUM1,
TFS_GOT_CHECKSUM2,
} 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,
STATE0_WAITING_FRAME = 0,
STATE1_GOT_DIGIT1,
STATE2_GOT_DIGIT2,
STATE3_GOT_MSB_DATA,
STATE4_GOT_LSB_DATA,
STATE5_GOT_CHKSUM1,
STATE6_GOT_CHKSUM2,
};
enum IslWorkingMode {
KEEP_HEIGHT = 0,
NUM_WORKING_MODE
};
// SF0X STYLE
/*enum ISL2950_PARSE_STATE {
ISL2950_PARSE_STATE0_UNSYNC = 0,
ISL2950_PARSE_STATE1_SYNC,
ISL2950_PARSE_STATE2_GOT_DIGIT0,
ISL2950_PARSE_STATE3_GOT_DOT,
ISL2950_PARSE_STATE4_GOT_DIGIT1,
ISL2950_PARSE_STATE5_GOT_DIGIT2,
ISL2950_PARSE_STATE6_GOT_CARRIAGE_RETURN
};*/
int isl2950_parser(uint8_t c, uint8_t *parserbuf,enum ISL2950_PARSE_STATE *state,uint16_t *crc16, int *dist);