mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 14:38:30 -04:00
StorageManager: use hal.storage->erase()
This commit is contained in:
parent
3ace15b04e
commit
4eb5e5e8d4
@ -21,6 +21,7 @@
|
|||||||
|
|
||||||
#include <AP_HAL/AP_HAL.h>
|
#include <AP_HAL/AP_HAL.h>
|
||||||
#include "StorageManager.h"
|
#include "StorageManager.h"
|
||||||
|
#include <stdio.h>
|
||||||
|
|
||||||
|
|
||||||
extern const AP_HAL::HAL& hal;
|
extern const AP_HAL::HAL& hal;
|
||||||
@ -110,21 +111,9 @@ const StorageManager::StorageArea *StorageManager::layout = layout_default;
|
|||||||
*/
|
*/
|
||||||
void StorageManager::erase(void)
|
void StorageManager::erase(void)
|
||||||
{
|
{
|
||||||
uint8_t blk[16];
|
if (!hal.storage->erase()) {
|
||||||
memset(blk, 0, sizeof(blk));
|
::printf("StorageManager: erase failed\n");
|
||||||
for (uint8_t i=0; i<STORAGE_NUM_AREAS; i++) {
|
|
||||||
const StorageManager::StorageArea &area = StorageManager::layout[i];
|
|
||||||
uint16_t length = area.length;
|
|
||||||
uint16_t offset = area.offset;
|
|
||||||
for (uint16_t ofs=0; ofs<length; ofs += sizeof(blk)) {
|
|
||||||
uint8_t n = 16;
|
|
||||||
if (ofs + n > length) {
|
|
||||||
n = length - ofs;
|
|
||||||
}
|
|
||||||
hal.storage->write_block(offset + ofs, blk, n);
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
|
Loading…
Reference in New Issue
Block a user