Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-03 12:14:10 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Baro: Commonize the CRC4 method

Browse Source
This commit is contained in:
murata 2019-04-24 15:51:54 +09:00 committed by Tom Pittenger
parent 7f42be5d3e
commit 716cc26082
2 changed files with 3 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
libraries/AP_Baro/AP_Baro_MS5611.cpp
View File

@ -18,6 +18,7 @@
#include <stdio.h> #include <stdio.h>
#include <AP_Math/AP_Math.h> #include <AP_Math/AP_Math.h>
#include <AP_Math/crc.h>
extern const AP_HAL::HAL &hal; extern const AP_HAL::HAL &hal;
@ -150,34 +151,6 @@ bool AP_Baro_MS56XX::_init()
return true; return true;
} }
/**
* MS56XX crc4 method from datasheet for 16 bytes (8 short values)
*/
uint16_t AP_Baro_MS56XX::crc4(uint16_t *data)
{
uint16_t n_rem = 0;
uint8_t n_bit;
for (uint8_t cnt = 0; cnt < 16; cnt++) {
/* uneven bytes */
if (cnt & 1) {
n_rem ^= (uint8_t)((data[cnt >> 1]) & 0x00FF);
} else {
n_rem ^= (uint8_t)(data[cnt >> 1] >> 8);
}
for (n_bit = 8; n_bit > 0; n_bit--) {
if (n_rem & 0x8000) {
n_rem = (n_rem << 1) ^ 0x3000;
} else {
n_rem = (n_rem << 1);
}
}
}
return (n_rem >> 12) & 0xF;
}
uint16_t AP_Baro_MS56XX::_read_prom_word(uint8_t word) uint16_t AP_Baro_MS56XX::_read_prom_word(uint8_t word)
{ {
const uint8_t reg = CMD_MS56XX_PROM + (word << 1); const uint8_t reg = CMD_MS56XX_PROM + (word << 1);
@ -224,7 +197,7 @@ bool AP_Baro_MS56XX::_read_prom_5611(uint16_t prom[8])
/* remove CRC byte */ /* remove CRC byte */
prom[7] &= 0xff00; prom[7] &= 0xff00;
return crc_read == crc4(prom); return crc_read == crc_crc4(prom);
} }
bool AP_Baro_MS56XX::_read_prom_5637(uint16_t prom[8]) bool AP_Baro_MS56XX::_read_prom_5637(uint16_t prom[8])
@ -257,7 +230,7 @@ bool AP_Baro_MS56XX::_read_prom_5637(uint16_t prom[8])
/* remove CRC byte */ /* remove CRC byte */
prom[0] &= ~0xf000; prom[0] &= ~0xf000;
return crc_read == crc4(prom); return crc_read == crc_crc4(prom);
} }
/* /*

2
libraries/AP_Baro/AP_Baro_MS5611.h
View File

@ -40,8 +40,6 @@ public:
static AP_Baro_Backend *probe(AP_Baro &baro, AP_HAL::OwnPtr<AP_HAL::Device> dev, enum MS56XX_TYPE ms56xx_type = BARO_MS5611); static AP_Baro_Backend *probe(AP_Baro &baro, AP_HAL::OwnPtr<AP_HAL::Device> dev, enum MS56XX_TYPE ms56xx_type = BARO_MS5611);
static uint16_t crc4(uint16_t *data);
private: private:
/* /*
* Update @accum and @count with the new sample in @val, taking into * Update @accum and @count with the new sample in @val, taking into