Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Proximity: Fix Cygbot_D1 driver CRC

This commit is contained in:
Rishabh 2021-12-13 03:26:40 +05:30 committed by Randy Mackay
parent 372fc1a840
commit ef20d749bf
1 changed files with 13 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
libraries/AP_Proximity/AP_Proximity_Cygbot_D1.cpp
View File

@ -95,28 +95,26 @@ bool AP_Proximity_Cygbot_D1::parse_byte(uint8_t data)
case Payload_Header: case Payload_Header:
if (data == CYGBOT_PAYLOAD_HEADER) { if (data == CYGBOT_PAYLOAD_HEADER) {
_parse_state = Payload_Data; _parse_state = Payload_Data;
_msg.payload_counter = 0; _msg.payload_counter = 1;
_msg.payload[_msg.payload_counter] = data; _msg.payload[_msg.payload_counter] = data;
_msg.payload_counter ++;
return true; return true;
} }
return false; return false;
case Payload_Data: case Payload_Data:
if (_msg.payload_counter < (_msg.payload_len - 1)) { if (_msg.payload_counter < (_msg.payload_len)) {
_msg.payload[_msg.payload_counter] = data;
_msg.payload_counter++; _msg.payload_counter++;
_msg.payload[_msg.payload_counter] = data;
return true; return true;
} }
_parse_state = CheckSum; _parse_state = CheckSum;
return true; FALLTHROUGH;
case CheckSum: { case CheckSum: {
// To-DO: FIX CHECKSUM below. It does not pass correctly const uint8_t checksum_num = calc_checksum(_msg.payload, _msg.payload_len);
// const uint8_t checksum_num = calc_checksum(_msg.payload, _msg.payload_len); if (data != checksum_num) {
// if (data != checksum_num) { return false;
// return false; }
// }
// checksum is valid, parse payload // checksum is valid, parse payload
_last_distance_received_ms = AP_HAL::millis(); _last_distance_received_ms = AP_HAL::millis();
parse_payload(); parse_payload();
@ -139,7 +137,8 @@ void AP_Proximity_Cygbot_D1::parse_payload()
// current horizontal angle in the payload // current horizontal angle in the payload
float sampled_angle = CYGBOT_2D_START_ANGLE; float sampled_angle = CYGBOT_2D_START_ANGLE;
for (uint16_t i = 1; i < _msg.payload_len - 1; i += 2) { // start from second byte as first byte is part of the header
for (uint16_t i = 2; i < _msg.payload_len; i += 2) {
const uint16_t distance_mm = UINT16_VALUE(_msg.payload[i], _msg.payload[i+1]); const uint16_t distance_mm = UINT16_VALUE(_msg.payload[i], _msg.payload[i+1]);
float distance_m = distance_mm * 0.001f; float distance_m = distance_mm * 0.001f;
if (distance_m > distance_min() && distance_m < distance_max()) { if (distance_m > distance_min() && distance_m < distance_max()) {
@ -163,11 +162,10 @@ void AP_Proximity_Cygbot_D1::parse_payload()
// Checksum // Checksum
uint8_t AP_Proximity_Cygbot_D1::calc_checksum(uint8_t *buff, int buffSize) uint8_t AP_Proximity_Cygbot_D1::calc_checksum(uint8_t *buff, int buffSize)
{ {
uint8_t check_sum_num = 0x00; uint8_t check_sum_num = 0;
check_sum_num ^= _msg.payload_len_flags_low;
check_sum_num ^= _msg.payload_len_flags_high; check_sum_num ^= _msg.payload_len_flags_high;
check_sum_num ^= _msg.payload_len_flags_low;
for (uint16_t i = 0; i < buffSize - 1; i++) { for (uint16_t i = 0; i <= buffSize; i++) {
check_sum_num ^= buff[i]; check_sum_num ^= buff[i];
} }
return check_sum_num; return check_sum_num;