AP_FlashIface: setup examples and driver for use with bootloader

This commit is contained in:
Siddharth Purohit 2021-06-02 23:23:04 +05:30 committed by Andrew Tridgell
parent 0120d8eeec
commit 17e6cab729
3 changed files with 54 additions and 61 deletions

View File

@ -23,6 +23,8 @@
#include <AP_HAL/AP_HAL.h>
#include "AP_FlashIface_JEDEC.h"
#include <AP_Math/AP_Math.h>
#include <AP_HAL_ChibiOS/QSPIDevice.h>
#include "../../Tools/AP_Bootloader/support.h"
extern const AP_HAL::HAL& hal;
@ -70,12 +72,14 @@ static const struct supported_device supported_devices[] = {
#define MAX_READ_SIZE 1024UL
static ChibiOS::QSPIDeviceManager qspi;
bool AP_FlashIface_JEDEC::init()
{
// Get device bus by name
_dev = nullptr;
for (uint8_t i = 0; i < ARRAY_SIZE(supported_devices); i++) {
_dev = hal.qspi->get_device(supported_devices[i].name);
_dev = qspi.get_device(supported_devices[i].name);
if (_dev) {
_dev_list_idx = i;
break;

View File

@ -1,62 +1,54 @@
#include <AP_HAL/AP_HAL.h>
#include <AP_BoardConfig/AP_BoardConfig.h>
#include <AP_FlashIface/AP_FlashIface.h>
#include <stdio.h>
const AP_HAL::HAL& hal = AP_HAL::get_HAL();
#include <support.h>
AP_FlashIface_JEDEC jedec_dev;
void setup();
void loop();
static AP_BoardConfig board_config;
static UNUSED_FUNCTION void test_page_program()
{
uint8_t *data = new uint8_t[jedec_dev.get_page_size()];
if (data == nullptr) {
hal.console->printf("Failed to allocate data for program");
uprintf("Failed to allocate data for program");
}
uint8_t *rdata = new uint8_t[jedec_dev.get_page_size()];
if (rdata == nullptr) {
hal.console->printf("Failed to allocate data for read");
uprintf("Failed to allocate data for read");
}
// fill program data with its own adress
for (uint32_t i = 0; i < jedec_dev.get_page_size(); i++) {
data[i] = i;
}
hal.console->printf("Writing Page #1\n");
uprintf("Writing Page #1\n");
uint32_t delay_us, timeout_us;
uint64_t start_time_us = AP_HAL::micros64();
if (!jedec_dev.start_program_page(0, data, delay_us, timeout_us)) {
hal.console->printf("Page write command failed\n");
uprintf("Page write command failed\n");
return;
}
while (true) {
hal.scheduler->delay_microseconds(delay_us);
chThdSleep(chTimeUS2I(delay_us));
if (AP_HAL::micros64() > (start_time_us+delay_us)) {
if (!jedec_dev.is_device_busy()) {
hal.console->printf("Page Program Successful, elapsed %lld us\n", AP_HAL::micros64() - start_time_us);
uprintf("Page Program Successful, elapsed %ld us\n", uint32_t(AP_HAL::micros64() - start_time_us));
break;
} else {
hal.console->printf("Typical page program time reached, Still Busy?!\n");
uprintf("Typical page program time reached, Still Busy?!\n");
}
}
if (AP_HAL::micros64() > (start_time_us+timeout_us)) {
hal.console->printf("Page Program Timed out, elapsed %lld us\n", AP_HAL::micros64() - start_time_us);
uprintf("Page Program Timed out, elapsed %lld us\n", AP_HAL::micros64() - start_time_us);
return;
}
}
if (!jedec_dev.read(0, rdata, jedec_dev.get_page_size())) {
hal.console->printf("Failed to read Flash page\n");
uprintf("Failed to read Flash page\n");
} else {
if (memcmp(data, rdata, jedec_dev.get_page_size()) != 0) {
hal.console->printf("Program Data Mismatch!\n");
uprintf("Program Data Mismatch!\n");
} else {
hal.console->printf("Program Data Verified Good!\n");
uprintf("Program Data Verified Good!\n");
}
}
}
@ -65,38 +57,38 @@ static UNUSED_FUNCTION void test_sector_erase()
{
uint32_t delay_ms, timeout_ms;
if (!jedec_dev.start_sector_erase(0, delay_ms, timeout_ms)) { // erase first sector
hal.console->printf("Sector erase command failed\n");
uprintf("Sector erase command failed\n");
return;
}
uint32_t erase_start = AP_HAL::millis();
uint32_t next_check_ms = 0;
hal.console->printf("Erasing Sector #1 ");
uprintf("Erasing Sector #1 ");
while (true) {
if (AP_HAL::millis() > next_check_ms) {
hal.console->printf("\n");
uprintf("\n");
if (!jedec_dev.is_device_busy()) {
if (next_check_ms == 0) {
hal.console->printf("Sector Erase happened too fast\n");
uprintf("Sector Erase happened too fast\n");
return;
}
hal.console->printf("Sector Erase Successful, elapsed %ld ms\n", AP_HAL::millis() - erase_start);
uprintf("Sector Erase Successful, elapsed %ld ms\n", AP_HAL::millis() - erase_start);
break;
} else {
hal.console->printf("Still busy erasing, elapsed %ld ms\n", AP_HAL::millis() - erase_start);
uprintf("Still busy erasing, elapsed %ld ms\n", AP_HAL::millis() - erase_start);
}
if ((AP_HAL::millis() - erase_start) > timeout_ms) {
hal.console->printf("Sector Erase Timed Out, elapsed %ld ms\n", AP_HAL::millis() - erase_start);
uprintf("Sector Erase Timed Out, elapsed %ld ms\n", AP_HAL::millis() - erase_start);
return;
}
next_check_ms = erase_start+delay_ms;
}
hal.scheduler->delay((delay_ms/100) + 10);
hal.console->printf("*");
chThdSleep(chTimeMS2I((delay_ms/100) + 10));
uprintf("*");
}
if (!jedec_dev.verify_sector_erase(0)) {
hal.console->printf("Erase Verification Failed!\n");
uprintf("Erase Verification Failed!\n");
} else {
hal.console->printf("Erase Verification Successful!\n");
uprintf("Erase Verification Successful!\n");
}
}
@ -104,54 +96,48 @@ static UNUSED_FUNCTION void test_mass_erase()
{
uint32_t delay_ms, timeout_ms;
if (!jedec_dev.start_mass_erase(delay_ms, timeout_ms)) {
hal.console->printf("Mass erase command failed\n");
uprintf("Mass erase command failed\n");
return;
}
uint32_t erase_start = AP_HAL::millis();
uint32_t next_check_ms = 0;
hal.console->printf("Mass Erasing ");
uprintf("Mass Erasing ");
while (true) {
if (AP_HAL::millis() > next_check_ms) {
hal.console->printf("\n");
uprintf("\n");
if (!jedec_dev.is_device_busy()) {
if (next_check_ms == 0) {
hal.console->printf("Sector Erase happened too fast\n");
uprintf("Sector Erase happened too fast\n");
return;
}
hal.console->printf("Mass Erase Successful, elapsed %ld ms\n", AP_HAL::millis() - erase_start);
uprintf("Mass Erase Successful, elapsed %ld ms\n", AP_HAL::millis() - erase_start);
return;
} else {
hal.console->printf("Still busy erasing, elapsed %ld ms\n", AP_HAL::millis() - erase_start);
uprintf("Still busy erasing, elapsed %ld ms\n", AP_HAL::millis() - erase_start);
}
if ((AP_HAL::millis() - erase_start) > timeout_ms) {
hal.console->printf("Mass Erase Timed Out, elapsed %ld ms\n", AP_HAL::millis() - erase_start);
uprintf("Mass Erase Timed Out, elapsed %ld ms\n", AP_HAL::millis() - erase_start);
return;
}
next_check_ms = erase_start+delay_ms;
}
hal.scheduler->delay(delay_ms/100);
hal.console->printf("*");
chThdSleep(chTimeMS2I(delay_ms/100));
uprintf("*");
}
}
void setup()
int main()
{
board_config.init();
}
init_uarts();
void loop()
{
// Start on user input
while (!hal.console->available()) {
hal.scheduler->delay(100);
while (true) {
// Start on user input
while (cin(0) < 0) {}
uprintf("\n\n******************Starting Test********************\n");
jedec_dev.init();
test_page_program();
test_sector_erase();
// test_mass_erase();
chThdSleep(chTimeMS2I(1000));
}
hal.console->printf("\n\n******************Starting Test********************\n");
jedec_dev.init();
test_page_program();
test_sector_erase();
// test_mass_erase();
hal.scheduler->delay(1000);
}
AP_HAL_MAIN();

View File

@ -2,9 +2,12 @@
# encoding: utf-8
def build(bld):
bld.ap_example(
bld.ap_program(
source=['../../../../Tools/AP_Bootloader/support.cpp', 'jedec_test.cpp'],
use=['ap','JEDEC_libs'],
program_groups='examples',
includes=bld.env.SRCROOT + '/Tools/AP_Bootloader/'
)
bld.ap_stlib(name= 'JEDEC_libs',
ap_vehicle='AP_Periph',
ap_libraries= ['AP_FlashIface'])
ap_vehicle='AP_Bootloader',
ap_libraries= ['AP_FlashIface', 'AP_HAL_Empty'])