/* This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program. If not, see <http://www.gnu.org/licenses/>. */ /* simple bitmask class */ #pragma once #include <stdint.h> #include <string.h> #include <AP_InternalError/AP_InternalError.h> template<uint16_t num_bits> class Bitmask { public: Bitmask() : numbits(num_bits), numwords((num_bits+31)/32) { clearall(); } Bitmask &operator=(const Bitmask&other) { memcpy(bits, other.bits, sizeof(bits[0])*other.numwords); return *this; } Bitmask(const Bitmask &other) = delete; // set given bitnumber void set(uint16_t bit) { // ignore an invalid bit number if (bit >= numbits) { AP::internalerror().error(AP_InternalError::error_t::bitmask_range); return; } uint16_t word = bit/32; uint8_t ofs = bit & 0x1f; bits[word] |= (1U << ofs); } // set all bits void setall(void) { // set all words to 111... except the last one. for (uint16_t i=0; i<numwords-1; i++) { bits[i] = 0xffffffff; } // set most of the last word to 111.., leaving out-of-range bits to be 0 uint16_t num_valid_bits = numbits % 32; bits[numwords-1] = (1 << num_valid_bits) - 1; } // clear given bitnumber void clear(uint16_t bit) { uint16_t word = bit/32; uint8_t ofs = bit & 0x1f; bits[word] &= ~(1U << ofs); } // clear all bits void clearall(void) { memset(bits, 0, numwords*sizeof(bits[0])); } // return true if given bitnumber is set bool get(uint16_t bit) const { uint16_t word = bit/32; uint8_t ofs = bit & 0x1f; #if CONFIG_HAL_BOARD == HAL_BOARD_SITL if (bit >= numbits) { AP::internalerror().error(AP_InternalError::error_t::bitmask_range); return false; } #endif return (bits[word] & (1U << ofs)) != 0; } // return true if all bits are clear bool empty(void) const { for (uint16_t i=0; i<numwords; i++) { if (bits[i] != 0) { return false; } } return true; } // return number of bits set uint16_t count() const { uint16_t sum = 0; for (uint16_t i=0; i<numwords; i++) { if (sizeof(bits[i]) <= sizeof(int)) { sum += __builtin_popcount(bits[i]); } else if (sizeof(bits[i]) <= sizeof(long)) { sum += __builtin_popcountl(bits[i]); } else { sum += __builtin_popcountll(bits[i]); } } return sum; } // return first bit set, or -1 if none set int16_t first_set() const { for (uint16_t i=0; i<numwords; i++) { if (bits[i] == 0) { continue; } int fs; if (sizeof(bits[i]) <= sizeof(int)) { fs = __builtin_ffs(bits[i]); } else if (sizeof(bits[i]) <= sizeof(long)) { fs = __builtin_ffsl(bits[i]); } else { fs = __builtin_ffsll(bits[i]); } return i*32 + fs - 1; } return -1; } // return number of bits available uint16_t size() const { return numbits; } private: uint16_t numbits; uint16_t numwords; uint32_t bits[(num_bits+31)/32]; };