|
|
|
@ -104,12 +104,16 @@ void FlashTest::write(uint16_t offset, const uint8_t *data, uint16_t length)
@@ -104,12 +104,16 @@ void FlashTest::write(uint16_t offset, const uint8_t *data, uint16_t length)
|
|
|
|
|
* test flash storage |
|
|
|
|
*/ |
|
|
|
|
void FlashTest::setup(void) |
|
|
|
|
{ |
|
|
|
|
hal.console->printf("AP_FlashStorage test\n"); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void FlashTest::loop(void) |
|
|
|
|
{ |
|
|
|
|
flash[0] = (uint8_t *)malloc(flash_sector_size); |
|
|
|
|
flash[1] = (uint8_t *)malloc(flash_sector_size); |
|
|
|
|
flash_erase(0); |
|
|
|
|
flash_erase(1); |
|
|
|
|
hal.console->printf("AP_FlashStorage test\n"); |
|
|
|
|
|
|
|
|
|
if (!storage.init()) { |
|
|
|
|
AP_HAL::panic("Failed first init()"); |
|
|
|
@ -154,12 +158,10 @@ void FlashTest::setup(void)
@@ -154,12 +158,10 @@ void FlashTest::setup(void)
|
|
|
|
|
if (memcmp(mem_buffer, mem_mirror, sizeof(mem_buffer)) != 0) { |
|
|
|
|
AP_HAL::panic("FATAL: data mis-match"); |
|
|
|
|
} |
|
|
|
|
AP_HAL::panic("TEST PASSED"); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void FlashTest::loop(void) |
|
|
|
|
{ |
|
|
|
|
hal.console->printf("loop\n"); |
|
|
|
|
while (true) { |
|
|
|
|
hal.console->printf("TEST PASSED"); |
|
|
|
|
hal.scheduler->delay(20000); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
FlashTest flashtest; |
|
|
|
|