HAL_PX4: fixed dirty_mask calculation in FRAM storage

this could lead to a number of bytes on 512 byte boundaries not being
written when changed in ram, so they would revert on next boot
This commit is contained in:
Andrew Tridgell 2014-09-11 18:52:31 +10:00
parent 7aad03df28
commit 74525cad89

View File

@ -177,10 +177,10 @@ void PX4Storage::_storage_open(void)
void PX4Storage::_mark_dirty(uint16_t loc, uint16_t length)
{
uint16_t end = loc + length;
while (loc < end) {
uint8_t line = (loc >> PX4_STORAGE_LINE_SHIFT);
_dirty_mask |= 1 << line;
loc += PX4_STORAGE_LINE_SIZE;
for (uint8_t line=loc>>PX4_STORAGE_LINE_SHIFT;
line <= end>>PX4_STORAGE_LINE_SHIFT;
line++) {
_dirty_mask |= 1U << line;
}
}