ardupilot/libraries/AP_HAL/examples/Storage/Storage.cpp

48 lines
854 B
C++
Raw Normal View History

2015-06-07 15:08:30 -03:00
/*
simple test of Storage API
*/
#include <AP_HAL/AP_HAL.h>
2015-06-07 15:08:30 -03:00
void setup();
void loop();
const AP_HAL::HAL& hal = AP_HAL::get_HAL();
2015-06-07 15:08:30 -03:00
void setup(void)
{
/*
init Storage API
*/
2017-07-02 19:55:44 -03:00
AP_HAL::Storage *st = hal.storage;
hal.console->printf("Starting AP_HAL::Storage test\r\n");
st->init();
2015-06-07 15:08:30 -03:00
/*
Calculate XOR of the full conent of memory
Do it by block of 8 bytes
*/
unsigned char buff[8], XOR_res = 0;
for (uint32_t i = 0; i < HAL_STORAGE_SIZE; i += 8) {
2015-06-07 15:08:30 -03:00
st->read_block((void *) buff, i, 8);
for(uint32_t j = 0; j < 8; j++) {
2015-06-07 15:08:30 -03:00
XOR_res ^= buff[j];
}
2015-06-07 15:08:30 -03:00
}
/*
print XORed result
*/
hal.console->printf("XORed ememory: %u\r\n", (unsigned) XOR_res);
2015-06-07 15:08:30 -03:00
}
// In main loop do nothing
void loop(void)
{
hal.scheduler->delay(1000);
}
AP_HAL_MAIN();