Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 02:55:07 +08:00
Code Issues Actions 19 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
+10 -4
View File
@@ -246,6 +246,7 @@ do { /* create a scope to handle exit conditions using break */
ret = -1; ret = -1;
break; break;
} }
_class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH);
/* get a publish handle on the range finder topic */ /* get a publish handle on the range finder topic */
@@ -360,21 +361,24 @@ CM8JL65::collect()
} else if (bytes_read > 0) { } else if (bytes_read > 0) {
// printf("Bytes read: %d \n",bytes_read); // printf("Bytes read: %d \n",bytes_read);
i = bytes_read - 6 ; i = bytes_read - 6 ;
while ((i >=0) && (!crc_valid))
{ while ((i >= 0) && (!crc_valid)) {
if (_linebuf[i] == START_FRAME_DIGIT1) { if (_linebuf[i] == START_FRAME_DIGIT1) {
bytes_processed = i; 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]); // 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; crc_valid = true;
} }
bytes_processed++; bytes_processed++;
} }
_parse_state = STATE0_WAITING_FRAME; _parse_state = STATE0_WAITING_FRAME;
} }
// else {printf("Starting frame wrong. Index: %d value 0x%02X \n",i,_linebuf[i]);} // else {printf("Starting frame wrong. Index: %d value 0x%02X \n",i,_linebuf[i]);}
i--; i--;
} }
@@ -566,11 +570,13 @@ start(const char *port, uint8_t rotation)
PX4_ERR("failed to set baudrate %d", B115200); PX4_ERR("failed to set baudrate %d", B115200);
goto fail; goto fail;
} }
PX4_DEBUG("cm8jl65::start() succeeded"); PX4_DEBUG("cm8jl65::start() succeeded");
return 0; return 0;
fail: fail:
PX4_DEBUG("cm8jl65::start() failed"); PX4_DEBUG("cm8jl65::start() failed");
if (g_dev != nullptr) { if (g_dev != nullptr) {
delete g_dev; delete g_dev;
g_dev = nullptr; 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_high_byte = 0xFF;
unsigned char crc_low_byte = 0xFF; unsigned char crc_low_byte = 0xFF;
int i; int i;
while (crc16_length--) { while (crc16_length--) {
i = crc_low_byte ^ *(dataFrame++); i = crc_low_byte ^ *(dataFrame++);
crc_low_byte = (unsigned char)(crc_high_byte ^ crc_msb_vector[i]); crc_low_byte = (unsigned char)(crc_high_byte ^ crc_msb_vector[i]);
crc_high_byte = crc_lsb_vector[i]; crc_high_byte = crc_lsb_vector[i];
} }
return (unsigned short)(crc_high_byte << 8 | crc_low_byte); 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; *state = STATE1_GOT_DIGIT1;
//printf("Got SFD1 \n"); //printf("Got SFD1 \n");
} }
break; break;
case STATE1_GOT_DIGIT1: 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; *state = STATE2_GOT_DIGIT2;
//printf("Got SFD2 \n"); //printf("Got SFD2 \n");
} }
// @NOTE: (claudio@auterion.com): if second byte is wrong we skip all the frame and restart parsing !! // @NOTE: (claudio@auterion.com): if second byte is wrong we skip all the frame and restart parsing !!
else if (c == START_FRAME_DIGIT1) { else if (c == START_FRAME_DIGIT1) {
*state = STATE1_GOT_DIGIT1; *state = STATE1_GOT_DIGIT1;
//printf("Discard previous SFD1, Got new SFD1 \n"); //printf("Discard previous SFD1, Got new SFD1 \n");
} else { } else {
*state = STATE0_WAITING_FRAME; *state = STATE0_WAITING_FRAME;
} }
break; break;
case STATE2_GOT_DIGIT2: 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: case STATE4_GOT_LSB_DATA:
if (c == (*crc16 >> 8)) { if (c == (*crc16 >> 8)) {
*state = STATE5_GOT_CHKSUM1; *state = STATE5_GOT_CHKSUM1;
}
else { } else {
// printf("Checksum invalid on high byte: 0x%02X, calculated: 0x%04X \n",c, *crc16); // printf("Checksum invalid on high byte: 0x%02X, calculated: 0x%04X \n",c, *crc16);
*state = STATE0_WAITING_FRAME; *state = STATE0_WAITING_FRAME;
} }
break; break;
case STATE5_GOT_CHKSUM1: case STATE5_GOT_CHKSUM1:
// Here, reset state to `NOT-STARTED` no matter crc ok or not // Here, reset state to `NOT-STARTED` no matter crc ok or not
*state = STATE0_WAITING_FRAME; *state = STATE0_WAITING_FRAME;
if (c == (*crc16 & 0xFF)) { if (c == (*crc16 & 0xFF)) {
// printf("Checksum verified \n"); // printf("Checksum verified \n");
*dist = (parserbuf[DISTANCE_MSB_POS] << 8) | parserbuf[DISTANCE_LSB_POS]; *dist = (parserbuf[DISTANCE_MSB_POS] << 8) | parserbuf[DISTANCE_LSB_POS];
return 0; return 0;
} }
/*else { /*else {
//printf("Checksum invalidon low byte: 0x%02X, calculated: 0x%04X \n",c, *crc16); //printf("Checksum invalidon low byte: 0x%02X, calculated: 0x%04X \n",c, *crc16);
}*/ }*/