mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-09 01:18:29 -04:00
Tools: make CPUInfo test fairer, add data for external flash
fix CPUInfo on linux make cache disabling optional in CPUInfo
This commit is contained in:
parent
55db600c10
commit
f6c07df162
Tools
AP_Bootloader
CPUInfo
@ -32,7 +32,7 @@
|
|||||||
#include "bl_protocol.h"
|
#include "bl_protocol.h"
|
||||||
#include "can.h"
|
#include "can.h"
|
||||||
#include <stdio.h>
|
#include <stdio.h>
|
||||||
#if EXTERNAL_PROG_FLASH_MB
|
#if EXT_FLASH_SIZE_MB
|
||||||
#include <AP_FlashIface/AP_FlashIface_JEDEC.h>
|
#include <AP_FlashIface/AP_FlashIface_JEDEC.h>
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
@ -44,7 +44,7 @@ struct boardinfo board_info = {
|
|||||||
.board_type = APJ_BOARD_ID,
|
.board_type = APJ_BOARD_ID,
|
||||||
.board_rev = 0,
|
.board_rev = 0,
|
||||||
.fw_size = (BOARD_FLASH_SIZE - (FLASH_BOOTLOADER_LOAD_KB + FLASH_RESERVE_END_KB + APP_START_OFFSET_KB))*1024,
|
.fw_size = (BOARD_FLASH_SIZE - (FLASH_BOOTLOADER_LOAD_KB + FLASH_RESERVE_END_KB + APP_START_OFFSET_KB))*1024,
|
||||||
.extf_size = (EXTERNAL_PROG_FLASH_MB * 1024 * 1024)
|
.extf_size = (EXT_FLASH_SIZE_MB * 1024 * 1024)
|
||||||
};
|
};
|
||||||
|
|
||||||
#ifndef HAL_BOOTLOADER_TIMEOUT
|
#ifndef HAL_BOOTLOADER_TIMEOUT
|
||||||
@ -55,7 +55,7 @@ struct boardinfo board_info = {
|
|||||||
#define HAL_STAY_IN_BOOTLOADER_VALUE 0
|
#define HAL_STAY_IN_BOOTLOADER_VALUE 0
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#if EXTERNAL_PROG_FLASH_MB
|
#if EXT_FLASH_SIZE_MB
|
||||||
AP_FlashIface_JEDEC ext_flash;
|
AP_FlashIface_JEDEC ext_flash;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
@ -151,7 +151,7 @@ int main(void)
|
|||||||
flash_init();
|
flash_init();
|
||||||
|
|
||||||
|
|
||||||
#if EXTERNAL_PROG_FLASH_MB
|
#if EXT_FLASH_SIZE_MB
|
||||||
while (!ext_flash.init()) {
|
while (!ext_flash.init()) {
|
||||||
// keep trying until we get it working
|
// keep trying until we get it working
|
||||||
// there's no future without it
|
// there's no future without it
|
||||||
|
@ -50,7 +50,7 @@
|
|||||||
#include "support.h"
|
#include "support.h"
|
||||||
#include "can.h"
|
#include "can.h"
|
||||||
#include <AP_HAL_ChibiOS/hwdef/common/watchdog.h>
|
#include <AP_HAL_ChibiOS/hwdef/common/watchdog.h>
|
||||||
#if EXTERNAL_PROG_FLASH_MB
|
#if EXT_FLASH_SIZE_MB
|
||||||
#include <AP_FlashIface/AP_FlashIface_JEDEC.h>
|
#include <AP_FlashIface/AP_FlashIface_JEDEC.h>
|
||||||
#endif
|
#endif
|
||||||
// #pragma GCC optimize("O0")
|
// #pragma GCC optimize("O0")
|
||||||
@ -148,7 +148,7 @@ volatile unsigned timer[NTIMERS];
|
|||||||
#define RESERVE_LEAD_WORDS 8
|
#define RESERVE_LEAD_WORDS 8
|
||||||
|
|
||||||
|
|
||||||
#if EXTERNAL_PROG_FLASH_MB
|
#if EXT_FLASH_SIZE_MB
|
||||||
extern AP_FlashIface_JEDEC ext_flash;
|
extern AP_FlashIface_JEDEC ext_flash;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
@ -233,7 +233,7 @@ jump_to_app()
|
|||||||
const uint32_t *app_base = (const uint32_t *)(APP_START_ADDRESS);
|
const uint32_t *app_base = (const uint32_t *)(APP_START_ADDRESS);
|
||||||
|
|
||||||
// If we have QSPI chip start it
|
// If we have QSPI chip start it
|
||||||
#if EXTERNAL_PROG_FLASH_MB
|
#if EXT_FLASH_SIZE_MB
|
||||||
uint8_t* ext_flash_start_addr;
|
uint8_t* ext_flash_start_addr;
|
||||||
if (!ext_flash.start_xip_mode((void**)&ext_flash_start_addr)) {
|
if (!ext_flash.start_xip_mode((void**)&ext_flash_start_addr)) {
|
||||||
return;
|
return;
|
||||||
@ -314,7 +314,7 @@ jump_to_app()
|
|||||||
/* extract the stack and entrypoint from the app vector table and go */
|
/* extract the stack and entrypoint from the app vector table and go */
|
||||||
do_jump(app_base[0], app_base[1]);
|
do_jump(app_base[0], app_base[1]);
|
||||||
exit:
|
exit:
|
||||||
#if EXTERNAL_PROG_FLASH_MB
|
#if EXT_FLASH_SIZE_MB
|
||||||
ext_flash.stop_xip_mode();
|
ext_flash.stop_xip_mode();
|
||||||
#endif
|
#endif
|
||||||
return;
|
return;
|
||||||
@ -432,7 +432,7 @@ bootloader(unsigned timeout)
|
|||||||
#endif
|
#endif
|
||||||
|
|
||||||
uint32_t address = board_info.fw_size; /* force erase before upload will work */
|
uint32_t address = board_info.fw_size; /* force erase before upload will work */
|
||||||
#if EXTERNAL_PROG_FLASH_MB
|
#if EXT_FLASH_SIZE_MB
|
||||||
uint32_t extf_address = board_info.extf_size; /* force erase before upload will work */
|
uint32_t extf_address = board_info.extf_size; /* force erase before upload will work */
|
||||||
#endif
|
#endif
|
||||||
uint32_t read_address = 0;
|
uint32_t read_address = 0;
|
||||||
@ -627,7 +627,7 @@ bootloader(unsigned timeout)
|
|||||||
// readback failure: INSYNC/FAILURE
|
// readback failure: INSYNC/FAILURE
|
||||||
//
|
//
|
||||||
case PROTO_EXTF_ERASE:
|
case PROTO_EXTF_ERASE:
|
||||||
#if EXTERNAL_PROG_FLASH_MB
|
#if EXT_FLASH_SIZE_MB
|
||||||
{
|
{
|
||||||
if (!done_sync || !CHECK_GET_DEVICE_FINISHED(done_get_device_flags)) {
|
if (!done_sync || !CHECK_GET_DEVICE_FINISHED(done_get_device_flags)) {
|
||||||
// lower chance of random data on a uart triggering erase
|
// lower chance of random data on a uart triggering erase
|
||||||
@ -684,7 +684,7 @@ bootloader(unsigned timeout)
|
|||||||
}
|
}
|
||||||
#else
|
#else
|
||||||
goto cmd_bad;
|
goto cmd_bad;
|
||||||
#endif // EXTERNAL_PROG_FLASH_MB
|
#endif // EXT_FLASH_SIZE_MB
|
||||||
break;
|
break;
|
||||||
|
|
||||||
// program bytes at current external flash address
|
// program bytes at current external flash address
|
||||||
@ -696,7 +696,7 @@ bootloader(unsigned timeout)
|
|||||||
//
|
//
|
||||||
case PROTO_EXTF_PROG_MULTI:
|
case PROTO_EXTF_PROG_MULTI:
|
||||||
{
|
{
|
||||||
#if EXTERNAL_PROG_FLASH_MB
|
#if EXT_FLASH_SIZE_MB
|
||||||
if (!done_sync || !CHECK_GET_DEVICE_FINISHED(done_get_device_flags)) {
|
if (!done_sync || !CHECK_GET_DEVICE_FINISHED(done_get_device_flags)) {
|
||||||
// lower chance of random data on a uart triggering erase
|
// lower chance of random data on a uart triggering erase
|
||||||
goto cmd_bad;
|
goto cmd_bad;
|
||||||
@ -883,7 +883,7 @@ bootloader(unsigned timeout)
|
|||||||
// reply: <crc:4>/INSYNC/OK
|
// reply: <crc:4>/INSYNC/OK
|
||||||
//
|
//
|
||||||
case PROTO_EXTF_GET_CRC: {
|
case PROTO_EXTF_GET_CRC: {
|
||||||
#if EXTERNAL_PROG_FLASH_MB
|
#if EXT_FLASH_SIZE_MB
|
||||||
// expect EOC
|
// expect EOC
|
||||||
uint32_t cmd_verify_bytes;
|
uint32_t cmd_verify_bytes;
|
||||||
if (cin_word(&cmd_verify_bytes, 100)) {
|
if (cin_word(&cmd_verify_bytes, 100)) {
|
||||||
|
@ -157,6 +157,8 @@ AP_HW_SierraF412 1055
|
|||||||
AP_HW_BEASTF7v2 1057
|
AP_HW_BEASTF7v2 1057
|
||||||
AP_HW_KakuteH7Mini 1058
|
AP_HW_KakuteH7Mini 1058
|
||||||
AP_HW_JHEMCUGSF405A 1059
|
AP_HW_JHEMCUGSF405A 1059
|
||||||
|
AP_HW_SPRACINGH7 1060
|
||||||
|
AP_HW_DEVEBOXH7 1061
|
||||||
|
|
||||||
AP_HW_CUBEORANGE_PERIPH 1400
|
AP_HW_CUBEORANGE_PERIPH 1400
|
||||||
AP_HW_CUBEBLACK_PERIPH 1401
|
AP_HW_CUBEBLACK_PERIPH 1401
|
||||||
|
@ -5,7 +5,7 @@ def build(bld):
|
|||||||
if not bld.env.BOOTLOADER:
|
if not bld.env.BOOTLOADER:
|
||||||
return
|
return
|
||||||
|
|
||||||
if bld.env.EXTERNAL_PROG_FLASH_MB:
|
if bld.env.EXT_FLASH_SIZE_MB:
|
||||||
flashiface_lib = ['AP_HAL', 'AP_FlashIface', 'AP_HAL_Empty']
|
flashiface_lib = ['AP_HAL', 'AP_FlashIface', 'AP_HAL_Empty']
|
||||||
else:
|
else:
|
||||||
flashiface_lib = []
|
flashiface_lib = []
|
||||||
|
@ -10,6 +10,7 @@
|
|||||||
#include <AP_HAL/AP_HAL.h>
|
#include <AP_HAL/AP_HAL.h>
|
||||||
#include <AP_Common/AP_Common.h>
|
#include <AP_Common/AP_Common.h>
|
||||||
#include <AP_Math/AP_Math.h>
|
#include <AP_Math/AP_Math.h>
|
||||||
|
#include <AP_ESC_Telem/AP_ESC_Telem.h>
|
||||||
#include "EKF_Maths.h"
|
#include "EKF_Maths.h"
|
||||||
|
|
||||||
void setup();
|
void setup();
|
||||||
@ -17,6 +18,13 @@ void loop();
|
|||||||
|
|
||||||
const AP_HAL::HAL& hal = AP_HAL::get_HAL();
|
const AP_HAL::HAL& hal = AP_HAL::get_HAL();
|
||||||
|
|
||||||
|
#if CONFIG_HAL_BOARD != HAL_BOARD_LINUX
|
||||||
|
|
||||||
|
// On H750 we want to measure external flash to ram performance
|
||||||
|
#if defined(EXT_FLASH_SIZE_MB) && defined(STM32H7)
|
||||||
|
#define DISABLE_CACHES
|
||||||
|
#endif
|
||||||
|
|
||||||
#ifdef STM32_SYS_CK
|
#ifdef STM32_SYS_CK
|
||||||
static uint32_t sysclk = STM32_SYS_CK;
|
static uint32_t sysclk = STM32_SYS_CK;
|
||||||
#elif defined(STM32_SYSCLK)
|
#elif defined(STM32_SYSCLK)
|
||||||
@ -28,8 +36,13 @@ static uint32_t sysclk = 0;
|
|||||||
static EKF_Maths ekf;
|
static EKF_Maths ekf;
|
||||||
|
|
||||||
HAL_Semaphore sem;
|
HAL_Semaphore sem;
|
||||||
|
AP_ESC_Telem telem;
|
||||||
|
|
||||||
void setup() {
|
void setup() {
|
||||||
|
#ifdef DISABLE_CACHES
|
||||||
|
SCB_DisableDCache();
|
||||||
|
SCB_DisableICache();
|
||||||
|
#endif
|
||||||
ekf.init();
|
ekf.init();
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -86,6 +99,7 @@ static void show_timings(void)
|
|||||||
v_f = 1+(AP_HAL::micros() % 5);
|
v_f = 1+(AP_HAL::micros() % 5);
|
||||||
v_out = 1+(AP_HAL::micros() % 3);
|
v_out = 1+(AP_HAL::micros() % 3);
|
||||||
|
|
||||||
|
v_32 = AP_HAL::millis();
|
||||||
v_32 = 1+(AP_HAL::micros() % 5);
|
v_32 = 1+(AP_HAL::micros() % 5);
|
||||||
v_out_32 = 1+(AP_HAL::micros() % 3);
|
v_out_32 = 1+(AP_HAL::micros() % 3);
|
||||||
|
|
||||||
@ -174,4 +188,9 @@ void loop()
|
|||||||
hal.scheduler->delay(3000);
|
hal.scheduler->delay(3000);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#else
|
||||||
|
void loop() {}
|
||||||
|
void setup() {}
|
||||||
|
#endif
|
||||||
|
|
||||||
AP_HAL_MAIN();
|
AP_HAL_MAIN();
|
||||||
|
@ -1,6 +1,7 @@
|
|||||||
dcache enabled, 384MHz
|
ICache and DCache disabled
|
||||||
|
|
||||||
|
SYSCLK 400MHz
|
||||||
|
|
||||||
SYSCLK 384MHz
|
|
||||||
Type sizes:
|
Type sizes:
|
||||||
char : 1
|
char : 1
|
||||||
short : 2
|
short : 2
|
||||||
@ -15,48 +16,54 @@ printing -Inf: -inf
|
|||||||
|
|
||||||
Operation timings:
|
Operation timings:
|
||||||
Note: timings for some operations are very data dependent
|
Note: timings for some operations are very data dependent
|
||||||
nop 0.00 usec/call
|
nop 0.0032 usec/call
|
||||||
micros() 0.39 usec/call
|
micros() 1.1417 usec/call
|
||||||
millis() 0.42 usec/call
|
millis() 1.8733 usec/call
|
||||||
fadd 0.00 usec/call
|
micros64() 1.1517 usec/call
|
||||||
fsub 0.00 usec/call
|
fadd 0.0422 usec/call
|
||||||
fmul 0.00 usec/call
|
fsub 0.0380 usec/call
|
||||||
fdiv /= 0.04 usec/call
|
fmul 0.0402 usec/call
|
||||||
fdiv 2/x 0.04 usec/call
|
fdiv /= 0.0446 usec/call
|
||||||
dadd 0.14 usec/call
|
fdiv 2/x 0.0768 usec/call
|
||||||
dsub 0.14 usec/call
|
dadd 0.0698 usec/call
|
||||||
dmul 0.06 usec/call
|
dsub 0.0648 usec/call
|
||||||
ddiv 0.08 usec/call
|
dmul 0.0704 usec/call
|
||||||
sinf() 0.30 usec/call
|
ddiv 0.0648 usec/call
|
||||||
cosf() 0.30 usec/call
|
sinf() 0.7500 usec/call
|
||||||
tanf() 0.50 usec/call
|
cosf() 0.7590 usec/call
|
||||||
acosf() 0.40 usec/call
|
tanf() 1.6720 usec/call
|
||||||
asinf() 0.30 usec/call
|
acosf() 1.1660 usec/call
|
||||||
atan2f() 0.40 usec/call
|
asinf() 1.2430 usec/call
|
||||||
sqrtf() 0.10 usec/call
|
atan2f() 1.6100 usec/call
|
||||||
sin() 4.40 usec/call
|
sqrtf() 0.0910 usec/call
|
||||||
cos() 4.10 usec/call
|
sin() 0.7500 usec/call
|
||||||
tan() 8.40 usec/call
|
cos() 0.7560 usec/call
|
||||||
acos() 5.70 usec/call
|
tan() 1.6920 usec/call
|
||||||
asin() 5.10 usec/call
|
acos() 1.1510 usec/call
|
||||||
atan2() 7.50 usec/call
|
asin() 1.2290 usec/call
|
||||||
sqrt() 1.20 usec/call
|
atan2() 1.5860 usec/call
|
||||||
iadd8 0.00 usec/call
|
sqrt() 0.0920 usec/call
|
||||||
isub8 0.00 usec/call
|
sq() 0.0400 usec/call
|
||||||
imul8 0.00 usec/call
|
powf(v,2) 0.0400 usec/call
|
||||||
idiv8 0.02 usec/call
|
powf(v,3.1) 1.2070 usec/call
|
||||||
iadd16 0.00 usec/call
|
EKF 72.4120 usec/call
|
||||||
isub16 0.00 usec/call
|
iadd8 0.0496 usec/call
|
||||||
imul16 0.00 usec/call
|
isub8 0.0474 usec/call
|
||||||
idiv16 0.02 usec/call
|
imul8 0.0514 usec/call
|
||||||
iadd32 0.00 usec/call
|
idiv8 0.0584 usec/call
|
||||||
isub32 0.00 usec/call
|
iadd16 0.0488 usec/call
|
||||||
imul32 0.00 usec/call
|
isub16 0.0484 usec/call
|
||||||
idiv32 0.02 usec/call
|
imul16 0.0442 usec/call
|
||||||
iadd64 0.00 usec/call
|
idiv16 0.0678 usec/call
|
||||||
isub64 0.00 usec/call
|
iadd32 0.0388 usec/call
|
||||||
imul64 0.02 usec/call
|
isub32 0.0404 usec/call
|
||||||
idiv64 0.16 usec/call
|
imul32 0.0404 usec/call
|
||||||
memcpy128 1.06 usec/call
|
idiv32 0.0564 usec/call
|
||||||
memset128 1.06 usec/call
|
iadd64 0.0636 usec/call
|
||||||
delay(1) 1000.00 usec/call
|
isub64 0.0946 usec/call
|
||||||
|
imul64 0.0880 usec/call
|
||||||
|
idiv64 0.5328 usec/call
|
||||||
|
memcpy128 4.3514 usec/call
|
||||||
|
memset128 9.7858 usec/call
|
||||||
|
delay(1) 1010.236 usec/call
|
||||||
|
SEM 2.2752 usec/call
|
||||||
|
Loading…
Reference in New Issue
Block a user