diff --git a/libraries/AP_Math/crc.cpp b/libraries/AP_Math/crc.cpp index dd4b7a267d..9835f49595 100644 --- a/libraries/AP_Math/crc.cpp +++ b/libraries/AP_Math/crc.cpp @@ -19,6 +19,8 @@ #include #include "crc.h" +#include + /** * crc4 method from datasheet for 16 bytes (8 short values) * @@ -540,7 +542,29 @@ uint64_t crc_crc64(const uint32_t *data, uint16_t num_words) return crc; } +// return the parity of byte - "1" if there is an odd number of bits +// set, "0" if there is an even number of bits set note that +// __builtin_parity causes hardfaults on Pixracer-periph - and is +// slower on 1 byte than this: uint8_t parity(uint8_t byte) { - return __builtin_parity(byte); + uint8_t p = 0; + + p ^= byte & 0x1; + byte >>= 1; + p ^= byte & 0x1; + byte >>= 1; + p ^= byte & 0x1; + byte >>= 1; + p ^= byte & 0x1; + byte >>= 1; + p ^= byte & 0x1; + byte >>= 1; + p ^= byte & 0x1; + byte >>= 1; + p ^= byte & 0x1; + byte >>= 1; + p ^= byte & 0x1; + + return p; } diff --git a/libraries/AP_Math/tests/test_math.cpp b/libraries/AP_Math/tests/test_math.cpp index ccc959d2fe..0043a38c5b 100644 --- a/libraries/AP_Math/tests/test_math.cpp +++ b/libraries/AP_Math/tests/test_math.cpp @@ -680,6 +680,18 @@ TEST(MathTest, FIXEDWINGTURNRATE) EXPECT_NEAR(56.187965393066406f, fixedwing_turn_rate(45, 10.0f), accuracy); } +TEST(CRCTest, parity) +{ + EXPECT_EQ(parity(0b1), 1); + EXPECT_EQ(parity(0b10), 1); + EXPECT_EQ(parity(0b100), 1); + + EXPECT_EQ(parity(0b11), 0); + EXPECT_EQ(parity(0b110), 0); + EXPECT_EQ(parity(0b111), 1); + EXPECT_EQ(parity(0b11111111), 0); +} + AP_GTEST_PANIC() AP_GTEST_MAIN()