Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 11:06:04 +08:00
Code Issues Actions 17 Packages Projects Releases Wiki Activity

Formatted code.

Browse Source 
Signed-off-by: Claudio Micheli <claudio@auterion.com>
This commit is contained in:
Claudio Micheli
2019-01-04 12:12:56 +01:00
committed by Beat Küng
parent 7124cbf3af
commit ca0cd27c6b
3 changed files with 314 additions and 298 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/distance_sensor/cm8jl65/cm8jl65.cpp
+47 -41
View File
@@ -41,33 +41,33 @@
*
*/
#include <px4_config.h>
#include <px4_getopt.h>
#include <px4_workqueue.h>
#include <px4_config.h>
#include <px4_getopt.h>
#include <px4_workqueue.h>
#include <sys/types.h>
#include <stdint.h>
#include <stdlib.h>
#include <stdbool.h>
#include <string.h>
#include <poll.h>
#include <stdio.h>
#include <unistd.h>
#include <termios.h>
#include <sys/types.h>
#include <stdint.h>
#include <stdlib.h>
#include <stdbool.h>
#include <string.h>
#include <poll.h>
#include <stdio.h>
#include <unistd.h>
#include <termios.h>
#include <perf/perf_counter.h>
#include <perf/perf_counter.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_range_finder.h>
#include <drivers/device/device.h>
#include <drivers/device/ringbuffer.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_range_finder.h>
#include <drivers/device/device.h>
#include <drivers/device/ringbuffer.h>
#include <uORB/uORB.h>
#include <uORB/topics/distance_sensor.h>
#include <uORB/uORB.h>
#include <uORB/topics/distance_sensor.h>
#include "cm8jl65_parser.h"
#include "cm8jl65_parser.h"
/* Configuration Constants */
/* Configuration Constants */
#ifndef CONFIG_SCHED_WORKQUEUE
# error This requires CONFIG_SCHED_WORKQUEUE.
@@ -82,9 +82,9 @@
#define CM8JL65_CONVERSION_INTERVAL 50*1000UL/* 50ms */
class CM8JL65 : public cdev::CDev
{
public:
class CM8JL65 : public cdev::CDev
{
public:
// Constructor
CM8JL65(const char *port = CM8JL65_DEFAULT_PORT, uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
@@ -100,7 +100,7 @@
*/
void print_info();
private:
private:
char _port[20];
uint8_t _rotation;
@@ -161,12 +161,12 @@
*/
static void cycle_trampoline(void *arg);
};
};
/*
/*
* Driver 'main' command.
*/
extern "C" __EXPORT int cm8jl65_main(int argc, char *argv[]);
extern "C" __EXPORT int cm8jl65_main(int argc, char *argv[]);
/**
* Method : Constructor
@@ -174,7 +174,7 @@
* @note This method initializes the class variables
*/
CM8JL65::CM8JL65(const char *port, uint8_t rotation) :
CM8JL65::CM8JL65(const char *port, uint8_t rotation) :
CDev(RANGE_FINDER0_DEVICE_PATH),
_rotation(rotation),
_min_distance(0.10f),
@@ -192,7 +192,7 @@
_distance_sensor_topic(nullptr),
_sample_perf(perf_alloc(PC_ELAPSED, "cm8jl65_read")),
_comms_errors(perf_alloc(PC_COUNT, "cm8jl65_com_err"))
{
{
/* store port name */
strncpy(_port, port, sizeof(_port));
/* enforce null termination */
@@ -229,9 +229,9 @@ int
CM8JL65::init()
{
/* status */
int ret = 0;
int ret = 0;
do { /* create a scope to handle exit conditions using break */
do { /* create a scope to handle exit conditions using break */
/* do regular cdev init */
ret = CDev::init();
@@ -246,6 +246,7 @@ do { /* create a scope to handle exit conditions using break */
ret = -1;
break;
}
_class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH);
/* get a publish handle on the range finder topic */
@@ -357,24 +358,27 @@ CM8JL65::collect()
perf_count(_comms_errors);
perf_end(_sample_perf);
} else if (bytes_read > 0){
} else if (bytes_read > 0) {
// printf("Bytes read: %d \n",bytes_read);
i = bytes_read - 6 ;
while ((i >=0) && (!crc_valid))
{
while ((i >= 0) && (!crc_valid)) {
if (_linebuf[i] == START_FRAME_DIGIT1) {
bytes_processed = i;
while ((bytes_processed < bytes_read) && (!crc_valid))
{
while ((bytes_processed < bytes_read) && (!crc_valid)) {
// printf("In the cycle, processing byte %d, 0x%02X \n",bytes_processed, _linebuf[bytes_processed]);
if (OK == cm8jl65_parser(_linebuf[bytes_processed],_frame_data, &_parse_state,&_crc16, &_distance_mm)){
if (OK == cm8jl65_parser(_linebuf[bytes_processed], _frame_data, &_parse_state, &_crc16, &_distance_mm)) {
crc_valid = true;
}
bytes_processed++;
}
_parse_state = STATE0_WAITING_FRAME;
}
// else {printf("Starting frame wrong. Index: %d value 0x%02X \n",i,_linebuf[i]);}
i--;
}
@@ -393,7 +397,7 @@ CM8JL65::collect()
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;
@@ -455,7 +459,7 @@ CM8JL65::cycle()
/* fds initialized? */
if (_fd < 0) {
/* open fd */
_fd = ::open(_port,O_RDWR);
_fd = ::open(_port, O_RDWR);
if (_fd < 0) {
PX4_ERR("open failed (%i)", errno);
@@ -504,7 +508,7 @@ CM8JL65::cycle()
/* schedule a fresh cycle call when a complete packet has been received */
//work_queue(HPWORK,&_work,(worker_t)&CM8JL65::cycle_trampoline,this,USEC2TICK(_conversion_interval - _cycle_counter * 1000LL));
work_queue(HPWORK,&_work,(worker_t)&CM8JL65::cycle_trampoline,this,USEC2TICK(_conversion_interval));
work_queue(HPWORK, &_work, (worker_t)&CM8JL65::cycle_trampoline, this, USEC2TICK(_conversion_interval));
_cycle_counter = 0;
}
@@ -566,11 +570,13 @@ start(const char *port, uint8_t rotation)
PX4_ERR("failed to set baudrate %d", B115200);
goto fail;
}
PX4_DEBUG("cm8jl65::start() succeeded");
return 0;
fail:
PX4_DEBUG("cm8jl65::start() failed");
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
src/drivers/distance_sensor/cm8jl65/cm8jl65_parser.cpp
+13 -3
View File
@@ -103,15 +103,18 @@ static const unsigned char crc_lsb_vector[] = {
unsigned short crc16_calc(unsigned char *dataFrame,uint8_t crc16_length) {
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 (unsigned short)(crc_high_byte << 8 | crc_low_byte);
}
@@ -127,6 +130,7 @@ int cm8jl65_parser(uint8_t c, uint8_t *parserbuf, CM8JL65_PARSE_STATE *state, ui
*state = STATE1_GOT_DIGIT1;
//printf("Got SFD1 \n");
}
break;
case STATE1_GOT_DIGIT1:
@@ -134,13 +138,16 @@ int cm8jl65_parser(uint8_t c, uint8_t *parserbuf, CM8JL65_PARSE_STATE *state, ui
*state = STATE2_GOT_DIGIT2;
//printf("Got SFD2 \n");
}
// @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 = STATE0_WAITING_FRAME;
}
break;
case STATE2_GOT_DIGIT2:
@@ -163,22 +170,25 @@ int cm8jl65_parser(uint8_t c, uint8_t *parserbuf, CM8JL65_PARSE_STATE *state, ui
case STATE4_GOT_LSB_DATA:
if (c == (*crc16 >> 8)) {
*state = STATE5_GOT_CHKSUM1;
}
else {
} else {
// printf("Checksum invalid on high byte: 0x%02X, calculated: 0x%04X \n",c, *crc16);
*state = STATE0_WAITING_FRAME;
}
break;
case STATE5_GOT_CHKSUM1:
// Here, reset state to `NOT-STARTED` no matter crc ok or not
*state = STATE0_WAITING_FRAME;
if (c == (*crc16 & 0xFF)) {
// printf("Checksum verified \n");
*dist = (parserbuf[DISTANCE_MSB_POS] << 8) | parserbuf[DISTANCE_LSB_POS];
return 0;
}
/*else {
//printf("Checksum invalidon low byte: 0x%02X, calculated: 0x%04X \n",c, *crc16);
}*/
src/drivers/distance_sensor/cm8jl65/cm8jl65_parser.h
+2 -2
View File
@@ -31,7 +31,7 @@
*
****************************************************************************/
/**
/**
* @file cm8jl65.cpp
* @author Claudio Micheli <claudio@auterion.com>
*
@@ -69,4 +69,4 @@ enum CM8JL65_PARSE_STATE {
int cm8jl65_parser(uint8_t c, uint8_t *parserbuf,enum CM8JL65_PARSE_STATE *state,uint16_t *crc16, int *dist);
int cm8jl65_parser(uint8_t c, uint8_t *parserbuf, enum CM8JL65_PARSE_STATE *state, uint16_t *crc16, int *dist);