PX4 Firmware
PX4 Autopilot Software http://px4.io
|
#include <CM8JL65.hpp>
Public Member Functions | |
CM8JL65 (const char *port, uint8_t rotation=distance_sensor_s::ROTATION_DOWNWARD_FACING) | |
Default Constructor. More... | |
virtual | ~CM8JL65 () override |
Virtual destructor. More... | |
int | init () |
Method : init() This method initializes the general driver for a range finder sensor. More... | |
void | print_info () |
Diagnostics - print some basic information about the driver. More... | |
Private Types | |
enum | PARSE_STATE { PARSE_STATE::WAITING_FRAME = 0, PARSE_STATE::DIGIT_1, PARSE_STATE::DIGIT_2, PARSE_STATE::MSB_DATA, PARSE_STATE::LSB_DATA, PARSE_STATE::CHECKSUM } |
Private Member Functions | |
uint16_t | crc16_calc (const unsigned char *data_frame, uint8_t crc16_length) |
Calculates the 16 byte crc value for the data frame. More... | |
int | collect () |
Reads data from serial UART and places it into a buffer. More... | |
int | data_parser (const uint8_t check_byte, uint8_t parserbuf[PARSER_BUF_LENGTH], PARSE_STATE &state, uint16_t &crc16, int &distance) |
int | open_serial_port (const speed_t speed=B115200) |
Opens and configures the UART serial communications port. More... | |
void | Run () override |
Perform a reading cycle; collect from the previous measurement and start a new one. More... | |
void | start () |
Initialise the automatic measurement state machine and start it. More... | |
void | stop () |
Stops the automatic measurement state machine. More... | |
Private Attributes | |
PX4Rangefinder | _px4_rangefinder |
char | _port [20] {} |
unsigned char | _frame_data [PARSER_BUF_LENGTH] {START_FRAME_DIGIT1, START_FRAME_DIGIT2, 0, 0} |
int | _file_descriptor {-1} |
uint8_t | _linebuf [25] {} |
uint16_t | _crc16 {0} |
PARSE_STATE | _parse_state {PARSE_STATE::WAITING_FRAME} |
perf_counter_t | _comms_errors {perf_alloc(PC_COUNT, MODULE_NAME": com_err")} |
perf_counter_t | _sample_perf {perf_alloc(PC_ELAPSED, MODULE_NAME": read")} |
Definition at line 75 of file CM8JL65.hpp.
|
strongprivate |
Enumerator | |
---|---|
WAITING_FRAME | |
DIGIT_1 | |
DIGIT_2 | |
MSB_DATA | |
LSB_DATA | |
CHECKSUM |
Definition at line 101 of file CM8JL65.hpp.
CM8JL65::CM8JL65 | ( | const char * | port, |
uint8_t | rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING |
||
) |
Default Constructor.
port | The serial port to open for communicating with the sensor. |
rotation | The sensor rotation relative to the vehicle body. |
Definition at line 86 of file CM8JL65.cpp.
References _port, _px4_rangefinder, f(), PX4Rangefinder::set_fov(), PX4Rangefinder::set_max_distance(), and PX4Rangefinder::set_min_distance().
|
overridevirtual |
Virtual destructor.
Definition at line 102 of file CM8JL65.cpp.
References _comms_errors, _sample_perf, perf_free(), and stop().
|
private |
Reads data from serial UART and places it into a buffer.
Definition at line 132 of file CM8JL65.cpp.
References _comms_errors, _crc16, _file_descriptor, _frame_data, _linebuf, _parse_state, _px4_rangefinder, _sample_perf, data_parser(), f(), hrt_absolute_time(), hrt_abstime, OK, perf_begin(), perf_count(), perf_end(), read(), START_FRAME_DIGIT1, PX4Rangefinder::update(), and WAITING_FRAME.
Referenced by Run().
|
private |
Calculates the 16 byte crc value for the data frame.
data_frame | The data frame to compute a checksum for. |
crc16_length | The length of the data frame. |
Definition at line 112 of file CM8JL65.cpp.
References crc16(), crc_lsb_vector, and crc_msb_vector.
Referenced by data_parser().
|
private |
Definition at line 190 of file CM8JL65.cpp.
References CHECKSUM, crc16_calc(), DIGIT_1, DIGIT_2, DISTANCE_LSB_POS, DISTANCE_MSB_POS, LSB_DATA, MSB_DATA, START_FRAME_DIGIT1, START_FRAME_DIGIT2, and WAITING_FRAME.
Referenced by collect().
int CM8JL65::init | ( | ) |
Method : init() This method initializes the general driver for a range finder sensor.
Definition at line 254 of file CM8JL65.cpp.
References start().
Referenced by cm8jl65::start().
|
private |
Opens and configures the UART serial communications port.
speed | The baudrate (speed) to configure the serial UART port. |
Definition at line 262 of file CM8JL65.cpp.
References _file_descriptor, and _port.
Referenced by Run().
void CM8JL65::print_info | ( | ) |
Diagnostics - print some basic information about the driver.
Definition at line 324 of file CM8JL65.cpp.
References _comms_errors, _px4_rangefinder, _sample_perf, perf_print_counter(), and PX4Rangefinder::print_status().
Referenced by cm8jl65::status().
|
overrideprivate |
Perform a reading cycle; collect from the previous measurement and start a new one.
Definition at line 333 of file CM8JL65.cpp.
References collect(), and open_serial_port().
|
private |
Initialise the automatic measurement state machine and start it.
Definition at line 343 of file CM8JL65.cpp.
References CM8JL65_MEASURE_INTERVAL.
Referenced by init().
|
private |
Stops the automatic measurement state machine.
Definition at line 352 of file CM8JL65.cpp.
References _file_descriptor.
Referenced by ~CM8JL65().
|
private |
Definition at line 164 of file CM8JL65.hpp.
Referenced by collect(), print_info(), and ~CM8JL65().
|
private |
Definition at line 160 of file CM8JL65.hpp.
Referenced by collect().
|
private |
Definition at line 156 of file CM8JL65.hpp.
Referenced by collect(), open_serial_port(), and stop().
|
private |
Definition at line 154 of file CM8JL65.hpp.
Referenced by collect().
|
private |
Definition at line 158 of file CM8JL65.hpp.
Referenced by collect().
|
private |
Definition at line 162 of file CM8JL65.hpp.
Referenced by collect().
|
private |
Definition at line 152 of file CM8JL65.hpp.
Referenced by CM8JL65(), and open_serial_port().
|
private |
Definition at line 150 of file CM8JL65.hpp.
Referenced by CM8JL65(), collect(), and print_info().
|
private |
Definition at line 165 of file CM8JL65.hpp.
Referenced by collect(), print_info(), and ~CM8JL65().