diff --git a/libraries/AP_FlashIface/AP_FlashIface_JEDEC.cpp b/libraries/AP_FlashIface/AP_FlashIface_JEDEC.cpp index 28f57016b1..1a8e051881 100644 --- a/libraries/AP_FlashIface/AP_FlashIface_JEDEC.cpp +++ b/libraries/AP_FlashIface/AP_FlashIface_JEDEC.cpp @@ -23,8 +23,8 @@ #include #include "AP_FlashIface_JEDEC.h" #include -#include #ifdef HAL_BOOTLOADER_BUILD +#include #include "../../Tools/AP_Bootloader/support.h" #else extern const AP_HAL::HAL& hal; @@ -83,14 +83,20 @@ static const struct supported_device supported_devices[] = { #endif #define MAX_READ_SIZE 1024UL +#ifdef HAL_BOOTLOADER_BUILD static ChibiOS::QSPIDeviceManager qspi; +#endif bool AP_FlashIface_JEDEC::init() { // Get device bus by name _dev = nullptr; for (uint8_t i = 0; i < ARRAY_SIZE(supported_devices); i++) { +#ifdef HAL_BOOTLOADER_BUILD _dev = qspi.get_device(supported_devices[i].name); +#else + _dev = hal.qspi->get_device(supported_devices[i].name); +#endif if (_dev) { _dev_list_idx = i; break; diff --git a/libraries/AP_FlashIface/examples/jedec_test/jedec_test.cpp b/libraries/AP_FlashIface/examples/jedec_test/jedec_test.cpp index a6d4ed9241..bb7e6544c4 100644 --- a/libraries/AP_FlashIface/examples/jedec_test/jedec_test.cpp +++ b/libraries/AP_FlashIface/examples/jedec_test/jedec_test.cpp @@ -1,70 +1,95 @@ #include +#include +#include +#include +#include #include #include -#include AP_FlashIface_JEDEC jedec_dev; +const AP_HAL::HAL& hal = AP_HAL::get_HAL(); + +void setup(); +void loop(); + +GCS_Dummy _gcs; + + +#ifdef HAL_BOOTLOADER_BUILD +#define DELAY_MILLIS(x) do { chThdSleepMilliseconds(x); } while(0) +#define DELAY_MICROS(x) do { chThdSleepMicroseconds(x); } while(0) +#else +#define DELAY_MILLIS(x) do { hal.scheduler->delay(x); } while(0) +#define DELAY_MICROS(x) do { hal.scheduler->delay_microseconds(x); } while(0) +#endif + +const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { + AP_GROUPEND +}; + +static AP_SerialManager serial_manager; +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) { - uprintf("Failed to allocate data for program"); + hal.console->printf("Failed to allocate data for program"); } uint8_t *rdata = new uint8_t[jedec_dev.get_page_size()]; if (rdata == nullptr) { - uprintf("Failed to allocate data for read"); + hal.console->printf("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; } - uprintf("Writing Page #1\n"); + hal.console->printf("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)) { - uprintf("Page write command failed\n"); + hal.console->printf("Page write command failed\n"); return; } while (true) { - chThdSleep(chTimeUS2I(delay_us)); + DELAY_MICROS(delay_us); if (AP_HAL::micros64() > (start_time_us+delay_us)) { if (!jedec_dev.is_device_busy()) { - uprintf("Page Program Successful, elapsed %ld us\n", uint32_t(AP_HAL::micros64() - start_time_us)); + hal.console->printf("Page Program Successful, elapsed %ld us\n", (unsigned long)(AP_HAL::micros64() - start_time_us)); break; } else { - uprintf("Typical page program time reached, Still Busy?!\n"); + hal.console->printf("Typical page program time reached, Still Busy?!\n"); } } if (AP_HAL::micros64() > (start_time_us+timeout_us)) { - uprintf("Page Program Timed out, elapsed %lld us\n", AP_HAL::micros64() - start_time_us); + hal.console->printf("Page Program Timed out, elapsed %lld us\n", (unsigned long long)(AP_HAL::micros64() - start_time_us)); return; } } if (!jedec_dev.read(0, rdata, jedec_dev.get_page_size())) { - uprintf("Failed to read Flash page\n"); + hal.console->printf("Failed to read Flash page\n"); } else { if (memcmp(data, rdata, jedec_dev.get_page_size()) != 0) { - uprintf("Program Data Mismatch!\n"); + hal.console->printf("Program Data Mismatch!\n"); } else { - uprintf("Program Data Verified Good!\n"); + hal.console->printf("Program Data Verified Good!\n"); } } // Now test XIP mode here as well uint8_t *chip_data = nullptr; if (!jedec_dev.start_xip_mode((void**)&chip_data)) { - uprintf("Failed to setup XIP mode\n"); + hal.console->printf("Failed to setup XIP mode\n"); } if (chip_data == nullptr) { - uprintf("Invalid address!\n"); + hal.console->printf("Invalid address!\n"); } // Here comes the future! if (memcmp(data, chip_data, jedec_dev.get_page_size()) != 0) { - uprintf("Program Data Mismatch in XIP mode!\n"); + hal.console->printf("Program Data Mismatch in XIP mode!\n"); } else { - uprintf("Program Data Verified Good in XIP mode!\n"); + hal.console->printf("Program Data Verified Good in XIP mode!\n"); } jedec_dev.stop_xip_mode(); } @@ -73,38 +98,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 - uprintf("Sector erase command failed\n"); + hal.console->printf("Sector erase command failed\n"); return; } uint32_t erase_start = AP_HAL::millis(); uint32_t next_check_ms = 0; - uprintf("Erasing Sector #1 "); + hal.console->printf("Erasing Sector #1 "); while (true) { if (AP_HAL::millis() > next_check_ms) { - uprintf("\n"); + hal.console->printf("\n"); if (!jedec_dev.is_device_busy()) { if (next_check_ms == 0) { - uprintf("Sector Erase happened too fast\n"); + hal.console->printf("Sector Erase happened too fast\n"); return; } - uprintf("Sector Erase Successful, elapsed %ld ms\n", AP_HAL::millis() - erase_start); + hal.console->printf("Sector Erase Successful, elapsed %ld ms\n", (unsigned long)(AP_HAL::millis() - erase_start)); break; } else { - uprintf("Still busy erasing, elapsed %ld ms\n", AP_HAL::millis() - erase_start); + hal.console->printf("Still busy erasing, elapsed %ld ms\n", (unsigned long)(AP_HAL::millis() - erase_start)); } if ((AP_HAL::millis() - erase_start) > timeout_ms) { - uprintf("Sector Erase Timed Out, elapsed %ld ms\n", AP_HAL::millis() - erase_start); + hal.console->printf("Sector Erase Timed Out, elapsed %ld ms\n", (unsigned long)(AP_HAL::millis() - erase_start)); return; } next_check_ms = erase_start+delay_ms; } - chThdSleep(chTimeMS2I((delay_ms/100) + 10)); - uprintf("*"); + DELAY_MILLIS((delay_ms/100) + 10); + hal.console->printf("*"); } if (!jedec_dev.verify_sector_erase(0)) { - uprintf("Erase Verification Failed!\n"); + hal.console->printf("Erase Verification Failed!\n"); } else { - uprintf("Erase Verification Successful!\n"); + hal.console->printf("Erase Verification Successful!\n"); } } @@ -112,48 +137,51 @@ static UNUSED_FUNCTION void test_mass_erase() { uint32_t delay_ms, timeout_ms; if (!jedec_dev.start_mass_erase(delay_ms, timeout_ms)) { - uprintf("Mass erase command failed\n"); + hal.console->printf("Mass erase command failed\n"); return; } uint32_t erase_start = AP_HAL::millis(); uint32_t next_check_ms = 0; - uprintf("Mass Erasing "); + hal.console->printf("Mass Erasing "); while (true) { if (AP_HAL::millis() > next_check_ms) { - uprintf("\n"); + hal.console->printf("\n"); if (!jedec_dev.is_device_busy()) { if (next_check_ms == 0) { - uprintf("Sector Erase happened too fast\n"); + hal.console->printf("Sector Erase happened too fast\n"); return; } - uprintf("Mass Erase Successful, elapsed %ld ms\n", AP_HAL::millis() - erase_start); + hal.console->printf("Mass Erase Successful, elapsed %ld ms\n",(unsigned long)(AP_HAL::millis() - erase_start)); return; } else { - uprintf("Still busy erasing, elapsed %ld ms\n", AP_HAL::millis() - erase_start); + hal.console->printf("Still busy erasing, elapsed %ld ms\n", (unsigned long)(AP_HAL::millis() - erase_start)); } if ((AP_HAL::millis() - erase_start) > timeout_ms) { - uprintf("Mass Erase Timed Out, elapsed %ld ms\n", AP_HAL::millis() - erase_start); + hal.console->printf("Mass Erase Timed Out, elapsed %ld ms\n", (unsigned long)(AP_HAL::millis() - erase_start)); return; } next_check_ms = erase_start+delay_ms; } - chThdSleep(chTimeMS2I(delay_ms/100)); - uprintf("*"); + DELAY_MILLIS(delay_ms/100); + hal.console->printf("*"); } } -int main() +void setup() { - init_uarts(); - - while (true) { - // Start on user input - while (cin(0) < 0) {} - uprintf("\n\n******************Starting Test********************\n"); - jedec_dev.init(); - test_sector_erase(); - test_page_program(); - // test_mass_erase(); - chThdSleep(chTimeMS2I(1000)); - } + board_config.init(); + serial_manager.init(); } + +void loop() +{ + // Start on user input + hal.console->printf("\n\n******************Starting Test********************\n"); + jedec_dev.init(); + test_sector_erase(); + test_page_program(); + // test_mass_erase(); + hal.scheduler->delay(1000); +} + +AP_HAL_MAIN(); diff --git a/libraries/AP_FlashIface/examples/jedec_test/wscript b/libraries/AP_FlashIface/examples/jedec_test/wscript index be5d237ccd..719ec151ba 100644 --- a/libraries/AP_FlashIface/examples/jedec_test/wscript +++ b/libraries/AP_FlashIface/examples/jedec_test/wscript @@ -2,12 +2,6 @@ # encoding: utf-8 def build(bld): - 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_example( + use='ap', ) - bld.ap_stlib(name= 'JEDEC_libs', - ap_vehicle='AP_Bootloader', - ap_libraries= ['AP_FlashIface', 'AP_HAL_Empty'])