Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-04 23:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Beacon: Common modbus crc method

Browse Source
This commit is contained in:
murata 2019-04-19 19:47:11 +09:00 committed by Andrew Tridgell
parent d7b2271197
commit 89484a8f64
2 changed files with 1 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
libraries/AP_Beacon/AP_Beacon_Marvelmind.cpp
View File

@ -19,7 +19,7 @@
*/
#include <AP_HAL/AP_HAL.h>
#include <AP_Math/crc.h>
#include "AP_Beacon_Marvelmind.h"
#define AP_BEACON_MARVELMIND_POSITION_DATAGRAM_ID 0x0001
@ -56,30 +56,6 @@ AP_Beacon_Marvelmind::AP_Beacon_Marvelmind(AP_Beacon &frontend, AP_SerialManager
}
}
//////////////////////////////////////////////////////////////////////////////
// Calculate Modbus CRC16 for array of bytes
// buf: input buffer
// len: size of buffer
// returncode: CRC value
//////////////////////////////////////////////////////////////////////////////
uint16_t AP_Beacon_Marvelmind::calc_crc_modbus(uint8_t *buf, uint16_t len)
{
uint16_t crc = 0xFFFF;
for (uint16_t pos = 0; pos < len; pos++) {
crc ^= (uint16_t) buf[pos]; // XOR byte into least sig. byte of crc
for (uint8_t i = 8; i != 0; i--) { // Loop over each bit
if ((crc & 0x0001) != 0) { // If the LSB is set
crc >>= 1; // Shift right and XOR 0xA001
crc ^= 0xA001;
} else {
// Else LSB is not set
crc >>= 1; // Just shift right
}
}
}
return crc;
}
void AP_Beacon_Marvelmind::process_position_datagram()
{
hedge.cur_position.address = input_buffer[16];

1
libraries/AP_Beacon/AP_Beacon_Marvelmind.h
View File

@ -79,7 +79,6 @@ private:
uint16_t num_bytes_in_block_received;
uint16_t data_id;
uint16_t calc_crc_modbus(uint8_t *buf, uint16_t len);
StationaryBeaconPosition* get_or_alloc_beacon(uint8_t address);
void process_beacons_positions_datagram();
void process_beacons_positions_highres_datagram();