From c31867f104abc4fde1a771d8b21295326f1fba87 Mon Sep 17 00:00:00 2001 From: Jiangxuan Chen <89390126+j-chen-opteran@users.noreply.github.com> Date: Mon, 23 Jan 2023 00:41:39 +0000 Subject: [PATCH] support for KakuteH7v2, KakuteH7mini, and BMI270 IMU driver (#20545) Signed-off-by: Julian Oes --- .../kakutef7/nuttx-config/nsh/defconfig | 2 +- boards/holybro/kakuteh7/default.px4board | 1 + boards/holybro/kakuteh7/init/rc.board_sensors | 5 +- .../kakuteh7/nuttx-config/nsh/defconfig | 2 +- .../nuttx-config/scripts/bootloader_script.ld | 10 +- .../kakuteh7/nuttx-config/scripts/script.ld | 6 +- boards/holybro/kakuteh7/src/init.c | 3 +- boards/holybro/kakuteh7/src/spi.cpp | 1 + .../holybro/kakuteh7mini/bootloader.px4board | 3 + boards/holybro/kakuteh7mini/default.px4board | 95 ++ .../holybro_kakuteh7mini_bootloader.bin | Bin 0 -> 41736 bytes .../holybro/kakuteh7mini/firmware.prototype | 13 + .../kakuteh7mini/init/rc.board_defaults | 51 + .../holybro/kakuteh7mini/init/rc.board_extras | 13 + .../kakuteh7mini/init/rc.board_sensors | 9 + .../nuttx-config/bootloader/defconfig | 114 +++ .../kakuteh7mini/nuttx-config/include/board.h | 419 ++++++++ .../nuttx-config/include/board_dma_map.h | 40 + .../kakuteh7mini/nuttx-config/nsh/defconfig | 272 ++++++ .../nuttx-config/scripts/bootloader_script.ld | 215 +++++ .../nuttx-config/scripts/script.ld | 228 +++++ .../holybro/kakuteh7mini/src/CMakeLists.txt | 67 ++ .../holybro/kakuteh7mini/src/board_config.h | 214 +++++ .../kakuteh7mini/src/bootloader_main.c | 75 ++ boards/holybro/kakuteh7mini/src/hw_config.h | 125 +++ boards/holybro/kakuteh7mini/src/i2c.cpp | 38 + boards/holybro/kakuteh7mini/src/init.c | 261 +++++ boards/holybro/kakuteh7mini/src/led.c | 223 +++++ boards/holybro/kakuteh7mini/src/spi.cpp | 52 + .../holybro/kakuteh7mini/src/timer_config.cpp | 55 ++ boards/holybro/kakuteh7mini/src/usb.c | 105 +++ boards/holybro/kakuteh7v2/bootloader.px4board | 3 + boards/holybro/kakuteh7v2/default.px4board | 96 ++ .../extras/holybro_kakuteh7v2_bootloader.bin | Bin 0 -> 41736 bytes boards/holybro/kakuteh7v2/firmware.prototype | 13 + .../holybro/kakuteh7v2/init/rc.board_defaults | 45 + .../holybro/kakuteh7v2/init/rc.board_extras | 13 + .../holybro/kakuteh7v2/init/rc.board_sensors | 9 + .../nuttx-config/bootloader/defconfig | 114 +++ .../kakuteh7v2/nuttx-config/include/board.h | 419 ++++++++ .../nuttx-config/include/board_dma_map.h | 40 + .../kakuteh7v2/nuttx-config/nsh/defconfig | 272 ++++++ .../nuttx-config/scripts/bootloader_script.ld | 215 +++++ .../kakuteh7v2/nuttx-config/scripts/script.ld | 228 +++++ boards/holybro/kakuteh7v2/src/CMakeLists.txt | 67 ++ boards/holybro/kakuteh7v2/src/board_config.h | 213 +++++ .../holybro/kakuteh7v2/src/bootloader_main.c | 75 ++ boards/holybro/kakuteh7v2/src/hw_config.h | 125 +++ boards/holybro/kakuteh7v2/src/i2c.cpp | 38 + boards/holybro/kakuteh7v2/src/init.c | 261 +++++ boards/holybro/kakuteh7v2/src/led.c | 223 +++++ boards/holybro/kakuteh7v2/src/spi.cpp | 52 + .../holybro/kakuteh7v2/src/timer_config.cpp | 55 ++ boards/holybro/kakuteh7v2/src/usb.c | 105 +++ src/drivers/drv_sensor.h | 2 +- src/drivers/imu/bosch/bmi270/BMI270.cpp | 891 ++++++++++++++++++ src/drivers/imu/bosch/bmi270/BMI270.hpp | 193 ++++ .../bosch/bmi270/Bosch_BMI270_registers.hpp | 206 ++++ src/drivers/imu/bosch/bmi270/CMakeLists.txt | 48 + src/drivers/imu/bosch/bmi270/Kconfig | 5 + src/drivers/imu/bosch/bmi270/bmi270_main.cpp | 85 ++ 61 files changed, 6814 insertions(+), 14 deletions(-) create mode 100644 boards/holybro/kakuteh7mini/bootloader.px4board create mode 100644 boards/holybro/kakuteh7mini/default.px4board create mode 100755 boards/holybro/kakuteh7mini/extras/holybro_kakuteh7mini_bootloader.bin create mode 100644 boards/holybro/kakuteh7mini/firmware.prototype create mode 100644 boards/holybro/kakuteh7mini/init/rc.board_defaults create mode 100644 boards/holybro/kakuteh7mini/init/rc.board_extras create mode 100644 boards/holybro/kakuteh7mini/init/rc.board_sensors create mode 100644 boards/holybro/kakuteh7mini/nuttx-config/bootloader/defconfig create mode 100644 boards/holybro/kakuteh7mini/nuttx-config/include/board.h create mode 100644 boards/holybro/kakuteh7mini/nuttx-config/include/board_dma_map.h create mode 100644 boards/holybro/kakuteh7mini/nuttx-config/nsh/defconfig create mode 100644 boards/holybro/kakuteh7mini/nuttx-config/scripts/bootloader_script.ld create mode 100644 boards/holybro/kakuteh7mini/nuttx-config/scripts/script.ld create mode 100644 boards/holybro/kakuteh7mini/src/CMakeLists.txt create mode 100644 boards/holybro/kakuteh7mini/src/board_config.h create mode 100644 boards/holybro/kakuteh7mini/src/bootloader_main.c create mode 100644 boards/holybro/kakuteh7mini/src/hw_config.h create mode 100644 boards/holybro/kakuteh7mini/src/i2c.cpp create mode 100644 boards/holybro/kakuteh7mini/src/init.c create mode 100644 boards/holybro/kakuteh7mini/src/led.c create mode 100644 boards/holybro/kakuteh7mini/src/spi.cpp create mode 100644 boards/holybro/kakuteh7mini/src/timer_config.cpp create mode 100644 boards/holybro/kakuteh7mini/src/usb.c create mode 100644 boards/holybro/kakuteh7v2/bootloader.px4board create mode 100644 boards/holybro/kakuteh7v2/default.px4board create mode 100755 boards/holybro/kakuteh7v2/extras/holybro_kakuteh7v2_bootloader.bin create mode 100644 boards/holybro/kakuteh7v2/firmware.prototype create mode 100644 boards/holybro/kakuteh7v2/init/rc.board_defaults create mode 100644 boards/holybro/kakuteh7v2/init/rc.board_extras create mode 100644 boards/holybro/kakuteh7v2/init/rc.board_sensors create mode 100644 boards/holybro/kakuteh7v2/nuttx-config/bootloader/defconfig create mode 100644 boards/holybro/kakuteh7v2/nuttx-config/include/board.h create mode 100644 boards/holybro/kakuteh7v2/nuttx-config/include/board_dma_map.h create mode 100644 boards/holybro/kakuteh7v2/nuttx-config/nsh/defconfig create mode 100644 boards/holybro/kakuteh7v2/nuttx-config/scripts/bootloader_script.ld create mode 100644 boards/holybro/kakuteh7v2/nuttx-config/scripts/script.ld create mode 100644 boards/holybro/kakuteh7v2/src/CMakeLists.txt create mode 100644 boards/holybro/kakuteh7v2/src/board_config.h create mode 100644 boards/holybro/kakuteh7v2/src/bootloader_main.c create mode 100644 boards/holybro/kakuteh7v2/src/hw_config.h create mode 100644 boards/holybro/kakuteh7v2/src/i2c.cpp create mode 100644 boards/holybro/kakuteh7v2/src/init.c create mode 100644 boards/holybro/kakuteh7v2/src/led.c create mode 100644 boards/holybro/kakuteh7v2/src/spi.cpp create mode 100644 boards/holybro/kakuteh7v2/src/timer_config.cpp create mode 100644 boards/holybro/kakuteh7v2/src/usb.c create mode 100644 src/drivers/imu/bosch/bmi270/BMI270.cpp create mode 100644 src/drivers/imu/bosch/bmi270/BMI270.hpp create mode 100644 src/drivers/imu/bosch/bmi270/Bosch_BMI270_registers.hpp create mode 100644 src/drivers/imu/bosch/bmi270/CMakeLists.txt create mode 100644 src/drivers/imu/bosch/bmi270/Kconfig create mode 100644 src/drivers/imu/bosch/bmi270/bmi270_main.cpp diff --git a/boards/holybro/kakutef7/nuttx-config/nsh/defconfig b/boards/holybro/kakutef7/nuttx-config/nsh/defconfig index 95d1117245..faf8655412 100644 --- a/boards/holybro/kakutef7/nuttx-config/nsh/defconfig +++ b/boards/holybro/kakutef7/nuttx-config/nsh/defconfig @@ -75,7 +75,7 @@ CONFIG_BOARD_RESET_ON_ASSERT=2 CONFIG_BUILTIN=y CONFIG_CDCACM=y CONFIG_CDCACM_IFLOWCONTROL=y -CONFIG_CDCACM_PRODUCTID=0x0016 +CONFIG_CDCACM_PRODUCTID=0x0050 CONFIG_CDCACM_PRODUCTSTR="PX4 KakuteF7" CONFIG_CDCACM_RXBUFSIZE=600 CONFIG_CDCACM_TXBUFSIZE=12000 diff --git a/boards/holybro/kakuteh7/default.px4board b/boards/holybro/kakuteh7/default.px4board index d3b5c29ce3..d1b4ff6007 100644 --- a/boards/holybro/kakuteh7/default.px4board +++ b/boards/holybro/kakuteh7/default.px4board @@ -15,6 +15,7 @@ CONFIG_DRIVERS_DSHOT=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM20689=y CONFIG_DRIVERS_IMU_INVENSENSE_MPU6000=y +CONFIG_DRIVERS_IMU_BOSCH_BMI270=y CONFIG_COMMON_LIGHT=y CONFIG_COMMON_MAGNETOMETER=y CONFIG_COMMON_OPTICAL_FLOW=y diff --git a/boards/holybro/kakuteh7/init/rc.board_sensors b/boards/holybro/kakuteh7/init/rc.board_sensors index 2711927fe3..c6c9960c39 100644 --- a/boards/holybro/kakuteh7/init/rc.board_sensors +++ b/boards/holybro/kakuteh7/init/rc.board_sensors @@ -1,10 +1,11 @@ #!/bin/sh # -# Holybro KakuteH7 specific board sensors init +# Holybro KakuteH7v1 specific board sensors init #------------------------------------------------------------------------------ board_adc start -# The default IMU is an ICM20689, but there might also be an MPU6000 +# The KakuteH7 v1 comes by default with an ICM20689, +# but there might also be an MPU6000 if ! mpu6000 -R 6 -s start then icm20689 -R 6 -s start diff --git a/boards/holybro/kakuteh7/nuttx-config/nsh/defconfig b/boards/holybro/kakuteh7/nuttx-config/nsh/defconfig index 7cb5571c4f..c7d29ddb92 100644 --- a/boards/holybro/kakuteh7/nuttx-config/nsh/defconfig +++ b/boards/holybro/kakuteh7/nuttx-config/nsh/defconfig @@ -75,7 +75,7 @@ CONFIG_BOARD_RESET_ON_ASSERT=2 CONFIG_BUILTIN=y CONFIG_CDCACM=y CONFIG_CDCACM_IFLOWCONTROL=y -CONFIG_CDCACM_PRODUCTID=0x004b +CONFIG_CDCACM_PRODUCTID=0x0050 CONFIG_CDCACM_PRODUCTSTR="PX4 KakuteH7" CONFIG_CDCACM_RXBUFSIZE=600 CONFIG_CDCACM_TXBUFSIZE=12000 diff --git a/boards/holybro/kakuteh7/nuttx-config/scripts/bootloader_script.ld b/boards/holybro/kakuteh7/nuttx-config/scripts/bootloader_script.ld index 6f9f9e1b5f..fc0a5589be 100644 --- a/boards/holybro/kakuteh7/nuttx-config/scripts/bootloader_script.ld +++ b/boards/holybro/kakuteh7/nuttx-config/scripts/bootloader_script.ld @@ -59,9 +59,9 @@ * 2) BOOT=1: Boot address defined by user option byte BOOT_ADD1[15:0]. * ST programmed value: System bootloader at 0x1FF0:0000 * - * The KakuteH7 has a Swtich on board, the BOOT0 pin is at ground so by default, - * the STM32 will boot to address 0x0800:0000 in FLASH unless the swiutch is - * drepresed, then the boot will be from 0x1FF0:0000 + * The KakuteH7 has a switch on board, the BOOT0 pin is at ground so by default, + * the STM32 will boot to address 0x0800:0000 in FLASH unless the switch is + * depressed, then the boot will be from 0x1FF0:0000 * * The STM32H743VI also has 1024Kb of data SRAM. * SRAM is split up into several blocks and into three power domains: @@ -105,12 +105,14 @@ * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 * where the code expects to begin execution by jumping to the entry point in * the 0x0800:0000 address range. + * + * The bootloader uses the first sector of the flash, which is 128K in length. */ MEMORY { itcm (rwx) : ORIGIN = 0x00000000, LENGTH = 64K - flash (rx) : ORIGIN = 0x08000000, LENGTH = 2048K + flash (rx) : ORIGIN = 0x08000000, LENGTH = 128K dtcm1 (rwx) : ORIGIN = 0x20000000, LENGTH = 64K dtcm2 (rwx) : ORIGIN = 0x20010000, LENGTH = 64K sram (rwx) : ORIGIN = 0x24000000, LENGTH = 512K diff --git a/boards/holybro/kakuteh7/nuttx-config/scripts/script.ld b/boards/holybro/kakuteh7/nuttx-config/scripts/script.ld index aa5405fd5f..ae07f4bfca 100644 --- a/boards/holybro/kakuteh7/nuttx-config/scripts/script.ld +++ b/boards/holybro/kakuteh7/nuttx-config/scripts/script.ld @@ -59,9 +59,9 @@ * 2) BOOT=1: Boot address defined by user option byte BOOT_ADD1[15:0]. * ST programmed value: System bootloader at 0x1FF0:0000 * - * The KakuteH7 has a Swtich on board, the BOOT0 pin is at ground so by default, - * the STM32 will boot to address 0x0800:0000 in FLASH unless the swiutch is - * drepresed, then the boot will be from 0x1FF0:0000 + * The KakuteH7 has a switch on board, the BOOT0 pin is at ground so by default, + * the STM32 will boot to address 0x0800:0000 in FLASH unless the switch is + * depressed, then the boot will be from 0x1FF0:0000 * * The STM32H743VI also has 1024Kb of data SRAM. * SRAM is split up into several blocks and into three power domains: diff --git a/boards/holybro/kakuteh7/src/init.c b/boards/holybro/kakuteh7/src/init.c index a9fe66548e..cf73735aed 100644 --- a/boards/holybro/kakuteh7/src/init.c +++ b/boards/holybro/kakuteh7/src/init.c @@ -240,8 +240,7 @@ __EXPORT int board_app_initialize(uintptr_t arg) int result = mmcsd_spislotinitialize(CONFIG_NSH_MMCSDMINOR, CONFIG_NSH_MMCSDSLOTNO, spi_dev); if (result != OK) { - led_on(LED_BLUE); - syslog(LOG_ERR, "[boot] FAILED to bind SPI port 1 to the MMCSD driver\n"); + syslog(LOG_ERR, "[boot] Could not bind MMCSD driver, expected on Kakute H7 V2\n"); } up_udelay(20); diff --git a/boards/holybro/kakuteh7/src/spi.cpp b/boards/holybro/kakuteh7/src/spi.cpp index cb19b56827..1ff5b7f35c 100644 --- a/boards/holybro/kakuteh7/src/spi.cpp +++ b/boards/holybro/kakuteh7/src/spi.cpp @@ -45,6 +45,7 @@ constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { initSPIBus(SPI::Bus::SPI4, { initSPIDevice(DRV_IMU_DEVTYPE_ICM20689, SPI::CS{GPIO::PortE, GPIO::Pin4}, SPI::DRDY{GPIO::PortE, GPIO::Pin1}), initSPIDevice(DRV_IMU_DEVTYPE_MPU6000, SPI::CS{GPIO::PortE, GPIO::Pin4}, SPI::DRDY{GPIO::PortE, GPIO::Pin1}), + initSPIDevice(DRV_IMU_DEVTYPE_BMI270, SPI::CS{GPIO::PortE, GPIO::Pin4}, SPI::DRDY{GPIO::PortE, GPIO::Pin1}), }), }; diff --git a/boards/holybro/kakuteh7mini/bootloader.px4board b/boards/holybro/kakuteh7mini/bootloader.px4board new file mode 100644 index 0000000000..19b6e662be --- /dev/null +++ b/boards/holybro/kakuteh7mini/bootloader.px4board @@ -0,0 +1,3 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_ROMFSROOT="" diff --git a/boards/holybro/kakuteh7mini/default.px4board b/boards/holybro/kakuteh7mini/default.px4board new file mode 100644 index 0000000000..d58d3ca941 --- /dev/null +++ b/boards/holybro/kakuteh7mini/default.px4board @@ -0,0 +1,95 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS3" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS0" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS1" +CONFIG_BOARD_SERIAL_RC="/dev/ttyS4" +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_COMMON_BAROMETERS=y +CONFIG_DRIVERS_BATT_SMBUS=y +CONFIG_DRIVERS_CAMERA_CAPTURE=y +CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y +CONFIG_COMMON_DISTANCE_SENSOR=y +CONFIG_DRIVERS_DSHOT=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20689=n +CONFIG_DRIVERS_IMU_INVENSENSE_MPU6000=y +CONFIG_DRIVERS_IMU_BOSCH_BMI270=y +CONFIG_COMMON_LIGHT=y +CONFIG_COMMON_MAGNETOMETER=y +CONFIG_COMMON_OPTICAL_FLOW=y +CONFIG_DRIVERS_OSD=y +CONFIG_DRIVERS_POWER_MONITOR_INA226=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_MODULES_SIMULATION_PWM_OUT_SIM=y +CONFIG_DRIVERS_RC_INPUT=y +CONFIG_DRIVERS_ROBOCLAW=y +CONFIG_DRIVERS_RPM=y +CONFIG_DRIVERS_SMART_BATTERY_BATMON=y +CONFIG_COMMON_TELEMETRY=y +CONFIG_DRIVERS_TONE_ALARM=y +CONFIG_MODULES_AIRSPEED_SELECTOR=y +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y +CONFIG_MODULES_BATTERY_STATUS=y +CONFIG_MODULES_CAMERA_FEEDBACK=y +CONFIG_MODULES_COMMANDER=y +CONFIG_MODULES_CONTROL_ALLOCATOR=y +CONFIG_MODULES_DATAMAN=y +CONFIG_MODULES_EKF2=y +CONFIG_MODULES_ESC_BATTERY=y +CONFIG_MODULES_EVENTS=y +CONFIG_MODULES_FLIGHT_MODE_MANAGER=y +CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y +CONFIG_MODULES_FW_POS_CONTROL_L1=y +CONFIG_MODULES_FW_RATE_CONTROL=y +CONFIG_MODULES_GIMBAL=y +CONFIG_MODULES_GYRO_CALIBRATION=y +CONFIG_MODULES_GYRO_FFT=y +CONFIG_MODULES_LAND_DETECTOR=y +CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y +CONFIG_MODULES_LOAD_MON=y +CONFIG_MODULES_LOGGER=y +CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MANUAL_CONTROL=y +CONFIG_MODULES_MAVLINK=y +CONFIG_MODULES_MC_ATT_CONTROL=y +CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y +CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y +CONFIG_MODULES_MC_POS_CONTROL=y +CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_NAVIGATOR=y +CONFIG_MODULES_RC_UPDATE=y +CONFIG_MODULES_ROVER_POS_CONTROL=y +CONFIG_MODULES_SENSORS=y +# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set +CONFIG_MODULES_SIMULATION_SIH=y +CONFIG_MODULES_TEMPERATURE_COMPENSATION=y +CONFIG_MODULES_VTOL_ATT_CONTROL=y +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_BL_UPDATE=y +CONFIG_SYSTEMCMDS_DMESG=y +CONFIG_SYSTEMCMDS_DUMPFILE=y +CONFIG_SYSTEMCMDS_GPIO=y +CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MFT=y +CONFIG_SYSTEMCMDS_MIXER=y +CONFIG_SYSTEMCMDS_MOTOR_TEST=y +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_PWM=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_SD_BENCH=y +CONFIG_SYSTEMCMDS_SD_STRESS=y +CONFIG_SYSTEMCMDS_SYSTEM_TIME=y +CONFIG_SYSTEMCMDS_TOP=y +CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y +CONFIG_SYSTEMCMDS_TUNE_CONTROL=y +CONFIG_SYSTEMCMDS_UORB=y +CONFIG_SYSTEMCMDS_USB_CONNECTED=y +CONFIG_SYSTEMCMDS_VER=y +CONFIG_SYSTEMCMDS_WORK_QUEUE=y diff --git a/boards/holybro/kakuteh7mini/extras/holybro_kakuteh7mini_bootloader.bin b/boards/holybro/kakuteh7mini/extras/holybro_kakuteh7mini_bootloader.bin new file mode 100755 index 0000000000000000000000000000000000000000..fca97ea49d7c894d56529048ab9297e29299d66e GIT binary patch literal 41736 zcmeFZdwf$>x;MV|CCN@(XiAHOwm>cg+onipL5nzJNXqUdU<>FBml+KdJ-gI_3aF?I zq!hJ?&LB7rP#qC;h7lD~$Q%pSBH(Qt&q;uK3dNxUvY|!Yp@r;T^L|%Cof&oB-|zkJ z{o~E&v$NLTd+l|3*0Y}VTvzr8lA+;<@Mre_UH=aT|M%bkx$%o!IkNeezt(*4ZKuXN zTHzDfz_*?H>gU8IA{&y}Z#$D}Mk|KIgr@Od8=l#;Z~W!H(hBjcni(L<(w=>n`buMO z_gyhOdi9 z;-#sh^QiX$_X*XQVd<(`WKj}Xry`(DW5x+J9nabpITZ)))Bkbb zyy>6rO&N1aj;fg%v+-|_{^w_~*GTSB)saz(XE`WOGMvip*+-Gm{3fQinWjldo#+wC z*i7h?giNQRMGFa;=w+s}%i%<8(flU9xA``dFo02ec9l>1^2nJdz4r3UZL-4`DlH^@cpoVst;vv)TKC3t}KJy zS5-)7tB$+#TJBz^j+vSzlP2O|=N7$ZCc{i7@+iycCa?@sD>M;eBC`$ttQo|_SWenKpI<#(%wO)OrbzN}~-_md1M@r1Rd!+Y7Jze3E@E@ad zLBF;8Z}fd^?CU3Qe9cMLc7{1$ALn8{^c#EQx_!hl{p!Mn`>s8{Hok>e9M^X;i!+mu zi_0|PpB$pI&TMAtHagiac3gYB>avWP&ttY+lVeYrFBa#2b>rFf=(966=8L%ezyHg# z;dsX6|Ic^N25AJPBg6vi0tHDK4-5p>rHmhZOXuM>f85Lj(I&+=q>EV|W)o4asaX=U zl_<5nDGkJ$BiTGAk=tj-Jn~VvrPR0-o~!9hsf|&oNuW%0O-@1^wP>RXYpQlUtvfN( z;Gg)#j*x*LlM{D_3wd1JwS}&D4`bze_^uQWF{QWNnIp8M&#>pG7bHZV6UE@UD&LpG zw-{%XMW1%cYjHPxMpg9giu}7A_Ef17u^SqbW^9T+J0~05d}h3d3zmY)C3}v`noJgX zlBderXwGZxloK=02X$`RI&h_l|B|!`xDs#~aAkarv<=VvbX2Bd+QI^fQxduP>0Bt^ zx-hSfOjG&zjn3X6EnzIiIvZ80cqf7yjRHNyWg0byNiKv!7`{dt~*AgcGOMof?)yG;xV3BS%{^Nz>j= zPzOJAfGE7myh|Em_k6ael_=cMg`G7+A9tU)G*qF_2d^ZRnM!c&3^DA#%a6aTjLFQ_ zwfZipV|L)$8Omfcv%{y)sxN(6+P*pd@0a40St>u(fooiw?~;-1z-2}*AGv&zj@&Bb zR*@0NsmQ5h7|k)rF+*sMLyi+@PKTV1qq#)n5*sfmV_57jBpbxKaj=KX?f?ERHtzqE z@nvRDZlz;Hn`2BWJGHbu*|oTHjH|RgrQrU~F$JaV!>2#cIc9ok`%U71bdC{A+eZ{i zons11+l@sPonwkh+ea2J=^RsB+MZfc**T`9v^~xJVCNV&?SE-|`V7?hX$)qcou=dA zvwz=8;WgM6O?QTP_MkjCzW?29@Y#{;tM=0G>Aaz@*1>vW^T@MUm&6oD1UoLl&nW-d z%V4DdCrpB$C`6_I;Fq#p0fWLMa^jQ#b33OpX-szhDaAjnof-QF-COmx0DHStWfySS z##6u0O>ftTH#MbJ@gZ1z;_ zd)7CyN@;wvgVhPQe{`8Va9-xJKW|ZFCi~;y$SS6dWcLQE3bCUo`ge#4saX&iCH^3< zMBa^DVTu^#=LboXkiZg33-6f0Snrtmtc4F%X>C$xArpdUv+lh$peDiDeAd zEQeZ&a#+Ybm>WkaqR}SpRS0N(bFA55b%w|1Pd$xOU@u2iI%3CZ`agriKV#rc?R` znh~BC(YQ;y@)s>OlNWgO_mpZ9DSabNGk-?}3H8x(vWjN`@ILwJe zXhT0=&lLO#E&7t#s@=Z8$95cMXG>cPweD02mq%zM7LEJ5oGQ|9X!*gh((eVzGyDi` zy%n@I60RMnL4P&wG|A@Elnj%GHKabvbp&LV$+`?*5vhZC^e@c#w!t%p^=$dSD(C|*scY?l^Mm~6Osq(-G{oX{7>!Eh|ija zil1HPb2%lb@?yY8%pcZp&nIkSpO4?hJRi613%n!GXNg3~Q#GSCr@pK1c3)Ma zN&HG8%A?(3d6^Qg>5|9zwJ(v)aj1`(M64r$?Vm+>%;6Zn?j`mG?Mt6sCX# zUso)~m7wg4L`hcp(dSwdwok)bqU^w3YqZ8+cT-FRHGG;hjY=lM_{-r^UK}bio41LW zk*9ySd!AMMcHr`!R!JE#wW!3-OwKP!l4_kpS`E@+tlBhnQi(<+nbU)j`lo?=fFGIC z#*9yr-f3dSGE=e~b5!#_uA_SBp%6)&5d1HdI3o35dH#wTX?dpZdi~3)-84_+OygG4 zUFU(7pd?eyZ?r1qT(XM~6|m=PKs(X*^=kS)M!B~q68?B#1Skxryag&^eKZmd49o?e zhmrl3natSVmXoAXg(M7d7n3AUs)T+Mw&VEncRLm(@~y1mi+qgMUy#ex9Fh=X5>lo2 zCFb@x$8^;~WZ*wt-!I{!>zl6gNp1=$l9kf5LEI+lM3OOFEEbKHDm$k<*;^z&t=2j? zWmT`3KJ!qH$WF=^wH@mjMtQDVi+yXoKr#ygq(g|?9TJ57BL9=a*p7?P`}=x%abH2P z$V@83-K5^T3tFb91&OIdR2FGYSI0?fImcVmLW2*I~^X z24#Wo$uW#_cXw><85;Bb$?kON{6sBNd!GEEibDKpY*4>_76EOcpm9A-n z-)QD>_pNI3?UGt04xF|D)GQv zRDdXT=nM9cZd#L8`2h8BN)!HB<;8&xnJ8Ni5#_~0kt@W?m+-|-HP680{n6GFjD#Hr zDcz*>GT+aO#{Gw-eLk!zDye->)=F@bv1t-Bm2<=w5y_!i%VKicuF7N|g~WZ>4I<$; zBec~81H_b%y<>3CQT-#tk!~$^hEY-^T+pBBt9)>(=Ok^ZhFGFBb=~s6P?>NYBvSU4 zqyTqIN`Sc~CAj24vlQnd8A}$oMx{ccBtd%4>o8)+e~7H6p~rxw~?>wsT^w(^BWC*?unY%{_;Jn9$L&QWsyO= zW^D;UwD%)Yyr#HaW^7*v8!s6gtWpQq+w~+P6IVWVWG2H5TxOLEkV}|JrkpS&s>meT zGM#l2nxUa@?}(H&@S(e-`CB9T4Jea)>~a#FN#*T6@QTWwK1GgwVThKpMlu_WsE1WX zM~n@$+`8VYawPowNS6#gdD;+Bzls;L_7~V$<&P02Qzx#W{imG4_r_^md%a$38y3u3 z#fORrg^tH(lT70I`zs~u1H`&(F;NyrqI|-*18p>YP0Ct@x+tbn*h8a1VNW$0yDE}> z#>Z-%LhyJ!)pF$dvZeW+CDL|Jo17*o^3SkO_xkQBVwIx;{luG@>EJS zp6HDJpoctW@#}h4*)Tw7?U4NA<8XklVU^zxM0=Xq95k3% zRXadGKI+wC2g(UJq@w*qIo?Y$-GLS8LA|uj(<#+y`IA|z80$tyva7~pla8Wi+`=Dk zlMl^cXsdS&Sg^nR15T0fE4}!l_Q#;DwF$xR!}q|zeLb~mE&XDG5MWMYMQQCK_NQu4 zGVTA!*0mZN7v#Z56J=PBb*;|!ZP2z#uw4n#Cm(iSk}1zLh<6_tiJNcczco z#_jv1%6CMvhrnv`o}&rgdtzQy5Nwr{~Uvjr=H1{v2HLQ@dp#(;=-TuVZLfRs~U4xeCdZuksn#6CO zCZ5}97cFS@T+N~f?{Tbak7tKFo@|dt9jsE@uc?bsDDYPOcEDqJ?+!@&kt-VTkm;a$ zpY%lG)=gJvBz*iJrBi~Rbu*?kye;XD-Fqby)`|i6c(MtaMo9U@DwGTZ#jXY=VLOg; zb5|X95ru(_!{o)=LfEOQ3SYy%nD2j5nh$+lDlJW_og<^AH6auEh4gNew)_we=-Ugx zoR9jceh>U^8?E#2L78IrlRqraDzEqdgKCi`+AikM^^}A?aAv8(>P@%q5TD#<2bACF zuas8!CahxqOe~+N37hgMUkWWDQJ(6aBGM9T`=%$cSPFmG-8fFjqYoZW=-8P<=g09}1B%-|EH@KVFeOzrC&zuBh@>02nN?%ol27e@cH3EMA^kOHg2z|UK zwpSP$(W4*H;Bue}+92~_d@YlgRv+&rR&H^OGoU;RzF7~LP+kJ7iBit!4!>5Dtq*D* z%&ui}ht+3$iDlY)78HKPjlShKMy{xkH#6N?cX>=lFOz~yH~qi)#{Y-N(|cCArAH$c z`AEp4%Xv6cG)|rfyn3g1uQMFR&r;;x$?Hi^bbw8Wo!Uq=ZCN({YG z!l5BI8}7P!g-J9-OT749iT>_b9S+nX{8Nc1qb1Zv1NK-WE)%Xn+V}6E&lS_vCiT+Q zMrCXIE#=aOi)AHausgyUv2Uo@stP;!Q*<_Bobhh?uU*#MTvz zhaMsnh=jlCCz)RdFUnKIxmXil^xHuN7|REjiIoM1cu~&rl|!ezORDsom#)irUht-; zRh}$C-U((yf}>xx^=rf@8%Rz7ymDN~zyNmODI?+Sk&k3ziIw@U{;%bel#w%K-P;-6_SqcsR-d zqLvM@S~h^vl293I5>qBYTD7vu@gA~0uJ{EfQ`Z%Y)k@nZlsebOzNtk{5Z{mT=oFsB zdYgZp#L%7N$?r?+FI40-#8wKkm1@|{k1@MhWomDRbQOHf5a@UvzJYL8B%>n5XI;I= z$pr_({orWx8#AP9a^%V|(DF3N`W<=BB*eY>2A zL93MJ+b+E-#T9J#%=N{A_q+oX&f^)RE=Dm;x-9b;{kWd1l;b8La8!}M=L7Ef{sd9y z)`)Y|i3YCY7?<`~tBz~NKB(qAn%~GOH}~|&nV|0)H@nv0pQGj)=)Ru|YHEFyt_Awe z;E$Ks)%$(vt6y+xkI$DDH5|T7a_4#kPjXut=7`=IJVlVM653+Ie6G0hP zTiR|&PB=*|8CqzrtfpMZe2Ijyj`VS`9dB9)8f)*CkD(6iorM}b)qs%_KvaNVSb>%661Kh)Je zl$)Vy?WOHSdd~W}O4_(Au1Ak|+|Xt1TmpkUEtBV28sNJedFaobl>zkf&V(k`O7KNO z8!eyEJ~Ucar^nk6Z98tT?F^I2R7c9tZd|lYj;oo)gd#tEFT^AcX+fR$`>&B@u7LlV zW0^DX`Ze#8(;=dn-VvwSFKK%8*BBEmnSj2K9zmJjo}lrA!c1s+T{FI!b1_*fJjsFc znxtl4N#IM{Yq>-dAKF*IDGA(3RY=+Dc*S9ECrqO2m@2G{NeRRR4&-UpJWuA#KbNz_ zdt_o-BNOXZnX$eovty6Q+}OjiwViPP9mRaAbD?v1xIPk2Yl>Wn&ATBw?*g57`y5|t zMQrBjyc?V;O-3=QU&LM;`&RJt%H$qi$-_0VXNcP<85_?gw;b zKyJJwICRPr4KbS!qfOF<9weuhMt2)Y1qf(8Z96Tpe>)usaKSS_a zsl@yvdReW~qgthwr9@$)Iu%BQEy7RELTc!R{j0=l6^fau)W+8v)IvudwOJ{T^ioJL z%6sn;_+otf-dles_~rZKW{d?K9*fw00>*3cwLf5qjyJ#2DruslZM>9^u}+MRm5!70 zkKRg?@BGGAo@r*3Zt#|{OV5EG&wA|g(7f_;s#nrYtf}Nhj9c@B&sDj{IrP-g%50H< zOB@JS_MWTc#O#7|<%=q3r!6WsV&5RvUYVmhSJOywa5RxCR7)N|V;uPYRsETwM)`VR zu%B0N^s@^6)ZXZ473764zSBupVLr6t5HUrx`rhBjC>#?BzZ%IY$P_aa9ltQ`!^=bO zc?dJY!A4jGK5CQy*r=L)^c~r25g%)qGOEFu*))ZYfg72sbDL3vx!@TmA56HdQQXwX z6oh0zdFnObH-#sqrTKm`_I-&Lr$6xI10OuFu)zpSbzJ?%y5s6Mrt)$?hfruj#%Xd? zp0s3%J3=S9wn}3`+XDmPwx08q3oFkpjW0;{lvkWvYK7m#5p}&CJjC+zOJTV-EmLk{ z&oAA+NU_lSzm=U|Iv*T5rKJYHYbh^shte>I>zt#4sWYrnas%CSg~DB-ljO+dk;x{{ zNy|t;|M%Tii3^>CEp}LmRXVD!H+e1d!Ml2w(>4r^(;hf!d4Fm31Fzyc+OM_U=aFFzOrgGH~=n@s{>q}#n;_{^?F(xJcP(C8oOtJg{%*Q}X4`uML zU&?f~tk|KJ85o6OERvRGpRi$ozY()GND$}T_7KSaWR|4#R#_>WdbOZ`A! zwry#g1ivN1DvU<({QXz=EqzE}Itm$I@Ac5)tlLj96Ct%aiUkwOY+q{dY+oAmY*Bw^ zu$C}cY&&7|g3hkPi?*mgC;Ch_AY|HukL{VphDYg+W# zJWn-!_RwhGz02rx??cs|Y**dV3-zfU%IzqP?{_*`vy#X-(6*k}CnUKJ%WRg=gqjz9 zEbmAAzH4pEa1XDYPk?I!mm}XEt=zt}Y{`Cgw#m`XIuF;!d5C2f?x<99I5zqp(87Ic znSF}SrI?rtpv3bdG2YtRwdo-ije?!vu6ZeP=99c^)WVhNIaC# zRN6&WfJ<<_pw2ZIM5>i{n2DL*&oj_FX#S_sdzWJ8ws^k$6{CC*A*NIENb!Q4yMGJ( zBJ7Mk%N!&&=Jdj4mwm~obwnV!i>ys)V!-FGM{8RogLt&lo4j6~i+pS1dUc+iQQnWV z-Jgk_;OxNXGD;abqPy3dkPwemzMy(YfA}{MVxhUDsN_5lPCuCM|DaM^oGtziPY1#& zkdcfHG~XY7Hp&fGXVbNrB5q!KqN1*TtM)Z@wmrG|0I+<6HnEv%pC2ALfM=;%Eq!WF z)3VX0lb{ziG$t!U6Tb}%wI(mRf?gc0OcA%Br#4?rWnF~@7O<^_R?#TJerE0l&z+Bc z60=6!wREJ1;>T$BcF-z7)iSE6KYX>nZD}ODpx3l45}w=ZStj@*;k$cxE~RjuBU!tC zJxA|@hG5PdPo}tYX_7lhigzS?==*yNSb<$_<#ZLg+NYIo@kGKi2X-zcrew*b5cpjU z;gUU!^)0V^*6;5pi59VCQo0Me-n@jyqPVfqmz0sxf@pYsrJm{?DFO6GbXP z_4mhYOdaU^5cFLW%!NI`YmP_nxAgB^nhZKXTcYDQ6&=Ut7>Ciy!Q}wXyZ12s3Wk$#e1$kHqFMvN$a8YcH6PXI= zVtwukZXws;T1c$!h2?8ZYmRmv)|eXFB(mDmd6X-E}Zanb?xUITgFEWRx9p;6rKLXydywyt&ohSrso!(OKF7Inx3yTnnkO4610NE3l5@oM&PdFX!CDUjfYe2Ds7)JqH&&$A8~RI zm*kGMWrOzgySCb&#hk|4JM=H@ku{7}TTC(@XdA9I&(yYR zKUqb0Tc#O0+@?tQIq-bb*05_n1;6hcHls;!>)ji4$pzpx;CEOQrE94k*P3O3qU(_; z-`5na6MbitwJ@6Ap!wgkvpi+Iw)zvFHp|iiIkATcWjTV77+(?-dbscu=X-B>^1+={?zk3b! zOIV;wIBBVv-(X#nTC{)}Id_^u7LF_{U$dxl9WcMDF<3hiJZ>Z`!D@ECcGz9>M<*3n z*7$lmL$evTYtuYe`}?kV_z-&>oNLQ0>zW13{JBnrcg{y0)JL!U5$hUaI~}ZCt+g!b z)FoH01h!(n4HRE9*X`4O0qs~ZpBE=>buycK&QM=OVh|j5KL>s=byQmFHYYh_2k&>Z zQ#Y1X`8!yZ`vvmTah0@ry&TI%t`EKL@%o zM0ELFYg$3i$r>*>&T|oKMH(zV)W@J3(h`N*XZ>Bkv>RCcJ^RiwnK^TaFHl8usA z*Vnvp?$`Aczc!YcqP^P^F|Dx`GZs*f{B;hKXl-XXN(DB#-?#x2+yuD)^$$yjMB(t{I@(yH8~i2y62i~pnba0FDe^( z?2+*P2yZE0O)HFnG=T^VE%T^SEVSC`t@ zS)112#yW9u?niPrT&sP*7u6=wRp!7dGp`xxE@6((?MUBTU*EASG4AwDC5-jBI!V}wm3c&+l>d=DO6-vH zHe`zvv7e1Bh=lKn{9}z;8=hFLiLTHmR_1Q#tQ}tPR{b95G;zImWc zGGZ4ti>>g38nEWvHNKRLg=`&Hs-)}Bue3g_@6{C9#gE{tLGcagPG)i>JZfO8`j}lC zI!2;csLbF@x24}&i)WE=BHsOEqHequPiv9;A~@VlEx*J(yomb!mnzIKc-v;iTj0r@ z32K8#0Fr6x8dwBslj*QnpRsmTfs5if>pIqIT^vZ{=3zdP)yev@YjX4m^`PfKWylxWutnA=nA7VDgYm%yxEQQ z<6Ogu$Fza8g0>RCTT9@mZT*30?l9JM6AX9N1`+mefW3ODhpg>(sy=}NVqwo zM(x?I)sRLNg)`Y4$~rH(w)x>@ipWsz`oVsh<3k=22|v*P0^msZ{q;y+0F-0-$LxR= z<}yu_HpJECm}6a6{_AALlc)*sq2Zt&umbi@X*-~fs5+pIG#pUVOo=^D54@n>bXcgr zss7ziLgKiV#H)fZSItpK)8t!;@GY+CxC(EJrC%Y<%8y+Wk(T26Y;x?HiE^gZqK(e5IiM~An8=i#)r4_c1GhMA~^t&lBk zui<7idGaXFVl7xeIZ>|sQP}z)1#cOJ$I*VD{2!10>Cx*l?fIXYN%reO+8S#c5hN4V zc9ri%twfu7=kTii!G2ZHb4i=NOUGBXMzAD_5t+4ImpMz9%v-L?8jB(? z`|d5}qdVk@=+nf#ofl7Q*LIEHTh^}S)^=s>T|42<>*My8ucn%1)^=x?X|JW^%U{E0 zg!qYpa#uT(d<6N?Zp$;t4l4DmTVY+dR>d@q0hJXkJ~4TD5!sj(cwJz0hN>?TWn3 zVBCFgzjx8HdKTV>-=-~gy&Y>&d3z8q2f9h-+O8{ao1cl)>q_DrV|>mocDZHUxr>nI zm-onn`$Vr2iG*>1c9lN4@2tA7zEw?ew5moEqKNW<^DC^56o*y0Tn@?03h8VGT@RnC zDX_Wj@s8yZn^9J3#SS^zachD@dDDyg{D^sLNm>FTr<9ky4{*bpUj$cboQr6w%;AnV z>Y2?X+o9xC;~7eBDygfdC5Kg$oMERomN4dLXHqJfDGpN6{;<-0Niz9ZX?8`NFH781 zXx79v*A#yorL1WcoNp^Ui`POrJJWqmJFoe1_oyWrXw4f!+QivsG+oND`lvSk#p?Q~ zHhu?E?}X{W4?5$9lkw96XVt^i>EpD$`lmiuCp|=CQt@RdLuq)KJKaf?it0%Cbl(lz`fpowOtV2s@+=Tc zxpQT5vpBO;Hw|%LrR{|y=c$|tGnLP_bAl0`-ecgcGPPs`_LX9lwO6ioV$~^}$Ceyy zRwtR~ANn2Ipgi-FNNFDX9ixG7)PMF=LPDT?uTfzWxd3a;E}=CT6(KP`P#GPy3D$f{ zF@N4KSS``Fj|jRDqx;XGL7pXAv1|NUrFEL%DZh?;73-L`f<3I~6FG+Yfnd6c^u{Tg z4ia}7{fWEw(&M=e+f|L7_URdI@dRr-$EN})r2`e+IGadp2bf1xAI zF^F8aNck?hN722B?o-6zC3KHEtA1P!?Tr4cdhFHRup~2eExKPdSHgF{IPJJ<$OqRU zF_r~tTov1+znbf0HybI>#f@={0B&Y3

KOox02)9lsHL;9Ifw5)Ubusy3GXBh#Hp_FzoN)&*4_(_eHz9b2~X%XM4#kqhc=%PpW zdZQ(nI_iHZgwllt0<-0*;Jk}5xSxpjF247_Wm@0wyN_JV-!M=+Rk}6RSn#%UchfE6 zn4YK9Y0-DP)8I?lJ!L~}!JkyY`$tyIfk=S-$963`kYeK_; z?N4<7u6sEAsfAX*ng)$f#}8*;=T9{7J0mp&`m|eAv_% zP90$2R}l&SMy32rCwMZ74^=mOGQ8u6H6}q*36o0D;LCbryxq{5kTs+%aRF+L$wB{G z@?q13`o8ez{lHArf3GTtm!sO!D)2|KHf%F5NmPgRapcEk6JoYtg-lZ)HYJEz%lpFn z`X4r#MT^*vXQSK^6Xo2l*CSF&k1HPWphz>379lM{Dj}7S)*`J%x*O?kq#q;wI39W_ z4>&yVeJQvm;Y!D4%tX8*ye?bR%GHa3ZIvtO$OrpOVf)(>)qh+~$hRtF1#llTkbrfO zkUx*JLwC5Hn6^!6*&=nY=ZXz zr|L-~W~XPsa#+n_j;v%KX2pbFhELD^*3KD(DXB(HNJ1@MrG3XILtZgVx5RZ8Ekm?C%9!^HNN^V3m0THm+QgOF7J0Q4ioXhw``QJw$mK zdJA$dqDAxRELefLxX-8eE_gB1Jao<0@R8!{OQgTe4H1`;G%khX$O`mI&OnD^p0CpeHKa)!}CAd_>FI=iI zeT!2Gjzuh8#FSB-PD|xPOBIj|l$wE3IVhE|5v4}BQ;N1Pq_{WVGp{1G#H_5HX6{XWvkg0dKshZ}&&=pZMqdG}u}H`M&DLy&Clyxe2~G z^Hoh~47X5vW27+F3jT~4M7X{}ZMyNyO@k9T6#j1n)Ta@Te~ z(ifi4&%uY+#FA?}9@-f)knIZ%xrV%!h0nmpiFz>W5HDguEJq~#rwH{or=D;`+13!b zVsPIE-$ac#!v`-t<&AD;>!9Bu)#dW=e;}C&h{E8NmEZt@<<#>?8BjTi*gDUIWs}$* z_4#CBV~%e?q*6mwVa#T%5d|p$Z4XzS>6#99820Mndn&BHky4V+Bvm^#CtmOzgLgaU zI|Ex^s#piV+j##!KaMI7Qy>y z&}+O|b(mZ0EuIfl$24lGt7^O(`m@&nYs&c*Ab&R?{8wX{^r0^KWOSdc28~&6Pp%(n{5kKIPdz|hVOC#D2Vpw0%=d(z=q&J*dC(cP*Jn_DB(rJ%XnwyrG znQ+QluyCYj)=bY>&)_)h;(SjeJULP>3DKx^3JY3u25dQM<5*}D>|ze?XGGowv_F&6 z#s5MyE9F98iBR1vd>_qy;XX)GG(L!~oNYaVFU5|PQ`9FPz`B7}(-RNNtN`DfszSG9 z^z_K^VvR4poGmwJ&CqBdxG%ie5b0ALljo3#Cq5UCoF!c z9-~-Q7$xsRmZMr3wCl0Y&v389f#DQ5|=n3aZZlJzE zRfW`dsH!jkzoFcQcu!tKiHBIJr%+X4vL`Ih$ECx*4lm`+M7fTCQjoCD=C^c*zer z{X12J#({-oA%aK$WPq0zJ86r5Q7_1R!L6x@K61-fXDTbk6?7~-;5-@VsF&dr`Vi>p0IQmos9UIke%EQkB8}bM7cxdL#xR#)mw&G zZR9^j{%>V5zmi+veawqF_WtnC;o*kecH&7jW!A3{Uy%$VRhHz?O@+; zLEq^-bBGz!i05Hz7zPcK0AGGi*)+h7)}4-o(+4<-w_OW7YwHXwaM~Ie>sKD8&H4|Efy#NaDxDLB_p+f)LotaoGWR+RgEz!Y5xY}spdZevrjSQA~H+(b|WH;rfi zeGw6Ui_3jC5uBrG%ropCcM@AS;($5P;NR?vD-htd$=El+SCd#wVrrGX&7d+f5T)!U zu!1^LnS9VJ5znR)TW=707co_2@{(4Ote&7DmHGm(1-$4Uy~!yAl2v95Vx!YYRya5; zwJ*F5F;vtZH56Qs-GAaTx%*krun!j2K)UeGp*!+OS2b3xz46aZHu(NRvlx<@Tp@2v z_9fhbkDGN_UJu^la0OPZAN$&$VHt`6o1jJUhvOV6q~ z-UAp7W3_pxb`3kw)I-i|CtTAzk8dpu$?s5li?e2;7Z@X&>inQzmU#GRQNN#m#8&(k z5vF9^7piB|7HEX227)Nx)*K9tO+m{zOEggs6^{nOM-}E%>CKSZdk|6qYYgc)mq0z#4DHzkxq4AGk)=Hk>#ZdOR^3 z?X66_8UITX&B2T=@8Z|$3%X2;56cHb*@=n4HqAW1l5MuOpI-$HzwJ*=w7+A-`tssw)FjNK9$udMWF;Zd6`(DpH1Sr>ddQh@+N}Fi2dC4Yd()YAJCC_~s06l-#E0AoO`F86;vYK%y}m=x z8nJ3B8txEDjzf88-A@ZniEQ1mi>1nV{So;7+4N0vg3mPNlX{y(Tlq>pTK|P=+QDq` zxS8$g&a_~%DEyhvi!Wfdjr0LiM%uG-0;wTB@s`%j({oN!tf6CPTba7;(=$)Up_feE zt`0%4Lfgk}`AbuFd;1EFmC2*MCcy1?kd6olIIAML;E?k`QzVt#oTBIx(}Pn*=%Tk3 zjeftLdHy3v1zaeS--Tg`g0oMfUypqpBHSAtdX~pIP_!(+ZM*ZfCSgyhVi!&rx2PKL ziu4WBIjqL?(-)wxX$VE`E`?4vqR^I zZ=D$J;VJF-=D&tW+<0?rzU}6NA${W5GqF1!y^agnnVmATo#Ix>^x7tUD4(TTrx%*+ z<|PHtmvz@R4O5fTw-$aRx51jj-1?zRTW5iZ@5?sxahaL;Mkp@sllq|iDOL+33<;U^ zSW8o%Lu9W-m)H!f&anl?UmH!)nuiDxOk#!vbhlKnv!PtSrT#*PeDbi*dKG zV;VCC{T_)mtPN1Oalx{7hQq00Ug6C+s|B=qayOrLp`MmtULeXl-YkWQE0{h8)X#hM zfVbePfd$>`)VH(HlX1Z;B}tDOdr~pqi2m?`cVL$k3{ywu*rDd@*bM7OqmD(=z%M)9TXZ4bSCa*?L4*{Zpx z$Jf0vwLc;o&hZ#1})9j!cPYKd&kzopMGT*6QePZ z<1ec&_y<0gE>amZy~&Ebh_2z`Vrlz@Rd&SvSVR{%LjQn1`+zt#T1tP$p8MK#4Y=tQ z9Rha+GAFbUHI$|?f!lIJ^3QX>g&j0pYWQYTi4JElq$s2yy~+2DBc93d<3IF`p(iP5 zZvS$*#%?#${kk{Y)PXDF+%GVA&`!HJbFPq`SYE2x=m|pq%j>_l1HaubaM)w5XM!(QBBYL54 z-#3NvuXiM9n5n{){h)*$!Cc?i_ODhjd8q|2LPvd*Xm@iRA97p1CQsPE(Z#>=bvA1F zC|Wg^g|iGce=%EJ zAd;=cfMe1C3E30o(MTc?xqMUrA@89n?-#Xau`wIqpeI>X{ zjzoXot|esBxNDUOF5+#w?XjcNTyu92VH4=K0V(l*_}ZH1ID#|9;Qyy1l=mJI(h+du zkn2b$e+GO6;kN_yIpG-irErEiUGb1J4bP%T>(^e8v= z^P?}FzA78Adi~vY(7?OUuOX*tNvebRYd6VhIsM`dsqZPILHYB`+OqMfrtwU0%!Kro zMkjW*rk{w};-5M+`rh!ap3aK;3ZX;8+X}WgI~VfD8$x3eQ%;qNx;A2_bvB@duG61k z#orFulJ?>RWXlg)`GVK5CmfY2K6eMqVu`4r9vyJ>G1TcU=v??sRNAsP48YIz#(3<8 zMps2AG*3V4HZ56HvB`F^d?z|@SqyY2k%RPHF}*u&RkG5LlQ!vBLgP_g4`lG05m|n{ zm2}yjwO=}gI}28fiyP{y4Ww#8@?63VvyC6;g4Ppo+lLSkv~Sc!zeF6 zhapFHgiPFA=iF&R;|h#}d7F$~Y>I?i@vW|vnXFZrcz*n~AX7(7Ru43=kefL4h_%rk z#vP(Q7qo4IKM7v^Ox~!#A}JC6up>_;y6fP%Ps$NTmABoQbevNv2RP-n0irbaaLP2~ z_3#Vil*jQ;rA|&UHz8t-utFrl2yf!-O#{wei=S~fVy!Gb`p?{6K>zc2L`iDVhrEVe zlC^;~amv+!T~?OpVTb+>p@t5{b$v#oryl20#kKXRW<|4xBgWX$~NF!76x@69JtwsGiPAGB1+CW0@%!qaGNL} zcomvcPT4WQ)tRRnY?oS?orn*D4~Kp(V#;tmjqA5R|Hn0QUM&BgNFSb0gk8AK*u*x{)S!fy@%I(!~Ae3{_qvr?cxbLeTj+1#D%7SX!NVH_w=p0E;;67^9_B&sg}=jV*Z7IUJk_^@5o#Q>Y>? zbr|$`1DM&ZB=hjqoNA=onC%SkpUM4PZdHka_UR5pt8>bo{ouE3o16}%!^=-u3V(^q4a4s9lb#lc3?lDYiS>@1Gp%SI5rFy*2v&mfhK4-zaDaZQ;@q-;01zl{c^rdL&tCLZ{Cq(4if zc={eK4V&=Fis|#;_SkGw8ZrG_;{T6q?Gl=%FphbJN?eCJ#Pa$lPPw_aPz7D?yq8N@BV(pw<3K= zkYcZ9H*1c)f24o0DW39YrRZZLT`_4~noS>^h{!;!2zYC7%GkcAoaY0_!5t3E`$x_5 z`VkMk<*#2wzy2+O_j&AmWr8%{pXg4UK{5qI7^0T$A;l&Sr`*zwGkbhSx+~|#&vcoM zQnrUx{*IkS)0*iqNmNrg5PtGteq$|S1kPZ-Ujq)bAYB!usr(ERY#TcU@O_>&N;B#6 zWPc*)H?bw29_prb;`EK^u7^@~Y9G+MO&+5(5T4h=Sip-gCY1QQ%4B{6$xsImM_LDM zgSN-IDW5>=U>oRj*nh9{%LQ?Z)8J1IAVPQf7U(WZuQN!;1$)ChTH?hqIGJD|e5g03 zNjJ7}O1ST;9KVXOX@WCWF*%w5nNmd%u+3IHkgf5gmsmgPzW0 zcU2-n^7+;!TMMCg&kP6X{emU*^fH6PSV%eN>CfWytnP94T5)_m zjiSeyvy=BYTbjt&UC*Y$2Q@BG{_ObrolB?<0lwef``5?#W$dapJ5JC}dInZ6oP|PT z?&(=5v9Cr4&#hvWi2hsyW8R527glXv0_{a86)|D=_3lKxV6nJ)i3z?=RMYkhzCFLH zyn)_ z(bzP^66G~)hvgwSJDTc_jZJazr6c2;f|S}3n>&EB)5?Y`4oxvVZRi;<_16H*sFjVv zY-c6)SErEJT(PO}%jYb=(W3{?J8ur|jQ0OupRxf8uMP6KRXd@*Pe&{eyN^{4M4zdU`^ulk{|LTK4*Q3~mh8}xP?_fE zD(`+4XGiRWzk`v>LoCfMj~kX^Qw97vdMHM+XG~ZZD|_}jF9m5NF_ji-q@m()k>bl} z!GXSv>Cuh8tCfr|g$=^N66LfJ0-0MIyyNR{3cU#h12*T0!kc+$a}459Iu+ExzfBQ4@Dn9dIg@~kn5(C46(gXDYQdQaL$Wl%RGsD|)p(rjzbwaP0ne2T z_|px6CIB*1f1Fv7f#W8hxTi8xT&FmMx+NcXdW}^32U$HhNwl{8bA55BFYcrh8p25U zT}WhxR;o|9gqpjeH8a6D^S827jMcvfC9a?LO=ph(u|v-+7h zqa$3^{VI3|YVW78A|aDN&t;?#9owxLJ*pL>fBJ`bd!E8bGiQJKvr&q%J{BD}x|EFn zAC+v%IZeNJcT>%S@^ozNtcP8Kwn=ARRd*rgj+pB}&7id#f3|hev%7BePY$*~zsbab z?&&+Dyc(+dNr*;1i#t2$-9Y&C zjWVNA2Hrg8e2l{1MDXt*oW%AMjCUHN{IABo1-ywW?f*B zB~UH}MANjL7K#gktL3()l**JM4F#pH?nf$RErQAdyUR@lii+a8O`$Fouqf&(etagN zyFkIMq8p*emvWnFQ=0GhPKxk-%l@B#o@a7q=A1Kg=3L+NUViVuCNI_!lKIgdFSDz* zauRgYXx^!H(JG<|961=qI?^IOrK`9*K8_W4$NvfOuo3Sbg_wIwuQk4Rga(9>sx|A&EnD); zZ3AM?d5O+@&Ha1}+O_Im+tu>Yg%`v9B=Xz^%&GC1Q@6(*R}t9KAtNhmvAooR6qwy@ ztiD%VfJVnXp0j&>ad&(2=Ze?w#;SPMzk9tC_b@Xc=ePeR-<^J-KU=$g0rJuNxk*R- z3)b)aId`wP>y-ZTCaM0dSYB+9iPk%*Pbq3FNn3(Avrq~wk!Li z-*MHvjQ01>b-5a$2{&9rXlbn7Ukl2p{p{<`-=ZgmGHRXA`Kj)Rt4MaYv5f46Uv)%z z--EUAcRn`!0yv2mfbC{2YL$wM$}lILg~T-7X)P^-wS22{fj`VMxObVW&_|EC);7kX zFIa9BS6V%^=9jp*z}unP%JiRm4j0(nUGlJOmnFu*GWbG>`gH>10^a~ZgnBp^IHR5o z!`WAmO%DPmiFzFuIMGCXN~g~2v5+Klf#Q%A6qg=IlvWpAFN3`@Y9f(FS)z+*fOKgt zQ0zbr7g0_9bPL~b58EzE-;6cN`gDAwN-yC&tW8O|XwM&#Uk{z|XP5S-y{$Jg=XVZp z6u`4ujk?yJkE-O;d2bvJZd98;688-_V4a(+6-SB-*1ubSdwwwc?(`8Ys1ioB3g(q7 zz^@GJ_|5u2Ee-CmlpnpdX!Tu-g6++C- z`s776v<^L63%c$g_NLu59eo>Wf~Ab~4tZ%@{kskUG$B?{aW3w1mKPPC^=RWc`o@<- z?>gM#58ewea^qi7cyoOHg0iN1-^1zk+}I0iXw-zC`IVpgC#(#;oP{VQbskJPQz)OTcp;8v2)ANZt1^TC@*nJ7kl*SC`6Agn#5w)oR z%@Mb>0Z;Aiq4{j<2jBuFx}}i3`2z)W+`W6mr%TsEZ{o(S{Qa=fw(YWLhH{gtah4@T zNpUX0^4hXFE@s*0I_rvCHfPz{Ww4=l&8Zk%5ot9R*~Bm0oHc4$h7^w)MvU#Jdg4lu zEj(HMDWLnK*j0=wLVGIoJmfJ#-}aWQ=_I{f>WQSg6`M!ngvzZIH^Cy^tvqZF$rHO; zmh#eE`mXYD=tM~4W3CjNN3*U;Ttnz4_n2Lnx^fQy-Yc;GLd<%zzN~s*HR;h>rMb|H z!@dC{E&OI$iK?X@kfl!XQv1Ch8V#$>7lYp}O$8=qimF}AtQQ{NRi5aPP?uZryxK)W zu=dytB**-l@}oJ?0`p~D!k8abr4;e_1ROLnw)(yc6srTV>Od;e&N+A7>m-+;DNrqdU=#l(^V$)32wN#(rI zwQ~_mdD?q_0cSY|ZR>Ptvr=asB=OcPpexknwLEUaE~1w<6*NRhel}FpQtjj{^!Cll z0i@G(rxWm$OWtZ|iiP72s&RFBPTcZCu(eku= zKn-y=eEphEe6jFY$GU6WX!K5jb{EERzsPL{uI9Yb1oZb5ac3TI;b?Ao8NE)s(f^Rd zj1wxryIanTWL4i+G(sDtRAg2bxPTs*hVyvXD{jdnjy=(!*RsOBFj`y()B^@ST=!U( z|J=n!+yq8$7W7`UZ9Djur9DLsyqQzr`DHYJw^WnJZQqPKqHp>f_fnfJ<)cKpH(I0` zX~Mn1&Bf*6e)A6{$GyWzJ?BHk*&oz(E)jKlx~f2h0PEhx*RH zs66h)UHh@N_0`q+>D#FH#)v&Dz0mvb8E~2&t~sq#&e%}%x0+{svnutIi1PSPmGa<^ zXH6)cdI}ukBrl(y?&ZKaY;q5iSYUQ1gY~}^`cT^V4Gwq-h;R34YI+HA?m7o3JU&(2 z+hh~dm;LCjgq9$cl%+(RW!ADPeT%Ve2bY3pECbKq4=trYRjRb1k_D~>p&yn?Jg0B# zsee5by5e`^`nPV#xs`{7I2R{;>E}{^$(^<>4h^&kwm2$$Y?uqX1?ckK zLcE#IY*_K$^H-%VRCBO&=l{y_2y#4(zL4NL=|2punTLEcD}{-C+JlgmV$Y!&K6zJa zUU!acgufCDLOQsP2bBfFHun}~p|EY)^Zvxtu|C6y3`!?#aE$b2Lr?NEq}^7t7wNV? zU85|tomM2w+6TQ%*pktF+@pi#)$rN?&W1`mc@6!3rjX;;`mrG}Ujf-vkLA#)2jBr9ae1 z)UR{I`YhlgYyuB~RyqzG1a1Q9?)6jGx+NWdam1gVfwB0Hj__`;HuRbQFOKie{PRV# zqB+WqE-D*W&nfz&1F)a`cFlH`v#?Hm#|eM>+Ce!Zs!%qRv(9T3HB&!J%m*El-1(M& zEc!9wyUYW=OGE~2@&lLMz0MI;Oy_&u9#G5BDRhU?Uq-0dFLQ`m*8QfX{Q3+hm&2#Z zr}@+ystuq<*K5G;$60bYJXnBB@BRvQlf9h>lm)g;?#*bwP0IrCrNjbfaaD+zix^c{ zub>@Im`Y`bDegbr7o^^x3TJ^TJmuW16x;ay|B5ZM0N65`R8u#$Oty+8vssz*zYu0N zIq`n;E&2DI4M3NoDYqot%}B zQ|8)$u9w_G6uBEaivQy+A`v@^2<$w!I*iy+Bw|MqQ9n2C!+O&AF7qe8^aEmvE*f4x zCI0HadB!g6Cc@94-NY{J3eNd=tuMhn+D!}(?**v zt0iM8YWyCwxTihR*1K(KjLEyMk*~toq= z-lOzuke($TP%KAZs^7gnrel3)ulmio8ZXkQ8b%N5dd zARze8xA{?_NBve&c+{!@G%!cM|UG9~`@@HIR@qzP6eh^{F2#I}nZ< zF^ByUo*nEWH<8wL&azL@JHJrOu62r*yh#YXz%^tkuXs=Rvph%39P3wi?^ii5EZCZe zKJFoQ*YQ0lL82dS*58s#=sh+rgD6{~0~=8H)<&Wh$adh-CKpgFG3QQyxh}~FI$=3&)LN$pc-Z`N5!qJ@g%gU|t>1LwqVcU07b9Qjl=r~TKqe@uD4pIj26R?f zA0+$gv~=3bK_lyfX)`XxJm8mN_95_s#;`eK*h(zozd ztx({Kuo_+KT%WmIM|2OK_v;>P#&L$mJG+6h&PF-E#4!!WLpWHzEAL5!T#C7&msgWA z*ee|}SanHJu4%5BrTNPi1XgKYaPgMoF1saiS)}EKWu}(f@h3}bmc8K8bjLBv)MbQ! zbvt%r>1VERZpulqoaG-_qc5vGWZK6NXXbe~T6aj&Q1lF=5u>s#b1_gldJgEO1$t6)m$?R4WlJWzah4dldS&rVAC!x^u-ppR88m zaBVQPJXNg>a$A=dE#JNTyXA>1idXo7G_mW+p-(M%YUs+MmF37O9F57TRuY+iWqE0} zVs%dsL>dW%Fvc-CCOE-q&Sx%SDGyvxzbUVVMvkctX762 z^#~jzaoCPlE2G#IJTC(&Mk96%a*jpJICHf!9bY|)oNt|j)_9GG6^Z}LyF1f z`>}oostBdb2tFJacoXv&X@Upk%J*5o(Wo@6hHnC*tF0%$V4-x&>9x#`J@aer22t|7 zUg?H)pW#-XlFVF<+~nHIR~)*tndoUhbpo%DMYcSOm`?eh@QA>TZE*NcEpoo$Vgl_i z`@`!8@d!KRbLuX4O(i$kaMCGt%3lJtoHz{~a@PBQX09Q{(&^c@9P|AwFsP$nn;Dv; zbf;53)cJ$Dha%rQHDZ5pAhfgKBj8pQ{@?7<4!XaYv@IVJx5BpiU{fTdY@b8Ay-tic z$BoKI+y4&sCj8laq<*6-rR6O2Pa}c<2;Ie_JnUr)o8~#@q1LBgS_cdrZLdv|z@2LR zNMUYm>lc(tm{rKgb*YT?G1M;nKo_eHgFkyIL% zG%NS&xu`Y`N9A}B z`_xJqjbzK9f%cb?=^*2gtm2hS|f{3uyx8W2V)z}M@FVygPjCxi{-idU|BP= zS0ik)Tvz=g(>lDct4O~NsVSdN*p#>JCrN(X0zNvk((bRH*xRY8tf^yZx!xC*1RHSw zd2a8Ci+sijcw1wEX^`TMJNPPniU}t!&AeNAq3sci^L+kqjTW&SG;LZDFx9o6T+}=ji$14q>(6DAt;(0E*^!4aPGfwL zo{?$&eIfb75MO!7b73vn)AQ+lytUF0tb|c57Q}13n{#!izB|K`=$RJrl)qRSDkg|P z77^;H`S3_@GRe+?@vKJnMD3AZevJJ$LP^EbK4R{ehcv($k!P4^(#O3^^4sydYXBUt?iAkj`8xP z&v=f*K6;x};pquaAU(Xz;N$iIZ?QaJVy--Dhw^|9XW?twck0BNx{a_lV5xJxy#o3e z&}@2cFZa7XP3G;8o;BW{~%M`hcWX`hklEM-lw!jwgkJ|%Iup|njnp7)Nk3@ zNIV_2S3aM530grSJ!NO!Ir!g+K&p6rO{)-B2z3H^%$EvUPJJh~b{B0fS z0K(682sH=2M$KOK2k17Vd?siBYPYaWy6xvLYM^Cwt5Z8jb5^q#evH7$C7*B62l_YY z-i8b{o?77a&ow>f=b9#Z7dmpV^)nyfrD~}eR(tb2*3tscc4?9a7DolKYR%Fa)v*y$ zi#u(ah56!6n|^bTHVt*$nZL{jw9+1f@$1#nTu(gS(ER^=1GjOuhkpxJ>Hq5;=hb&m zf9p_Z%1b|g)A76BSB;Yg?jWS;76N8aD7rA@g}A{eCAkp zt3~G1L6@0s3^^;z@y(}HG>>8~gs@g=W?-RpTBtyln7v|5aKyKDpS&DQVY&4OrX-nD7-ks!)<_nO6Oi_3r6r>1o9FxdUzI*t=ls)>v_bs}+n9gg+l;zD(--RA@#g~Vj%XX&DGr2hM9jxQ?Vw)G z{M+1Cn*By&CW&ZCRT zWes$agUtupP2_`bZL>b=>yZ2kQqauB0yzh{N%64js;vr~r#q019SY05YwElnmc&`P z4cuP+<}huc$-!(7q_4!zoeo1Y=1oU2?8axolJsM&d_+eC<=&V*jaZ$u+cr6&i;gww zbV%~lwVUi?JRhxf1k9R`stA7%dcgr5dR7N&m18neYSFXo=rf8935Qs`qSC%tw1ZyFOPr z^N@t~@+f@hBu;4LgjvphE7P5WSH_9OuvCb0&T zVfDmHbsg@2JpyPAkPOoIa*#0IPD6T$Q8J2hY^R z3@s^QcA3?=x~v!o&rGOwW@29CFfY>cG8_5pUqcdO-W@J!^f?p02J7*X z)s7zEjwi8pc?2{&!69AP4xE5+P9}WkPl9DVmGV4z4PMj(^;)7~$S+e+zV9v^fiOCT zB5&zL$?m~%O0S92MtqeHUr*6rhoyRoybxy}HE$2DRmS*r>6@wTrD9r_(STCJkh_b7TGd6~Q24_(k| zyLYR#%T}P9|5U%zqgiEw=atdsp!}oi;g9xmqcJA`(*Er_$Q^+EOq@AIF8hGw$m-LN zg#Fq4`}fjr$OKw%3;buWlnGvzHr~rp$Nn45sB9?;<78;(;3?dgrOs&S6Rh8Zu=@jOD%_AdPC=?dWrEfRx&}C$9ID%djws5(EAkNs|*ki=Ik8gY!Gg zE!2m4wQQAMb?k2CG<#Y#+Fh+$ZmaVrpKc1#6}FCdoM9a-)dNaZ*q2Wa==f&C7Z6M4 zCT??DrQV<(Vb^e;cBMh!kYZjzY?bOMC|k;P9)(oA)lYe$<(eI%e(xl&LmCVVkMXy? zLH{32;D0C!(I0Ns!&1;4ls9&f9}VgsnTokBDn={neZ&_fYMW@@&x$Gy_RQyPQyk~oontP@HH|z8wZ@j9@LIbx)SpH-%;z*pwH-{$g`u zg-WC69S8Ge9^uAx$X_To{kL_x!818Xm`S7y;0Co7EhaXShXG}JP+oqxS7T7_(;3^M zY3t7eM$+~SydvbL2) z@nS~fT#@u7UvmUcHpd?leV`mRJ58SB(s#}+j@Mgxo&8LtbTY0WC(@bjG)aVFu*8XX zF~VGl%|4DZD$&}YuFa^O3+_c^tAU-xzU_C;p0EQl3CEpj(rb>moHS?pnPA*ovHNDE zNmHG9k|DquCS_+kDfi+a{XZ5w;MnMWtIenxh2A8~qeAPTKgOcofn{ezE3YYo1}^vv z&N6zxjmLR!dcW;Mf3~Xm8kg;Gp7qm{CI%DLdip3xT;T_bKgz$Ly z#(WithgZ3=@VBnw`f$+c|Mn0e|HL|J9AcHI*wDRZL^(ADh1ulm%as>iC$fPkrIU

o>b)4C$mbc8NOaSN0Hc5W}# z^})QM7qxqN;LH>%7W7!{-k?0T%K)2uGidRk+>Eu-i6?T~jK~LH!rM|sBhpyA#yF#~ zt0&uu4wZ&JDwUPS$8Ca#p5DOHNf0B?rIzna4nBI39~I@IwUB>_8Be_!##1>aaT&)@>pZYn zcr_hev8F7fA-okJKy3)h5-h8Nz-82mX83Rg-GSXMS$Tvmb5mhI2As4XF~8C}SA%s9 zsTz-a`bdX8$ias0vnk76AF!sitC{hc#)?Lk z`T=-J=oMM|7Go1jNkF@^EUIA^__py+ZAQ}*7q$PzQX`sKdR#LLlq=^IZ434dR>DWJ zJIIcorN<*?!Cf)o`!gNIt=YUQFC4PKL;sTEPF(g~akul$YMhGlg-eAJjS83Q4|hrx zNVzK)jfjsnzV=g8S=QzLuvCtkJM@aa?C_OO%MM-<3-(;eeWvruU!UoB#T?iF%7#d4 zyFXuJZl?YMle@z#;-48`(TXpjKRxJs!LeB^JyYND+|`*Lu0wO}qc(vPfIwyH&~#?O zyW3{_^h*i(iKY*N%FV$Y>t$oG-v!*y)&~u3TOE;S5?Xm)-xjjD1B0u!I_y^ysxPe{gU4PeLNd7(DRjD zyxF8aEpP9v!r-!uR)gtiH)cO!cjEXihx`8@_gTbzBk3!3nR5 zYH?17E$U9Bb#Uy_G!=;DS5CTs$)8G^lg+Ancg(Lp`{@6h-b zjwAFB&B+=(^$T~l7=iCV%77tg?wNCm%ZU!Mt~YWirTQJCG$-G{I(5ix6!)}7YweeK z`wsu7ZGvIH-O^#`oCa@nLWkix*P*|bklju-Ifdce6Kn^nB9$Z}7^{}R7>q^KKl<7w2q`UyHVv zZyJLAGSE}9^ug$B>$+$#srCXEgU0<$N9|USeXVe*`#6I-rJK_$T<~|18LT z>ttiA#<;N66wPJU`bA(03gTw0WgS~43tB}mOjG3f zA&m86ivS$1F^c>MbRp^NW99T1?Ah#^Lg>5-@VeNp2ioYbUCmNqHVgcwodx~`Pt0WX zwFy{`WcNtx)jr(c1dDA3+XkYBDSi=f5@Uck&5|5I1%&)6iOy#lf?qSpx6SI6C9rp< z<+>Ty;6cG7$FT6->u&W9N{n(a7UGNzD65kNOdZn$oHP7<{rmLh4Z(2C<@P%<{{(Mp z=lh_?tiW>ZfmI6{xbP!oqZKiGP>*!lxf)K>(*)>45q-u2@$ciS+whJXNEfI&wA!Ha z-MMar?^u)u2j=hYeL6F4tD zdd`vH;?8VInD4U31+bg`+CMw_qQ5ZN?|&ru2h94w-CFVk_A&qM$NG&j9Y&j=r_EqD zUN(Cb@?nHBrvT+EQIoI8(@2xJ$r7byTnbidAKi1@bC@xhn2;OP8rAfN^_0d2 zY_xDb!USA>%~)g3>jJISVxgLK0mZgvGS;&Pv#+~=DO@u-!IEB(P%*D!TgABw1#56B zo{`kW39|f0@QFq|A61#fO1=pyTeLkxkeH4K_>=v;$VcgItIg*G}@OO z2x$glM-%}L!Jy&#)6^r`klgHd9GOkSVD+oWBnNah{tD8EGKJo

C$)F?AS5OdOs% zU(qB+=Xb&mGR*mXOX-v!RTRJd=D{)CkWc)~&I+k7Q@t4K40Ss>jd2f-`M=S9LUy7b z?_#Ro(!y-&RYG8uy|9St@hjxf4AK?o@=Z;~L}(2Jw3-1jUwJF3eG>N@WbHYuzKEw}? zz`lbqQ&SS`r&!`FXblX=))fpWxhdB|%Q91?kb?fpC_jB82YC&Ar=u^xXBK#JvV6S* z`g84|hC!8rYBzL#br%1+ulcESoSL?p(b>`*rz_9OEy^{*m&aBhkVc&5Zm^y}B0Zza zuabV}sDbTlcxnO@gI4T-EcEI=)X-NzSJ#!AIDmA z%;xU;qEyC)9w!OC0eY2ff`+T)Y@yUxpj|Uioa5k zM;D_UU2-|3d{fj?)PxM!Q{<^A;8R0d>qL?2;fYYIH2u13fhqAqT#6X4;jgz8hvWs& zKB0Kx>&#QJ3`@Zww;h7a8MQ}u9|~Dv`9%5XYO0#%A@w__b(szkCq6Coqep*(jivac z&_U0;4#>8L^#MJnWi(nzzORK$E&QJ$X`!WzFB?O?RC+LyS85@zr2Qw!8T#TW9Lhkt zG9S5DOZ&0rf8c(50Dr^XdmGLVy1V3dNTy1fi5u7}o-3Y(=<_p1cX|Iu?UR_`yezS zG$S-4G$HJR(1(kNO6De0tK1DEPd3g zz33lw4_*j-0$Oe_G^36{_uL4}JdzV=byqOc-ATWpYqazzO1jOF;j6$|0?tPG>Two} zvsB+zob|z3l5ceBqbTt<$6#Lt&P+Iq^VQ?bh_k-Ft2oo*%na=nVSKohkv?JEuX6wD zyZe2Gs-C!3M>>;Q-JjG}dY9(jL|uVTF%8#TZ&}bAp59E8>HE8|4rp12F2xeUJOrz^ z2HiCJJ{fjNDU2{xH3h}6O215c2fzYhK>8Id#s=cn7k3JHW8hQ)9z)Cx4@)~7 z$=I9bvaKb5Ob9~@IN8>j>i zNjhn(BhKqvybg6nCBWNRWFpFu#07bNvx9Hb@W2I`q4ye6v}nPQ{QHpL`6*k~s~7>V#Psij)#e<0EiQ`J%e7P2FLS?r?cYMREDki4%p~nKP%)oG4^1sVFI0__TnVOVx5jvhF0p z%#uZ=%U6_8iG}Q(?2%d74+@AYEm|ThtSEh|q~cD}?uUs*rAtZ{3MQRY<;x#`a>Ch{W`=IwXofNF9RufD;$g z_D6exCP3&;W{ldNUJy+o9tbWbW8dPvW`rMcOhaKc&v**)AT2^+mx1Z%`nZwlDKtkj z145NAMAw^pvIvCE3()98=&l~bq7d#(9>PorkA6Cw^+4FNXe^6H$RZ}Q7=)!uA7@L8 zma>$kXzWy~iH-A_iqADFJB-)W>0oTbppOrKiqQVU4%c2m)hTdMY)DY!SL$>%wJEdba`@^AqRXAi*y2C#9ZCmNr_`oxL-p%{= z*VPXU&rWFlZd}|06W80md+F)i`wy=@{m1%~JMPPPbMM#In^PuCe)D?Ht(ezGzw*cR zUmD+^o19(#&cYP^XH@<8CE3-0%?^h($JDU;?|;k+e`i*9|E0QyhJ%~L8|@EV!w+VRejxk{U*wOUJLcK3^D8N-O3!QXkIr2;WMV{J%9YS6PL28h`5VUV zh`K#*6l!uHr!fPU7&-4bmtgR!<)v{oo*o}P)WGtYJ@eTJHUS6y2%fG6^!T%Q29<-Z z)rh8t0xqWL8O$Ob+mhQs)5dQ$~%|P5Y-RBggMhk2vPJhm?-b + +#ifndef __ASSEMBLY__ +# include +#endif + +#include "stm32_rcc.h" +#include "stm32_sdmmc.h" + +/************************************************************************************ + * Pre-processor Definitions + ************************************************************************************/ + +/* Clocking *************************************************************************/ +/* The holybro KakuteH7 board provides the following clock sources: + * + * X1: 8 MHz crystal for HSE + * + * So we have these clock source available within the STM32 + * + * HSI: 16 MHz RC factory-trimmed + * HSE: 8 MHz crystal for HSE + */ + +#define STM32_BOARD_XTAL 8000000ul + +#define STM32_HSI_FREQUENCY 16000000ul +#define STM32_LSI_FREQUENCY 32000 +#define STM32_HSE_FREQUENCY STM32_BOARD_XTAL +#define STM32_LSE_FREQUENCY 32768 + +/* Main PLL Configuration. + * + * PLL source is HSE = 8,000,000 + * + * PLL_VCOx = (STM32_HSE_FREQUENCY / PLLM) * PLLN + * Subject to: + * + * 1 <= PLLM <= 63 + * 4 <= PLLN <= 512 + * 150 MHz <= PLL_VCOL <= 420MHz + * 192 MHz <= PLL_VCOH <= 836MHz + * + * SYSCLK = PLL_VCO / PLLP + * CPUCLK = SYSCLK / D1CPRE + * Subject to + * + * PLLP1 = {2, 4, 6, 8, ..., 128} + * PLLP2,3 = {2, 3, 4, ..., 128} + * CPUCLK <= 480 MHz + */ + +#define STM32_BOARD_USEHSE + +#define STM32_PLLCFG_PLLSRC RCC_PLLCKSELR_PLLSRC_HSE + +/* PLL1, wide 4 - 8 MHz input, enable DIVP, DIVQ, DIVR + * + * PLL1_VCO = (8,000,000 / 1) * 120 = 960 MHz + * + * PLL1P = PLL1_VCO/2 = 960 MHz / 2 = 480 MHz + * PLL1Q = PLL1_VCO/4 = 960 MHz / 4 = 240 MHz + * PLL1R = PLL1_VCO/8 = 960 MHz / 8 = 120 MHz + */ + +#define STM32_PLLCFG_PLL1CFG (RCC_PLLCFGR_PLL1VCOSEL_WIDE | \ + RCC_PLLCFGR_PLL1RGE_4_8_MHZ | \ + RCC_PLLCFGR_DIVP1EN | \ + RCC_PLLCFGR_DIVQ1EN | \ + RCC_PLLCFGR_DIVR1EN) +#define STM32_PLLCFG_PLL1M RCC_PLLCKSELR_DIVM1(1) +#define STM32_PLLCFG_PLL1N RCC_PLL1DIVR_N1(120) +#define STM32_PLLCFG_PLL1P RCC_PLL1DIVR_P1(2) +#define STM32_PLLCFG_PLL1Q RCC_PLL1DIVR_Q1(4) +#define STM32_PLLCFG_PLL1R RCC_PLL1DIVR_R1(8) + +#define STM32_VCO1_FREQUENCY ((STM32_HSE_FREQUENCY / 1) * 120) +#define STM32_PLL1P_FREQUENCY (STM32_VCO1_FREQUENCY / 2) +#define STM32_PLL1Q_FREQUENCY (STM32_VCO1_FREQUENCY / 4) +#define STM32_PLL1R_FREQUENCY (STM32_VCO1_FREQUENCY / 8) + +/* PLL2 */ + +#define STM32_PLLCFG_PLL2CFG (RCC_PLLCFGR_PLL2VCOSEL_WIDE | \ + RCC_PLLCFGR_PLL2RGE_4_8_MHZ | \ + RCC_PLLCFGR_DIVP2EN | \ + RCC_PLLCFGR_DIVQ2EN | \ + RCC_PLLCFGR_DIVR2EN) +#define STM32_PLLCFG_PLL2M RCC_PLLCKSELR_DIVM2(2) +#define STM32_PLLCFG_PLL2N RCC_PLL2DIVR_N2(48) +#define STM32_PLLCFG_PLL2P RCC_PLL2DIVR_P2(2) +#define STM32_PLLCFG_PLL2Q RCC_PLL2DIVR_Q2(2) +#define STM32_PLLCFG_PLL2R RCC_PLL2DIVR_R2(2) + +#define STM32_VCO2_FREQUENCY ((STM32_HSE_FREQUENCY / 2) * 48) +#define STM32_PLL2P_FREQUENCY (STM32_VCO2_FREQUENCY / 2) +#define STM32_PLL2Q_FREQUENCY (STM32_VCO2_FREQUENCY / 2) +#define STM32_PLL2R_FREQUENCY (STM32_VCO2_FREQUENCY / 2) + +/* PLL3 */ + +#define STM32_PLLCFG_PLL3CFG (RCC_PLLCFGR_PLL3VCOSEL_WIDE | \ + RCC_PLLCFGR_PLL3RGE_4_8_MHZ | \ + RCC_PLLCFGR_DIVQ3EN) +#define STM32_PLLCFG_PLL3M RCC_PLLCKSELR_DIVM3(2) +#define STM32_PLLCFG_PLL3N RCC_PLL3DIVR_N3(48) +#define STM32_PLLCFG_PLL3P RCC_PLL3DIVR_P3(2) +#define STM32_PLLCFG_PLL3Q RCC_PLL3DIVR_Q3(4) +#define STM32_PLLCFG_PLL3R RCC_PLL3DIVR_R3(2) + +#define STM32_VCO3_FREQUENCY ((STM32_HSE_FREQUENCY / 2) * 48) +#define STM32_PLL3P_FREQUENCY (STM32_VCO3_FREQUENCY / 2) +#define STM32_PLL3Q_FREQUENCY (STM32_VCO3_FREQUENCY / 4) +#define STM32_PLL3R_FREQUENCY (STM32_VCO3_FREQUENCY / 2) + +/* SYSCLK = PLL1P = 480MHz + * CPUCLK = SYSCLK / 1 = 480 MHz + */ + +#define STM32_RCC_D1CFGR_D1CPRE (RCC_D1CFGR_D1CPRE_SYSCLK) +#define STM32_SYSCLK_FREQUENCY (STM32_PLL1P_FREQUENCY) +#define STM32_CPUCLK_FREQUENCY (STM32_SYSCLK_FREQUENCY / 1) + +/* Configure Clock Assignments */ + +/* AHB clock (HCLK) is SYSCLK/2 (240 MHz max) + * HCLK1 = HCLK2 = HCLK3 = HCLK4 = 240 + */ + +#define STM32_RCC_D1CFGR_HPRE RCC_D1CFGR_HPRE_SYSCLKd2 /* HCLK = SYSCLK / 2 */ +#define STM32_ACLK_FREQUENCY (STM32_CPUCLK_FREQUENCY / 2) /* ACLK in D1, HCLK3 in D1 */ +#define STM32_HCLK_FREQUENCY (STM32_CPUCLK_FREQUENCY / 2) /* HCLK in D2, HCLK4 in D3 */ +#define STM32_BOARD_HCLK STM32_HCLK_FREQUENCY /* same as above, to satisfy compiler */ + +/* APB1 clock (PCLK1) is HCLK/2 (120 MHz) */ + +#define STM32_RCC_D2CFGR_D2PPRE1 RCC_D2CFGR_D2PPRE1_HCLKd2 /* PCLK1 = HCLK / 2 */ +#define STM32_PCLK1_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* APB2 clock (PCLK2) is HCLK/2 (120 MHz) */ + +#define STM32_RCC_D2CFGR_D2PPRE2 RCC_D2CFGR_D2PPRE2_HCLKd2 /* PCLK2 = HCLK / 2 */ +#define STM32_PCLK2_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* APB3 clock (PCLK3) is HCLK/2 (120 MHz) */ + +#define STM32_RCC_D1CFGR_D1PPRE RCC_D1CFGR_D1PPRE_HCLKd2 /* PCLK3 = HCLK / 2 */ +#define STM32_PCLK3_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* APB4 clock (PCLK4) is HCLK/4 (120 MHz) */ + +#define STM32_RCC_D3CFGR_D3PPRE RCC_D3CFGR_D3PPRE_HCLKd2 /* PCLK4 = HCLK / 2 */ +#define STM32_PCLK4_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* Timer clock frequencies */ + +/* Timers driven from APB1 will be twice PCLK1 */ + +#define STM32_APB1_TIM2_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM3_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM4_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM5_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM6_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM7_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM12_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM13_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM14_CLKIN (2*STM32_PCLK1_FREQUENCY) + +/* Timers driven from APB2 will be twice PCLK2 */ + +#define STM32_APB2_TIM1_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM8_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM15_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM16_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM17_CLKIN (2*STM32_PCLK2_FREQUENCY) + +/* Kernel Clock Configuration + * + * Note: look at Table 54 in ST Manual + */ + +/* I2C123 clock source */ + +#define STM32_RCC_D2CCIP2R_I2C123SRC RCC_D2CCIP2R_I2C123SEL_HSI + +/* I2C4 clock source */ + +#define STM32_RCC_D3CCIPR_I2C4SRC RCC_D3CCIPR_I2C4SEL_HSI + +/* SPI123 clock source */ + +#define STM32_RCC_D2CCIP1R_SPI123SRC RCC_D2CCIP1R_SPI123SEL_PLL2 + +/* SPI45 clock source */ + +#define STM32_RCC_D2CCIP1R_SPI45SRC RCC_D2CCIP1R_SPI45SEL_PLL2 + +/* SPI6 clock source */ + +#define STM32_RCC_D3CCIPR_SPI6SRC RCC_D3CCIPR_SPI6SEL_PLL2 + +/* USB 1 and 2 clock source */ + +#define STM32_RCC_D2CCIP2R_USBSRC RCC_D2CCIP2R_USBSEL_PLL3 + +/* ADC 1 2 3 clock source */ + +#define STM32_RCC_D3CCIPR_ADCSEL RCC_D3CCIPR_ADCSEL_PLL2 + +/* FDCAN 1 2 clock source */ + +#define STM32_RCC_D2CCIP1R_FDCANSEL RCC_D2CCIP1R_FDCANSEL_HSE /* FDCAN 1 2 clock source */ + +#define STM32_FDCANCLK STM32_HSE_FREQUENCY + +/* FLASH wait states + * + * ------------ ---------- ----------- + * Vcore MAX ACLK WAIT STATES + * ------------ ---------- ----------- + * 1.15-1.26 V 70 MHz 0 + * (VOS1 level) 140 MHz 1 + * 210 MHz 2 + * 1.05-1.15 V 55 MHz 0 + * (VOS2 level) 110 MHz 1 + * 165 MHz 2 + * 220 MHz 3 + * 0.95-1.05 V 45 MHz 0 + * (VOS3 level) 90 MHz 1 + * 135 MHz 2 + * 180 MHz 3 + * 225 MHz 4 + * ------------ ---------- ----------- + */ + +#define BOARD_FLASH_WAITSTATES 2 + +/* SDMMC definitions ********************************************************/ + +/* Init 400kHz, freq = PLL1Q/(2*div) div = PLL1Q/(2*freq) */ + +#define STM32_SDMMC_INIT_CLKDIV (300 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) + +/* 25 MHz Max for now, 25 mHZ = PLL1Q/(2*div), div = PLL1Q/(2*freq) + * div = 4.8 = 240 / 50, So round up to 5 for default speed 24 MB/s + */ + +#if defined(CONFIG_STM32H7_SDMMC_XDMA) || defined(CONFIG_STM32H7_SDMMC_IDMA) +# define STM32_SDMMC_MMCXFR_CLKDIV (5 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#else +# define STM32_SDMMC_MMCXFR_CLKDIV (100 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#endif +#if defined(CONFIG_STM32H7_SDMMC_XDMA) || defined(CONFIG_STM32H7_SDMMC_IDMA) +# define STM32_SDMMC_SDXFR_CLKDIV (5 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#else +# define STM32_SDMMC_SDXFR_CLKDIV (100 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#endif + +#define STM32_SDMMC_CLKCR_EDGE STM32_SDMMC_CLKCR_NEGEDGE + +/* LED definitions ******************************************************************/ +/* The holybro KakuteH7 board has three, LED_GREEN a Green LED, LED_BLUE + * a Blue LED and LED_RED a Red LED, that can be controlled by software. + * + * If CONFIG_ARCH_LEDS is not defined, then the user can control the LEDs in any way. + * The following definitions are used to access individual LEDs. + */ + +/* LED index values for use with board_userled() */ + +#define BOARD_LED1 0 +#define BOARD_NLEDS 1 + +#define BOARD_LED_RED BOARD_LED1 + +/* LED bits for use with board_userled_all() */ + +#define BOARD_LED1_BIT (1 << BOARD_LED1) + +/* If CONFIG_ARCH_LEDS is defined, the usage by the board port is defined in + * include/board.h and src/stm32_leds.c. The LEDs are used to encode OS-related + * events as follows: + * + * + * SYMBOL Meaning LED state + * Red Green Blue + * ---------------------- -------------------------- ------ ------ ----*/ + +#define LED_STARTED 0 /* NuttX has been started OFF OFF OFF */ +#define LED_HEAPALLOCATE 1 /* Heap has been allocated OFF OFF ON */ +#define LED_IRQSENABLED 2 /* Interrupts enabled OFF ON OFF */ +#define LED_STACKCREATED 3 /* Idle stack created OFF ON ON */ +#define LED_INIRQ 4 /* In an interrupt N/C N/C GLOW */ +#define LED_SIGNAL 5 /* In a signal handler N/C GLOW N/C */ +#define LED_ASSERTION 6 /* An assertion failed GLOW N/C GLOW */ +#define LED_PANIC 7 /* The system has crashed Blink OFF N/C */ +#define LED_IDLE 8 /* MCU is is sleep mode ON OFF OFF */ + +/* Thus if the Green LED is statically on, NuttX has successfully booted and + * is, apparently, running normally. If the Red LED is flashing at + * approximately 2Hz, then a fatal error has been detected and the system + * has halted. + */ + +/* Alternate function pin selections ************************************************/ + + +#define GPIO_USART1_RX GPIO_USART1_RX_2 /* PA10 */ +#define GPIO_USART1_TX GPIO_USART1_TX_2 /* PA9 */ + +#define GPIO_USART2_RX GPIO_USART2_RX_2 /* PD6 */ +#define GPIO_USART2_TX GPIO_USART2_TX_2 /* PD5 */ + +#define GPIO_USART3_RX GPIO_USART3_RX_3 /* PD9 */ +#define GPIO_USART3_TX GPIO_USART3_TX_3 /* PD8 */ + +#define GPIO_UART4_RX GPIO_UART4_RX_5 /* PD0 */ +#define GPIO_UART4_TX GPIO_UART4_TX_5 /* PD1 */ + +#define GPIO_USART6_RX GPIO_USART6_RX_1 /* PC7 */ +#define GPIO_USART6_TX GPIO_USART6_TX_1 /* PC6 */ + +#define GPIO_UART7_RX GPIO_UART7_RX_3 /* PE7 */ +#define GPIO_UART7_TX GPIO_UART7_TX_3 /* PE8 */ + +/* SPI + * SPI1 SD Card + * SPI2 is OSD AT7456E + * SPI4 is IMU + */ + +#define GPIO_SPI1_MISO GPIO_SPI1_MISO_1 /* PA6 */ +#define GPIO_SPI1_MOSI GPIO_SPI1_MOSI_1 /* PA7 */ +#define GPIO_SPI1_SCK GPIO_SPI1_SCK_1 /* PA5 */ + +#define GPIO_SPI2_MISO GPIO_SPI2_MISO_1 /* PB14 */ +#define GPIO_SPI2_MOSI GPIO_SPI2_MOSI_1 /* PB15 */ +#define GPIO_SPI2_SCK GPIO_SPI2_SCK_4 /* PB13 */ + +#define GPIO_SPI4_MISO GPIO_SPI4_MISO_2 /* PE5 */ +#define GPIO_SPI4_MOSI GPIO_SPI4_MOSI_2 /* PE6 */ +#define GPIO_SPI4_SCK GPIO_SPI4_SCK_2 /* PE2 */ + +/* I2C + */ + +#define GPIO_I2C1_SCL GPIO_I2C1_SCL_1 /* PB6 */ +#define GPIO_I2C1_SDA GPIO_I2C1_SDA_1 /* PB7 */ + +#define GPIO_I2C1_SCL_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTB | GPIO_PIN6) +#define GPIO_I2C1_SDA_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTB | GPIO_PIN7) + +/* SDMMC1 + * + * VDD 3.3 + * GND + * SDMMC1_CK PC12 + * SDMMC1_CMD PD2 + * SDMMC1_D0 PC8 + * SDMMC1_D1 PC9 + * SDMMC1_D2 PC10 + * SDMMC1_D3 PC11 + * GPIO_SDMMC1_NCD PG0 + */ + +/* USB + * + * OTG_FS_DM PA11 + * OTG_FS_DP PA12 + * VBUS PA9 + */ + + +/* Board provides GPIO or other Hardware for signaling to timing analyzer */ + +# define PROBE_INIT(mask) +# define PROBE(n,s) +# define PROBE_MARK(n) + diff --git a/boards/holybro/kakuteh7mini/nuttx-config/include/board_dma_map.h b/boards/holybro/kakuteh7mini/nuttx-config/include/board_dma_map.h new file mode 100644 index 0000000000..1954ce3e68 --- /dev/null +++ b/boards/holybro/kakuteh7mini/nuttx-config/include/board_dma_map.h @@ -0,0 +1,40 @@ +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#define DMAMAP_SPI4_RX DMAMAP_DMA12_SPI4RX_1 /* DMA2 */ +#define DMAMAP_SPI4_TX DMAMAP_DMA12_SPI4TX_1 /* DMA2 */ + +#define DMAMAP_USART2_RX DMAMAP_DMA12_USART2RX_1 /* DMA2 */ + diff --git a/boards/holybro/kakuteh7mini/nuttx-config/nsh/defconfig b/boards/holybro/kakuteh7mini/nuttx-config/nsh/defconfig new file mode 100644 index 0000000000..c22da0c8a0 --- /dev/null +++ b/boards/holybro/kakuteh7mini/nuttx-config/nsh/defconfig @@ -0,0 +1,272 @@ +# +# This file is autogenerated: PLEASE DO NOT EDIT IT. +# +# You can use "make menuconfig" to make any modifications to the installed .config file. +# You can then do "make savedefconfig" to generate a new defconfig file that includes your +# modifications. +# +# CONFIG_DISABLE_ENVIRON is not set +# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set +# CONFIG_DISABLE_PTHREAD is not set +# CONFIG_MMCSD_HAVE_CARDDETECT is not set +# CONFIG_MMCSD_HAVE_WRITEPROTECT is not set +# CONFIG_MMCSD_MMCSUPPORT is not set +# CONFIG_NSH_DISABLEBG is not set +# CONFIG_NSH_DISABLESCRIPT is not set +# CONFIG_NSH_DISABLE_CAT is not set +# CONFIG_NSH_DISABLE_CD is not set +# CONFIG_NSH_DISABLE_CP is not set +# CONFIG_NSH_DISABLE_DATE is not set +# CONFIG_NSH_DISABLE_DF is not set +# CONFIG_NSH_DISABLE_ECHO is not set +# CONFIG_NSH_DISABLE_ENV is not set +# CONFIG_NSH_DISABLE_EXEC is not set +# CONFIG_NSH_DISABLE_EXIT is not set +# CONFIG_NSH_DISABLE_EXPORT is not set +# CONFIG_NSH_DISABLE_FREE is not set +# CONFIG_NSH_DISABLE_GET is not set +# CONFIG_NSH_DISABLE_HELP is not set +# CONFIG_NSH_DISABLE_ITEF is not set +# CONFIG_NSH_DISABLE_KILL is not set +# CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_LS is not set +# CONFIG_NSH_DISABLE_MKDIR is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set +# CONFIG_NSH_DISABLE_MOUNT is not set +# CONFIG_NSH_DISABLE_MV is not set +# CONFIG_NSH_DISABLE_PS is not set +# CONFIG_NSH_DISABLE_PSSTACKUSAGE is not set +# CONFIG_NSH_DISABLE_PWD is not set +# CONFIG_NSH_DISABLE_RM is not set +# CONFIG_NSH_DISABLE_RMDIR is not set +# CONFIG_NSH_DISABLE_SEMICOLON is not set +# CONFIG_NSH_DISABLE_SET is not set +# CONFIG_NSH_DISABLE_SLEEP is not set +# CONFIG_NSH_DISABLE_SOURCE is not set +# CONFIG_NSH_DISABLE_TEST is not set +# CONFIG_NSH_DISABLE_TIME is not set +# CONFIG_NSH_DISABLE_UMOUNT is not set +# CONFIG_NSH_DISABLE_UNSET is not set +# CONFIG_NSH_DISABLE_USLEEP is not set +# CONFIG_SPI_CALLBACK is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/holybro/kakuteh7mini/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="stm32h7" +CONFIG_ARCH_CHIP_STM32H743II=y +CONFIG_ARCH_CHIP_STM32H7=y +CONFIG_ARCH_INTERRUPTSTACK=512 +CONFIG_ARCH_STACKDUMP=y +CONFIG_ARMV7M_BASEPRI_WAR=y +CONFIG_ARMV7M_DCACHE=y +CONFIG_ARMV7M_DTCM=y +CONFIG_ARMV7M_ICACHE=y +CONFIG_ARMV7M_MEMCPY=y +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_ARM_MPU_EARLY_RESET=y +CONFIG_BOARDCTL_RESET=y +CONFIG_BOARDCTL=y +CONFIG_BOARD_CRASHDUMP=y +CONFIG_BOARD_LOOPSPERMSEC=95150 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_BOARD_ASSERT_RESET_VALUE=0 +CONFIG_BUILTIN=y +CONFIG_C99_BOOL8=y +CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y +CONFIG_CDCACM_PRODUCTID=0x0050 +CONFIG_CDCACM_PRODUCTSTR="PX4 KakuteH7Mini-nand" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_CDCACM_VENDORID=0x3162 +CONFIG_CDCACM_VENDORSTR="Holybro" +CONFIG_CLOCK_MONOTONIC=y +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_HARDFAULT_ALERT=y +CONFIG_DEBUG_MEMFAULT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEFAULT_SMALL=y +# CONFIG_NSH_DISABLE_ECHO is not set +# CONFIG_NSH_DISABLE_ENV is not set +# CONFIG_NSH_DISABLE_FREE is not set +# CONFIG_NSH_DISABLE_HELP is not set +# CONFIG_NSH_DISABLE_KILL is not set +# CONFIG_NSH_DISABLE_LS is not set +# CONFIG_NSH_DISABLE_MKDIR is not set +# CONFIG_NSH_DISABLE_MODCMDS is not set +# CONFIG_NSH_DISABLE_MOUNT is not set +# CONFIG_NSH_DISABLE_MV is not set +# CONFIG_NSH_DISABLE_PRINTF is not set +# CONFIG_NSH_DISABLE_PSSTACKUSAGE +# CONFIG_NSH_DISABLE_PWD is not set +# CONFIG_NSH_DISABLE_SOURCE is not set +# CONFIG_NSH_DISABLE_SLEEP is not set +# CONFIG_NSH_DISABLE_TEST is not set +# CONFIG_NSH_DISABLE_UMOUNT is not set +# CONFIG_NSH_DISABLE_UNSET is not set +# CONFIG_NSH_DISABLE_USLEEP is not set +CONFIG_DEV_FIFO_SIZE=0 +CONFIG_DEV_PIPE_MAXSIZE=1024 +CONFIG_DEV_PIPE_SIZE=70 +CONFIG_EXPERIMENTAL=y +CONFIG_FAT_DMAMEMORY=y +CONFIG_FAT_LCNAMES=y +CONFIG_FAT_LFN=y +CONFIG_FAT_LFN_ALIAS_HASH=y +CONFIG_FDCLONE_STDIO=y +CONFIG_FS_BINFS=y +CONFIG_FS_CROMFS=y +CONFIG_FS_FAT=y +CONFIG_FS_FATTIME=y +CONFIG_FS_PROCFS=y +CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y +CONFIG_FS_PROCFS_MAX_TASKS=64 +CONFIG_FS_PROCFS_REGISTER=y +CONFIG_FS_ROMFS=y +CONFIG_GRAN=y +CONFIG_GRAN_INTR=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +CONFIG_I2C=y +CONFIG_I2C_RESET=y +CONFIG_IDLETHREAD_STACKSIZE=750 +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_STRERROR=y +CONFIG_MEMSET_64BIT=y +CONFIG_MEMSET_OPTSPEED=y +CONFIG_MMCSD=y +CONFIG_MMCSD_SDIO=y +CONFIG_MM_REGIONS=4 +CONFIG_MTD=y +CONFIG_MTD_BYTE_WRITE=y +CONFIG_MTD_PARTITION=y +CONFIG_MTD_PROGMEM=y +CONFIG_MTD_RAMTRON=y +CONFIG_NAME_MAX=40 +CONFIG_NSH_ARCHINIT=y +CONFIG_NSH_ARGCAT=y +CONFIG_NSH_BUILTIN_APPS=y +CONFIG_NSH_CMDPARMS=y +CONFIG_NSH_CROMFSETC=y +CONFIG_NSH_DISABLE_IFCONFIG=y +CONFIG_NSH_DISABLE_IFUPDOWN=y +CONFIG_NSH_DISABLE_TELNETD=y +CONFIG_NSH_LINELEN=128 +CONFIG_NSH_MAXARGUMENTS=15 +CONFIG_NSH_MMCSDSPIPORTNO=1 +CONFIG_NSH_NESTDEPTH=8 +CONFIG_NSH_QUOTE=y +CONFIG_NSH_ROMFSETC=y +CONFIG_NSH_ROMFSSECTSIZE=128 +CONFIG_NSH_STRERROR=y +CONFIG_NSH_VARS=y +CONFIG_OTG_ID_GPIO_DISABLE=y +CONFIG_PIPES=y +CONFIG_PREALLOC_TIMERS=50 +CONFIG_PRIORITY_INHERITANCE=y +CONFIG_PTHREAD_MUTEX_ROBUST=y +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_SETSPEED=y +CONFIG_RAM_SIZE=245760 +CONFIG_RAM_START=0x20010000 +CONFIG_RAW_BINARY=y +CONFIG_READLINE_CMD_HISTORY=y +CONFIG_READLINE_TABCOMPLETION=y +CONFIG_RTC_DATETIME=y +CONFIG_LIBC_MAX_EXITFUNS=1 +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_HPWORKPRIORITY=249 +CONFIG_SCHED_HPWORKSTACKSIZE=1280 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y +CONFIG_SCHED_INSTRUMENTATION_SWITCH=y +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKSTACKSIZE=1632 +CONFIG_SCHED_WAITPID=y +CONFIG_SDCLONE_DISABLE=y +CONFIG_SEM_PREALLOCHOLDERS=32 +CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y +CONFIG_SERIAL_TERMIOS=y +CONFIG_SIG_DEFAULT=y +CONFIG_SIG_SIGALRM_ACTION=y +CONFIG_SIG_SIGUSR1_ACTION=y +CONFIG_SIG_SIGUSR2_ACTION=y +CONFIG_SIG_SIGWORK=4 +CONFIG_STACK_COLORATION=y +CONFIG_START_DAY=30 +CONFIG_START_MONTH=11 +CONFIG_STDIO_BUFFER_SIZE=256 +CONFIG_STM32H7_ADC1=y +CONFIG_STM32H7_ADC3=y +CONFIG_STM32H7_BBSRAM=y +CONFIG_STM32H7_BBSRAM_FILES=5 +CONFIG_STM32H7_BDMA=y +CONFIG_STM32H7_BKPSRAM=y +CONFIG_STM32H7_DMA1=y +CONFIG_STM32H7_DMA2=y +CONFIG_STM32H7_DMACAPABLE=y +CONFIG_STM32H7_FLOWCONTROL_BROKEN=y +CONFIG_STM32H7_I2C1=y +CONFIG_STM32H7_I2C_DYNTIMEO=y +CONFIG_STM32H7_I2C_DYNTIMEO_STARTSTOP=10 +CONFIG_STM32H7_OTGFS=y +CONFIG_STM32H7_PROGMEM=y +CONFIG_STM32H7_RTC=y +CONFIG_STM32H7_RTC_AUTO_LSECLOCK_START_DRV_CAPABILITY=y +CONFIG_STM32H7_RTC_MAGIC_REG=1 +CONFIG_STM32H7_SAVE_CRASHDUMP=y +CONFIG_STM32H7_SDMMC1=y +CONFIG_STM32H7_SERIALBRK_BSDCOMPAT=y +CONFIG_STM32H7_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32H7_SPI1=y +CONFIG_STM32H7_SPI2=y +CONFIG_STM32H7_SPI4=y +CONFIG_STM32H7_SPI4_DMA=y +CONFIG_STM32H7_SPI4_DMA_BUFFER=1024 +CONFIG_STM32H7_SPI_DMA=y +CONFIG_STM32H7_UART4=y +CONFIG_STM32H7_UART7=y +CONFIG_STM32H7_USART1=y +CONFIG_STM32H7_USART2=y +CONFIG_STM32H7_USART3=y +CONFIG_STM32H7_USART6=y +CONFIG_STM32H7_USART_BREAKS=y +CONFIG_STM32H7_USART_INVERT=y +CONFIG_STM32H7_USART_SINGLEWIRE=y +CONFIG_STM32H7_USART_SWAP=y +CONFIG_SYSTEM_CDCACM=y +CONFIG_SYSTEM_NSH=y +CONFIG_TASK_NAME_SIZE=24 +CONFIG_TTY_SIGINT=y +CONFIG_TTY_SIGTSTP=y +CONFIG_UART4_BAUD=57600 +CONFIG_UART4_RXBUFSIZE=600 +CONFIG_UART4_TXBUFSIZE=1500 +CONFIG_UART7_BAUD=57600 +CONFIG_UART7_RXBUFSIZE=600 +CONFIG_UART7_TXBUFSIZE=1500 +CONFIG_USART1_BAUD=57600 +CONFIG_USART1_RXBUFSIZE=600 +CONFIG_USART1_TXBUFSIZE=1500 +CONFIG_USART2_BAUD=57600 +CONFIG_USART2_RXBUFSIZE=600 +CONFIG_USART2_RXDMA=y +CONFIG_USART2_TXBUFSIZE=2500 +CONFIG_USART3_BAUD=57600 +CONFIG_USART3_RXBUFSIZE=600 +CONFIG_USART3_SERIAL_CONSOLE=y +CONFIG_USART3_TXBUFSIZE=1500 +CONFIG_USART6_BAUD=57600 +CONFIG_USART6_RXBUFSIZE=600 +CONFIG_USART6_TXBUFSIZE=1500 +CONFIG_USBDEV=y +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_MAXPOWER=500 +CONFIG_USEC_PER_TICK=1000 +CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_ENTRYPOINT="nsh_main" +CONFIG_WATCHDOG=y diff --git a/boards/holybro/kakuteh7mini/nuttx-config/scripts/bootloader_script.ld b/boards/holybro/kakuteh7mini/nuttx-config/scripts/bootloader_script.ld new file mode 100644 index 0000000000..fc0a5589be --- /dev/null +++ b/boards/holybro/kakuteh7mini/nuttx-config/scripts/bootloader_script.ld @@ -0,0 +1,215 @@ +/**************************************************************************** + * scripts/script.ld + * + * Copyright (C) 2016, 2019 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* The KakuteH7 uses an STM32H743VI has 2048Kb of main FLASH memory. + * The flash memory is partitioned into a User Flash memory and a System + * Flash memory. Each of these memories has two banks: + * + * 1) User Flash memory: + * + * Bank 1: Start address 0x0800:0000 to 0x080F:FFFF with 8 sectors, 128Kb each + * Bank 2: Start address 0x0810:0000 to 0x081F:FFFF with 8 sectors, 128Kb each + * + * 2) System Flash memory: + * + * Bank 1: Start address 0x1FF0:0000 to 0x1FF1:FFFF with 1 x 128Kb sector + * Bank 1: Start address 0x1FF4:0000 to 0x1FF5:FFFF with 1 x 128Kb sector + * + * 3) User option bytes for user configuration, only in Bank 1. + * + * In the STM32H743VI, two different boot spaces can be selected through + * the BOOT pin and the boot base address programmed in the BOOT_ADD0 and + * BOOT_ADD1 option bytes: + * + * 1) BOOT=0: Boot address defined by user option byte BOOT_ADD0[15:0]. + * ST programmed value: Flash memory at 0x0800:0000 + * 2) BOOT=1: Boot address defined by user option byte BOOT_ADD1[15:0]. + * ST programmed value: System bootloader at 0x1FF0:0000 + * + * The KakuteH7 has a switch on board, the BOOT0 pin is at ground so by default, + * the STM32 will boot to address 0x0800:0000 in FLASH unless the switch is + * depressed, then the boot will be from 0x1FF0:0000 + * + * The STM32H743VI also has 1024Kb of data SRAM. + * SRAM is split up into several blocks and into three power domains: + * + * 1) TCM SRAMs are dedicated to the Cortex-M7 and are accessible with + * 0 wait states by the Cortex-M7 and by MDMA through AHBS slave bus + * + * 1.1) 128Kb of DTCM-RAM beginning at address 0x2000:0000 + * + * The DTCM-RAM is organized as 2 x 64Kb DTCM-RAMs on 2 x 32 bit + * DTCM ports. The DTCM-RAM could be used for critical real-time + * data, such as interrupt service routines or stack / heap memory. + * Both DTCM-RAMs can be used in parallel (for load/store operations) + * thanks to the Cortex-M7 dual issue capability. + * + * 1.2) 64Kb of ITCM-RAM beginning at address 0x0000:0000 + * + * This RAM is connected to ITCM 64-bit interface designed for + * execution of critical real-times routines by the CPU. + * + * 2) AXI SRAM (D1 domain) accessible by all system masters except BDMA + * through D1 domain AXI bus matrix + * + * 2.1) 512Kb of SRAM beginning at address 0x2400:0000 + * + * 3) AHB SRAM (D2 domain) accessible by all system masters except BDMA + * through D2 domain AHB bus matrix + * + * 3.1) 128Kb of SRAM1 beginning at address 0x3000:0000 + * 3.2) 128Kb of SRAM2 beginning at address 0x3002:0000 + * 3.3) 32Kb of SRAM3 beginning at address 0x3004:0000 + * + * SRAM1 - SRAM3 are one contiguous block: 288Kb at address 0x3000:0000 + * + * 4) AHB SRAM (D3 domain) accessible by most of system masters + * through D3 domain AHB bus matrix + * + * 4.1) 64Kb of SRAM4 beginning at address 0x3800:0000 + * 4.1) 4Kb of backup RAM beginning at address 0x3880:0000 + * + * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 + * where the code expects to begin execution by jumping to the entry point in + * the 0x0800:0000 address range. + * + * The bootloader uses the first sector of the flash, which is 128K in length. + */ + +MEMORY +{ + itcm (rwx) : ORIGIN = 0x00000000, LENGTH = 64K + flash (rx) : ORIGIN = 0x08000000, LENGTH = 128K + dtcm1 (rwx) : ORIGIN = 0x20000000, LENGTH = 64K + dtcm2 (rwx) : ORIGIN = 0x20010000, LENGTH = 64K + sram (rwx) : ORIGIN = 0x24000000, LENGTH = 512K + sram1 (rwx) : ORIGIN = 0x30000000, LENGTH = 128K + sram2 (rwx) : ORIGIN = 0x30020000, LENGTH = 128K + sram3 (rwx) : ORIGIN = 0x30040000, LENGTH = 32K + sram4 (rwx) : ORIGIN = 0x38000000, LENGTH = 64K + bbram (rwx) : ORIGIN = 0x38800000, LENGTH = 4K +} + +OUTPUT_ARCH(arm) +EXTERN(_vectors) +ENTRY(_stext) + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) +EXTERN(_bootdelay_signature) + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + . = ALIGN(32); + /* + This signature provides the bootloader with a way to delay booting + */ + _bootdelay_signature = ABSOLUTE(.); + FILL(0xffecc2925d7d05c5) + . += 8; + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.glue_7) + *(.glue_7t) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + + } > flash + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > flash + + + .ARM.extab : { + *(.ARM.extab*) + } > flash + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > flash + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > sram AT > flash + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + . = ALIGN(4); + _ebss = ABSOLUTE(.); + } > sram + + /* Stabs debugging sections. */ + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } +} diff --git a/boards/holybro/kakuteh7mini/nuttx-config/scripts/script.ld b/boards/holybro/kakuteh7mini/nuttx-config/scripts/script.ld new file mode 100644 index 0000000000..ae07f4bfca --- /dev/null +++ b/boards/holybro/kakuteh7mini/nuttx-config/scripts/script.ld @@ -0,0 +1,228 @@ +/**************************************************************************** + * scripts/script.ld + * + * Copyright (C) 2016, 2019 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* The KakuteH7 uses an STM32H743VI has 2048Kb of main FLASH memory. + * The flash memory is partitioned into a User Flash memory and a System + * Flash memory. Each of these memories has two banks: + * + * 1) User Flash memory: + * + * Bank 1: Start address 0x0800:0000 to 0x080F:FFFF with 8 sectors, 128Kb each + * Bank 2: Start address 0x0810:0000 to 0x081F:FFFF with 8 sectors, 128Kb each + * + * 2) System Flash memory: + * + * Bank 1: Start address 0x1FF0:0000 to 0x1FF1:FFFF with 1 x 128Kb sector + * Bank 1: Start address 0x1FF4:0000 to 0x1FF5:FFFF with 1 x 128Kb sector + * + * 3) User option bytes for user configuration, only in Bank 1. + * + * In the STM32H743VI, two different boot spaces can be selected through + * the BOOT pin and the boot base address programmed in the BOOT_ADD0 and + * BOOT_ADD1 option bytes: + * + * 1) BOOT=0: Boot address defined by user option byte BOOT_ADD0[15:0]. + * ST programmed value: Flash memory at 0x0800:0000 + * 2) BOOT=1: Boot address defined by user option byte BOOT_ADD1[15:0]. + * ST programmed value: System bootloader at 0x1FF0:0000 + * + * The KakuteH7 has a switch on board, the BOOT0 pin is at ground so by default, + * the STM32 will boot to address 0x0800:0000 in FLASH unless the switch is + * depressed, then the boot will be from 0x1FF0:0000 + * + * The STM32H743VI also has 1024Kb of data SRAM. + * SRAM is split up into several blocks and into three power domains: + * + * 1) TCM SRAMs are dedicated to the Cortex-M7 and are accessible with + * 0 wait states by the Cortex-M7 and by MDMA through AHBS slave bus + * + * 1.1) 128Kb of DTCM-RAM beginning at address 0x2000:0000 + * + * The DTCM-RAM is organized as 2 x 64Kb DTCM-RAMs on 2 x 32 bit + * DTCM ports. The DTCM-RAM could be used for critical real-time + * data, such as interrupt service routines or stack / heap memory. + * Both DTCM-RAMs can be used in parallel (for load/store operations) + * thanks to the Cortex-M7 dual issue capability. + * + * 1.2) 64Kb of ITCM-RAM beginning at address 0x0000:0000 + * + * This RAM is connected to ITCM 64-bit interface designed for + * execution of critical real-times routines by the CPU. + * + * 2) AXI SRAM (D1 domain) accessible by all system masters except BDMA + * through D1 domain AXI bus matrix + * + * 2.1) 512Kb of SRAM beginning at address 0x2400:0000 + * + * 3) AHB SRAM (D2 domain) accessible by all system masters except BDMA + * through D2 domain AHB bus matrix + * + * 3.1) 128Kb of SRAM1 beginning at address 0x3000:0000 + * 3.2) 128Kb of SRAM2 beginning at address 0x3002:0000 + * 3.3) 32Kb of SRAM3 beginning at address 0x3004:0000 + * + * SRAM1 - SRAM3 are one contiguous block: 288Kb at address 0x3000:0000 + * + * 4) AHB SRAM (D3 domain) accessible by most of system masters + * through D3 domain AHB bus matrix + * + * 4.1) 64Kb of SRAM4 beginning at address 0x3800:0000 + * 4.1) 4Kb of backup RAM beginning at address 0x3880:0000 + * + * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 + * where the code expects to begin execution by jumping to the entry point in + * the 0x0800:0000 address range. + */ + +MEMORY +{ + ITCM_RAM (rwx) : ORIGIN = 0x00000000, LENGTH = 64K + FLASH (rx) : ORIGIN = 0x08020000, LENGTH = 1792K /* params in last sector */ + + DTCM1_RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 64K + DTCM2_RAM (rwx) : ORIGIN = 0x20010000, LENGTH = 64K + AXI_SRAM (rwx) : ORIGIN = 0x24000000, LENGTH = 512K /* D1 domain AXI bus */ + SRAM1 (rwx) : ORIGIN = 0x30000000, LENGTH = 128K /* D2 domain AHB bus */ + SRAM2 (rwx) : ORIGIN = 0x30020000, LENGTH = 128K /* D2 domain AHB bus */ + SRAM3 (rwx) : ORIGIN = 0x30040000, LENGTH = 32K /* D2 domain AHB bus */ + SRAM4 (rwx) : ORIGIN = 0x38000000, LENGTH = 64K /* D3 domain */ + BKPRAM (rwx) : ORIGIN = 0x38800000, LENGTH = 4K +} + +OUTPUT_ARCH(arm) +EXTERN(_vectors) +ENTRY(_stext) + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) +EXTERN(_bootdelay_signature) + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + . = ALIGN(32); + /* + This signature provides the bootloader with a way to delay booting + */ + _bootdelay_signature = ABSOLUTE(.); + FILL(0xffecc2925d7d05c5) + . += 8; + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.glue_7) + *(.glue_7t) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + + } > FLASH + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > FLASH + + + .ARM.extab : { + *(.ARM.extab*) + } > FLASH + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > FLASH + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + + /* Pad out last section as the STM32H7 Flash write size is 256 bits. 32 bytes */ + . = ALIGN(16); + FILL(0xffff) + . += 16; + } > AXI_SRAM AT > FLASH = 0xffff + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + . = ALIGN(4); + _ebss = ABSOLUTE(.); + } > AXI_SRAM + + /* Emit the the D3 power domain section for locating BDMA data */ + + .sram4_reserve (NOLOAD) : + { + *(.sram4) + . = ALIGN(4); + _sram4_heap_start = ABSOLUTE(.); + } > SRAM4 + + /* Stabs debugging sections. */ + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } +} diff --git a/boards/holybro/kakuteh7mini/src/CMakeLists.txt b/boards/holybro/kakuteh7mini/src/CMakeLists.txt new file mode 100644 index 0000000000..220642b2a5 --- /dev/null +++ b/boards/holybro/kakuteh7mini/src/CMakeLists.txt @@ -0,0 +1,67 @@ +############################################################################ +# +# Copyright (c) 2016 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +if("${PX4_BOARD_LABEL}" STREQUAL "bootloader") + add_library(drivers_board + bootloader_main.c + usb.c + ) + target_link_libraries(drivers_board + PRIVATE + nuttx_arch # sdio + nuttx_drivers # sdio + bootloader + ) + target_include_directories(drivers_board PRIVATE ${PX4_SOURCE_DIR}/platforms/nuttx/src/bootloader/common) + +else() + add_library(drivers_board + i2c.cpp + init.c + led.c + spi.cpp + timer_config.cpp + usb.c + ) + add_dependencies(drivers_board arch_board_hw_info) + + target_link_libraries(drivers_board + PRIVATE + arch_io_pins + arch_spi + arch_board_hw_info + drivers__led # drv_led_start + nuttx_arch # sdio + nuttx_drivers # sdio + px4_layer + ) +endif() diff --git a/boards/holybro/kakuteh7mini/src/board_config.h b/boards/holybro/kakuteh7mini/src/board_config.h new file mode 100644 index 0000000000..a96e548476 --- /dev/null +++ b/boards/holybro/kakuteh7mini/src/board_config.h @@ -0,0 +1,214 @@ +/**************************************************************************** + * + * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file board_config.h + * + * holybro KakuteH7 internal definitions + */ + +#pragma once + +/**************************************************************************************************** + * Included Files + ****************************************************************************************************/ + +#include +#include +#include + +#include + +/**************************************************************************************************** + * Definitions + ****************************************************************************************************/ + +/* Configuration ************************************************************************************/ + +# define BOARD_HAS_USB_VALID 1 +# define BOARD_HAS_NBAT_V 1 +# define BOARD_HAS_NBAT_I 1 + +/* Holybro KakuteH7 GPIOs ************************************************************************/ + +/* LEDs are driven with push open drain to support Anode to 5V or 3.3V */ + +#define GPIO_nLED_RED /* PC2 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN2) + +#define BOARD_HAS_CONTROL_STATUS_LEDS 1 +#define BOARD_OVERLOAD_LED LED_RED + +/* + * ADC channels + * + * These are the channel numbers of the ADCs of the microcontroller that + * can be used by the Px4 Firmware in the adc driver + */ + +/* ADC defines to be used in sensors.cpp to read from a particular channel */ + +#define ADC1_CH(n) (n) + +/* Define GPIO pins used as ADC N.B. Channel numbers must match below */ + +#define PX4_ADC_GPIO \ + /* PC0 */ GPIO_ADC123_INP10, \ + /* PC1 */ GPIO_ADC123_INP11, \ + /* PC5 */ GPIO_ADC12_INP8 + +/* Define Channel numbers must match above GPIO pin IN(n)*/ + +#define ADC_BATTERY_VOLTAGE_CHANNEL /* PC0 */ ADC1_CH(10) +#define ADC_BATTERY_CURRENT_CHANNEL /* PC1 */ ADC1_CH(11) +#define ADC_RSSI_IN_CHANNEL /* PC5 */ ADC1_CH(8) + +#define ADC_CHANNELS \ + ((1 << ADC_BATTERY_VOLTAGE_CHANNEL) | \ + (1 << ADC_BATTERY_CURRENT_CHANNEL) | \ + (1 << ADC_RSSI_IN_CHANNEL)) + +#define BOARD_ADC_OPEN_CIRCUIT_V (5.6f) + + + +/* PWM + */ +#define DIRECT_PWM_OUTPUT_CHANNELS 8 + +#define BOARD_NUM_IO_TIMERS 4 + + +/* Tone alarm output */ + +#define GPIO_TONE_ALARM_IDLE /* PC13 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13) +#define GPIO_TONE_ALARM_GPIO /* PC13 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) + +/* USB OTG FS + * + * PA9 OTG_FS_VBUS VBUS sensing + */ +#define GPIO_OTGFS_VBUS /* PA8 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_100MHz|GPIO_PORTA|GPIO_PIN8) + +/* High-resolution timer */ +#define HRT_TIMER 4 /* use timer3 for the HRT */ +#define HRT_TIMER_CHANNEL 1 /* use capture/compare channel 1 */ + +// So we can run CRSF on ttyS4, which corresponds to the TX6/RX6 pins +/* RC Serial port */ +#define RC_SERIAL_PORT "/dev/ttyS4" +#define BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT + +/* PWM input driver. Use FMU AUX5 pins attached to timer4 channel 2 */ + +#define PWMIN_TIMER 5 +#define PWMIN_TIMER_CHANNEL /* T5C1 */ 1 +#define GPIO_PWM_IN /* PA0 */ GPIO_TIM5_CH1IN + +#define GPIO_RSSI_IN /* PC5 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTC|GPIO_PIN5) + +#define GPIO_RF_SWITCH /* PE13 */ (GPIO_OUTPUT|GPIO_PULLDOWN|GPIO_PORTE|GPIO_PIN13) + +/* Power switch controls ******************************************************/ + +#define SDIO_SLOTNO 0 /* Only one slot */ +#define SDIO_MINOR 0 + +/* SD card bringup does not work if performed on the IDLE thread because it + * will cause waiting. Use either: + * + * CONFIG_LIB_BOARDCTL=y, OR + * CONFIG_BOARD_INITIALIZE=y && CONFIG_BOARD_INITTHREAD=y + */ + +#if defined(CONFIG_BOARD_INITIALIZE) && !defined(CONFIG_LIB_BOARDCTL) && \ + !defined(CONFIG_BOARD_INITTHREAD) +# warning SDIO initialization cannot be perfomed on the IDLE thread +#endif + +/* By Providing BOARD_ADC_USB_CONNECTED (using the px4_arch abstraction) + * this board support the ADC system_power interface, and therefore + * provides the true logic GPIO BOARD_ADC_xxxx macros. + */ +#define BOARD_ADC_USB_CONNECTED (px4_arch_gpioread(GPIO_OTGFS_VBUS)) + +/* Board never powers off the Servo rail */ + +#define BOARD_ADC_SERVO_VALID (1) + +#define BOARD_ADC_BRICK1_VALID (1) + +/* This board provides a DMA pool and APIs */ +#define BOARD_DMA_ALLOC_POOL_SIZE 5120 + +/* This board provides the board_on_reset interface */ + +#define BOARD_HAS_ON_RESET 1 + +#define PX4_GPIO_INIT_LIST { \ + PX4_ADC_GPIO, \ + GPIO_TONE_ALARM_IDLE, \ + GPIO_RSSI_IN, \ + GPIO_RF_SWITCH, \ + } + +#define BOARD_ENABLE_CONSOLE_BUFFER + +#define FLASH_BASED_PARAMS + +__BEGIN_DECLS + +/**************************************************************************************************** + * Public Types + ****************************************************************************************************/ + +/**************************************************************************************************** + * Public data + ****************************************************************************************************/ + +#ifndef __ASSEMBLY__ + +/**************************************************************************************************** + * Public Functions + ****************************************************************************************************/ + +extern void stm32_spiinitialize(void); + +extern void stm32_usbinitialize(void); + +extern void board_peripheral_reset(int ms); + +#include + +#endif /* __ASSEMBLY__ */ + +__END_DECLS diff --git a/boards/holybro/kakuteh7mini/src/bootloader_main.c b/boards/holybro/kakuteh7mini/src/bootloader_main.c new file mode 100644 index 0000000000..915b7f3d29 --- /dev/null +++ b/boards/holybro/kakuteh7mini/src/bootloader_main.c @@ -0,0 +1,75 @@ +/**************************************************************************** + * + * Copyright (c) 2019, 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file bootloader_main.c + * + * FMU-specific early startup code for bootloader +*/ + +#include "board_config.h" +#include "bl.h" + +#include +#include +#include +#include +#include +#include "arm_internal.h" +#include + +extern int sercon_main(int c, char **argv); + +__EXPORT void board_on_reset(int status) {} + +__EXPORT void stm32_boardinitialize(void) +{ + /* configure USB interfaces */ + stm32_usbinitialize(); +} + +__EXPORT int board_app_initialize(uintptr_t arg) +{ + return 0; +} + +void board_late_initialize(void) +{ + sercon_main(0, NULL); +} + +extern void sys_tick_handler(void); +void board_timerhook(void) +{ + sys_tick_handler(); +} diff --git a/boards/holybro/kakuteh7mini/src/hw_config.h b/boards/holybro/kakuteh7mini/src/hw_config.h new file mode 100644 index 0000000000..1ad290fd6c --- /dev/null +++ b/boards/holybro/kakuteh7mini/src/hw_config.h @@ -0,0 +1,125 @@ +/* + * hw_config.h + * + * Created on: May 17, 2015 + * Author: david_s5 + */ + +#ifndef HW_CONFIG_H_ +#define HW_CONFIG_H_ + +/**************************************************************************** + * 10-8--2016: + * To simplify the ripple effect on the tools, we will be using + * /dev/serial/by-id/PX4 to locate PX4 devices. Therefore + * moving forward all Bootloaders must contain the prefix "PX4 BL " + * in the USBDEVICESTRING + * This Change will be made in an upcoming BL release + ****************************************************************************/ +/* + * Define usage to configure a bootloader + * + * + * Constant example Usage + * APP_LOAD_ADDRESS 0x08004000 - The address in Linker Script, where the app fw is org-ed + * BOOTLOADER_DELAY 5000 - Ms to wait while under USB pwr or bootloader request + * BOARD_FMUV2 + * INTERFACE_USB 1 - (Optional) Scan and use the USB interface for bootloading + * INTERFACE_USART 1 - (Optional) Scan and use the Serial interface for bootloading + * USBDEVICESTRING "PX4 BL FMU v2.x" - USB id string + * USBPRODUCTID 0x0011 - PID Should match defconfig + * BOOT_DELAY_ADDRESS 0x000001a0 - (Optional) From the linker script from Linker Script to get a custom + * delay provided by an APP FW + * BOARD_TYPE 9 - Must match .prototype boad_id + * _FLASH_KBYTES (*(uint16_t *)0x1fff7a22) - Run time flash size detection + * BOARD_FLASH_SECTORS ((_FLASH_KBYTES == 0x400) ? 11 : 23) - Run time determine the physical last sector + * BOARD_FLASH_SECTORS 11 - Hard coded zero based last sector + * BOARD_FLASH_SIZE (_FLASH_KBYTES*1024)- Total Flash size of device, determined at run time. + * (1024 * 1024) - Hard coded Total Flash of device - The bootloader and app reserved will be deducted + * programmatically + * + * BOARD_FIRST_FLASH_SECTOR_TO_ERASE 2 - Optional sectors index in the flash_sectors table (F4 only), to begin erasing. + * This is to allow sectors to be reserved for app fw usage. That will NOT be erased + * during a FW upgrade. + * The default is 0, and selects the first sector to be erased, as the 0th entry in the + * flash_sectors table. Which is the second physical sector of FLASH in the device. + * The first physical sector of FLASH is used by the bootloader, and is not defined + * in the table. + * + * APP_RESERVATION_SIZE (BOARD_FIRST_FLASH_SECTOR_TO_ERASE * 16 * 1024) - Number of bytes reserved by the APP FW. This number plus + * BOOTLOADER_RESERVATION_SIZE will be deducted from + * BOARD_FLASH_SIZE to determine the size of the App FW + * and hence the address space of FLASH to erase and program. + * USBMFGSTRING "PX4 AP" - Optional USB MFG string (default is '3D Robotics' if not defined.) + * SERIAL_BREAK_DETECT_DISABLED - Optional prevent break selection on Serial port from entering or staying in BL + * + * * Other defines are somewhat self explanatory. + */ + +/* Boot device selection list*/ +#define USB0_DEV 0x01 +#define SERIAL0_DEV 0x02 +#define SERIAL1_DEV 0x04 + +#define APP_LOAD_ADDRESS 0x08020000 +#define BOOTLOADER_DELAY 3000 +#define INTERFACE_USB 1 +#define INTERFACE_USB_CONFIG "/dev/ttyACM0" +#define BOARD_VBUS MK_GPIO_INPUT(GPIO_OTGFS_VBUS) + +//#define USE_VBUS_PULL_DOWN +#define BOOT_DELAY_ADDRESS 0x000001a0 +#define BOARD_TYPE 1054 +#define BOARD_FLASH_SECTORS (14) +#define BOARD_FLASH_SIZE (16 * 128 * 1024) +#define APP_RESERVATION_SIZE (1 * 128 * 1024) + +#define OSC_FREQ 8 + +#define BOARD_PIN_LED_ACTIVITY GPIO_nLED_RED +#define BOARD_LED_ON 0 +#define BOARD_LED_OFF 1 + +#define SERIAL_BREAK_DETECT_DISABLED 1 + +/* + * Uncommenting this allows to force the bootloader through + * a PWM output pin. As this can accidentally initialize + * an ESC prematurely, it is not recommended. This feature + * has not been used and hence defaults now to off. + * + * # define BOARD_FORCE_BL_PIN_OUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN14) + * # define BOARD_FORCE_BL_PIN_IN (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN11) + * + * # define BOARD_POWER_PIN_OUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN4) + * # define BOARD_POWER_ON 1 + * # define BOARD_POWER_OFF 0 + * # undef BOARD_POWER_PIN_RELEASE // Leave pin enabling Power - un comment to release (disable power) + * +*/ + +#if !defined(ARCH_SN_MAX_LENGTH) +# define ARCH_SN_MAX_LENGTH 12 +#endif + +#if !defined(APP_RESERVATION_SIZE) +# define APP_RESERVATION_SIZE 0 +#endif + +#if !defined(BOARD_FIRST_FLASH_SECTOR_TO_ERASE) +# define BOARD_FIRST_FLASH_SECTOR_TO_ERASE 1 +#endif + +#if !defined(USB_DATA_ALIGN) +# define USB_DATA_ALIGN +#endif + +#ifndef BOOT_DEVICES_SELECTION +# define BOOT_DEVICES_SELECTION USB0_DEV|SERIAL0_DEV|SERIAL1_DEV +#endif + +#ifndef BOOT_DEVICES_FILTER_ONUSB +# define BOOT_DEVICES_FILTER_ONUSB USB0_DEV|SERIAL0_DEV|SERIAL1_DEV +#endif + +#endif /* HW_CONFIG_H_ */ diff --git a/boards/holybro/kakuteh7mini/src/i2c.cpp b/boards/holybro/kakuteh7mini/src/i2c.cpp new file mode 100644 index 0000000000..e9f26f9a9e --- /dev/null +++ b/boards/holybro/kakuteh7mini/src/i2c.cpp @@ -0,0 +1,38 @@ +/**************************************************************************** + * + * Copyright (C) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include + +constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { + initI2CBusExternal(1), +}; diff --git a/boards/holybro/kakuteh7mini/src/init.c b/boards/holybro/kakuteh7mini/src/init.c new file mode 100644 index 0000000000..c8f19df734 --- /dev/null +++ b/boards/holybro/kakuteh7mini/src/init.c @@ -0,0 +1,261 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file init.c + * + * PX4FMU-specific early startup code. This file implements the + * board_app_initialize() function that is called early by nsh during startup. + * + * Code here is run before the rcS script is invoked; it should start required + * subsystems and perform board-specific initialisation. + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include "board_config.h" + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include "arm_internal.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +# if defined(FLASH_BASED_PARAMS) +# include +#endif + + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ + +/* Configuration ************************************************************/ + +/* + * Ideally we'd be able to get these from arm_internal.h, + * but since we want to be able to disable the NuttX use + * of leds for system indication at will and there is no + * separate switch, we need to build independent of the + * CONFIG_ARCH_LEDS configuration switch. + */ +__BEGIN_DECLS +extern void led_init(void); +extern void led_on(int led); +extern void led_off(int led); +__END_DECLS + + +/************************************************************************************ + * Name: board_peripheral_reset + * + * Description: + * + ************************************************************************************/ +__EXPORT void board_peripheral_reset(int ms) +{ +} + +/************************************************************************************ + * Name: board_on_reset + * + * Description: + * Optionally provided function called on entry to board_system_reset + * It should perform any house keeping prior to the rest. + * + * status - 1 if resetting to boot loader + * 0 if just resetting + * + ************************************************************************************/ +__EXPORT void board_on_reset(int status) +{ + for (int i = 0; i < DIRECT_PWM_OUTPUT_CHANNELS; ++i) { + px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); + } + + if (status >= 0) { + up_mdelay(6); + } +} + +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the initialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ + +__EXPORT void +stm32_boardinitialize(void) +{ + board_on_reset(-1); /* Reset PWM first thing */ + + /* configure LEDs */ + + board_autoled_initialize(); + + /* configure pins */ + + const uint32_t gpio[] = PX4_GPIO_INIT_LIST; + px4_gpio_init(gpio, arraySize(gpio)); + + board_control_spi_sensors_power_configgpio(); + + /* Turn bluetooth off by default (no mavlink support yet) */ + px4_arch_gpiowrite(GPIO_RF_SWITCH, 0); + + /* configure USB interfaces */ + + stm32_usbinitialize(); + +} + +/**************************************************************************** + * Name: board_app_initialize + * + * Description: + * Perform application specific initialization. This function is never + * called directly from application code, but only indirectly via the + * (non-standard) boardctl() interface using the command BOARDIOC_INIT. + * + * Input Parameters: + * arg - The boardctl() argument is passed to the board_app_initialize() + * implementation without modification. The argument has no + * meaning to NuttX; the meaning of the argument is a contract + * between the board-specific initalization logic and the the + * matching application logic. The value cold be such things as a + * mode enumeration value, a set of DIP switch switch settings, a + * pointer to configuration data read from a file or serial FLASH, + * or whatever you would like to do with it. Every implementation + * should accept zero/NULL as a default configuration. + * + * Returned Value: + * Zero (OK) is returned on success; a negated errno value is returned on + * any failure to indicate the nature of the failure. + * + ****************************************************************************/ + + +__EXPORT int board_app_initialize(uintptr_t arg) +{ + /* Power on Interfaces */ + board_control_spi_sensors_power(true, 0xffff); + + /* Need hrt running before using the ADC */ + + px4_platform_init(); + + /* configure SPI interfaces */ + + stm32_spiinitialize(); + + /* configure the DMA allocator */ + + if (board_dma_alloc_init() < 0) { + syslog(LOG_ERR, "[boot] DMA alloc FAILED\n"); + } + +#if defined(SERIAL_HAVE_RXDMA) + // set up the serial DMA polling at 1ms intervals for received bytes that have not triggered a DMA event. + static struct hrt_call serial_dma_call; + hrt_call_every(&serial_dma_call, 1000, 1000, (hrt_callout)stm32_serial_dma_poll, NULL); +#endif + + /* initial LED state */ + drv_led_start(); + led_off(LED_RED); + + if (board_hardfault_init(2, true) != 0) { + led_on(LED_RED); + } + + // MARK: this will *not* work as the minis have a W25N NAND flash chip + /* Get the SPI port for the microSD slot */ + struct spi_dev_s *spi_dev = stm32_spibus_initialize(CONFIG_NSH_MMCSDSPIPORTNO); + + if (!spi_dev) { + syslog(LOG_ERR, "[boot] FAILED to initialize SPI port %d\n", CONFIG_NSH_MMCSDSPIPORTNO); + led_on(LED_BLUE); + } + + up_udelay(20); + +#if defined(FLASH_BASED_PARAMS) + static sector_descriptor_t params_sector_map[] = { + {15, 128 * 1024, 0x081E0000}, + {0, 0, 0}, + }; + + /* Initialize the flashfs layer to use heap allocated memory */ + int result = parameter_flashfs_init(params_sector_map, NULL, 0); + + if (result != OK) { + syslog(LOG_ERR, "[boot] FAILED to init params in FLASH %d\n", result); + led_on(LED_AMBER); + } + +#endif + + /* Configure the HW based on the manifest */ + + px4_platform_configure(); + + return OK; +} diff --git a/boards/holybro/kakuteh7mini/src/led.c b/boards/holybro/kakuteh7mini/src/led.c new file mode 100644 index 0000000000..945c383eb7 --- /dev/null +++ b/boards/holybro/kakuteh7mini/src/led.c @@ -0,0 +1,223 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include + +#include + +#include "chip.h" +#include "stm32_gpio.h" +#include "board_config.h" + +#include +#include + +/* + * Ideally we'd be able to get these from arm_internal.h, + * but since we want to be able to disable the NuttX use + * of leds for system indication at will and there is no + * separate switch, we need to build independent of the + * CONFIG_ARCH_LEDS configuration switch. + */ +__BEGIN_DECLS +extern void led_init(void); +extern void led_on(int led); +extern void led_off(int led); +extern void led_toggle(int led); +__END_DECLS + +#ifdef CONFIG_ARCH_LEDS +static bool nuttx_owns_leds = true; +// B R S G +// 0 1 2 3 +static const uint8_t xlatpx4[] = {1, 2, 4, 0}; +# define xlat(p) xlatpx4[(p)] +static uint32_t g_ledmap[] = { + GPIO_nLED_RED, // Indexed by BOARD_LED_RED +}; + +#else + +# define xlat(p) (p) +static uint32_t g_ledmap[] = { + GPIO_nLED_RED, // Indexed by LED_RED, LED_AMBER +}; + +#endif + +__EXPORT void led_init(void) +{ + for (size_t l = 0; l < (sizeof(g_ledmap) / sizeof(g_ledmap[0])); l++) { + if (g_ledmap[l] != 0) { + stm32_configgpio(g_ledmap[l]); + } + } +} + +static void phy_set_led(int led, bool state) +{ + /* Drive Low to switch on */ + + if (g_ledmap[led] != 0) { + stm32_gpiowrite(g_ledmap[led], !state); + } +} + +static bool phy_get_led(int led) +{ + /* If Low it is on */ + if (g_ledmap[led] != 0) { + return !stm32_gpioread(g_ledmap[led]); + } + + return false; +} + +__EXPORT void led_on(int led) +{ + phy_set_led(xlat(led), true); +} + +__EXPORT void led_off(int led) +{ + phy_set_led(xlat(led), false); +} + +__EXPORT void led_toggle(int led) +{ + phy_set_led(xlat(led), !phy_get_led(xlat(led))); +} + +#ifdef CONFIG_ARCH_LEDS +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: board_autoled_initialize + ****************************************************************************/ + +void board_autoled_initialize(void) +{ + led_init(); +} + +/**************************************************************************** + * Name: board_autoled_on + ****************************************************************************/ + +void board_autoled_on(int led) +{ + if (!nuttx_owns_leds) { + return; + } + + switch (led) { + default: + break; + + case LED_HEAPALLOCATE: + phy_set_led(BOARD_LED_BLUE, true); + break; + + case LED_IRQSENABLED: + phy_set_led(BOARD_LED_BLUE, false); + phy_set_led(BOARD_LED_GREEN, true); + break; + + case LED_STACKCREATED: + phy_set_led(BOARD_LED_GREEN, true); + phy_set_led(BOARD_LED_BLUE, true); + break; + + case LED_INIRQ: + phy_set_led(BOARD_LED_BLUE, true); + break; + + case LED_SIGNAL: + phy_set_led(BOARD_LED_GREEN, true); + break; + + case LED_ASSERTION: + phy_set_led(BOARD_LED_RED, true); + phy_set_led(BOARD_LED_BLUE, true); + break; + + case LED_PANIC: + phy_set_led(BOARD_LED_RED, true); + break; + + case LED_IDLE : /* IDLE */ + phy_set_led(BOARD_LED_RED, true); + break; + } +} + +/**************************************************************************** + * Name: board_autoled_off + ****************************************************************************/ + +void board_autoled_off(int led) +{ + if (!nuttx_owns_leds) { + return; + } + + switch (led) { + default: + break; + + case LED_SIGNAL: + phy_set_led(BOARD_LED_GREEN, false); + break; + + case LED_INIRQ: + phy_set_led(BOARD_LED_BLUE, false); + break; + + case LED_ASSERTION: + phy_set_led(BOARD_LED_RED, false); + phy_set_led(BOARD_LED_BLUE, false); + break; + + case LED_PANIC: + phy_set_led(BOARD_LED_RED, false); + break; + + case LED_IDLE : /* IDLE */ + phy_set_led(BOARD_LED_RED, false); + break; + } +} + +#endif /* CONFIG_ARCH_LEDS */ diff --git a/boards/holybro/kakuteh7mini/src/spi.cpp b/boards/holybro/kakuteh7mini/src/spi.cpp new file mode 100644 index 0000000000..1f47a54022 --- /dev/null +++ b/boards/holybro/kakuteh7mini/src/spi.cpp @@ -0,0 +1,52 @@ +/**************************************************************************** + * + * Copyright (C) 2020, 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include +#include + +constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(SPIDEV_MMCSD(0), SPI::CS{GPIO::PortA, GPIO::Pin4}) + }), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(DRV_OSD_DEVTYPE_ATXXXX, SPI::CS{GPIO::PortB, GPIO::Pin12}), + }), + initSPIBus(SPI::Bus::SPI4, { + // + +constexpr io_timers_t io_timers[MAX_IO_TIMERS] = { + initIOTimer(Timer::Timer3, DMA{DMA::Index1}), + initIOTimer(Timer::Timer2, DMA{DMA::Index1}), + initIOTimer(Timer::Timer5, DMA{DMA::Index1}), + initIOTimer(Timer::Timer8, DMA{DMA::Index1}), +}; + +constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = { + initIOTimerChannel(io_timers, {Timer::Timer3, Timer::Channel3}, {GPIO::PortB, GPIO::Pin0}), + initIOTimerChannel(io_timers, {Timer::Timer3, Timer::Channel4}, {GPIO::PortB, GPIO::Pin1}), + initIOTimerChannel(io_timers, {Timer::Timer2, Timer::Channel2}, {GPIO::PortB, GPIO::Pin3}), + initIOTimerChannel(io_timers, {Timer::Timer2, Timer::Channel3}, {GPIO::PortB, GPIO::Pin10}), + initIOTimerChannel(io_timers, {Timer::Timer5, Timer::Channel1}, {GPIO::PortA, GPIO::Pin0}), + initIOTimerChannel(io_timers, {Timer::Timer5, Timer::Channel3}, {GPIO::PortA, GPIO::Pin2}), + initIOTimerChannel(io_timers, {Timer::Timer8, Timer::Channel3}, {GPIO::PortC, GPIO::Pin8}), + initIOTimerChannel(io_timers, {Timer::Timer8, Timer::Channel4}, {GPIO::PortC, GPIO::Pin9}), +}; + +constexpr io_timers_channel_mapping_t io_timers_channel_mapping = + initIOTimerChannelMapping(io_timers, timer_io_channels); diff --git a/boards/holybro/kakuteh7mini/src/usb.c b/boards/holybro/kakuteh7mini/src/usb.c new file mode 100644 index 0000000000..6d42476b71 --- /dev/null +++ b/boards/holybro/kakuteh7mini/src/usb.c @@ -0,0 +1,105 @@ +/**************************************************************************** + * + * Copyright (C) 2016 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file px4fmu_usb.c + * + * Board-specific USB functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include "board_config.h" + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: stm32_usbinitialize + * + * Description: + * Called to setup USB-related GPIO pins for the PX4FMU board. + * + ************************************************************************************/ + +__EXPORT void stm32_usbinitialize(void) +{ + /* The OTG FS has an internal soft pull-up */ + + /* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */ + +#ifdef CONFIG_STM32H7_OTGFS + stm32_configgpio(GPIO_OTGFS_VBUS); +#endif +} + +/************************************************************************************ + * Name: stm32_usbsuspend + * + * Description: + * Board logic must provide the stm32_usbsuspend logic if the USBDEV driver is + * used. This function is called whenever the USB enters or leaves suspend mode. + * This is an opportunity for the board logic to shutdown clocks, power, etc. + * while the USB is suspended. + * + ************************************************************************************/ + +__EXPORT void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume) +{ + uinfo("resume: %d\n", resume); +} diff --git a/boards/holybro/kakuteh7v2/bootloader.px4board b/boards/holybro/kakuteh7v2/bootloader.px4board new file mode 100644 index 0000000000..19b6e662be --- /dev/null +++ b/boards/holybro/kakuteh7v2/bootloader.px4board @@ -0,0 +1,3 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_ROMFSROOT="" diff --git a/boards/holybro/kakuteh7v2/default.px4board b/boards/holybro/kakuteh7v2/default.px4board new file mode 100644 index 0000000000..6590eba150 --- /dev/null +++ b/boards/holybro/kakuteh7v2/default.px4board @@ -0,0 +1,96 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS3" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS0" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS1" +CONFIG_BOARD_SERIAL_RC="/dev/ttyS4" +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_COMMON_BAROMETERS=y +CONFIG_DRIVERS_BATT_SMBUS=y +CONFIG_DRIVERS_CAMERA_CAPTURE=y +CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y +CONFIG_COMMON_DISTANCE_SENSOR=y +CONFIG_DRIVERS_DSHOT=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20689=y +CONFIG_DRIVERS_IMU_INVENSENSE_MPU6000=y +CONFIG_DRIVERS_IMU_BOSCH_BMI270=y +CONFIG_COMMON_LIGHT=y +CONFIG_COMMON_MAGNETOMETER=y +CONFIG_COMMON_OPTICAL_FLOW=y +CONFIG_DRIVERS_OSD=y +CONFIG_DRIVERS_POWER_MONITOR_INA226=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_DRIVERS_RC_INPUT=y +CONFIG_DRIVERS_ROBOCLAW=y +CONFIG_DRIVERS_RPM=y +CONFIG_DRIVERS_SMART_BATTERY_BATMON=y +CONFIG_COMMON_TELEMETRY=y +CONFIG_DRIVERS_TONE_ALARM=y +CONFIG_MODULES_AIRSPEED_SELECTOR=y +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y +CONFIG_MODULES_BATTERY_STATUS=y +CONFIG_MODULES_CAMERA_FEEDBACK=y +CONFIG_MODULES_COMMANDER=y +CONFIG_MODULES_CONTROL_ALLOCATOR=y +CONFIG_MODULES_DATAMAN=y +CONFIG_MODULES_EKF2=y +CONFIG_MODULES_ESC_BATTERY=y +CONFIG_MODULES_EVENTS=y +CONFIG_MODULES_FLIGHT_MODE_MANAGER=y +CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y +CONFIG_MODULES_FW_POS_CONTROL_L1=y +CONFIG_MODULES_FW_RATE_CONTROL=y +CONFIG_MODULES_GIMBAL=y +CONFIG_MODULES_GYRO_CALIBRATION=y +CONFIG_MODULES_GYRO_FFT=y +CONFIG_MODULES_LAND_DETECTOR=y +CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y +CONFIG_MODULES_LOAD_MON=y +CONFIG_MODULES_LOGGER=y +CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MANUAL_CONTROL=y +CONFIG_MODULES_MAVLINK=y +CONFIG_MODULES_MC_ATT_CONTROL=y +CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y +CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y +CONFIG_MODULES_MC_POS_CONTROL=y +CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_MICRODDS_CLIENT=y +CONFIG_MODULES_NAVIGATOR=y +CONFIG_MODULES_RC_UPDATE=y +CONFIG_MODULES_ROVER_POS_CONTROL=y +CONFIG_MODULES_SENSORS=y +# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set +CONFIG_MODULES_SIMULATION_PWM_OUT_SIM=y +CONFIG_MODULES_SIMULATION_SENSOR_BARO_SIM=y +CONFIG_MODULES_SIMULATION_SENSOR_GPS_SIM=y +CONFIG_MODULES_SIMULATION_SENSOR_MAG_SIM=y +CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y +CONFIG_MODULES_TEMPERATURE_COMPENSATION=y +CONFIG_MODULES_VTOL_ATT_CONTROL=y +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_BL_UPDATE=y +CONFIG_SYSTEMCMDS_DMESG=y +CONFIG_SYSTEMCMDS_DUMPFILE=y +CONFIG_SYSTEMCMDS_GPIO=y +CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MFT=y +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_SD_BENCH=y +CONFIG_SYSTEMCMDS_SD_STRESS=y +CONFIG_SYSTEMCMDS_SYSTEM_TIME=y +CONFIG_SYSTEMCMDS_TOP=y +CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y +CONFIG_SYSTEMCMDS_TUNE_CONTROL=y +CONFIG_SYSTEMCMDS_UORB=y +CONFIG_SYSTEMCMDS_USB_CONNECTED=y +CONFIG_SYSTEMCMDS_VER=y +CONFIG_SYSTEMCMDS_WORK_QUEUE=y diff --git a/boards/holybro/kakuteh7v2/extras/holybro_kakuteh7v2_bootloader.bin b/boards/holybro/kakuteh7v2/extras/holybro_kakuteh7v2_bootloader.bin new file mode 100755 index 0000000000000000000000000000000000000000..7ed8c65c2f539aedb05b3a3ee7a5ea7db243885d GIT binary patch literal 41736 zcmeFZdwf$>x;MV|CCN@(XiAHOwm>cg+onipL5nzJNXqUdU<>FBml+KdJ-gI_3aF?I zq!hJ?&LB7rP#qC;h7lD~$Q%pSBH(Qt&q;uK3dNxUvY|!Yp@r;T^L|%Cof&oB-|zkJ z{o~E&v$NLTd+l|3*0Y}VTvzr8lA+;<@Mre_UH=aT|M%bkx$%o!IkNeezt(*4ZKuXN zTHzDfz_*?H>gU8IA{&y}Z#$D}Mk|KIgr@Od8=l#;Z~W!H(hBjcni(L<(w=>n`buMO z_gyhOdi9 z;-#sh^QiX$_X*XQVd<(`WKj}Xry`(DW5x+J9nabpITZ)))Bkbb zyy>6rO&N1aj;fg%v+-|_{^w_~*GTSB)saz(XE`WOGMvip*+-Gm{3fQinWjldo#+wC z*i7h?giNQRMGFa;=w+s}%i%<8(flU9xA``dFo02ec9l>1^2nJdz4r3UZL-4`DlH^@cpoVst;vv)TKC3t}KJy zS5-)7tB$+#TJBz^j+vSzlP2O|=N7$ZCc{i7@+iycCa?@sD>M;eBC`$ttQo|_SWenKpI<#(%wO)OrbzN}~-_md1M@r1Rd!+Y7Jze3E@E@ad zLBF;8Z}fd^?CU3Qe9cMLc7{1$ALn8{^c#EQx_!hl{p!Mn`>s8{Hok>e9M^X;i!+mu zi_0|PpB$pI&TMAtHagiac3gYB>avWP&ttY+lVeYrFBa#2b>rFf=(966=8L%ezyHg# z;dsX6|Ic^N25AJPBg6vi0tHDK4-5p>rHmhZOXuM>f85Lj(I&+=q>EV|W)o4asaX=U zl_<5nDGkJ$BiTGAk=tj-Jn~VvrPR0-o~!9hsf|&oNuW%0O-@1^wP>RXYpQlUtvfN( z;Gg)#j*x*LlM{D_3wd1JwS}&D4`bze_^uQWF{QWNnIp8M&#>pG7bHZV6UE@UD&LpG zw-{%XMW1%cYjHPxMpg9giu}7A_Ef17u^SqbW^9T+J0~05d}h3d3zmY)C3}v`noJgX zlBderXwGZxloK=02X$`RI&h_l|B|!`xDs#~aAkarv<=VvbX2Bd+QI^fQxduP>0Bt^ zx-hSfOjG&zjn3X6EnzIiIvZ80cqf7yjRHNyWg0byNiKv!7`{dt~*AgcGOMof?)yG;xV3BS%{^Nz>j= zPzOJAfGE7myh|Em_k6ael_=cMg`G7+A9tU)G*qF_2d^ZRnM!c&3^DA#%a6aTjLFQ_ zwfZipV|L)$8Omfcv%{y)sxN(6+P*pd@0a40St>u(fooiw?~;-1z-2}*AGv&zj@&Bb zR*@0NsmQ5h7|k)rF+*sMLyi+@PKTV1qq#)n5*sfmV_57jBpbxKaj=KX?f?ERHtzqE z@nvRDZlz;Hn`2BWJGHbu*|oTHjH|RgrQrU~F$JaV!>2#cIc9ok`%U71bdC{A+eZ{i zons11+l@sPonwkh+ea2J=^RsB+MZfc**T`9v^~xJVCNV&?SE-|`V7?hX$)qcou=dA zvwz=8;WgM6O?QTP_MkjCzW?29@Y#{;tM=0G>Aaz@*1>vW^T@MUm&6oD1UoLl&nW-d z%V4DdCrpB$C`6_I;Fq#p0fWLMa^jQ#b33OpX-szhDaAjnof-QF-COmx0DHStWfySS z##6u0O>ftTH#MbJ@gZ1z;_ zd)7CyN@;wvgVhPQe{`8Va9-xJKW|ZFCi~;y$SS6dWcLQE3bCUo`ge#4saX&iCH^3< zMBa^DVTu^#=LboXkiZg33-6f0Snrtmtc4F%X>C$xArpdUv+lh$peDiDeAd zEQeZ&a#+Ybm>WkaqR}SpRS0N(bFA55b%w|1Pd$xOU@u2iI%3CZ`agriKV#rc?R` znh~BC(YQ;y@)s>OlNWgO_mpZ9DSabNGk-?}3H8x(vWjN`@ILwJe zXhT0=&lLO#E&7t#s@=Z8$95cMXG>cPweD02mq%zM7LEJ5oGQ|9X!*gh((eVzGyDi` zy%n@I60RMnL4P&wG|A@Elnj%GHKabvbp&LV$+`?*5vhZC^e@c#w!t%p^=$dSD(C|*scY?l^Mm~6Osq(-G{oX{7>!Eh|ija zil1HPb2%lb@?yY8%pcZp&nIkSpO4?hJRi613%n!GXNg3~Q#GSCr@pK1c3)Ma zN&HG8%A?(3d6^Qg>5|9zwJ(v)aj1`(M64r$?Vm+>%;6Zn?j`mG?Mt6sCX# zUso)~m7wg4L`hcp(dSwdwok)bqU^w3YqZ8+cT-FRHGG;hjY=lM_{-r^UK}bio41LW zk*9ySd!AMMcHr`!R!JE#wW!3-OwKP!l4_kpS`E@+tlBhnQi(<+nbU)j`lo?=fFGIC z#*9yr-f3dSGE=e~b5!#_uA_SBp%6)&5d1HdI3o35dH#wTX?dpZdi~3)-84_+OygG4 zUFU(7pd?eyZ?r1qT(XM~6|m=PKs(X*^=kS)M!B~q68?B#1Skxryag&^eKZmd49o?e zhmrl3natSVmXoAXg(M7d7n3AUs)T+Mw&VEncRLm(@~y1mi+qgMUy#ex9Fh=X5>lo2 zCFb@x$8^;~WZ*wt-!I{!>zl6gNp1=$l9kf5LEI+lM3OOFEEbKHDm$k<*;^z&t=2j? zWmT`3KJ!qH$WF=^wH@mjMtQDVi+yXoKr#ygq(g|?9TJ57BL9=a*p7?P`}=x%abH2P z$V@83-K5^T3tFb91&OIdR2FGYSI0?fImcVmLW2*I~^X z24#Wo$uW#_cXw><85;Bb$?kON{6sBNd!GEEibDKpY*4>_76EOcpm9A-n z-)QD>_pNI3?UGt04xF|D)GQv zRDdXT=nM9cZd#L8`2h8BN)!HB<;8&xnJ8Ni5#_~0kt@W?m+-|-HP680{n6GFjD#Hr zDcz*>GT+aO#{Gw-eLk!zDye->)=F@bv1t-Bm2<=w5y_!i%VKicuF7N|g~WZ>4I<$; zBec~81H_b%y<>3CQT-#tk!~$^hEY-^T+pBBt9)>(=Ok^ZhFGFBb=~s6P?>NYBvSU4 zqyTqIN`Sc~CAj24vlQnd8A}$oMx{ccBtd%4>o8)+e~7H6p~rxw~?>wsT^w(^BWC*?unY%{_;Jn9$L&QWsyO= zW^D;UwD%)Yyr#HaW^7*v8!s6gtWpQq+w~+P6IVWVWG2H5TxOLEkV}|JrkpS&s>meT zGM#l2nxUa@?}(H&@S(e-`CB9T4Jea)>~a#FN#*T6@QTWwK1GgwVThKpMlu_WsE1WX zM~n@$+`8VYawPowNS6#gdD;+Bzls;L_7~V$<&P02Qzx#W{imG4_r_^md%a$38y3u3 z#fORrg^tH(lT70I`zs~u1H`&(F;NyrqI|-*18p>YP0Ct@x+tbn*h8a1VNW$0yDE}> z#>Z-%LhyJ!)pF$dvZeW+CDL|Jo17*o^3SkO_xkQBVwIx;{luG@>EJS zp6HDJpoctW@#}h4*)Tw7?U4NA<8XklVU^zxM0=Xq95k3% zRXadGKI+wC2g(UJq@w*qIo?Y$-GLS8LA|uj(<#+y`IA|z80$tyva7~pla8Wi+`=Dk zlMl^cXsdS&Sg^nR15T0fE4}!l_Q#;DwF$xR!}q|zeLb~mE&XDG5MWMYMQQCK_NQu4 zGVTA!*0mZN7v#Z56J=PBb*;|!ZP2z#uw4n#Cm(iSk}1zLh<6_tiJNcczco z#_jv1%6CMvhrnv`o}&rgdtzQy5Nwr{~Uvjr=H1{v2HLQ@dp#(;=-TuVZLfRs~U4xeCdZuksn#6CO zCZ5}97cFS@T+N~f?{Tbak7tKFo@|dt9jsE@uc?bsDDYPOcEDqJ?+!@&kt-VTkm;a$ zpY%lG)=gJvBz*iJrBi~Rbu*?kye;XD-Fqby)`|i6c(MtaMo9U@DwGTZ#jXY=VLOg; zb5|X95ru(_!{o)=LfEOQ3SYy%nD2j5nh$+lDlJW_og<^AH6auEh4gNew)_we=-Ugx zoR9jceh>U^8?E#2L78IrlRqraDzEqdgKCi`+AikM^^}A?aAv8(>P@%q5TD#<2bACF zuas8!CahxqOe~+N37hgMUkWWDQJ(6aBGM9T`=%$cSPFmG-8fFjqYoZW=-8P<=g09}1B%-|EH@KVFeOzrC&zuBh@>02nN?%ol27e@cH3EMA^kOHg2z|UK zwpSP$(W4*H;Bue}+92~_d@YlgRv+&rR&H^OGoU;RzF7~LP+kJ7iBit!4!>5Dtq*D* z%&ui}ht+3$iDlY)78HKPjlShKMy{xkH#6N?cX>=lFOz~yH~qi)#{Y-N(|cCArAH$c z`AEp4%Xv6cG)|rfyn3g1uQMFR&r;;x$?Hi^bbw8Wo!Uq=ZCN({YG z!l5BI8}7P!g-J9-OT749iT>_b9S+nX{8Nc1qb1Zv1NK-WE)%Xn+V}6E&lS_vCiT+Q zMrCXIE#=aOi)AHausgyUv2Uo@stP;!Q*<_Bobhh?uU*#MTvz zhaMsnh=jlCCz)RdFUnKIxmXil^xHuN7|REjiIoM1cu~&rl|!ezORDsom#)irUht-; zRh}$C-U((yf}>xx^=rf@8%Rz7ymDN~zyNmODI?+Sk&k3ziIw@U{;%bel#w%K-P;-6_SqcsR-d zqLvM@S~h^vl293I5>qBYTD7vu@gA~0uJ{EfQ`Z%Y)k@nZlsebOzNtk{5Z{mT=oFsB zdYgZp#L%7N$?r?+FI40-#8wKkm1@|{k1@MhWomDRbQOHf5a@UvzJYL8B%>n5XI;I= z$pr_({orWx8#AP9a^%V|(DF3N`W<=BB*eY>2A zL93MJ+b+E-#T9J#%=N{A_q+oX&f^)RE=Dm;x-9b;{kWd1l;b8La8!}M=L7Ef{sd9y z)`)Y|i3YCY7?<`~tBz~NKB(qAn%~GOH}~|&nV|0)H@nv0pQGj)=)Ru|YHEFyt_Awe z;E$Ks)%$(vt6y+xkI$DDH5|T7a_4#kPjXut=7`=IJVlVM653+Ie6G0hP zTiR|&PB=*|8CqzrtfpMZe2Ijyj`VS`9dB9)8f)*CkD(6iorM}b)qs%_KvaNVSb>%661Kh)Je zl$)Vy?WOHSdd~W}O4_(Au1Ak|+|Xt1TmpkUEtBV28sNJedFaobl>zkf&V(k`O7KNO z8!eyEJ~Ucar^nk6Z98tT?F^I2R7c9tZd|lYj;oo)gd#tEFT^AcX+fR$`>&B@u7LlV zW0^DX`Ze#8(;=dn-VvwSFKK%8*BBEmnSj2K9zmJjo}lrA!c1s+T{FI!b1_*fJjsFc znxtl4N#IM{Yq>-dAKF*IDGA(3RY=+Dc*S9ECrqO2m@2G{NeRRR4&-UpJWuA#KbNz_ zdt_o-BNOXZnX$eovty6Q+}OjiwViPP9mRaAbD?v1xIPk2Yl>Wn&ATBw?*g57`y5|t zMQrBjyc?V;O-3=QU&LM;`&RJt%H$qi$-_0VXNcP<85_?gw;b zKyJJwICRPr4KbS!qfOF<9weuhMt2)Y1qf(8Z96Tpe>)usaKSS_a zsl@yvdReW~qgthwr9@$)Iu%BQEy7RELTc!R{j0=l6^fau)W+8v)IvudwOJ{T^ioJL z%6sn;_+otf-dles_~rZKW{d?K9*fw00>*3cwLf5qjyJ#2DruslZM>9^u}+MRm5!70 zkKRg?@BGGAo@r*3Zt#|{OV5EG&wA|g(7f_;s#nrYtf}Nhj9c@B&sDj{IrP-g%50H< zOB@JS_MWTc#O#7|<%=q3r!6WsV&5RvUYVmhSJOywa5RxCR7)N|V;uPYRsETwM)`VR zu%B0N^s@^6)ZXZ473764zSBupVLr6t5HUrx`rhBjC>#?BzZ%IY$P_aa9ltQ`!^=bO zc?dJY!A4jGK5CQy*r=L)^c~r25g%)qGOEFu*))ZYfg72sbDL3vx!@TmA56HdQQXwX z6oh0zdFnObH-#sqrTKm`_I-&Lr$6xI10OuFu)zpSbzJ?%y5s6Mrt)$?hfruj#%Xd? zp0s3%J3=S9wn}3`+XDmPwx08q3oFkpjW0;{lvkWvYK7m#5p}&CJjC+zOJTV-EmLk{ z&oAA+NU_lSzm=U|Iv*T5rKJYHYbh^shte>I>zt#4sWYrnas%CSg~DB-ljO+dk;x{{ zNy|t;|M%Tii3^>CEp}LmRXVD!H+e1d!Ml2w(>4r^(;hf!d4Fm31Fzyc+OM_U=aFFzOrgGH~=n@s{>q}#n;_{^?F(xJcP(C8oOtJg{%*Q}X4`uML zU&?f~tk|KJ85o6OERvRGpRi$ozY()GND$}T_7KSaWR|4#R#_>WdbOZ`A! zwry#g1ivN1DvU<({QXz=EqzE}Itm$I@Ac5)tlLj96Ct%aiUkwOY+q{dY+oAmY*Bw^ zu$C}cY&&7|g3hkPi?*mgC;Ch_AY|HukL{VphDYg+W# zJWn-!_RwhGz02rx??cs|Y**dV3-zfU%IzqP?{_*`vy#X-(6*k}CnUKJ%WRg=gqjz9 zEbmAAzH4pEa1XDYPk?I!mm}XEt=zt}Y{`Cgw#m`XIuF;!d5C2f?x<99I5zqp(87Ic znSF}SrI?rtpv3bdG2YtRwdo-ije?!vu6ZeP=99c^)WVhNIaC# zRN6&WfJ<<_pw2ZIM5>i{n2DL*&oj_FX#S_sdzWJ8ws^k$6{CC*A*NIENb!Q4yMGJ( zBJ7Mk%N!&&=Jdj4mwm~obwnV!i>ys)V!-FGM{8RogLt&lo4j6~i+pS1dUc+iQQnWV z-Jgk_;OxNXGD;abqPy3dkPwemzMy(YfA}{MVxhUDsN_5lPCuCM|DaM^oGtziPY1#& zkdcfHG~XY7Hp&fGXVbNrB5q!KqN1*TtM)Z@wmrG|0I+<6HnEv%pC2ALfM=;%Eq!WF z)3VX0lb{ziG$t!U6Tb}%wI(mRf?gc0OcA%Br#4?rWnF~@7O<^_R?#TJerE0l&z+Bc z60=6!wREJ1;>T$BcF-z7)iSE6KYX>nZD}ODpx3l45}w=ZStj@*;k$cxE~RjuBU!tC zJxA|@hG5PdPo}tYX_7lhigzS?==*yNSb<$_<#ZLg+NYIo@kGKi2X-zcrew*b5cpjU z;gUU!^)0V^*6;5pi59VCQo0Me-n@jyqPVfqmz0sxf@pYsrJm{?DFO6GbXP z_4mhYOdaU^5cFLW%!NI`YmP_nxAgB^nhZKXTcYDQ6&=Ut7>Ciy!Q}wXyZ12s3Wk$#e1$kHqFMvN$a8YcH6PXI= zVtwukZXws;T1c$!h2?8ZYmRmv)|eXFB(mDmd6X-E}Zanb?xUITgFEWRx9p;6rKLXydywyt&ohSrso!(OKF7Inx3yTnnkO4610NE3l5@oM&PdFX!CDUjfYe2Ds7)JqH&&$A8~RI zm*kGMWrOzgySCb&#hk|4JM=H@ku{7}TTC(@XdA9I&(yYR zKUqb0Tc#O0+@?tQIq-bb*05_n1;6hcHls;!>)ji4$pzpx;CEOQrE94k*P3O3qU(_; z-`5na6MbitwJ@6Ap!wgkvpi+Iw)zvFHp|iiIkATcWjTV77+(?-dbscu=X-B>^1+={?zk3b! zOIV;wIBBVv-(X#nTC{)}Id_^u7LF_{U$dxl9WcMDF<3hiJZ>Z`!D@ECcGz9>M<*3n z*7$lmL$evTYtuYe`}?kV_z-&>oNLQ0>zW13{JBnrcg{y0)JL!U5$hUaI~}ZCt+g!b z)FoH01h!(n4HRE9*X`4O0qs~ZpBE=>buycK&QM=OVh|j5KL>s=byQmFHYYh_2k&>Z zQ#Y1X`8!yZ`vvmTah0@ry&TI%t`EKL@%o zM0ELFYg$3i$r>*>&T|oKMH(zV)W@J3(h`N*XZ>Bkv>RCcJ^RiwnK^TaFHl8usA z*Vnvp?$`Aczc!YcqP^P^F|Dx`GZs*f{B;hKXl-XXN(DB#-?#x2+yuD)^$$yjMB(t{I@(yH8~i2y62i~pnba0FDe^( z?2+*P2yZE0O)HFnG=T^VE%T^SEVSC`t@ zS)112#yW9u?niPrT&sP*7u6=wRp!7dGp`xxE@6((?MUBTU*EASG4AwDC5-jBI!V}wm3c&+l>d=DO6-vH zHe`zvv7e1Bh=lKn{9}z;8=hFLiLTHmR_1Q#tQ}tPR{b95G;zImWc zGGZ4ti>>g38nEWvHNKRLg=`&Hs-)}Bue3g_@6{C9#gE{tLGcagPG)i>JZfO8`j}lC zI!2;csLbF@x24}&i)WE=BHsOEqHequPiv9;A~@VlEx*J(yomb!mnzIKc-v;iTj0r@ z32K8#0Fr6x8dwBslj*QnpRsmTfs5if>pIqIT^vZ{=3zdP)yev@YjX4m^`PfKWylxWutnA=nA7VDgYm%yxEQQ z<6Ogu$Fza8g0>RCTT9@mZT*30?l9JM6AX9N1`+mefW3ODhpg>(sy=}NVqwo zM(x?I)sRLNg)`Y4$~rH(w)x>@ipWsz`oVsh<3k=22|v*P0^msZ{q;y+0F-0-$LxR= z<}yu_HpJECm}6a6{_AALlc)*sq2Zt&umbi@X*-~fs5+pIG#pUVOo=^D54@n>bXcgr zss7ziLgKiV#H)fZSItpK)8t!;@GY+CxC(EJrC%Y<%8y+Wk(T26Y;x?HiE^gZqK(e5IiM~An8=i#)r4_c1GhMA~^t&lBk zui<7idGaXFVl7xeIZ>|sQP}z)1#cOJ$I*VD{2!10>Cx*l?fIXYN%reO+8S#c5hN4V zc9ri%twfu7=kTii!G2ZHb4i=NOUGBXMzAD_5t+4ImpMz9%v-L?8jB(? z`|d5}qdVk@=+nf#ofl7Q*LIEHTh^}S)^=s>T|42<>*My8ucn%1)^=x?X|JW^%U{E0 zg!qYpa#uT(d<6N?Zp$;t4l4DmTVY+dR>d@q0hJXkJ~4TD5!sj(cwJz0hN>?TWn3 zVBCFgzjx8HdKTV>-=-~gy&Y>&d3z8q2f9h-+O8{ao1cl)>q_DrV|>mocDZHUxr>nI zm-onn`$Vr2iG*>1c9lN4@2tA7zEw?ew5moEqKNW<^DC^56o*y0Tn@?03h8VGT@RnC zDX_Wj@s8yZn^9J3#SS^zachD@dDDyg{D^sLNm>FTr<9ky4{*bpUj$cboQr6w%;AnV z>Y2?X+o9xC;~7eBDygfdC5Kg$oMERomN4dLXHqJfDGpN6{;<-0Niz9ZX?8`NFH781 zXx79v*A#yorL1WcoNp^Ui`POrJJWqmJFoe1_oyWrXw4f!+QivsG+oND`lvSk#p?Q~ zHhu?E?}X{W4?5$9lkw96XVt^i>EpD$`lmiuCp|=CQt@RdLuq)KJKaf?it0%Cbl(lz`fpowOtV2s@+=Tc zxpQT5vpBO;Hw|%LrR{|y=c$|tGnLP_bAl0`-ecgcGPPs`_LX9lwO6ioV$~^}$Ceyy zRwtR~ANn2Ipgi-FNNFDX9ixG7)PMF=LPDT?uTfzWxd3a;E}=CT6(KP`P#GPy3D$f{ zF@N4KSS``Fj|jRDqx;XGL7pXAv1|NUrFEL%DZh?;73-L`f<3I~6FG+Yfnd6c^u{Tg z4ia}7{fWEw(&M=e+f|L7_URdI@dRr-$EN})r2`e+IGadp2bf1xAI zF^F8aNck?hN722B?o-6zC3KHEtA1P!?Tr4cdhFHRup~2eExKPdSHgF{IPJJ<$OqRU zF_r~tTov1+znbf0HybI>#f@={0B&Y3

KOox02)9lsHL;9Ifw5)Ubusy3GXBh#Hp_FzoN)&*4_(_eHz9b2~X%XM4#kqhc=%PpW zdZQ(nI_iHZgwllt0<-0*;Jk}5xSxpjF247_Wm@0wyN_JV-!M=+Rk}6RSn#%UchfE6 zn4YK9Y0-DP)8I?lJ!L~}!JkyY`$tyIfk=S-$963`kYeK_; z?N4<7u6sEAsfAX*ng)$f#}8*;=T9{7J0mp&`m|eAv_% zP90$2R}l&SMy32rCwMZ74^=mOGQ8u6H6}q*36o0D;LCbryxq{5kTs+%aRF+L$wB{G z@?q13`o8ez{lHArf3GTtm!sO!D)2|KHf%F5NmPgRapcEk6JoYtg-lZ)HYJEz%lpFn z`X4r#MT^*vXQSK^6Xo2l*CSF&k1HPWphz>379lM{Dj}7S)*`J%x*O?kq#q;wI39W_ z4>&yVeJQvm;Y!D4%tX8*ye?bR%GHa3ZIvtO$OrpOVf)(>)qh+~$hRtF1#llTkbrfO zkUx*JLwC5Hn6^!6*&=nY=ZXz zr|L-~W~XPsa#+n_j;v%KX2pbFhELD^*3KD(DXB(HNJ1@MrG3XILtZgVx5RZ8Ekm?C%9!^HNN^V3m0THm+QgOF7J0Q4ioXhw``QJw$mK zdJA$dqDAxRELefLxX-8eE_gB1Jao<0@R8!{OQgTe4H1`;G%khX$O`mI&OnD^p0CpeHKa)!}CAd_>FI=iI zeT!2Gjzuh8#FSB-PD|xPOBIj|l$wE3IVhE|5v4}BQ;N1Pq_{WVGp{1G#H_5HX6{XWvkg0dKshZ}&&=pZMqdG}u}H`M&DLy&Clyxe2~G z^Hoh~47X5vW27+F3jT~4M7X{}ZMyNyO@k9T6#j1n)Ta@Te~ z(ifi4&%uY+#FA?}9@-f)knIZ%xrV%!h0nmpiFz>W5HDguEJq~#rwH{or=D;`+13!b zVsPIE-$ac#!v`-t<&AD;>!9Bu)#dW=e;}C&h{E8NmEZt@<<#>?8BjTi*gDUIWs}$* z_4#CBV~%e?q*6mwVa#T%5d|p$Z4XzS>6#99820Mndn&BHky4V+Bvm^#CtmOzgLgaU zI|Ex^s#piV+j##!KaMI7Qy>y z&}+O|b(mZ0EuIfl$24lGt7^O(`m@&nYs&c*Ab&R?{8wX{^r0^KWOSdc28~&6Pp%(n{5kKIPdz|hVOC#D2Vpw0%=d(z=q&J*dC(cP*Jn_DB(rJ%XnwyrG znQ+QluyCYj)=bY>&)_)h;(SjeJULP>3DKx^3JY3u25dQM<5*}D>|ze?XGGowv_F&6 z#s5MyE9F98iBR1vd>_qy;XX)GG(L!~oNYaVFU5|PQ`9FPz`B7}(-RNNtN`DfszSG9 z^z_K^VvR4poGmwJ&CqBdxG%ie5b0ALljo3#Cq5UCoF!c z9-~-Q7$xsRmZMr3wCl0Y&v389f#DQ5|=n3aZZlJzE zRfW`dsH!jkzoFcQcu!tKiHBIJr%+X4vL`Ih$ECx*4lm`+M7fTCQjoCD=C^c*zer z{X12J#({-oA%aK$WPq0zJ86r5Q7_1R!L6x@K61-fXDTbk6?7~-;5-@VsF&dr`Vi>p0IQmos9UIke%EQkB8}bM7cxdL#xR#)mw&G zZR9^j{%>V5zmi+veawqF_WtnC;o*kecH&7jW!A3{Uy%$VRhHz?O@+; zLEq^-bBGz!i05Hz7zPcK0AGGi*)+h7)}4-o(+4<-w_OW7YwHXwaM~Ie>sKD8&H4|Efy#NaDxDLB_p+f)LotaoGWR+RgEz!Y5xY}spdZevrjSQA~H+(b|WH;rfi zeGw6Ui_3jC5uBrG%ropCcM@AS;($5P;NR?vD-htd$=El+SCd#wVrrGX&7d+f5T)!U zu!1^LnS9VJ5znR)TW=707co_2@{(4Ote&7DmHGm(1-$4Uy~!yAl2v95Vx!YYRya5; zwJ*F5F;vtZH56Qs-GAaTx%*krun!j2K)UeGp*!+OS2b3xz46aZHu(NRvlx<@Tp@2v z_9fhbkDGN_UJu^la0OPZAN$&$VHt`6o1jJUhvOV6q~ z-UAp7W3_pxb`3kw)I-i|CtTAzk8dpu$?s5li?e2;7Z@X&>inQzmU#GRQNN#m#8&(k z5vF9^7piB|7HEX227)Nx)*K9tO+m{zOEggs6^{nOM-}E%>CKSZdk|6qYYgc)mq0z#4DHzkxq4AGk)=Hk>#ZdOR^3 z?X66_8UITX&B2T=@8Z|$3%X2;56cHb*@=n4HqAW1l5MuOpI-$HzwJ*=w7+A-`tssw)FjNK9$udMWF;Zd6`(DpH1Sr>ddQh@+N}Fi2dC4Yd()YAJCC_~s06l-#E0AoO`F86;vYK%y}m=x z8nJ3B8txEDjzf88-A@ZniEQ1mi>1nV{So;7+4N0vg3mPNlX{y(Tlq>pTK|P=+QDq` zxS8$g&a_~%DEyhvi!Wfdjr0LiM%uG-0;wTB@s`%j({oN!tf6CPTba7;(=$)Up_feE zt`0%4Lfgk}`AbuFd;1EFmC2*MCcy1?kd6olIIAML;E?k`QzVt#oTBIx(}Pn*=%Tk3 zjeftLdHy3v1zaeS--Tg`g0oMfUypqpBHSAtdX~pIP_!(+ZM*ZfCSgyhVi!&rx2PKL ziu4WBIjqL?(-)wxX$VE`E`?4vqR^I zZ=D$J;VJF-=D&tW+<0?rzU}6NA${W5GqF1!y^agnnVmATo#Ix>^x7tUD4(TTrx%*+ z<|PHtmvz@R4O5fTw-$aRx51jj-1?zRTW5iZ@5?sxahaL;Mkp@sllq|iDOL+33<;U^ zSW8o%Lu9W-m)H!f&anl?UmH!)nuiDxOk#!vbhlKnv!PtSrT#*PeDbi*dKG zV;VCC{T_)mtPN1Oalx{7hQq00Ug6C+s|B=qayOrLp`MmtULeXl-YkWQE0{h8)X#hM zfVbePfd$>`)VH(HlX1Z;B}tDOdr~pqi2m?`cVL$k3{ywu*rDd@*bM7OqmD(=z%M)9TXZ4bSCa*?L4*{Zpx z$Jf0vwLc;o&hZ#1})9j!cPYKd&kzopMGT*6QePZ z<1ec&_y<0gE>amZy~&Ebh_2z`Vrlz@Rd&SvSVR{%LjQn1`+zt#T1tP$p8MK#4Y=tQ z9Rha+GAFbUHI$|?f!lIJ^3QX>g&j0pYWQYTi4JElq$s2yy~+2DBc93d<3IF`p(iP5 zZvS$*#%?#${kk{Y)PXDF+%GVA&`!HJbFPq`SYE2x=m|pq%j>_l1HaubaM)w5XM!(QBBYL54 z-#3NvuXiM9n5n{){h)*$!Cc?i_ODhjd8q|2LPvd*Xm@iRA97p1CQsPE(Z#>=bvA1F zC|Wg^g|iGce=%EJ zAd;=cfMe1C3E30o(MTc?xqMUrA@89n?-#Xau`wIqpeI>X{ zjzoXot|esBxNDUOF5+#w?XjcNTyu92VH4=K0V(l*_}ZH1ID#|9;Qyy1l=mJI(h+du zkn2b$e+GO6;kN_yIpG-irErEiUGb1J4bP%T>(^e8v= z^P?}FzA78Adi~vY(7?OUuOX*tNvebRYd6VhIsM`dsqZPILHYB`+OqMfrtwU0%!Kro zMkjW*rk{w};-5M+`rh!ap3aK;3ZX;8+X}WgI~VfD8$x3eQ%;qNx;A2_bvB@duG61k z#orFulJ?>RWXlg)`GVK5CmfY2K6eMqVu`4r9vyJ>G1TcU=v??sRNAsP48YIz#(3<8 zMps2AG*3V4HZ56HvB`F^d?z|@SqyY2k%RPHF}*u&RkG5LlQ!vBLgP_g4`lG05m|n{ zm2}yjwO=}gI}28fiyP{y4Ww#8@?63VvyC6;g4Ppo+lLSkv~Sc!zeF6 zhapFHgiPFA=iF&R;|h#}d7F$~Y>I?i@vW|vnXFZrcz*n~AX7(7Ru43=kefL4h_%rk z#vP(Q7qo4IKM7v^Ox~!#A}JC6up>_;y6fP%Ps$NTmABoQbevNv2RP-n0irbaaLP2~ z_3#Vil*jQ;rA|&UHz8t-utFrl2yf!-O#{wei=S~fVy!Gb`p?{6K>zc2L`iDVhrEVe zlC^;~amv+!T~?OpVTb+>p@t5{b$v#oryl20#kKXRW<|4xBgWX$~NF!76x@69JtwsGiPAGB1+CW0@%!qaGNL} zcomvcPT4WQ)tRRnY?oS?orn*D4~Kp(V#;tmjqA5R|Hn0QUM&BgNFSb0gk8AK*u*x{)S!fy@%I(!~Ae3{_qvr?cxbLeTj+1#D%7SX!NVH_w=p0E;;67^9_B&sg}=jV*Z7IUJk_^@5o#Q>Y>? zbr|$`1DM&ZB=hjqoNA=onC%SkpUM4PZdHka_UR5pt8>bo{ouE3o16}%!^=-u3V(^q4a4s9lb#lc3?lDYiS>@1Gp%SI5rFy*2v&mfhK4-zaDaZQ;@q-;01zl{c^rdL&tCLZ{Cq(4if zc={eK4V&=Fis|#;_SkGw8ZrG_;{T6q?Gl=%FphbJN?eCJ#Pa$lPPw_aPz7D?yq8N@BV(pw<3K= zkYcZ9H*1c)f24o0DW39YrRZZLT`_4~noS>^h{!;!2zYC7%GkcAoaY0_!5t3E`$x_5 z`VkMk<*#2wzy2+O_j&AmWr8%{pXg4UK{5qI7^0T$A;l&Sr`*zwGkbhSx+~|#&vcoM zQnrUx{*IkS)0*iqNmNrg5PtGteq$|S1kPZ-Ujq)bAYB!usr(ERY#TcU@O_>&N;B#6 zWPc*)H?bw29_prb;`EK^u7^@~Y9G+MO&+5(5T4h=Sip-gCY1QQ%4B{6$xsImM_LDM zgSN-IDW5>=U>oRj*nh9{%LQ?Z)8J1IAVPQf7U(WZuQN!;1$)ChTH?hqIGJD|e5g03 zNjJ7}O1ST;9KVXOX@WCWF*%w5nNmd%u+3IHkgf5gmsmgPzW0 zcU2-n^7+;!TMMCg&kP6X{emU*^fH6PSV%eN>CfWytnP94T5)_m zjiSeyvy=BYTbjt&UC*Y$2Q@BG{_ObrolB?<0lwef``5?#W$dapJ5JC}dInZ6oP|PT z?&(=5v9Cr4&#hvWi2hsyW8R527glXv0_{a86)|D=_3lKxV6nJ)i3z?=RMYkhzCFLH zyn)_ z(bzP^66G~)hvgwSJDTc_jZJazr6c2;f|S}3n>&EB)5?Y`4oxvVZRi;<_16H*sFjVv zY-c6)SErEJT(PO}%jYb=(W3{?J8ur|jQ0OupRxf8uMP6KRXd@*Pe&{eyN^{4M4zdU`^ulk{|LTK4*Q3~mh8}xP?_fE zD(`+4XGiRWzk`v>LoCfMj~kX^Qw97vdMHM+XG~ZZD|_}jF9m5NF_ji-q@m()k>bl} z!GXSv>Cuh8tCfr|g$=^N66LfJ0-0MIyyNR{3cU#h12*T0!kc+$a}459Iu+ExzfBQ4@Dn9dIg@~kn5(C46(gXDYQdQaL$Wl%RGsD|)p(rjzbwaP0ne2T z_|px6CIB*1f1Fv7f#W8hxTi8xT&FmMx+NcXdW}^32U$HhNwl{8bA55BFYcrh8p25U zT}WhxR;o|9gqpjeH8a6D^S827jMcvfC9a?LO=ph(u|v-+7h zqa$3^{VI3|YVW78A|aDN&t;?#9owxLJ*pL>fBJ`bd!E8bGiQJKvr&q%J{BD}x|EFn zAC+v%IZeNJcT>%S@^ozNtcP8Kwn=ARRd*rgj+pB}&7id#f3|hev%7BePY$*~zsbab z?&&+Dyc(+dNr*;1i#t2$-9Y&C zjWVNA2Hrg8e2l{1MDXt*oW%AMjCUHN{IABo1-ywW?f*B zB~UH}MANjL7K#gktL3()l**JM4F#pH?nf$RErQAdyUR@lii+a8O`$Fouqf&(etagN zyFkIMq8p*emvWnFQ=0GhPKxk-%l@B#o@a7q=A1Kg=3L+NUViVuCNI_!lKIgdFSDz* zauRgYXx^!H(JG<|961=qI?^IOrK`9*K8_W4$NvfOuo3Sbg_wIwuQk4Rga(9>sx|A&EnD); zZ3AM?d5O+@&Ha1}+O_Im+tu>Yg%`v9B=Xz^%&GC1Q@6(*R}t9KAtNhmvAooR6qwy@ ztiD%VfJVnXp0j&>ad&(2=Ze?w#;SPMzk9tC_b@Xc=ePeR-<^J-KU=$g0rJuNxk*R- z3)b)aId`wP>y-ZTCaM0dSYB+9iPk%*Pbq3FNn3(Avrq~wk!Li z-*MHvjQ01>b-5a$2{&9rXlbn7Ukl2p{p{<`-=ZgmGHRXA`Kj)Rt4MaYv5f46Uv)%z z--EUAcRn`!0yv2mfbC{2YL$wM$}lILg~T-7X)P^-wS22{fj`VMxObVW&_|EC);7kX zFIa9BS6V%^=9jp*z}unP%JiRm4j0(nUGlJOmnFu*GWbG>`gH>10^a~ZgnBp^IHR5o z!`WAmO%DPmiFzFuIMGCXN~g~2v5+Klf#Q%A6qg=IlvWpAFN3`@Y9f(FS)z+*fOKgt zQ0zbr7g0_9bPL~b58EzE-;6cN`gDAwN-yC&tW8O|XwM&#Uk{z|XP5S-y{$Jg=XVZp z6u`4ujk?yJkE-O;d2bvJZd98;688-_V4a(+6-SB-*1ubSdwwwc?(`8Ys1ioB3g(q7 zz^@GJ_|5u2Ee-CmlpnpdX!Tu-g6++C- z`s776v<^L63%c$g_NLu59eo>Wf~Ab~4tZ%@{kskUG$B?{aW3w1mKPPC^=RWc`o@<- z?>gM#58ewea^qi7cyoOHg0iN1-^1zk+}I0iXw-zC`IVpgC#(#;oP{VQbskJPQz)OTcp;8v2)ANZt1^TC@*nJ7kl*SC`6Agn#5w)oR z%@Mb>0Z;Aiq4{j<2jBuFx}}i3`2z)W+`W6mr%TsEZ{o(S{Qa=fw(YWLhH{gtah4@T zNpUX0^4hXFE@s*0I_rvCHfPz{Ww4=l&8Zk%5ot9R*~Bm0oHc4$h7^w)MvU#Jdg4lu zEj(HMDWLnK*j0=wLVGIoJmfJ#-}aWQ=_I{f>WQSg6`M!ngvzZIH^Cy^tvqZF$rHO; zmh#eE`mXYD=tM~4W3CjNN3*U;Ttnz4_n2Lnx^fQy-Yc;GLd<%zzN~s*HR;h>rMb|H z!@dC{E&OI$iK?X@kfl!XQv1Ch8V#$>7lYp}O$8=qimF}AtQQ{NRi5aPP?uZryxK)W zu=dytB**-l@}oJ?0`p~D!k8abr4;e_1ROLnw)(yc6srTV>Od;e&N+A7>m-+;DNrqdU=#l(^V$)32wN#(rI zwQ~_mdD?q_0cSY|ZR>Ptvr=asB=OcPpexknwLEUaE~1w<6*NRhel}FpQtjj{^!Cll z0i@G(rxWm$OWtZ|iiP72s&RFBPTcZCu(eku= zKn-y=eEphEe6jFY$GU6WX!K5jb{EERzsPL{uI9Yb1oZb5ac3TI;b?Ao8NE)s(f^Rd zj1wxryIanTWL4i+G(sDtRAg2bxPTs*hVyvXD{jdnjy=(!*RsOBFj`y()B^@ST=!U( z|J=n!+yq8$7W7`UZ9Djur9DLsyqQzr`DHYJw^WnJZQqPKqHp>f_fnfJ<)cKpH(I0` zX~Mn1&Bf*6e)A6{$GyWzJ?BHk*&oz(E)jKlx~f2h0PEhx*RH zs66h)UHh@N_0`q+>D#FH#)v&Dz0mvb8E~2&t~sq#&e%}%x0+{svnutIi1PSPmGa<^ zXH6)cdI}ukBrl(y?&ZKaY;q5iSYUQ1gY~}^`cT^V4Gwq-h;R34YI+HA?m7o3JU&(2 z+hh~dm;LCjgq9$cl%+(RW!ADPeT%Ve2bY3pECbKq4=trYRjRb1k_D~>p&yn?Jg0B# zsee5by5e`^`nPV#xs`{7I2R{;>E}{^$(^<>4h^&kwm2$$Y?uqX1?ckK zLcE#IY*_K$^H-%VRCBO&=l{y_2y#4(zL4NL=|2punTLEcD}{-C+JlgmV$Y!&K6zJa zUU!acgufCDLOQsP2bBfFHun}~p|EY)^Zvxtu|C6y3`!?#aE$b2Lr?NEq}^7t7wNV? zU85|tomM2w+6TQ%*pktF+@pi#)$rN?&W1`mc@6!3rjX;;`mrG}Ujf-vkLA#)2jBr9ae1 z)UR{I`YhlgYyuB~RyqzG1a1Q9?)6jGx+NWdam1gVfwB0Hj__`;HuRbQFOKie{PRV# zqB+WqE-D*W&nfz&1F)a`cFlH`v#?Hm#|eM>+Ce!Zs!%qRv(9T3HB&!J%m*El-1(M& zEc!9wyUYW=OGE~2@&lLMz0MI;Oy_&u9#G5BDRhU?Uq-0dFLQ`m*8QfX{Q3+hm&2#Z zr}@+ystuq<*K5G;$60bYJXnBB@BRvQlf9h>lm)g;?#*bwP0IrCrNjbfaaD+zix^c{ zub>@Im`Y`bDegbr7o^^x3TJ^TJmuW16x;ay|B5ZM0N65`R8u#$Oty+8vssz*zYu0N zIq`n;E&2DI4M3NoDYqot%}B zQ|8)$u9w_G6uBEaivQy+A`v@^2<$w!I*iy+Bw|MqQ9n2C!+O&AF7qe8^aEmvE*f4x zCI0HadB!g6Cc@94-NY{J3eNd=tuMhn+D!}(?**v zt0iM8YWyCwxTihR*1K(KjLEyMk*~toq= z-lOzuke($TP%KAZs^7gnrel3)ulmio8ZXkQ8b%N5dd zARze8xA{?_NBve&c+{!@G%!cM|UG9~`@@HIR@qzP6eh^{F2#I}nZ< zF^ByUo*nEWH<8wL&azL@JHJrOu62r*yh#YXz%^tkuXs=Rvph%39P3wi?^ii5EZCZe zKJFoQ*YQ0lL82dS*58s#=sh+rgD6{~0~=8H)<&Wh$adh-CKpgFG3QQyxh}~FI$=3&)LN$pc-Z`N5!qJ@g%gU|t>1LwqVcU07b9Qjl=r~TKqe@uD4pIj26R?f zA0+$gv~=3bK_lyfX)`XxJm8mN_95_s#;`eK*h(zozd ztx({Kuo_+KT%WmIM|2OK_v;>P#&L$mJG+6h&PF-E#4!!WLpWHzD{nbMF2&r?%d1Hl z?3E4~th%Hq*EHA6()?u$0;@DHxOmHPm)#P%EYkAAGE>X#_>-kI%U*D4y5pE->N3K= zx*fZ*^fOmDH|3;Q&hihe(U(;oGVNoCGxNL~tve)XD0+s`h*8-Va$C7Xjb(ZuB6_Fm z!ZHTP_BWQbxTXiprZ*~dLbbwL7Pu_Sik4X`s+EYAGH4z2JqrC>(}fCV-MQkQPgW~& zxHgzto~l*`xvk5KmhWEv-SWg0#VhhCvD~Zg%vb?lf zvAU-RB8`gzs|?jjvaMPf;;vRKF8k1GWoS#alH{sZQk+YXE6o;-Tpo|l0XqY*m>ImaSqoVi*VkJwDMx73NfYPB)}$3&r8nS>PgA;o0# z{a8N(RfJMz1RstIyoq^?G{J*%<@+q)XjB?j!#4rZ)z*_=uu!_?^jc=ep82(QgD81k zuXMw@&u}YGNoKA_ZgOqqD-PY+O!Ty$I)PWnB3m9sOsD)$ctqgFHaPsJ7CGN=F@g4% z{o(b4c!ZtuIdzx2rjna%IO&u+GW({j`@BT7}U|P%?!;^ zy3;8i>ij|7Ly_;D8nM4P5ZYPr5pb&t|8I6_2i@OH+LjNATVY#$uqhH!w$CBmUMEJJ z<3{D9?SBV*6aH*IQoqra(sCC1r;)&agzn-|9`>?@P4gV{Q0vn#tpkRRw$~;};7&Du zq%gO(^$SWR%qrw_9&_T%HWz2F{(P1%o_Dgqv#K73^71KYwQyt1qm9GP`=Z*oNGc6W znw5L?TvVHeqjEfmavW-==S;tJTssTx4U1nlUDJ8^4RjWM1D&0B*20#AfjWp0qwuc6 zTY53zVZ#SEudZxpJr)yxmIa2O24b3*OP9<#;Bd}TER414`c8K4lQKN%;5N$cbdYga#iiJ3n+>s&F$2~%38TeB4*sKl;!U3!Z7yJLT?Fnm3tYTK zbJghf??D@c9#7%fUkkY=o+a`KH8OABl@_$sn@q&&c!#>^2bLfbk$t&zni*gEBxgRza~BO}wU!A^p;#q!*Ju&f!` zs}VL?uB-l$X&qkJRis~s)RfOBY|7jAlO#WG0Uw=NY4_Jp?CsQ4*3_}ITo!GH5} zmAnXTH`^%B$W#S-=iKN4l_OI-ys?n`D&TDC#JI|lX-4nRseNoir*MIbN*UxrS>AMK zazToeAn}zW(G||N-dbr0R>G(j3*t52&AGZ$-<@Ge^h}F*%3mxE6%#}t zi-;$1Fza!AK@|U=^d0=?eyZ~wSZ}D_;Y$qV{twR>@EsOPCeKdn)JWGZFUYJYTL#LO zLAJtY(TT-wTIBRh~J?d)T0@C zxaFvkH>kDDUfot2sqaIQrxkmGe~>Be!bLA{ zB%Y4iE1yrj1g#*Ep0YFV9Q^M@AXPlRrqxS2`KSD_^>6p1-gH{>W7Svt9co`I{Gr19Y2FJ`*$mwOiOG-S+brHPAA;)u|n%Ijh+VKSto>lFzs31N|Fx zZ$pL}Pc3lz=b9e#b4?Sy3mrMw`k4>#Qnl0!tG#(1YiWUJyEMrIi=%>AwPxvz>evXW z#ho_I!hCV3O~1KEn}#~>%wOgMT4|5L`1NXOt|uOEX#Rh`f!jFS!@mWq^#Apa^XfaO zzjdfH<)xp$>G)l5^4!Q;Wo$Sm>3p++a*DQ5bumbyLOuEu@S{y4xIU?mcoWkeK69+Q z)uMcSX(Ti%=c-b_9IP1@%&p6vj+VEO_I_`oC3ciEp;JBsG$X5PpA-ij%Vsq%Aq?r& zd)uVQ#`scLH{%Ktw_EQ>#nL`#glt>OjowxTEC$sp$;NiC-vCX=27{krtOdG+aE`wZ5T{?!SLWEfbX$m5|4en`H$$R26!E_bvqy)?e8){_Y7 zASC}i|gvvf?{B4`93c=c{$`q zZpQ8B3cg*i`bjU{UY3qoc8yjLRc)fx_laRElA&T_uJ#;R@w7FsaQ?SHy_9>KVyOPK zgZ$0h_$&Thj`y4XB=r%YYcT^JSUP)ncIC!^lVo&#hC6*d`_^(9`%o{CagIFerTJak zj#+oi6Yw0srJ0&?BpL}HLepRkS}jJrUA6)EX6ycQi~H=KTL#_+wA?X^4T^%r&p;>l z3O(n&F3H&=|I>>a9Si&g^*|a2CQxZ(6keR+;WofOrE{;e1tWN`@&m@(qwohtYsp3b z2CR3-q}kv>f80d=4fTW80zZ3;IKLTd8~hZ)6Tyhs^W0mcJ$#hBv*E2aF$=UGnMZML8J-%q8xAyU} zWW3eh&GUW!uS%b+bN*>b+Ms@gZOlLLZARUn=?iu5_;Z1FN3;#?6bHgLBIe_uc2KWo z{%vk6&3>aXlSDK_7d7eZ+OW@gPIK&{w&W~Yss*boQ8wf+2R5h3r&B#ImcidhNcJH% z>?dpn*5|!y?bM=;YmxI9^msCl=&-7U^KM1nNaTgyGv2qWX|c2kDK{bIyXsR&uKgLF zvIaWI!R7<)Ci20zwpkzbbx3{%DQMz2JZjJ*xw?$}t%!wdh%P^qY>*PW;Pw4wWbUw+jMGi##9qEdT5_Xd^^UX_ub| z-wfK)E6qpr@Z8ldujZZ{d>TRtlna zN~gS0A@ht)q6H6=A%8IU7xF* zc}T)~c@#c$5+^ir!YpUMmFdpGE91msSSmz0XE_J0RQ$8x*&~c&PVWzY*D!ol_#XMc zuzF&px(;{19s#rlNCxS9IY=09ry;#W^7-fTw9Dneou1ZpB#YCe8@z8zt6|sAgJ)`D zhL#jDyUglbT~-W)XC~A-Gchl6m>21JnGOAbzkY9i)oJozL>7QGu;PPUmUrvUGeKB* zn^y%6|Ko}iE zk+*cBWcT1WrPsu1Bfd(9uczp*!%{s(-iY-R()wZ7&U5$Cc-J+fL=hek*j@&>(q? zQ(0nN@ai>9us36yparnJFq_*zd!j4d7RvCe;)nshYz%xgP#IoQeN{wZet4xT33s%K z*7ev4RRe_yPHw;KClf2!Z<(X2AT^U7#*Q2tT%@JD;O(HN6|Y5#T|0QHGY+?pHkZzS`#&TZ|kVZA`cJw(nKuT@g6W4pnWmuR*i2;9&q{#`NMNcK4!TBBL z7V1O2TDD5BI(D~mnmw%=?XFfWx7GQRPdA0=3R}lJ&ae)a>H(!H?8~PIbbPbn3y39i z6Sq07Qg2X?uxmI^yV4+VNHMPZiQWa?OrWzju;XpUduB;r(vV0&NcUtap@tKZI`_rHakhOxD7i!++IbV(M3FP zMeevY5Z;v1ovYzxOaHVY_v<2A7j(R+QV=&8g*IS)>T9-QBpTlsi@Y!FfvGqVm(tzFpjbSB!O z4784uE!+5OgB zwDsqKqYyPGmtC!Q?`kvZg7SeNr1Lwz$TgkTo}2^f-HsZ0Lm7iGC?8j;1m)#Nz!S=-8@ zcrl}Ku1I>4uQ`Gzo8u3OK2Q#uohHw5={x5Z$Lp=U&VD9RIvH1x6X{HMnj}IoSmMOH z7-6o&W*^5Hm1u2H*Jjkt1@|Jd)xgeT-}XCaPuKyOgyYUM=`}}OPMS0QOfc@P*nKn7 zq^Zt4$q?WSld`j&lzVZI{vQh-aBTFx)n?R;LT{4gQK5CvA7fGPz_K%v&jmvg8&-&>}6N8CrJ$)1;uJ8lbr9Y1n>AA9O2#rEaHZW4= z`vV6|QWSWKJqDCYpBn~z6+8Rui<b(R$j))v#K7U89DRszt$~JdYHRGCxE*S$?ZE2g_%i1ef8a3b(6fNG-CkxqLU=rU zW4?;S!>im__*++TeK_d!e|w0Ke`1|94zWs9Z0KGyqMVw7!ff*O<;n}M6WKtN(n-dt zXZrsT)QpOSw=u)iqp`Qg4(#jN1*E1mf%-S~bINBFM|rpjn$$7SYFfi+Hgx4hAK)AK z?p-wD-e^>O9NIIR4n)FF&)T|W9**|qfkGwf?Z~YfV8{gJX3wo?}Z%`iFWq{4S8MJs%ZpK>a#1pw~M&yGp;ccm+5oxSlW1P|0 z)syW+he|^qmC8!v<2J!VPj6u9B#4pc(@QO4Y>tdMw^d3jWr^0DR;TY_qRHW16&hp) zGg(=HiVuZxKlBCMMBb?{a1>=bbp@+I2Oqu2kBV~9TFAe|jHg}<4=W}W03De67rReRDvl?jwL z@L4Kjfza{-G%r}1;tShjV2pqRN^_L4K?m9H&cIs$=zWV@z0E}XvLS%pC0tR;Mgpdo~iG6?&?er*P*%gQJcUCK%g>pXgagt z-EFgd`lW>YMAHXB<>ugy^|CS8?*i^;>w|{2t&Yeu39US@ZwuMnfx%T<9rmjURhs*Y zbLPhFsox(|0>+>@Bj*yI!-59+O*OSv%RrgC1Cdp4BDJAvVU8x#eo1ftKAsI3==sX7 z@+Y%LVOd8_P|E>u2vI=FUi9spZs{;|PJ=f(p~G;U>(F0I$Zn^aoWgMK3AO`OkxCK~j8#iu3`U~_nhyQd zD76&Ew$Ma(pvGN;I$Go2+omxV<%G-99*78+C5r{4yIxYu!f~k6F-uOql)LzhCbKBV ze#vOx8Psgv)f&z1a(pJfAwF_ymUq5qKd7T3+~Yb7vIXU6vk%TjT`gln!u5)_=TFN^fU^>fnj1+5|&rYZ9L z5XO43MF0-h7)5>rx{!4Cv2uD0_H1@dA#`2^cwKDQ18wxzu4bt)n+1N;&H{geCuXwx z+5{{|vU{ZUY9H=zg2gt2Z39um6u$^Ki7`N&W=Rg90z!V3MCUUN!LJ$Q+h+C364*P_ za@~w;@Sxz4V_5j^b+>v4B}O?I3vtE~TZY}u%`%4x>%b(`K+6 zFPpsz`7lD6Q-Jc7sL5C4X{1TqWQo!;E(I&KkM23{x@T;N^yzVLOvnvtjcWSCdP-vh zHd;6zVFIqcW~?#ib%EAuu~5yrfMQ!S8SB}D+1Fjb6t0<^U`a1XsF+u=t>RpTf;Bi5 z&q(Ux1X=zg_(UU~kmXHEa>1((;Kd6=K)J2Q-C-rNT%!p2pc8{SaWKML9fM&@8tqFC zgfs)OBZ>frV9;>=Y3h+|NN)B!j?AWEu=-VGk^?#$e+B77nL=+_s6-a@m^ut2CJs-X zuV@mZ^E+V&8Rq=HrF6=VDvIBJ^WYe6$R~bgXNA<4sa_0qhPs`c#<&N^{NLz4Av@8J zcQMs(X<;_?Dj~4SURXr+_!aVK2I&fP`KBggBD4kqTFn5bHc^%x2Pw|n!dgh8&E?DT z1g!aV%`9A^7C5T1Y*XlI(8w;Lx}`WxcUtSuX`zjZyz$-F+7#j!n#A6xIpL;UAL55c zVBf)*sVNEeQ!H^7v<3!b>k0;x+>~pfWtpi`NJ0N)l%KwlgS-a5)6o~;GYdR9S-#!@ z{ke8f!=Oq*wHrFWI*WhZ*ZkBuPEA|Q=xk|@)0JoC7Ude@%VR4LNFz>jH&{;~k)F}z zS4qEf)WCK&JT_R{1$V=wll-~zpBeR(B-Zog^(pNNHoBpz4 zS{bw?dqcKvfR5!&`EZE4?}m>!ikU}%ZtS-gOaq^TtEe9|{UunNKs zk2qevv_u1fSBg}HNL7ziIyKcNKm+WOkKUCISg8-t_>VlvZn&8p^xvisU+R{=k7KPl zW^;FaQ7U6YkCTMn0KLjKLBmyYwoqy;P_CYH0O3P4;=Mh zpHMvUb>^v9hNa+;+YUkIjM}5S4~49-e4>1GHC0XXkouj|x=e?N6Q36P(WAe?#!`Gz z=%D9a2V~pB`hcF(G8(NU-`7H>7XHtWw9rzV3ZcKbQ>g zS*q_U&iddi$v3+6QIvR_W3aCRXC|D*`RZ|I#93e8Rh(&YW`_2PFg{$$NS`q7SGj-n z-Tl5oRZm>2Bb`aD?oVnfy-Ra%qOQQFn1*Yvw=C!lPj9Bl^!;5}2ehn1mtqNF9)i_d zgKipqpA5UC6h@e;nu20jrC%n!17LwLApHs!V*_#Pi#r9_!ZVepF>tB?k0IuUhov2k zWbaptYw=zCpzPTRZjzC(QrybE>$?;5z)=E1(}jX<1Ai}!6?jQyLJZ%cuk zEF^y!nt@do-!lfXEzCJsXRSbWfqYMw52+D)7U@Ln4Ym0boGj^zpUTyz500ss4OD`M zB%QR?5$E+SUWYoP65#DDG7;rS;(|QC*}=DIc;JG}(0dIjTC`wD{(VUB{FJR~)mni{ zhdY%R%B+c=oz|Zv6;(X()cD~~j70dDR8+C>@uDY}tQb+urYu}iB0OBQyo`k~jfH>2 z!{=~H{8HUr;SO9=3gR(S{ORuN&$`0}<>f1ulou^5sd(&(qSB=-%jI%ZlrCNLOBXAx zST+*JV^1zEsdSYTuP9kqy1W9fzi(;z(xN9y?q~G8PX+*^q&wuhLp>ti!hHj-DKz5x zb%c@K@e#PDd{N!krtYvuceuQyqPTqN#EC-g%$d_?P8711RFo7gd|JTGrD{1MS$7g) zX33(`p=Va9R$ zm!K&exv=D^ktgmNaW~@qQ#`y@d=scAfzfOUVARMVU|10i| zMK~G9likOW;jq^j!Hgw1&f|CzM->iw;1h`Z(8i3%aD4>F6B*3-M_lK^pU^L#D0u>1 z>!aIc7>yHZ>jE6q)=P0vTmKe^7RMiOP@nq@haSf;B&N16L}Geb9TG(#qz*xSz=;cL z`=h-;6CiXaGe&JsFNmfP4+IyJv2XESGs2HJrlGK!XFP>?kQSk^%fNJWecZ_O6q=)% z0intlqU+5)Sp-7o1!(jkbXN~zQ3!V?4`C*RM?W3TdLV3BG?qmpWD%2D48qc-kF%vk zOIgZNGTozk(I{o&BQDjc#Y-C-a5wypGQeBhZr@8*5` z>*@!FXD76NH!kjhiR*3Oz4UbM{fF0{{$u^g9rtCtx%X@9%_$Qmzj;09R?O?8U-{$u zFOBcdP0lWVXJLx|Gpc_4lI&{0W{1O?V`^Ca_djNZzcVYl|59B;!@*7BjrIpF^3yXP z96iw1p1<DOW + +#ifndef __ASSEMBLY__ +# include +#endif + +#include "stm32_rcc.h" +#include "stm32_sdmmc.h" + +/************************************************************************************ + * Pre-processor Definitions + ************************************************************************************/ + +/* Clocking *************************************************************************/ +/* The holybro KakuteH7 board provides the following clock sources: + * + * X1: 8 MHz crystal for HSE + * + * So we have these clock source available within the STM32 + * + * HSI: 16 MHz RC factory-trimmed + * HSE: 8 MHz crystal for HSE + */ + +#define STM32_BOARD_XTAL 8000000ul + +#define STM32_HSI_FREQUENCY 16000000ul +#define STM32_LSI_FREQUENCY 32000 +#define STM32_HSE_FREQUENCY STM32_BOARD_XTAL +#define STM32_LSE_FREQUENCY 32768 + +/* Main PLL Configuration. + * + * PLL source is HSE = 8,000,000 + * + * PLL_VCOx = (STM32_HSE_FREQUENCY / PLLM) * PLLN + * Subject to: + * + * 1 <= PLLM <= 63 + * 4 <= PLLN <= 512 + * 150 MHz <= PLL_VCOL <= 420MHz + * 192 MHz <= PLL_VCOH <= 836MHz + * + * SYSCLK = PLL_VCO / PLLP + * CPUCLK = SYSCLK / D1CPRE + * Subject to + * + * PLLP1 = {2, 4, 6, 8, ..., 128} + * PLLP2,3 = {2, 3, 4, ..., 128} + * CPUCLK <= 480 MHz + */ + +#define STM32_BOARD_USEHSE + +#define STM32_PLLCFG_PLLSRC RCC_PLLCKSELR_PLLSRC_HSE + +/* PLL1, wide 4 - 8 MHz input, enable DIVP, DIVQ, DIVR + * + * PLL1_VCO = (8,000,000 / 1) * 120 = 960 MHz + * + * PLL1P = PLL1_VCO/2 = 960 MHz / 2 = 480 MHz + * PLL1Q = PLL1_VCO/4 = 960 MHz / 4 = 240 MHz + * PLL1R = PLL1_VCO/8 = 960 MHz / 8 = 120 MHz + */ + +#define STM32_PLLCFG_PLL1CFG (RCC_PLLCFGR_PLL1VCOSEL_WIDE | \ + RCC_PLLCFGR_PLL1RGE_4_8_MHZ | \ + RCC_PLLCFGR_DIVP1EN | \ + RCC_PLLCFGR_DIVQ1EN | \ + RCC_PLLCFGR_DIVR1EN) +#define STM32_PLLCFG_PLL1M RCC_PLLCKSELR_DIVM1(1) +#define STM32_PLLCFG_PLL1N RCC_PLL1DIVR_N1(120) +#define STM32_PLLCFG_PLL1P RCC_PLL1DIVR_P1(2) +#define STM32_PLLCFG_PLL1Q RCC_PLL1DIVR_Q1(4) +#define STM32_PLLCFG_PLL1R RCC_PLL1DIVR_R1(8) + +#define STM32_VCO1_FREQUENCY ((STM32_HSE_FREQUENCY / 1) * 120) +#define STM32_PLL1P_FREQUENCY (STM32_VCO1_FREQUENCY / 2) +#define STM32_PLL1Q_FREQUENCY (STM32_VCO1_FREQUENCY / 4) +#define STM32_PLL1R_FREQUENCY (STM32_VCO1_FREQUENCY / 8) + +/* PLL2 */ + +#define STM32_PLLCFG_PLL2CFG (RCC_PLLCFGR_PLL2VCOSEL_WIDE | \ + RCC_PLLCFGR_PLL2RGE_4_8_MHZ | \ + RCC_PLLCFGR_DIVP2EN | \ + RCC_PLLCFGR_DIVQ2EN | \ + RCC_PLLCFGR_DIVR2EN) +#define STM32_PLLCFG_PLL2M RCC_PLLCKSELR_DIVM2(2) +#define STM32_PLLCFG_PLL2N RCC_PLL2DIVR_N2(48) +#define STM32_PLLCFG_PLL2P RCC_PLL2DIVR_P2(2) +#define STM32_PLLCFG_PLL2Q RCC_PLL2DIVR_Q2(2) +#define STM32_PLLCFG_PLL2R RCC_PLL2DIVR_R2(2) + +#define STM32_VCO2_FREQUENCY ((STM32_HSE_FREQUENCY / 2) * 48) +#define STM32_PLL2P_FREQUENCY (STM32_VCO2_FREQUENCY / 2) +#define STM32_PLL2Q_FREQUENCY (STM32_VCO2_FREQUENCY / 2) +#define STM32_PLL2R_FREQUENCY (STM32_VCO2_FREQUENCY / 2) + +/* PLL3 */ + +#define STM32_PLLCFG_PLL3CFG (RCC_PLLCFGR_PLL3VCOSEL_WIDE | \ + RCC_PLLCFGR_PLL3RGE_4_8_MHZ | \ + RCC_PLLCFGR_DIVQ3EN) +#define STM32_PLLCFG_PLL3M RCC_PLLCKSELR_DIVM3(2) +#define STM32_PLLCFG_PLL3N RCC_PLL3DIVR_N3(48) +#define STM32_PLLCFG_PLL3P RCC_PLL3DIVR_P3(2) +#define STM32_PLLCFG_PLL3Q RCC_PLL3DIVR_Q3(4) +#define STM32_PLLCFG_PLL3R RCC_PLL3DIVR_R3(2) + +#define STM32_VCO3_FREQUENCY ((STM32_HSE_FREQUENCY / 2) * 48) +#define STM32_PLL3P_FREQUENCY (STM32_VCO3_FREQUENCY / 2) +#define STM32_PLL3Q_FREQUENCY (STM32_VCO3_FREQUENCY / 4) +#define STM32_PLL3R_FREQUENCY (STM32_VCO3_FREQUENCY / 2) + +/* SYSCLK = PLL1P = 480MHz + * CPUCLK = SYSCLK / 1 = 480 MHz + */ + +#define STM32_RCC_D1CFGR_D1CPRE (RCC_D1CFGR_D1CPRE_SYSCLK) +#define STM32_SYSCLK_FREQUENCY (STM32_PLL1P_FREQUENCY) +#define STM32_CPUCLK_FREQUENCY (STM32_SYSCLK_FREQUENCY / 1) + +/* Configure Clock Assignments */ + +/* AHB clock (HCLK) is SYSCLK/2 (240 MHz max) + * HCLK1 = HCLK2 = HCLK3 = HCLK4 = 240 + */ + +#define STM32_RCC_D1CFGR_HPRE RCC_D1CFGR_HPRE_SYSCLKd2 /* HCLK = SYSCLK / 2 */ +#define STM32_ACLK_FREQUENCY (STM32_CPUCLK_FREQUENCY / 2) /* ACLK in D1, HCLK3 in D1 */ +#define STM32_HCLK_FREQUENCY (STM32_CPUCLK_FREQUENCY / 2) /* HCLK in D2, HCLK4 in D3 */ +#define STM32_BOARD_HCLK STM32_HCLK_FREQUENCY /* same as above, to satisfy compiler */ + +/* APB1 clock (PCLK1) is HCLK/2 (120 MHz) */ + +#define STM32_RCC_D2CFGR_D2PPRE1 RCC_D2CFGR_D2PPRE1_HCLKd2 /* PCLK1 = HCLK / 2 */ +#define STM32_PCLK1_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* APB2 clock (PCLK2) is HCLK/2 (120 MHz) */ + +#define STM32_RCC_D2CFGR_D2PPRE2 RCC_D2CFGR_D2PPRE2_HCLKd2 /* PCLK2 = HCLK / 2 */ +#define STM32_PCLK2_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* APB3 clock (PCLK3) is HCLK/2 (120 MHz) */ + +#define STM32_RCC_D1CFGR_D1PPRE RCC_D1CFGR_D1PPRE_HCLKd2 /* PCLK3 = HCLK / 2 */ +#define STM32_PCLK3_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* APB4 clock (PCLK4) is HCLK/4 (120 MHz) */ + +#define STM32_RCC_D3CFGR_D3PPRE RCC_D3CFGR_D3PPRE_HCLKd2 /* PCLK4 = HCLK / 2 */ +#define STM32_PCLK4_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* Timer clock frequencies */ + +/* Timers driven from APB1 will be twice PCLK1 */ + +#define STM32_APB1_TIM2_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM3_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM4_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM5_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM6_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM7_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM12_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM13_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM14_CLKIN (2*STM32_PCLK1_FREQUENCY) + +/* Timers driven from APB2 will be twice PCLK2 */ + +#define STM32_APB2_TIM1_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM8_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM15_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM16_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM17_CLKIN (2*STM32_PCLK2_FREQUENCY) + +/* Kernel Clock Configuration + * + * Note: look at Table 54 in ST Manual + */ + +/* I2C123 clock source */ + +#define STM32_RCC_D2CCIP2R_I2C123SRC RCC_D2CCIP2R_I2C123SEL_HSI + +/* I2C4 clock source */ + +#define STM32_RCC_D3CCIPR_I2C4SRC RCC_D3CCIPR_I2C4SEL_HSI + +/* SPI123 clock source */ + +#define STM32_RCC_D2CCIP1R_SPI123SRC RCC_D2CCIP1R_SPI123SEL_PLL2 + +/* SPI45 clock source */ + +#define STM32_RCC_D2CCIP1R_SPI45SRC RCC_D2CCIP1R_SPI45SEL_PLL2 + +/* SPI6 clock source */ + +#define STM32_RCC_D3CCIPR_SPI6SRC RCC_D3CCIPR_SPI6SEL_PLL2 + +/* USB 1 and 2 clock source */ + +#define STM32_RCC_D2CCIP2R_USBSRC RCC_D2CCIP2R_USBSEL_PLL3 + +/* ADC 1 2 3 clock source */ + +#define STM32_RCC_D3CCIPR_ADCSEL RCC_D3CCIPR_ADCSEL_PLL2 + +/* FDCAN 1 2 clock source */ + +#define STM32_RCC_D2CCIP1R_FDCANSEL RCC_D2CCIP1R_FDCANSEL_HSE /* FDCAN 1 2 clock source */ + +#define STM32_FDCANCLK STM32_HSE_FREQUENCY + +/* FLASH wait states + * + * ------------ ---------- ----------- + * Vcore MAX ACLK WAIT STATES + * ------------ ---------- ----------- + * 1.15-1.26 V 70 MHz 0 + * (VOS1 level) 140 MHz 1 + * 210 MHz 2 + * 1.05-1.15 V 55 MHz 0 + * (VOS2 level) 110 MHz 1 + * 165 MHz 2 + * 220 MHz 3 + * 0.95-1.05 V 45 MHz 0 + * (VOS3 level) 90 MHz 1 + * 135 MHz 2 + * 180 MHz 3 + * 225 MHz 4 + * ------------ ---------- ----------- + */ + +#define BOARD_FLASH_WAITSTATES 2 + +/* SDMMC definitions ********************************************************/ + +/* Init 400kHz, freq = PLL1Q/(2*div) div = PLL1Q/(2*freq) */ + +#define STM32_SDMMC_INIT_CLKDIV (300 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) + +/* 25 MHz Max for now, 25 mHZ = PLL1Q/(2*div), div = PLL1Q/(2*freq) + * div = 4.8 = 240 / 50, So round up to 5 for default speed 24 MB/s + */ + +#if defined(CONFIG_STM32H7_SDMMC_XDMA) || defined(CONFIG_STM32H7_SDMMC_IDMA) +# define STM32_SDMMC_MMCXFR_CLKDIV (5 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#else +# define STM32_SDMMC_MMCXFR_CLKDIV (100 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#endif +#if defined(CONFIG_STM32H7_SDMMC_XDMA) || defined(CONFIG_STM32H7_SDMMC_IDMA) +# define STM32_SDMMC_SDXFR_CLKDIV (5 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#else +# define STM32_SDMMC_SDXFR_CLKDIV (100 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#endif + +#define STM32_SDMMC_CLKCR_EDGE STM32_SDMMC_CLKCR_NEGEDGE + +/* LED definitions ******************************************************************/ +/* The holybro KakuteH7 board has three, LED_GREEN a Green LED, LED_BLUE + * a Blue LED and LED_RED a Red LED, that can be controlled by software. + * + * If CONFIG_ARCH_LEDS is not defined, then the user can control the LEDs in any way. + * The following definitions are used to access individual LEDs. + */ + +/* LED index values for use with board_userled() */ + +#define BOARD_LED1 0 +#define BOARD_NLEDS 1 + +#define BOARD_LED_RED BOARD_LED1 + +/* LED bits for use with board_userled_all() */ + +#define BOARD_LED1_BIT (1 << BOARD_LED1) + +/* If CONFIG_ARCH_LEDS is defined, the usage by the board port is defined in + * include/board.h and src/stm32_leds.c. The LEDs are used to encode OS-related + * events as follows: + * + * + * SYMBOL Meaning LED state + * Red Green Blue + * ---------------------- -------------------------- ------ ------ ----*/ + +#define LED_STARTED 0 /* NuttX has been started OFF OFF OFF */ +#define LED_HEAPALLOCATE 1 /* Heap has been allocated OFF OFF ON */ +#define LED_IRQSENABLED 2 /* Interrupts enabled OFF ON OFF */ +#define LED_STACKCREATED 3 /* Idle stack created OFF ON ON */ +#define LED_INIRQ 4 /* In an interrupt N/C N/C GLOW */ +#define LED_SIGNAL 5 /* In a signal handler N/C GLOW N/C */ +#define LED_ASSERTION 6 /* An assertion failed GLOW N/C GLOW */ +#define LED_PANIC 7 /* The system has crashed Blink OFF N/C */ +#define LED_IDLE 8 /* MCU is is sleep mode ON OFF OFF */ + +/* Thus if the Green LED is statically on, NuttX has successfully booted and + * is, apparently, running normally. If the Red LED is flashing at + * approximately 2Hz, then a fatal error has been detected and the system + * has halted. + */ + +/* Alternate function pin selections ************************************************/ + + +#define GPIO_USART1_RX GPIO_USART1_RX_2 /* PA10 */ +#define GPIO_USART1_TX GPIO_USART1_TX_2 /* PA9 */ + +#define GPIO_USART2_RX GPIO_USART2_RX_2 /* PD6 */ +#define GPIO_USART2_TX GPIO_USART2_TX_2 /* PD5 */ + +#define GPIO_USART3_RX GPIO_USART3_RX_3 /* PD9 */ +#define GPIO_USART3_TX GPIO_USART3_TX_3 /* PD8 */ + +#define GPIO_UART4_RX GPIO_UART4_RX_5 /* PD0 */ +#define GPIO_UART4_TX GPIO_UART4_TX_5 /* PD1 */ + +#define GPIO_USART6_RX GPIO_USART6_RX_1 /* PC7 */ +#define GPIO_USART6_TX GPIO_USART6_TX_1 /* PC6 */ + +#define GPIO_UART7_RX GPIO_UART7_RX_3 /* PE7 */ +#define GPIO_UART7_TX GPIO_UART7_TX_3 /* PE8 */ + +/* SPI + * SPI1 SD Card + * SPI2 is OSD AT7456E + * SPI4 is IMU + */ + +#define GPIO_SPI1_MISO GPIO_SPI1_MISO_1 /* PA6 */ +#define GPIO_SPI1_MOSI GPIO_SPI1_MOSI_1 /* PA7 */ +#define GPIO_SPI1_SCK GPIO_SPI1_SCK_1 /* PA5 */ + +#define GPIO_SPI2_MISO GPIO_SPI2_MISO_1 /* PB14 */ +#define GPIO_SPI2_MOSI GPIO_SPI2_MOSI_1 /* PB15 */ +#define GPIO_SPI2_SCK GPIO_SPI2_SCK_4 /* PB13 */ + +#define GPIO_SPI4_MISO GPIO_SPI4_MISO_2 /* PE5 */ +#define GPIO_SPI4_MOSI GPIO_SPI4_MOSI_2 /* PE6 */ +#define GPIO_SPI4_SCK GPIO_SPI4_SCK_2 /* PE2 */ + +/* I2C + */ + +#define GPIO_I2C1_SCL GPIO_I2C1_SCL_1 /* PB6 */ +#define GPIO_I2C1_SDA GPIO_I2C1_SDA_1 /* PB7 */ + +#define GPIO_I2C1_SCL_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTB | GPIO_PIN6) +#define GPIO_I2C1_SDA_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTB | GPIO_PIN7) + +/* SDMMC1 + * + * VDD 3.3 + * GND + * SDMMC1_CK PC12 + * SDMMC1_CMD PD2 + * SDMMC1_D0 PC8 + * SDMMC1_D1 PC9 + * SDMMC1_D2 PC10 + * SDMMC1_D3 PC11 + * GPIO_SDMMC1_NCD PG0 + */ + +/* USB + * + * OTG_FS_DM PA11 + * OTG_FS_DP PA12 + * VBUS PA9 + */ + + +/* Board provides GPIO or other Hardware for signaling to timing analyzer */ + +# define PROBE_INIT(mask) +# define PROBE(n,s) +# define PROBE_MARK(n) + diff --git a/boards/holybro/kakuteh7v2/nuttx-config/include/board_dma_map.h b/boards/holybro/kakuteh7v2/nuttx-config/include/board_dma_map.h new file mode 100644 index 0000000000..1954ce3e68 --- /dev/null +++ b/boards/holybro/kakuteh7v2/nuttx-config/include/board_dma_map.h @@ -0,0 +1,40 @@ +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#define DMAMAP_SPI4_RX DMAMAP_DMA12_SPI4RX_1 /* DMA2 */ +#define DMAMAP_SPI4_TX DMAMAP_DMA12_SPI4TX_1 /* DMA2 */ + +#define DMAMAP_USART2_RX DMAMAP_DMA12_USART2RX_1 /* DMA2 */ + diff --git a/boards/holybro/kakuteh7v2/nuttx-config/nsh/defconfig b/boards/holybro/kakuteh7v2/nuttx-config/nsh/defconfig new file mode 100644 index 0000000000..be0c62cbbc --- /dev/null +++ b/boards/holybro/kakuteh7v2/nuttx-config/nsh/defconfig @@ -0,0 +1,272 @@ +# +# This file is autogenerated: PLEASE DO NOT EDIT IT. +# +# You can use "make menuconfig" to make any modifications to the installed .config file. +# You can then do "make savedefconfig" to generate a new defconfig file that includes your +# modifications. +# +# CONFIG_DISABLE_ENVIRON is not set +# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set +# CONFIG_DISABLE_PTHREAD is not set +# CONFIG_MMCSD_HAVE_CARDDETECT is not set +# CONFIG_MMCSD_HAVE_WRITEPROTECT is not set +# CONFIG_MMCSD_MMCSUPPORT is not set +# CONFIG_NSH_DISABLEBG is not set +# CONFIG_NSH_DISABLESCRIPT is not set +# CONFIG_NSH_DISABLE_CAT is not set +# CONFIG_NSH_DISABLE_CD is not set +# CONFIG_NSH_DISABLE_CP is not set +# CONFIG_NSH_DISABLE_DATE is not set +# CONFIG_NSH_DISABLE_DF is not set +# CONFIG_NSH_DISABLE_ECHO is not set +# CONFIG_NSH_DISABLE_ENV is not set +# CONFIG_NSH_DISABLE_EXEC is not set +# CONFIG_NSH_DISABLE_EXIT is not set +# CONFIG_NSH_DISABLE_EXPORT is not set +# CONFIG_NSH_DISABLE_FREE is not set +# CONFIG_NSH_DISABLE_GET is not set +# CONFIG_NSH_DISABLE_HELP is not set +# CONFIG_NSH_DISABLE_ITEF is not set +# CONFIG_NSH_DISABLE_KILL is not set +# CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_LS is not set +# CONFIG_NSH_DISABLE_MKDIR is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set +# CONFIG_NSH_DISABLE_MOUNT is not set +# CONFIG_NSH_DISABLE_MV is not set +# CONFIG_NSH_DISABLE_PS is not set +# CONFIG_NSH_DISABLE_PSSTACKUSAGE is not set +# CONFIG_NSH_DISABLE_PWD is not set +# CONFIG_NSH_DISABLE_RM is not set +# CONFIG_NSH_DISABLE_RMDIR is not set +# CONFIG_NSH_DISABLE_SEMICOLON is not set +# CONFIG_NSH_DISABLE_SET is not set +# CONFIG_NSH_DISABLE_SLEEP is not set +# CONFIG_NSH_DISABLE_SOURCE is not set +# CONFIG_NSH_DISABLE_TEST is not set +# CONFIG_NSH_DISABLE_TIME is not set +# CONFIG_NSH_DISABLE_UMOUNT is not set +# CONFIG_NSH_DISABLE_UNSET is not set +# CONFIG_NSH_DISABLE_USLEEP is not set +# CONFIG_SPI_CALLBACK is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/holybro/kakuteh7v2/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="stm32h7" +CONFIG_ARCH_CHIP_STM32H743II=y +CONFIG_ARCH_CHIP_STM32H7=y +CONFIG_ARCH_INTERRUPTSTACK=512 +CONFIG_ARCH_STACKDUMP=y +CONFIG_ARMV7M_BASEPRI_WAR=y +CONFIG_ARMV7M_DCACHE=y +CONFIG_ARMV7M_DTCM=y +CONFIG_ARMV7M_ICACHE=y +CONFIG_ARMV7M_MEMCPY=y +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_ARM_MPU_EARLY_RESET=y +CONFIG_BOARDCTL_RESET=y +CONFIG_BOARDCTL=y +CONFIG_BOARD_CRASHDUMP=y +CONFIG_BOARD_LOOPSPERMSEC=95150 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_BOARD_ASSERT_RESET_VALUE=0 +CONFIG_BUILTIN=y +CONFIG_C99_BOOL8=y +CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y +CONFIG_CDCACM_PRODUCTID=0x0050 +CONFIG_CDCACM_PRODUCTSTR="PX4 KakuteH7v2" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_CDCACM_VENDORID=0x3162 +CONFIG_CDCACM_VENDORSTR="Holybro" +CONFIG_CLOCK_MONOTONIC=y +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_HARDFAULT_ALERT=y +CONFIG_DEBUG_MEMFAULT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEFAULT_SMALL=y +# CONFIG_NSH_DISABLE_ECHO is not set +# CONFIG_NSH_DISABLE_ENV is not set +# CONFIG_NSH_DISABLE_FREE is not set +# CONFIG_NSH_DISABLE_HELP is not set +# CONFIG_NSH_DISABLE_KILL is not set +# CONFIG_NSH_DISABLE_LS is not set +# CONFIG_NSH_DISABLE_MKDIR is not set +# CONFIG_NSH_DISABLE_MODCMDS is not set +# CONFIG_NSH_DISABLE_MOUNT is not set +# CONFIG_NSH_DISABLE_MV is not set +# CONFIG_NSH_DISABLE_PRINTF is not set +# CONFIG_NSH_DISABLE_PSSTACKUSAGE +# CONFIG_NSH_DISABLE_PWD is not set +# CONFIG_NSH_DISABLE_SOURCE is not set +# CONFIG_NSH_DISABLE_SLEEP is not set +# CONFIG_NSH_DISABLE_TEST is not set +# CONFIG_NSH_DISABLE_UMOUNT is not set +# CONFIG_NSH_DISABLE_UNSET is not set +# CONFIG_NSH_DISABLE_USLEEP is not set +CONFIG_DEV_FIFO_SIZE=0 +CONFIG_DEV_PIPE_MAXSIZE=1024 +CONFIG_DEV_PIPE_SIZE=70 +CONFIG_EXPERIMENTAL=y +CONFIG_FAT_DMAMEMORY=y +CONFIG_FAT_LCNAMES=y +CONFIG_FAT_LFN=y +CONFIG_FAT_LFN_ALIAS_HASH=y +CONFIG_FDCLONE_STDIO=y +CONFIG_FS_BINFS=y +CONFIG_FS_CROMFS=y +CONFIG_FS_FAT=y +CONFIG_FS_FATTIME=y +CONFIG_FS_PROCFS=y +CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y +CONFIG_FS_PROCFS_MAX_TASKS=64 +CONFIG_FS_PROCFS_REGISTER=y +CONFIG_FS_ROMFS=y +CONFIG_GRAN=y +CONFIG_GRAN_INTR=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +CONFIG_I2C=y +CONFIG_I2C_RESET=y +CONFIG_IDLETHREAD_STACKSIZE=750 +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_STRERROR=y +CONFIG_MEMSET_64BIT=y +CONFIG_MEMSET_OPTSPEED=y +CONFIG_MMCSD=y +CONFIG_MMCSD_SDIO=y +CONFIG_MM_REGIONS=4 +CONFIG_MTD=y +CONFIG_MTD_BYTE_WRITE=y +CONFIG_MTD_PARTITION=y +CONFIG_MTD_PROGMEM=y +CONFIG_MTD_RAMTRON=y +CONFIG_NAME_MAX=40 +CONFIG_NSH_ARCHINIT=y +CONFIG_NSH_ARGCAT=y +CONFIG_NSH_BUILTIN_APPS=y +CONFIG_NSH_CMDPARMS=y +CONFIG_NSH_CROMFSETC=y +CONFIG_NSH_DISABLE_IFCONFIG=y +CONFIG_NSH_DISABLE_IFUPDOWN=y +CONFIG_NSH_DISABLE_TELNETD=y +CONFIG_NSH_LINELEN=128 +CONFIG_NSH_MAXARGUMENTS=15 +CONFIG_NSH_MMCSDSPIPORTNO=1 +CONFIG_NSH_NESTDEPTH=8 +CONFIG_NSH_QUOTE=y +CONFIG_NSH_ROMFSETC=y +CONFIG_NSH_ROMFSSECTSIZE=128 +CONFIG_NSH_STRERROR=y +CONFIG_NSH_VARS=y +CONFIG_OTG_ID_GPIO_DISABLE=y +CONFIG_PIPES=y +CONFIG_PREALLOC_TIMERS=50 +CONFIG_PRIORITY_INHERITANCE=y +CONFIG_PTHREAD_MUTEX_ROBUST=y +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_SETSPEED=y +CONFIG_RAM_SIZE=245760 +CONFIG_RAM_START=0x20010000 +CONFIG_RAW_BINARY=y +CONFIG_READLINE_CMD_HISTORY=y +CONFIG_READLINE_TABCOMPLETION=y +CONFIG_RTC_DATETIME=y +CONFIG_LIBC_MAX_EXITFUNS=1 +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_HPWORKPRIORITY=249 +CONFIG_SCHED_HPWORKSTACKSIZE=1280 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y +CONFIG_SCHED_INSTRUMENTATION_SWITCH=y +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKSTACKSIZE=1632 +CONFIG_SCHED_WAITPID=y +CONFIG_SDCLONE_DISABLE=y +CONFIG_SEM_PREALLOCHOLDERS=32 +CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y +CONFIG_SERIAL_TERMIOS=y +CONFIG_SIG_DEFAULT=y +CONFIG_SIG_SIGALRM_ACTION=y +CONFIG_SIG_SIGUSR1_ACTION=y +CONFIG_SIG_SIGUSR2_ACTION=y +CONFIG_SIG_SIGWORK=4 +CONFIG_STACK_COLORATION=y +CONFIG_START_DAY=30 +CONFIG_START_MONTH=11 +CONFIG_STDIO_BUFFER_SIZE=256 +CONFIG_STM32H7_ADC1=y +CONFIG_STM32H7_ADC3=y +CONFIG_STM32H7_BBSRAM=y +CONFIG_STM32H7_BBSRAM_FILES=5 +CONFIG_STM32H7_BDMA=y +CONFIG_STM32H7_BKPSRAM=y +CONFIG_STM32H7_DMA1=y +CONFIG_STM32H7_DMA2=y +CONFIG_STM32H7_DMACAPABLE=y +CONFIG_STM32H7_FLOWCONTROL_BROKEN=y +CONFIG_STM32H7_I2C1=y +CONFIG_STM32H7_I2C_DYNTIMEO=y +CONFIG_STM32H7_I2C_DYNTIMEO_STARTSTOP=10 +CONFIG_STM32H7_OTGFS=y +CONFIG_STM32H7_PROGMEM=y +CONFIG_STM32H7_RTC=y +CONFIG_STM32H7_RTC_AUTO_LSECLOCK_START_DRV_CAPABILITY=y +CONFIG_STM32H7_RTC_MAGIC_REG=1 +CONFIG_STM32H7_SAVE_CRASHDUMP=y +CONFIG_STM32H7_SDMMC1=y +CONFIG_STM32H7_SERIALBRK_BSDCOMPAT=y +CONFIG_STM32H7_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32H7_SPI1=y +CONFIG_STM32H7_SPI2=y +CONFIG_STM32H7_SPI4=y +CONFIG_STM32H7_SPI4_DMA=y +CONFIG_STM32H7_SPI4_DMA_BUFFER=1024 +CONFIG_STM32H7_SPI_DMA=y +CONFIG_STM32H7_UART4=y +CONFIG_STM32H7_UART7=y +CONFIG_STM32H7_USART1=y +CONFIG_STM32H7_USART2=y +CONFIG_STM32H7_USART3=y +CONFIG_STM32H7_USART6=y +CONFIG_STM32H7_USART_BREAKS=y +CONFIG_STM32H7_USART_INVERT=y +CONFIG_STM32H7_USART_SINGLEWIRE=y +CONFIG_STM32H7_USART_SWAP=y +CONFIG_SYSTEM_CDCACM=y +CONFIG_SYSTEM_NSH=y +CONFIG_TASK_NAME_SIZE=24 +CONFIG_TTY_SIGINT=y +CONFIG_TTY_SIGTSTP=y +CONFIG_UART4_BAUD=57600 +CONFIG_UART4_RXBUFSIZE=600 +CONFIG_UART4_TXBUFSIZE=1500 +CONFIG_UART7_BAUD=57600 +CONFIG_UART7_RXBUFSIZE=600 +CONFIG_UART7_TXBUFSIZE=1500 +CONFIG_USART1_BAUD=57600 +CONFIG_USART1_RXBUFSIZE=600 +CONFIG_USART1_TXBUFSIZE=1500 +CONFIG_USART2_BAUD=57600 +CONFIG_USART2_RXBUFSIZE=600 +CONFIG_USART2_RXDMA=y +CONFIG_USART2_TXBUFSIZE=2500 +CONFIG_USART3_BAUD=57600 +CONFIG_USART3_RXBUFSIZE=600 +CONFIG_USART3_SERIAL_CONSOLE=y +CONFIG_USART3_TXBUFSIZE=1500 +CONFIG_USART6_BAUD=57600 +CONFIG_USART6_RXBUFSIZE=600 +CONFIG_USART6_TXBUFSIZE=1500 +CONFIG_USBDEV=y +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_MAXPOWER=500 +CONFIG_USEC_PER_TICK=1000 +CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_ENTRYPOINT="nsh_main" +CONFIG_WATCHDOG=y diff --git a/boards/holybro/kakuteh7v2/nuttx-config/scripts/bootloader_script.ld b/boards/holybro/kakuteh7v2/nuttx-config/scripts/bootloader_script.ld new file mode 100644 index 0000000000..fc0a5589be --- /dev/null +++ b/boards/holybro/kakuteh7v2/nuttx-config/scripts/bootloader_script.ld @@ -0,0 +1,215 @@ +/**************************************************************************** + * scripts/script.ld + * + * Copyright (C) 2016, 2019 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* The KakuteH7 uses an STM32H743VI has 2048Kb of main FLASH memory. + * The flash memory is partitioned into a User Flash memory and a System + * Flash memory. Each of these memories has two banks: + * + * 1) User Flash memory: + * + * Bank 1: Start address 0x0800:0000 to 0x080F:FFFF with 8 sectors, 128Kb each + * Bank 2: Start address 0x0810:0000 to 0x081F:FFFF with 8 sectors, 128Kb each + * + * 2) System Flash memory: + * + * Bank 1: Start address 0x1FF0:0000 to 0x1FF1:FFFF with 1 x 128Kb sector + * Bank 1: Start address 0x1FF4:0000 to 0x1FF5:FFFF with 1 x 128Kb sector + * + * 3) User option bytes for user configuration, only in Bank 1. + * + * In the STM32H743VI, two different boot spaces can be selected through + * the BOOT pin and the boot base address programmed in the BOOT_ADD0 and + * BOOT_ADD1 option bytes: + * + * 1) BOOT=0: Boot address defined by user option byte BOOT_ADD0[15:0]. + * ST programmed value: Flash memory at 0x0800:0000 + * 2) BOOT=1: Boot address defined by user option byte BOOT_ADD1[15:0]. + * ST programmed value: System bootloader at 0x1FF0:0000 + * + * The KakuteH7 has a switch on board, the BOOT0 pin is at ground so by default, + * the STM32 will boot to address 0x0800:0000 in FLASH unless the switch is + * depressed, then the boot will be from 0x1FF0:0000 + * + * The STM32H743VI also has 1024Kb of data SRAM. + * SRAM is split up into several blocks and into three power domains: + * + * 1) TCM SRAMs are dedicated to the Cortex-M7 and are accessible with + * 0 wait states by the Cortex-M7 and by MDMA through AHBS slave bus + * + * 1.1) 128Kb of DTCM-RAM beginning at address 0x2000:0000 + * + * The DTCM-RAM is organized as 2 x 64Kb DTCM-RAMs on 2 x 32 bit + * DTCM ports. The DTCM-RAM could be used for critical real-time + * data, such as interrupt service routines or stack / heap memory. + * Both DTCM-RAMs can be used in parallel (for load/store operations) + * thanks to the Cortex-M7 dual issue capability. + * + * 1.2) 64Kb of ITCM-RAM beginning at address 0x0000:0000 + * + * This RAM is connected to ITCM 64-bit interface designed for + * execution of critical real-times routines by the CPU. + * + * 2) AXI SRAM (D1 domain) accessible by all system masters except BDMA + * through D1 domain AXI bus matrix + * + * 2.1) 512Kb of SRAM beginning at address 0x2400:0000 + * + * 3) AHB SRAM (D2 domain) accessible by all system masters except BDMA + * through D2 domain AHB bus matrix + * + * 3.1) 128Kb of SRAM1 beginning at address 0x3000:0000 + * 3.2) 128Kb of SRAM2 beginning at address 0x3002:0000 + * 3.3) 32Kb of SRAM3 beginning at address 0x3004:0000 + * + * SRAM1 - SRAM3 are one contiguous block: 288Kb at address 0x3000:0000 + * + * 4) AHB SRAM (D3 domain) accessible by most of system masters + * through D3 domain AHB bus matrix + * + * 4.1) 64Kb of SRAM4 beginning at address 0x3800:0000 + * 4.1) 4Kb of backup RAM beginning at address 0x3880:0000 + * + * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 + * where the code expects to begin execution by jumping to the entry point in + * the 0x0800:0000 address range. + * + * The bootloader uses the first sector of the flash, which is 128K in length. + */ + +MEMORY +{ + itcm (rwx) : ORIGIN = 0x00000000, LENGTH = 64K + flash (rx) : ORIGIN = 0x08000000, LENGTH = 128K + dtcm1 (rwx) : ORIGIN = 0x20000000, LENGTH = 64K + dtcm2 (rwx) : ORIGIN = 0x20010000, LENGTH = 64K + sram (rwx) : ORIGIN = 0x24000000, LENGTH = 512K + sram1 (rwx) : ORIGIN = 0x30000000, LENGTH = 128K + sram2 (rwx) : ORIGIN = 0x30020000, LENGTH = 128K + sram3 (rwx) : ORIGIN = 0x30040000, LENGTH = 32K + sram4 (rwx) : ORIGIN = 0x38000000, LENGTH = 64K + bbram (rwx) : ORIGIN = 0x38800000, LENGTH = 4K +} + +OUTPUT_ARCH(arm) +EXTERN(_vectors) +ENTRY(_stext) + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) +EXTERN(_bootdelay_signature) + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + . = ALIGN(32); + /* + This signature provides the bootloader with a way to delay booting + */ + _bootdelay_signature = ABSOLUTE(.); + FILL(0xffecc2925d7d05c5) + . += 8; + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.glue_7) + *(.glue_7t) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + + } > flash + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > flash + + + .ARM.extab : { + *(.ARM.extab*) + } > flash + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > flash + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > sram AT > flash + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + . = ALIGN(4); + _ebss = ABSOLUTE(.); + } > sram + + /* Stabs debugging sections. */ + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } +} diff --git a/boards/holybro/kakuteh7v2/nuttx-config/scripts/script.ld b/boards/holybro/kakuteh7v2/nuttx-config/scripts/script.ld new file mode 100644 index 0000000000..ae07f4bfca --- /dev/null +++ b/boards/holybro/kakuteh7v2/nuttx-config/scripts/script.ld @@ -0,0 +1,228 @@ +/**************************************************************************** + * scripts/script.ld + * + * Copyright (C) 2016, 2019 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* The KakuteH7 uses an STM32H743VI has 2048Kb of main FLASH memory. + * The flash memory is partitioned into a User Flash memory and a System + * Flash memory. Each of these memories has two banks: + * + * 1) User Flash memory: + * + * Bank 1: Start address 0x0800:0000 to 0x080F:FFFF with 8 sectors, 128Kb each + * Bank 2: Start address 0x0810:0000 to 0x081F:FFFF with 8 sectors, 128Kb each + * + * 2) System Flash memory: + * + * Bank 1: Start address 0x1FF0:0000 to 0x1FF1:FFFF with 1 x 128Kb sector + * Bank 1: Start address 0x1FF4:0000 to 0x1FF5:FFFF with 1 x 128Kb sector + * + * 3) User option bytes for user configuration, only in Bank 1. + * + * In the STM32H743VI, two different boot spaces can be selected through + * the BOOT pin and the boot base address programmed in the BOOT_ADD0 and + * BOOT_ADD1 option bytes: + * + * 1) BOOT=0: Boot address defined by user option byte BOOT_ADD0[15:0]. + * ST programmed value: Flash memory at 0x0800:0000 + * 2) BOOT=1: Boot address defined by user option byte BOOT_ADD1[15:0]. + * ST programmed value: System bootloader at 0x1FF0:0000 + * + * The KakuteH7 has a switch on board, the BOOT0 pin is at ground so by default, + * the STM32 will boot to address 0x0800:0000 in FLASH unless the switch is + * depressed, then the boot will be from 0x1FF0:0000 + * + * The STM32H743VI also has 1024Kb of data SRAM. + * SRAM is split up into several blocks and into three power domains: + * + * 1) TCM SRAMs are dedicated to the Cortex-M7 and are accessible with + * 0 wait states by the Cortex-M7 and by MDMA through AHBS slave bus + * + * 1.1) 128Kb of DTCM-RAM beginning at address 0x2000:0000 + * + * The DTCM-RAM is organized as 2 x 64Kb DTCM-RAMs on 2 x 32 bit + * DTCM ports. The DTCM-RAM could be used for critical real-time + * data, such as interrupt service routines or stack / heap memory. + * Both DTCM-RAMs can be used in parallel (for load/store operations) + * thanks to the Cortex-M7 dual issue capability. + * + * 1.2) 64Kb of ITCM-RAM beginning at address 0x0000:0000 + * + * This RAM is connected to ITCM 64-bit interface designed for + * execution of critical real-times routines by the CPU. + * + * 2) AXI SRAM (D1 domain) accessible by all system masters except BDMA + * through D1 domain AXI bus matrix + * + * 2.1) 512Kb of SRAM beginning at address 0x2400:0000 + * + * 3) AHB SRAM (D2 domain) accessible by all system masters except BDMA + * through D2 domain AHB bus matrix + * + * 3.1) 128Kb of SRAM1 beginning at address 0x3000:0000 + * 3.2) 128Kb of SRAM2 beginning at address 0x3002:0000 + * 3.3) 32Kb of SRAM3 beginning at address 0x3004:0000 + * + * SRAM1 - SRAM3 are one contiguous block: 288Kb at address 0x3000:0000 + * + * 4) AHB SRAM (D3 domain) accessible by most of system masters + * through D3 domain AHB bus matrix + * + * 4.1) 64Kb of SRAM4 beginning at address 0x3800:0000 + * 4.1) 4Kb of backup RAM beginning at address 0x3880:0000 + * + * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 + * where the code expects to begin execution by jumping to the entry point in + * the 0x0800:0000 address range. + */ + +MEMORY +{ + ITCM_RAM (rwx) : ORIGIN = 0x00000000, LENGTH = 64K + FLASH (rx) : ORIGIN = 0x08020000, LENGTH = 1792K /* params in last sector */ + + DTCM1_RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 64K + DTCM2_RAM (rwx) : ORIGIN = 0x20010000, LENGTH = 64K + AXI_SRAM (rwx) : ORIGIN = 0x24000000, LENGTH = 512K /* D1 domain AXI bus */ + SRAM1 (rwx) : ORIGIN = 0x30000000, LENGTH = 128K /* D2 domain AHB bus */ + SRAM2 (rwx) : ORIGIN = 0x30020000, LENGTH = 128K /* D2 domain AHB bus */ + SRAM3 (rwx) : ORIGIN = 0x30040000, LENGTH = 32K /* D2 domain AHB bus */ + SRAM4 (rwx) : ORIGIN = 0x38000000, LENGTH = 64K /* D3 domain */ + BKPRAM (rwx) : ORIGIN = 0x38800000, LENGTH = 4K +} + +OUTPUT_ARCH(arm) +EXTERN(_vectors) +ENTRY(_stext) + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) +EXTERN(_bootdelay_signature) + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + . = ALIGN(32); + /* + This signature provides the bootloader with a way to delay booting + */ + _bootdelay_signature = ABSOLUTE(.); + FILL(0xffecc2925d7d05c5) + . += 8; + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.glue_7) + *(.glue_7t) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + + } > FLASH + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > FLASH + + + .ARM.extab : { + *(.ARM.extab*) + } > FLASH + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > FLASH + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + + /* Pad out last section as the STM32H7 Flash write size is 256 bits. 32 bytes */ + . = ALIGN(16); + FILL(0xffff) + . += 16; + } > AXI_SRAM AT > FLASH = 0xffff + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + . = ALIGN(4); + _ebss = ABSOLUTE(.); + } > AXI_SRAM + + /* Emit the the D3 power domain section for locating BDMA data */ + + .sram4_reserve (NOLOAD) : + { + *(.sram4) + . = ALIGN(4); + _sram4_heap_start = ABSOLUTE(.); + } > SRAM4 + + /* Stabs debugging sections. */ + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } +} diff --git a/boards/holybro/kakuteh7v2/src/CMakeLists.txt b/boards/holybro/kakuteh7v2/src/CMakeLists.txt new file mode 100644 index 0000000000..220642b2a5 --- /dev/null +++ b/boards/holybro/kakuteh7v2/src/CMakeLists.txt @@ -0,0 +1,67 @@ +############################################################################ +# +# Copyright (c) 2016 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +if("${PX4_BOARD_LABEL}" STREQUAL "bootloader") + add_library(drivers_board + bootloader_main.c + usb.c + ) + target_link_libraries(drivers_board + PRIVATE + nuttx_arch # sdio + nuttx_drivers # sdio + bootloader + ) + target_include_directories(drivers_board PRIVATE ${PX4_SOURCE_DIR}/platforms/nuttx/src/bootloader/common) + +else() + add_library(drivers_board + i2c.cpp + init.c + led.c + spi.cpp + timer_config.cpp + usb.c + ) + add_dependencies(drivers_board arch_board_hw_info) + + target_link_libraries(drivers_board + PRIVATE + arch_io_pins + arch_spi + arch_board_hw_info + drivers__led # drv_led_start + nuttx_arch # sdio + nuttx_drivers # sdio + px4_layer + ) +endif() diff --git a/boards/holybro/kakuteh7v2/src/board_config.h b/boards/holybro/kakuteh7v2/src/board_config.h new file mode 100644 index 0000000000..99395eabb9 --- /dev/null +++ b/boards/holybro/kakuteh7v2/src/board_config.h @@ -0,0 +1,213 @@ +/**************************************************************************** + * + * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file board_config.h + * + * holybro KakuteH7 internal definitions + */ + +#pragma once + +/**************************************************************************************************** + * Included Files + ****************************************************************************************************/ + +#include +#include +#include + +#include + +/**************************************************************************************************** + * Definitions + ****************************************************************************************************/ + +/* Configuration ************************************************************************************/ + +# define BOARD_HAS_USB_VALID 1 +# define BOARD_HAS_NBAT_V 1 +# define BOARD_HAS_NBAT_I 1 + +/* Holybro KakuteH7 GPIOs ************************************************************************/ + +/* LEDs are driven with push open drain to support Anode to 5V or 3.3V */ + +#define GPIO_nLED_RED /* PC2 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN2) + +#define BOARD_HAS_CONTROL_STATUS_LEDS 1 +#define BOARD_OVERLOAD_LED LED_RED + +/* + * ADC channels + * + * These are the channel numbers of the ADCs of the microcontroller that + * can be used by the Px4 Firmware in the adc driver + */ + +/* ADC defines to be used in sensors.cpp to read from a particular channel */ + +#define ADC1_CH(n) (n) + +/* Define GPIO pins used as ADC N.B. Channel numbers must match below */ + +#define PX4_ADC_GPIO \ + /* PC0 */ GPIO_ADC123_INP10, \ + /* PC1 */ GPIO_ADC123_INP11, \ + /* PC5 */ GPIO_ADC12_INP8 + +/* Define Channel numbers must match above GPIO pin IN(n)*/ + +#define ADC_BATTERY_VOLTAGE_CHANNEL /* PC0 */ ADC1_CH(10) +#define ADC_BATTERY_CURRENT_CHANNEL /* PC1 */ ADC1_CH(11) +#define ADC_RSSI_IN_CHANNEL /* PC5 */ ADC1_CH(8) + +#define ADC_CHANNELS \ + ((1 << ADC_BATTERY_VOLTAGE_CHANNEL) | \ + (1 << ADC_BATTERY_CURRENT_CHANNEL) | \ + (1 << ADC_RSSI_IN_CHANNEL)) + +#define BOARD_ADC_OPEN_CIRCUIT_V (5.6f) + + + +/* PWM + */ +#define DIRECT_PWM_OUTPUT_CHANNELS 8 + +#define BOARD_NUM_IO_TIMERS 4 + + +/* Tone alarm output */ + +#define GPIO_TONE_ALARM_IDLE /* PC13 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13) +#define GPIO_TONE_ALARM_GPIO /* PC13 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) + +/* USB OTG FS + * + * PA9 OTG_FS_VBUS VBUS sensing + */ +#define GPIO_OTGFS_VBUS /* PA8 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_100MHz|GPIO_PORTA|GPIO_PIN8) + +/* High-resolution timer */ +#define HRT_TIMER 4 /* use timer3 for the HRT */ +#define HRT_TIMER_CHANNEL 1 /* use capture/compare channel 1 */ + +/* RC Serial port */ + +/* PWM input driver. Use FMU AUX5 pins attached to timer4 channel 2 */ + +#define PWMIN_TIMER 5 +#define PWMIN_TIMER_CHANNEL /* T5C1 */ 1 +#define GPIO_PWM_IN /* PA0 */ GPIO_TIM5_CH1IN + +#define GPIO_RSSI_IN /* PC5 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTC|GPIO_PIN5) + +#define GPIO_RF_SWITCH /* PE13 */ (GPIO_OUTPUT|GPIO_PULLDOWN|GPIO_PORTE|GPIO_PIN13) + +/* Power switch controls ******************************************************/ + +#define SDIO_SLOTNO 0 /* Only one slot */ +#define SDIO_MINOR 0 + +/* SD card bringup does not work if performed on the IDLE thread because it + * will cause waiting. Use either: + * + * CONFIG_BOARDCTL=y, OR + * CONFIG_BOARD_INITIALIZE=y && CONFIG_BOARD_INITTHREAD=y + */ + +#if defined(CONFIG_BOARD_INITIALIZE) && !defined(CONFIG_BOARDCTL) && \ + !defined(CONFIG_BOARD_INITTHREAD) +# warning SDIO initialization cannot be perfomed on the IDLE thread +#endif + +/* By Providing BOARD_ADC_USB_CONNECTED (using the px4_arch abstraction) + * this board support the ADC system_power interface, and therefore + * provides the true logic GPIO BOARD_ADC_xxxx macros. + */ +#define BOARD_ADC_USB_CONNECTED (px4_arch_gpioread(GPIO_OTGFS_VBUS)) + +/* Board never powers off the Servo rail */ + +#define BOARD_ADC_SERVO_VALID (1) + +#define BOARD_ADC_BRICK1_VALID (1) + + + +/* This board provides a DMA pool and APIs */ +#define BOARD_DMA_ALLOC_POOL_SIZE 5120 + +/* This board provides the board_on_reset interface */ + +#define BOARD_HAS_ON_RESET 1 + +#define PX4_GPIO_INIT_LIST { \ + PX4_ADC_GPIO, \ + GPIO_TONE_ALARM_IDLE, \ + GPIO_RSSI_IN, \ + GPIO_RF_SWITCH, \ + } + +#define BOARD_ENABLE_CONSOLE_BUFFER + +#define FLASH_BASED_PARAMS + +__BEGIN_DECLS + +/**************************************************************************************************** + * Public Types + ****************************************************************************************************/ + +/**************************************************************************************************** + * Public data + ****************************************************************************************************/ + +#ifndef __ASSEMBLY__ + +/**************************************************************************************************** + * Public Functions + ****************************************************************************************************/ + +extern void stm32_spiinitialize(void); + +extern void stm32_usbinitialize(void); + +extern void board_peripheral_reset(int ms); + +#include + +#endif /* __ASSEMBLY__ */ + +__END_DECLS diff --git a/boards/holybro/kakuteh7v2/src/bootloader_main.c b/boards/holybro/kakuteh7v2/src/bootloader_main.c new file mode 100644 index 0000000000..915b7f3d29 --- /dev/null +++ b/boards/holybro/kakuteh7v2/src/bootloader_main.c @@ -0,0 +1,75 @@ +/**************************************************************************** + * + * Copyright (c) 2019, 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file bootloader_main.c + * + * FMU-specific early startup code for bootloader +*/ + +#include "board_config.h" +#include "bl.h" + +#include +#include +#include +#include +#include +#include "arm_internal.h" +#include + +extern int sercon_main(int c, char **argv); + +__EXPORT void board_on_reset(int status) {} + +__EXPORT void stm32_boardinitialize(void) +{ + /* configure USB interfaces */ + stm32_usbinitialize(); +} + +__EXPORT int board_app_initialize(uintptr_t arg) +{ + return 0; +} + +void board_late_initialize(void) +{ + sercon_main(0, NULL); +} + +extern void sys_tick_handler(void); +void board_timerhook(void) +{ + sys_tick_handler(); +} diff --git a/boards/holybro/kakuteh7v2/src/hw_config.h b/boards/holybro/kakuteh7v2/src/hw_config.h new file mode 100644 index 0000000000..72b3d6ac6c --- /dev/null +++ b/boards/holybro/kakuteh7v2/src/hw_config.h @@ -0,0 +1,125 @@ +/* + * hw_config.h + * + * Created on: May 17, 2015 + * Author: david_s5 + */ + +#ifndef HW_CONFIG_H_ +#define HW_CONFIG_H_ + +/**************************************************************************** + * 10-8--2016: + * To simplify the ripple effect on the tools, we will be using + * /dev/serial/by-id/PX4 to locate PX4 devices. Therefore + * moving forward all Bootloaders must contain the prefix "PX4 BL " + * in the USBDEVICESTRING + * This Change will be made in an upcoming BL release + ****************************************************************************/ +/* + * Define usage to configure a bootloader + * + * + * Constant example Usage + * APP_LOAD_ADDRESS 0x08004000 - The address in Linker Script, where the app fw is org-ed + * BOOTLOADER_DELAY 5000 - Ms to wait while under USB pwr or bootloader request + * BOARD_FMUV2 + * INTERFACE_USB 1 - (Optional) Scan and use the USB interface for bootloading + * INTERFACE_USART 1 - (Optional) Scan and use the Serial interface for bootloading + * USBDEVICESTRING "PX4 BL FMU v2.x" - USB id string + * USBPRODUCTID 0x0011 - PID Should match defconfig + * BOOT_DELAY_ADDRESS 0x000001a0 - (Optional) From the linker script from Linker Script to get a custom + * delay provided by an APP FW + * BOARD_TYPE 9 - Must match .prototype boad_id + * _FLASH_KBYTES (*(uint16_t *)0x1fff7a22) - Run time flash size detection + * BOARD_FLASH_SECTORS ((_FLASH_KBYTES == 0x400) ? 11 : 23) - Run time determine the physical last sector + * BOARD_FLASH_SECTORS 11 - Hard coded zero based last sector + * BOARD_FLASH_SIZE (_FLASH_KBYTES*1024)- Total Flash size of device, determined at run time. + * (1024 * 1024) - Hard coded Total Flash of device - The bootloader and app reserved will be deducted + * programmatically + * + * BOARD_FIRST_FLASH_SECTOR_TO_ERASE 2 - Optional sectors index in the flash_sectors table (F4 only), to begin erasing. + * This is to allow sectors to be reserved for app fw usage. That will NOT be erased + * during a FW upgrade. + * The default is 0, and selects the first sector to be erased, as the 0th entry in the + * flash_sectors table. Which is the second physical sector of FLASH in the device. + * The first physical sector of FLASH is used by the bootloader, and is not defined + * in the table. + * + * APP_RESERVATION_SIZE (BOARD_FIRST_FLASH_SECTOR_TO_ERASE * 16 * 1024) - Number of bytes reserved by the APP FW. This number plus + * BOOTLOADER_RESERVATION_SIZE will be deducted from + * BOARD_FLASH_SIZE to determine the size of the App FW + * and hence the address space of FLASH to erase and program. + * USBMFGSTRING "PX4 AP" - Optional USB MFG string (default is '3D Robotics' if not defined.) + * SERIAL_BREAK_DETECT_DISABLED - Optional prevent break selection on Serial port from entering or staying in BL + * + * * Other defines are somewhat self explanatory. + */ + +/* Boot device selection list*/ +#define USB0_DEV 0x01 +#define SERIAL0_DEV 0x02 +#define SERIAL1_DEV 0x04 + +#define APP_LOAD_ADDRESS 0x08020000 +#define BOOTLOADER_DELAY 3000 +#define INTERFACE_USB 1 +#define INTERFACE_USB_CONFIG "/dev/ttyACM0" +#define BOARD_VBUS MK_GPIO_INPUT(GPIO_OTGFS_VBUS) + +//#define USE_VBUS_PULL_DOWN +#define BOOT_DELAY_ADDRESS 0x000001a0 +#define BOARD_TYPE 1053 +#define BOARD_FLASH_SECTORS (14) +#define BOARD_FLASH_SIZE (16 * 128 * 1024) +#define APP_RESERVATION_SIZE (1 * 128 * 1024) + +#define OSC_FREQ 8 + +#define BOARD_PIN_LED_ACTIVITY GPIO_nLED_RED +#define BOARD_LED_ON 0 +#define BOARD_LED_OFF 1 + +#define SERIAL_BREAK_DETECT_DISABLED 1 + +/* + * Uncommenting this allows to force the bootloader through + * a PWM output pin. As this can accidentally initialize + * an ESC prematurely, it is not recommended. This feature + * has not been used and hence defaults now to off. + * + * # define BOARD_FORCE_BL_PIN_OUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN14) + * # define BOARD_FORCE_BL_PIN_IN (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN11) + * + * # define BOARD_POWER_PIN_OUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN4) + * # define BOARD_POWER_ON 1 + * # define BOARD_POWER_OFF 0 + * # undef BOARD_POWER_PIN_RELEASE // Leave pin enabling Power - un comment to release (disable power) + * +*/ + +#if !defined(ARCH_SN_MAX_LENGTH) +# define ARCH_SN_MAX_LENGTH 12 +#endif + +#if !defined(APP_RESERVATION_SIZE) +# define APP_RESERVATION_SIZE 0 +#endif + +#if !defined(BOARD_FIRST_FLASH_SECTOR_TO_ERASE) +# define BOARD_FIRST_FLASH_SECTOR_TO_ERASE 1 +#endif + +#if !defined(USB_DATA_ALIGN) +# define USB_DATA_ALIGN +#endif + +#ifndef BOOT_DEVICES_SELECTION +# define BOOT_DEVICES_SELECTION USB0_DEV|SERIAL0_DEV|SERIAL1_DEV +#endif + +#ifndef BOOT_DEVICES_FILTER_ONUSB +# define BOOT_DEVICES_FILTER_ONUSB USB0_DEV|SERIAL0_DEV|SERIAL1_DEV +#endif + +#endif /* HW_CONFIG_H_ */ diff --git a/boards/holybro/kakuteh7v2/src/i2c.cpp b/boards/holybro/kakuteh7v2/src/i2c.cpp new file mode 100644 index 0000000000..e9f26f9a9e --- /dev/null +++ b/boards/holybro/kakuteh7v2/src/i2c.cpp @@ -0,0 +1,38 @@ +/**************************************************************************** + * + * Copyright (C) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include + +constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { + initI2CBusExternal(1), +}; diff --git a/boards/holybro/kakuteh7v2/src/init.c b/boards/holybro/kakuteh7v2/src/init.c new file mode 100644 index 0000000000..7e55cc1770 --- /dev/null +++ b/boards/holybro/kakuteh7v2/src/init.c @@ -0,0 +1,261 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file init.c + * + * PX4FMU-specific early startup code. This file implements the + * board_app_initialize() function that is called early by nsh during startup. + * + * Code here is run before the rcS script is invoked; it should start required + * subsystems and perform board-specific initialisation. + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include "board_config.h" + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include "arm_internal.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +# if defined(FLASH_BASED_PARAMS) +# include +#endif + + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ + +/* Configuration ************************************************************/ + +/* + * Ideally we'd be able to get these from arm_internal.h, + * but since we want to be able to disable the NuttX use + * of leds for system indication at will and there is no + * separate switch, we need to build independent of the + * CONFIG_ARCH_LEDS configuration switch. + */ +__BEGIN_DECLS +extern void led_init(void); +extern void led_on(int led); +extern void led_off(int led); +__END_DECLS + + +/************************************************************************************ + * Name: board_peripheral_reset + * + * Description: + * + ************************************************************************************/ +__EXPORT void board_peripheral_reset(int ms) +{ +} + +/************************************************************************************ + * Name: board_on_reset + * + * Description: + * Optionally provided function called on entry to board_system_reset + * It should perform any house keeping prior to the rest. + * + * status - 1 if resetting to boot loader + * 0 if just resetting + * + ************************************************************************************/ +__EXPORT void board_on_reset(int status) +{ + for (int i = 0; i < DIRECT_PWM_OUTPUT_CHANNELS; ++i) { + px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); + } + + if (status >= 0) { + up_mdelay(6); + } +} + +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the initialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ + +__EXPORT void +stm32_boardinitialize(void) +{ + board_on_reset(-1); /* Reset PWM first thing */ + + /* configure LEDs */ + + board_autoled_initialize(); + + /* configure pins */ + + const uint32_t gpio[] = PX4_GPIO_INIT_LIST; + px4_gpio_init(gpio, arraySize(gpio)); + + board_control_spi_sensors_power_configgpio(); + + /* Turn bluetooth off by default (no mavlink support yet) */ + px4_arch_gpiowrite(GPIO_RF_SWITCH, 0); + + /* configure USB interfaces */ + + stm32_usbinitialize(); + +} + +/**************************************************************************** + * Name: board_app_initialize + * + * Description: + * Perform application specific initialization. This function is never + * called directly from application code, but only indirectly via the + * (non-standard) boardctl() interface using the command BOARDIOC_INIT. + * + * Input Parameters: + * arg - The boardctl() argument is passed to the board_app_initialize() + * implementation without modification. The argument has no + * meaning to NuttX; the meaning of the argument is a contract + * between the board-specific initalization logic and the the + * matching application logic. The value cold be such things as a + * mode enumeration value, a set of DIP switch switch settings, a + * pointer to configuration data read from a file or serial FLASH, + * or whatever you would like to do with it. Every implementation + * should accept zero/NULL as a default configuration. + * + * Returned Value: + * Zero (OK) is returned on success; a negated errno value is returned on + * any failure to indicate the nature of the failure. + * + ****************************************************************************/ + + +__EXPORT int board_app_initialize(uintptr_t arg) +{ + /* Power on Interfaces */ + board_control_spi_sensors_power(true, 0xffff); + + /* Need hrt running before using the ADC */ + + px4_platform_init(); + + /* configure SPI interfaces */ + + stm32_spiinitialize(); + + /* configure the DMA allocator */ + + if (board_dma_alloc_init() < 0) { + syslog(LOG_ERR, "[boot] DMA alloc FAILED\n"); + } + +#if defined(SERIAL_HAVE_RXDMA) + // set up the serial DMA polling at 1ms intervals for received bytes that have not triggered a DMA event. + static struct hrt_call serial_dma_call; + hrt_call_every(&serial_dma_call, 1000, 1000, (hrt_callout)stm32_serial_dma_poll, NULL); +#endif + + /* initial LED state */ + drv_led_start(); + led_off(LED_RED); + + if (board_hardfault_init(2, true) != 0) { + led_on(LED_RED); + } + + /* Get the SPI port for the microSD slot */ + struct spi_dev_s *spi_dev = stm32_spibus_initialize(CONFIG_NSH_MMCSDSPIPORTNO); + + if (!spi_dev) { + syslog(LOG_ERR, "[boot] FAILED to initialize SPI port %d\n", CONFIG_NSH_MMCSDSPIPORTNO); + led_on(LED_BLUE); + } + + up_udelay(20); + + +#if defined(FLASH_BASED_PARAMS) + static sector_descriptor_t params_sector_map[] = { + {15, 128 * 1024, 0x081E0000}, + {0, 0, 0}, + }; + + /* Initialize the flashfs layer to use heap allocated memory */ + int result = parameter_flashfs_init(params_sector_map, NULL, 0); + + if (result != OK) { + syslog(LOG_ERR, "[boot] FAILED to init params in FLASH %d\n", result); + led_on(LED_AMBER); + } + +#endif + + /* Configure the HW based on the manifest */ + + px4_platform_configure(); + + return OK; +} diff --git a/boards/holybro/kakuteh7v2/src/led.c b/boards/holybro/kakuteh7v2/src/led.c new file mode 100644 index 0000000000..945c383eb7 --- /dev/null +++ b/boards/holybro/kakuteh7v2/src/led.c @@ -0,0 +1,223 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include + +#include + +#include "chip.h" +#include "stm32_gpio.h" +#include "board_config.h" + +#include +#include + +/* + * Ideally we'd be able to get these from arm_internal.h, + * but since we want to be able to disable the NuttX use + * of leds for system indication at will and there is no + * separate switch, we need to build independent of the + * CONFIG_ARCH_LEDS configuration switch. + */ +__BEGIN_DECLS +extern void led_init(void); +extern void led_on(int led); +extern void led_off(int led); +extern void led_toggle(int led); +__END_DECLS + +#ifdef CONFIG_ARCH_LEDS +static bool nuttx_owns_leds = true; +// B R S G +// 0 1 2 3 +static const uint8_t xlatpx4[] = {1, 2, 4, 0}; +# define xlat(p) xlatpx4[(p)] +static uint32_t g_ledmap[] = { + GPIO_nLED_RED, // Indexed by BOARD_LED_RED +}; + +#else + +# define xlat(p) (p) +static uint32_t g_ledmap[] = { + GPIO_nLED_RED, // Indexed by LED_RED, LED_AMBER +}; + +#endif + +__EXPORT void led_init(void) +{ + for (size_t l = 0; l < (sizeof(g_ledmap) / sizeof(g_ledmap[0])); l++) { + if (g_ledmap[l] != 0) { + stm32_configgpio(g_ledmap[l]); + } + } +} + +static void phy_set_led(int led, bool state) +{ + /* Drive Low to switch on */ + + if (g_ledmap[led] != 0) { + stm32_gpiowrite(g_ledmap[led], !state); + } +} + +static bool phy_get_led(int led) +{ + /* If Low it is on */ + if (g_ledmap[led] != 0) { + return !stm32_gpioread(g_ledmap[led]); + } + + return false; +} + +__EXPORT void led_on(int led) +{ + phy_set_led(xlat(led), true); +} + +__EXPORT void led_off(int led) +{ + phy_set_led(xlat(led), false); +} + +__EXPORT void led_toggle(int led) +{ + phy_set_led(xlat(led), !phy_get_led(xlat(led))); +} + +#ifdef CONFIG_ARCH_LEDS +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: board_autoled_initialize + ****************************************************************************/ + +void board_autoled_initialize(void) +{ + led_init(); +} + +/**************************************************************************** + * Name: board_autoled_on + ****************************************************************************/ + +void board_autoled_on(int led) +{ + if (!nuttx_owns_leds) { + return; + } + + switch (led) { + default: + break; + + case LED_HEAPALLOCATE: + phy_set_led(BOARD_LED_BLUE, true); + break; + + case LED_IRQSENABLED: + phy_set_led(BOARD_LED_BLUE, false); + phy_set_led(BOARD_LED_GREEN, true); + break; + + case LED_STACKCREATED: + phy_set_led(BOARD_LED_GREEN, true); + phy_set_led(BOARD_LED_BLUE, true); + break; + + case LED_INIRQ: + phy_set_led(BOARD_LED_BLUE, true); + break; + + case LED_SIGNAL: + phy_set_led(BOARD_LED_GREEN, true); + break; + + case LED_ASSERTION: + phy_set_led(BOARD_LED_RED, true); + phy_set_led(BOARD_LED_BLUE, true); + break; + + case LED_PANIC: + phy_set_led(BOARD_LED_RED, true); + break; + + case LED_IDLE : /* IDLE */ + phy_set_led(BOARD_LED_RED, true); + break; + } +} + +/**************************************************************************** + * Name: board_autoled_off + ****************************************************************************/ + +void board_autoled_off(int led) +{ + if (!nuttx_owns_leds) { + return; + } + + switch (led) { + default: + break; + + case LED_SIGNAL: + phy_set_led(BOARD_LED_GREEN, false); + break; + + case LED_INIRQ: + phy_set_led(BOARD_LED_BLUE, false); + break; + + case LED_ASSERTION: + phy_set_led(BOARD_LED_RED, false); + phy_set_led(BOARD_LED_BLUE, false); + break; + + case LED_PANIC: + phy_set_led(BOARD_LED_RED, false); + break; + + case LED_IDLE : /* IDLE */ + phy_set_led(BOARD_LED_RED, false); + break; + } +} + +#endif /* CONFIG_ARCH_LEDS */ diff --git a/boards/holybro/kakuteh7v2/src/spi.cpp b/boards/holybro/kakuteh7v2/src/spi.cpp new file mode 100644 index 0000000000..a8ce482c5d --- /dev/null +++ b/boards/holybro/kakuteh7v2/src/spi.cpp @@ -0,0 +1,52 @@ +/**************************************************************************** + * + * Copyright (C) 2020, 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include +#include + +constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(SPIDEV_MMCSD(0), SPI::CS{GPIO::PortA, GPIO::Pin4}) + }), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(DRV_OSD_DEVTYPE_ATXXXX, SPI::CS{GPIO::PortB, GPIO::Pin12}), + }), + initSPIBus(SPI::Bus::SPI4, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM20689, SPI::CS{GPIO::PortE, GPIO::Pin4}, SPI::DRDY{GPIO::PortE, GPIO::Pin1}), + initSPIDevice(DRV_IMU_DEVTYPE_MPU6000, SPI::CS{GPIO::PortE, GPIO::Pin4}, SPI::DRDY{GPIO::PortE, GPIO::Pin1}), + initSPIDevice(DRV_IMU_DEVTYPE_BMI270, SPI::CS{GPIO::PortE, GPIO::Pin4}, SPI::DRDY{GPIO::PortE, GPIO::Pin1}), + }), +}; + +static constexpr bool unused = validateSPIConfig(px4_spi_buses); diff --git a/boards/holybro/kakuteh7v2/src/timer_config.cpp b/boards/holybro/kakuteh7v2/src/timer_config.cpp new file mode 100644 index 0000000000..20eb0f15d8 --- /dev/null +++ b/boards/holybro/kakuteh7v2/src/timer_config.cpp @@ -0,0 +1,55 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include + +constexpr io_timers_t io_timers[MAX_IO_TIMERS] = { + initIOTimer(Timer::Timer3, DMA{DMA::Index1}), + initIOTimer(Timer::Timer2, DMA{DMA::Index1}), + initIOTimer(Timer::Timer5, DMA{DMA::Index1}), + initIOTimer(Timer::Timer8, DMA{DMA::Index1}), +}; + +constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = { + initIOTimerChannel(io_timers, {Timer::Timer3, Timer::Channel3}, {GPIO::PortB, GPIO::Pin0}), + initIOTimerChannel(io_timers, {Timer::Timer3, Timer::Channel4}, {GPIO::PortB, GPIO::Pin1}), + initIOTimerChannel(io_timers, {Timer::Timer2, Timer::Channel2}, {GPIO::PortB, GPIO::Pin3}), + initIOTimerChannel(io_timers, {Timer::Timer2, Timer::Channel3}, {GPIO::PortB, GPIO::Pin10}), + initIOTimerChannel(io_timers, {Timer::Timer5, Timer::Channel1}, {GPIO::PortA, GPIO::Pin0}), + initIOTimerChannel(io_timers, {Timer::Timer5, Timer::Channel3}, {GPIO::PortA, GPIO::Pin2}), + initIOTimerChannel(io_timers, {Timer::Timer8, Timer::Channel3}, {GPIO::PortC, GPIO::Pin8}), + initIOTimerChannel(io_timers, {Timer::Timer8, Timer::Channel4}, {GPIO::PortC, GPIO::Pin9}), +}; + +constexpr io_timers_channel_mapping_t io_timers_channel_mapping = + initIOTimerChannelMapping(io_timers, timer_io_channels); diff --git a/boards/holybro/kakuteh7v2/src/usb.c b/boards/holybro/kakuteh7v2/src/usb.c new file mode 100644 index 0000000000..6d42476b71 --- /dev/null +++ b/boards/holybro/kakuteh7v2/src/usb.c @@ -0,0 +1,105 @@ +/**************************************************************************** + * + * Copyright (C) 2016 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file px4fmu_usb.c + * + * Board-specific USB functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include "board_config.h" + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: stm32_usbinitialize + * + * Description: + * Called to setup USB-related GPIO pins for the PX4FMU board. + * + ************************************************************************************/ + +__EXPORT void stm32_usbinitialize(void) +{ + /* The OTG FS has an internal soft pull-up */ + + /* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */ + +#ifdef CONFIG_STM32H7_OTGFS + stm32_configgpio(GPIO_OTGFS_VBUS); +#endif +} + +/************************************************************************************ + * Name: stm32_usbsuspend + * + * Description: + * Board logic must provide the stm32_usbsuspend logic if the USBDEV driver is + * used. This function is called whenever the USB enters or leaves suspend mode. + * This is an opportunity for the board logic to shutdown clocks, power, etc. + * while the USB is suspended. + * + ************************************************************************************/ + +__EXPORT void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume) +{ + uinfo("resume: %d\n", resume); +} diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index b264086bd6..df21cd8875 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -87,7 +87,7 @@ #define DRV_GYR_DEVTYPE_MPU6050 0x35 #define DRV_IMU_DEVTYPE_MPU6500 0x36 - +#define DRV_IMU_DEVTYPE_BMI270 0x37 #define DRV_IMU_DEVTYPE_ICM20602 0x38 #define DRV_IMU_DEVTYPE_ICM20608G 0x3A diff --git a/src/drivers/imu/bosch/bmi270/BMI270.cpp b/src/drivers/imu/bosch/bmi270/BMI270.cpp new file mode 100644 index 0000000000..34e4b3d31f --- /dev/null +++ b/src/drivers/imu/bosch/bmi270/BMI270.cpp @@ -0,0 +1,891 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +#include "BMI270.hpp" +#define BMI270_DEBUG +#define BMI270_MAX_FIFO_SAMPLES 8 + +using namespace time_literals; + +static constexpr int16_t combine(uint8_t msb, uint8_t lsb) +{ + return (msb << 8u) | lsb; +} + +/** +* The following device config microcode has the following copyright: +* +* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. +* +* BSD-3-Clause +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* 1. Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* +* 2. Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in the +* documentation and/or other materials provided with the distribution. +* +* 3. Neither the name of the copyright holder nor the names of its +* contributors may be used to endorse or promote products derived from +* this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) +* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, +* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING +* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*/ +uint8_t maximum_fifo_config_file[] = { 0x5E, + 0xc8, 0x2e, 0x00, 0x2e, 0x80, 0x2e, 0x1a, 0x00, 0xc8, 0x2e, 0x00, 0x2e, 0xc8, 0x2e, 0x00, 0x2e, 0xc8, 0x2e, 0x00, + 0x2e, 0xc8, 0x2e, 0x00, 0x2e, 0xc8, 0x2e, 0x00, 0x2e, 0xc8, 0x2e, 0x00, 0x2e, 0x90, 0x32, 0x21, 0x2e, 0x59, 0xf5, + 0x10, 0x30, 0x21, 0x2e, 0x6a, 0xf5, 0x1a, 0x24, 0x22, 0x00, 0x80, 0x2e, 0x3b, 0x00, 0xc8, 0x2e, 0x44, 0x47, 0x22, + 0x00, 0x37, 0x00, 0xa4, 0x00, 0xff, 0x0f, 0xd1, 0x00, 0x07, 0xad, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x11, 0x24, 0xfc, 0xf5, 0x80, 0x30, 0x40, 0x42, 0x50, 0x50, 0x00, 0x30, 0x12, 0x24, 0xeb, + 0x00, 0x03, 0x30, 0x00, 0x2e, 0xc1, 0x86, 0x5a, 0x0e, 0xfb, 0x2f, 0x21, 0x2e, 0xfc, 0xf5, 0x13, 0x24, 0x63, 0xf5, + 0xe0, 0x3c, 0x48, 0x00, 0x22, 0x30, 0xf7, 0x80, 0xc2, 0x42, 0xe1, 0x7f, 0x3a, 0x25, 0xfc, 0x86, 0xf0, 0x7f, 0x41, + 0x33, 0x98, 0x2e, 0xc2, 0xc4, 0xd6, 0x6f, 0xf1, 0x30, 0xf1, 0x08, 0xc4, 0x6f, 0x11, 0x24, 0xff, 0x03, 0x12, 0x24, + 0x00, 0xfc, 0x61, 0x09, 0xa2, 0x08, 0x36, 0xbe, 0x2a, 0xb9, 0x13, 0x24, 0x38, 0x00, 0x64, 0xbb, 0xd1, 0xbe, 0x94, + 0x0a, 0x71, 0x08, 0xd5, 0x42, 0x21, 0xbd, 0x91, 0xbc, 0xd2, 0x42, 0xc1, 0x42, 0x00, 0xb2, 0xfe, 0x82, 0x05, 0x2f, + 0x50, 0x30, 0x21, 0x2e, 0x21, 0xf2, 0x00, 0x2e, 0x00, 0x2e, 0xd0, 0x2e, 0xf0, 0x6f, 0x02, 0x30, 0x02, 0x42, 0x20, + 0x26, 0xe0, 0x6f, 0x02, 0x31, 0x03, 0x40, 0x9a, 0x0a, 0x02, 0x42, 0xf0, 0x37, 0x05, 0x2e, 0x5e, 0xf7, 0x10, 0x08, + 0x12, 0x24, 0x1e, 0xf2, 0x80, 0x42, 0x83, 0x84, 0xf1, 0x7f, 0x0a, 0x25, 0x13, 0x30, 0x83, 0x42, 0x3b, 0x82, 0xf0, + 0x6f, 0x00, 0x2e, 0x00, 0x2e, 0xd0, 0x2e, 0x12, 0x40, 0x52, 0x42, 0x00, 0x2e, 0x12, 0x40, 0x52, 0x42, 0x3e, 0x84, + 0x00, 0x40, 0x40, 0x42, 0x7e, 0x82, 0xe1, 0x7f, 0xf2, 0x7f, 0x98, 0x2e, 0x6a, 0xd6, 0x21, 0x30, 0x23, 0x2e, 0x61, + 0xf5, 0xeb, 0x2c, 0xe1, 0x6f + }; +// end of Bosch microcode copyright + + +BMI270::BMI270(const I2CSPIDriverConfig &config) : + SPI(config), + I2CSPIDriver(config), + _drdy_gpio(config.drdy_gpio), + _px4_accel(get_device_id(), config.rotation), + _px4_gyro(get_device_id(), config.rotation) +{ + if (_drdy_gpio != 0) { + _drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed"); + } + + ConfigureSampleRate(_px4_gyro.get_max_rate_hz()); +} + +BMI270::~BMI270() +{ + perf_free(_bad_register_perf); + perf_free(_bad_transfer_perf); + perf_free(_fifo_empty_perf); + perf_free(_fifo_overflow_perf); + perf_free(_fifo_reset_perf); + perf_free(_drdy_missed_perf); +} + +int BMI270::init() +{ + int ret = SPI::init(); + + if (ret != PX4_OK) { + DEVICE_DEBUG("SPI::init failed (%i)", ret); + return ret; + } + + PX4_DEBUG("init function called, resetting..."); + + + return Reset() ? 0 : -1; +} + +bool BMI270::Reset() +{ + _state = STATE::RESET; + DataReadyInterruptDisable(); + ScheduleClear(); + ScheduleNow(); + return true; +} + +// Debug function that Ardupilot is equipped with +void BMI270::CheckErrorRegister() +{ +#ifdef BMI270_DEBUG + uint8_t err = RegisterRead(Register::ERR_REG); + + if (err) { + if ((err & 1) == 1) { + uint8_t status = RegisterRead(Register::INTERNAL_STATUS); + + switch (status & 0xF) { + case 0: + PX4_DEBUG("BMI270: not_init"); + break; + + case 2: + PX4_DEBUG("BMI270: init_err"); + break; + + case 3: + PX4_DEBUG("BMI270: drv_err"); + break; + + case 4: + PX4_DEBUG("BMI270: sns_stop"); + break; + + case 5: + PX4_DEBUG("BMI270: nvm_error"); + break; + + case 6: + PX4_DEBUG("BMI270: start_up_error"); + break; + + case 7: + PX4_DEBUG("BMI270: compat_error"); + break; + + case 1: // init ok + if ((status >> 5 & 1) == 1) { + PX4_DEBUG("BMI270: axes_remap_error"); + + } else if ((status >> 6 & 1) == 1) { + PX4_DEBUG("BMI270: odr_50hz_error"); + } + + break; + } + + } else if ((err >> 6 & 1) == 1) { + PX4_DEBUG("BMI270: fifo_err"); + + } else if ((err >> 7 & 1) == 1) { + PX4_DEBUG("BMI270: aux_err"); + + } else { + PX4_DEBUG("BMI270: internal error detected %d", err >> 1 & 0xF); + } + } + +#endif + +} + +void BMI270::exit_and_cleanup() +{ + DataReadyInterruptDisable(); + I2CSPIDriverBase::exit_and_cleanup(); +} + +void BMI270::print_status() +{ + I2CSPIDriverBase::print_status(); + + PX4_INFO("FIFO empty interval: %d us (%.1f Hz)", _fifo_empty_interval_us, 1e6 / _fifo_empty_interval_us); + + perf_print_counter(_bad_register_perf); + perf_print_counter(_bad_transfer_perf); + perf_print_counter(_fifo_empty_perf); + perf_print_counter(_fifo_overflow_perf); + perf_print_counter(_fifo_reset_perf); + perf_print_counter(_drdy_missed_perf); +} + +int BMI270::probe() +{ + // When starting communication with the BMI270, according page 123 of the + // datasheet a rising edge on CSB is required to start SPI. + // It is recommended to just read the CHIP_ID register once to do that + // but ignore the result. + RegisterRead(Register::CHIP_ID); + + // It takes 200us for the device to start SPI communication. + px4_usleep(200); + + const uint8_t CHIP_ID = RegisterRead(Register::CHIP_ID); + + if (CHIP_ID != chip_id) { + DEVICE_DEBUG("unexpected CHIP_ID 0x%02x", CHIP_ID); + return PX4_ERROR; + } + + return PX4_OK; +} + +void BMI270::RunImpl() +{ + const hrt_abstime now = hrt_absolute_time(); + + switch (_state) { + case STATE::RESET: + + // 0xB6 is written to the CMD register for a soft reset + RegisterWrite(Register::CMD, 0xB6); + _reset_timestamp = now; + _failure_count = 0; + _state = STATE::WAIT_FOR_RESET; + ScheduleDelayed(1_ms); // Following a delay of 1 ms, all configuration settings are overwritten with their reset value. + break; + + case STATE::WAIT_FOR_RESET: + + /* + Hardware initialization steps according to datasheet: + 1. Disable PWR_CONF.adv_power_save and wait for 450us + 2. Write 0x00 to INIT_CTRL + 3. Burst write initialization file to INIT_DATA + 4. Write 0x01 to INIT_CTRL + 5. Wait 150ms and read register INTERNAL_STATUS for value 0b001 + 6. If step 5 passed, enter configure state + + + */ + if ((RegisterRead(Register::CHIP_ID) == chip_id)) { + PX4_DEBUG("Read from CHIP_ID register and the IDs match"); + + // 1. Disable PWR_CONF.adv_power_save and wait for 450us + + RegisterWrite(Register::PWR_CONF, 0x00); + _state = STATE::MICROCODE_LOAD; + ScheduleDelayed(450_us); + + } else { + // RESET not complete + if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + PX4_DEBUG("Reset failed, retrying"); + _state = STATE::RESET; + ScheduleDelayed(100_ms); + + } else { + PX4_DEBUG("Reset not complete, check again in 10 ms"); + ScheduleDelayed(10_ms); + } + } + + break; + + case STATE::MICROCODE_LOAD: + + { + + // 2. Write 0x00 to INIT_CTRL + + RegisterWrite(Register::CONFIG1, 0x00); + // give it the maximum FIFO config file + PX4_DEBUG("attempting to upload initialization file onto BMI270"); + + // 3. Burst write initialization file to INIT_DATA + + int res = transfer(maximum_fifo_config_file, nullptr, sizeof(maximum_fifo_config_file)); + + if (res == PX4_OK) { + RegisterWrite(Register::CONFIG1, 1); + PX4_DEBUG("Successfully uploaded initialization file onto BMI270"); + PX4_DEBUG("Preparing to read INTERNAL_STATUS register"); + _state = STATE::CONFIGURE; + ScheduleDelayed(150_ms); + + + } else { + PX4_DEBUG("Failed to upload initialization file onto BMI270, resetting"); + _state = STATE::RESET; + ScheduleDelayed(10_ms); + } + + break; + } + + case STATE::CONFIGURE: + + PX4_DEBUG("IMU now in configure state"); + + if (Configure()) { + + // if configure succeeded then start reading from FIFO + + if (DataReadyInterruptConfigure()) { + _data_ready_interrupt_enabled = true; + + // backup schedule as a watchdog timeout + ScheduleDelayed(100_ms); + + } else { + _data_ready_interrupt_enabled = false; + ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us); + } + + FIFOReset(); + _state = STATE::FIFO_READ; + + + } else { + // CONFIGURE not complete + if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + PX4_DEBUG("Configure failed, resetting"); + _state = STATE::RESET; + + } else { + PX4_DEBUG("Configure failed, retrying"); + } + + ScheduleDelayed(100_ms); + } + + break; + + case STATE::FIFO_READ: { + + PX4_DEBUG("reading from FIFO"); + + hrt_abstime timestamp_sample = now; + + if (_data_ready_interrupt_enabled) { + PX4_DEBUG("data ready interrupt enabled"); + // scheduled from interrupt if _drdy_timestamp_sample was set as expected + const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0); + + if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) { + timestamp_sample = drdy_timestamp_sample; + + } else { + perf_count(_drdy_missed_perf); + } + + // push backup schedule back + ScheduleDelayed(_fifo_empty_interval_us * 2); + } + + + + bool success = false; + const uint16_t fifo_count = FIFOReadCount(); + + // more bytes than what the buffer takes so an overflow + if (fifo_count >= FIFO::SIZE) { + FIFOReset(); + perf_count(_fifo_overflow_perf); + + } else if (fifo_count == 0) { + perf_count(_fifo_empty_perf); + + } else { + + uint8_t samples = fifo_count / sizeof(FIFO::Data); + + // tolerate minor jitter, leave sample to next iteration if behind by only 1 + if (samples == _fifo_gyro_samples + 1) { + timestamp_sample -= static_cast(FIFO_SAMPLE_DT); + samples--; + } + + if (samples > FIFO_MAX_SAMPLES) { + // not technically an overflow, but more samples than we expected or can publish + FIFOReset(); + perf_count(_fifo_overflow_perf); + + } else if (samples >= _fifo_gyro_samples) { + if (FIFORead(timestamp_sample, fifo_count)) { + success = true; + + if (_failure_count > 0) { + _failure_count--; + } + } + } + + } + + if (!success) { + _failure_count++; + + // full reset if things are failing consistently + if (_failure_count > 10) { + PX4_DEBUG("failure count > 10, resetting..."); + Reset(); + return; + } + } + + if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) { + // check configuration registers periodically or immediately following any failure + if (RegisterCheck(_register_cfg[_checked_register])) { + _last_config_check_timestamp = now; + _checked_register = (_checked_register + 1) % size_register_cfg; + + } else { + PX4_DEBUG("register check failed, resetting..."); + + // register check failed, force reset + perf_count(_bad_register_perf); + // Reset(); + } + + } else { + // periodically update temperature (~1 Hz) + if (hrt_elapsed_time(&_temperature_update_timestamp) >= 1_s) { + UpdateTemperature(); + _temperature_update_timestamp = now; + } + } + + } + + break; + } +} + +void BMI270::SetAccelScaleAndRange() +{ + const uint8_t ACC_RANGE = RegisterRead(Register::ACC_RANGE) & (Bit1 | Bit0); + + switch (ACC_RANGE) { + case acc_range_2g: + _px4_accel.set_scale(2.f * CONSTANTS_ONE_G / 32768.f); + _px4_accel.set_range(2.f * CONSTANTS_ONE_G); + break; + + case acc_range_4g: + _px4_accel.set_scale(4.f * CONSTANTS_ONE_G / 32768.f); + _px4_accel.set_range(4.f * CONSTANTS_ONE_G); + break; + + case acc_range_8g: + _px4_accel.set_scale(8.f * CONSTANTS_ONE_G / 32768.f); + _px4_accel.set_range(8.f * CONSTANTS_ONE_G); + break; + + case acc_range_16g: + _px4_accel.set_scale(16.f * CONSTANTS_ONE_G / 32768.f); + _px4_accel.set_range(16.f * CONSTANTS_ONE_G); + break; + } +} + +void BMI270::SetGyroScale() +{ + // data is 16 bits with 2000dps range + const float scale = math::radians(2000.0f) / 32767.0f; + _px4_gyro.set_scale(scale); + +} + +void BMI270::ConfigureSampleRate(int sample_rate) +{ + // round down to nearest FIFO sample dt * SAMPLES_PER_TRANSFER + const float min_interval = FIFO_SAMPLE_DT; + _fifo_empty_interval_us = math::max(roundf((1e6f / (float)sample_rate) / min_interval) * min_interval, min_interval); + + // works out to be 2 ... + _fifo_gyro_samples = math::min((float)_fifo_empty_interval_us / (1e6f / RATE), (float)FIFO_MAX_SAMPLES); + + // recompute FIFO empty interval (us) with actual sample limit + _fifo_empty_interval_us = _fifo_gyro_samples * (1e6f / RATE); + + ConfigureFIFOWatermark(_fifo_gyro_samples); +} + + +// when this register is set an interrupt is triggered when the FIFO reaches this many samples +void BMI270::ConfigureFIFOWatermark(uint8_t samples) +{ + // FIFO_WTM: 13 bit FIFO watermark level value + // unit of the fifo watermark is one byte + const uint16_t fifo_watermark_threshold = samples * sizeof(FIFO::Data); + + for (auto &r : _register_cfg) { + if (r.reg == Register::FIFO_WTM_0) { + // fifo_water_mark[7:0] + r.set_bits = fifo_watermark_threshold & 0x00FF; + r.clear_bits = ~r.set_bits; + + } else if (r.reg == Register::FIFO_WTM_1) { + // fifo_water_mark[12:8] + r.set_bits = (fifo_watermark_threshold & 0x0700) >> 8; + r.clear_bits = ~r.set_bits; + } + } +} + +bool BMI270::Configure() +{ + + bool success = false; + + // check internal status first as per datasheet + uint8_t internal_status = RegisterRead(Register::INTERNAL_STATUS); + PX4_DEBUG("Internal status register value: 0x%02hhX", internal_status); + + if ((internal_status & 1) == 1) { + PX4_DEBUG("INTERNAL_STATUS 0x01, ready for configure"); + + } else { + + PX4_DEBUG("INTERNAL_STATUS check failed, resetting"); + _state = STATE::RESET; + ScheduleDelayed(10_ms); + + }; + + + // first set and clear all configured register bits + for (const auto ®_cfg : _register_cfg) { + RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits); + } + + // now check that all are configured + + for (const auto ®_cfg : _register_cfg) { + if (!RegisterCheck(reg_cfg)) { + success = false; + } + } + + SetAccelScaleAndRange(); + SetGyroScale(); + + success = true; + return success; +} + +int BMI270::DataReadyInterruptCallback(int irq, void *context, void *arg) +{ + PX4_DEBUG("actual data ready interrupt called"); + static_cast(arg)->DataReady(); + return 0; +} + +void BMI270::DataReady() +{ + _drdy_timestamp_sample.store(hrt_absolute_time()); + ScheduleNow(); +} + +bool BMI270::DataReadyInterruptConfigure() +{ + if (_drdy_gpio == 0) { + return false; + } + + // Setup data ready on falling edge + return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0; +} + +bool BMI270::DataReadyInterruptDisable() +{ + if (_drdy_gpio == 0) { + return false; + } + + return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0; +} + +bool BMI270::RegisterCheck(const register_config_t ®_cfg) +{ + bool success = true; + + const uint8_t reg_value = RegisterRead(reg_cfg.reg); + + PX4_DEBUG("0x%02hhX: 0x%02hhX", (uint8_t)reg_cfg.reg, reg_value); + + if (reg_cfg.set_bits && ((reg_value & reg_cfg.set_bits) != reg_cfg.set_bits)) { + PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits); + success = false; + } + + if (reg_cfg.clear_bits && ((reg_value & reg_cfg.clear_bits) != 0)) { + PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits); + success = false; + } + + return success; +} + +uint8_t BMI270::RegisterRead(Register reg) +{ + // 6.1.2 SPI interface of accelerometer part + // + // In case of read operations of the accelerometer part, the requested data + // is not sent immediately, but instead first a dummy byte is sent, and + // after this dummy byte the actual requested register content is transmitted. + uint8_t cmd[3] {}; + cmd[0] = static_cast(reg) | DIR_READ; + // cmd[1] dummy byte + transfer(cmd, cmd, sizeof(cmd)); + return cmd[2]; +} + +void BMI270::RegisterWrite(Register reg, uint8_t value) +{ + uint8_t cmd[2] { (uint8_t)reg, value }; + transfer(cmd, cmd, sizeof(cmd)); +} + +void BMI270::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits) +{ + const uint8_t orig_val = RegisterRead(reg); + + uint8_t val = (orig_val & ~clearbits) | setbits; + + if (orig_val != val) { + RegisterWrite(reg, val); + } +} + + +// Checks how many bytes are in the FIFO +uint16_t BMI270::FIFOReadCount() +{ + CheckErrorRegister(); + PX4_DEBUG("Attempting to determine FIFO fill level"); + + // FIFO length registers FIFO_LENGTH_1 and FIFO_LENGTH_0 contain the 14 bit FIFO byte + FIFOLengthReadBuffer buffer {}; + + if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, sizeof(buffer)) != PX4_OK) { + PX4_DEBUG("Bad transfer"); + perf_count(_bad_transfer_perf); + return 0; + } + + uint16_t fifo_fill_level = combine(buffer.FIFO_LENGTH_1 & 0x3F, buffer.FIFO_LENGTH_0); + + return fifo_fill_level; +} + + +// writes a gyro frame into the FIFO buffer the first argument points to +void BMI270::ProcessGyro(sensor_gyro_fifo_s *gyro, FIFO::Data *frame) +{ + const uint8_t samples = gyro->samples; + + const int16_t gyro_x = combine(frame->x_msb, frame->x_lsb); + const int16_t gyro_y = combine(frame->y_msb, frame->y_lsb); + const int16_t gyro_z = combine(frame->z_msb, frame->z_lsb); + + // Rotate from FLU to NED + gyro->x[samples] = gyro_x; + gyro->y[samples] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y; + gyro->z[samples] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z; + + gyro->samples++; +} + +// writes an accelerometer frame into the FIFO buffer the first argument points to +void BMI270::ProcessAccel(sensor_accel_fifo_s *accel, FIFO::Data *frame) +{ + + const uint8_t samples = accel->samples; + + const int16_t accel_x = combine(frame->x_msb, frame->x_lsb); + const int16_t accel_y = combine(frame->y_msb, frame->y_lsb); + const int16_t accel_z = combine(frame->z_msb, frame->z_lsb); + + // Rotate from FLU to NED + accel->x[samples] = accel_x; + accel->y[samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y; + accel->z[samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z; + + accel->samples++; +} + + +bool BMI270::FIFORead(const hrt_abstime ×tamp_sample, uint16_t fifo_bytes) +{ + + uint8_t err = RegisterRead(Register::ERR_REG); + + if ((err >> 6 & 1) == 1) { + FIFOReset(); + return false; + } + + FIFOReadBuffer buffer{}; + + // Reads from the FIFO_DATA register as much data as is available, + // plus 2 bytes for the sent command and dummy byte. + if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, fifo_bytes + 2) != PX4_OK) { + PX4_DEBUG("buffer transfer failed"); + perf_count(_bad_transfer_perf); + return false; + } + + sensor_accel_fifo_s accel_buffer{}; + accel_buffer.timestamp_sample = timestamp_sample; + accel_buffer.dt = FIFO_SAMPLE_DT; + + sensor_gyro_fifo_s gyro_buffer{}; + gyro_buffer.timestamp_sample = timestamp_sample; + gyro_buffer.dt = FIFO_SAMPLE_DT; + + uint8_t *data_buffer = (uint8_t *)&buffer.f[0]; + unsigned fifo_buffer_index = 0; // start of buffer + + while (fifo_buffer_index < fifo_bytes) { + // look for header signature (first 6 bits) followed by two bits indicating the status of INT1 and INT2 + switch (data_buffer[fifo_buffer_index]) { + case ((uint8_t)FIFO::Header::sensor_accel_frame | (uint8_t)FIFO::Header::sensor_gyro_frame): { + // Move past header and padding for Aux + fifo_buffer_index += 1; + BMI270::ProcessGyro(&gyro_buffer, (FIFO::Data *)&data_buffer[fifo_buffer_index]); + // Move forward to next record + fifo_buffer_index += 6; + BMI270::ProcessAccel(&accel_buffer, (FIFO::Data *)&data_buffer[fifo_buffer_index]); + // Move forward to next record + fifo_buffer_index += 6; + } + break; + + case (uint8_t)FIFO::Header::sensor_gyro_frame: { + // Move past header. + fifo_buffer_index += 1; + BMI270::ProcessGyro(&gyro_buffer, (FIFO::Data *)&data_buffer[fifo_buffer_index]); + // Move forward to next record + fifo_buffer_index += 6; + } + break; + + case (uint8_t)FIFO::Header::sensor_accel_frame: { + // Move past header. + fifo_buffer_index += 1; + BMI270::ProcessAccel(&accel_buffer, (FIFO::Data *)&data_buffer[fifo_buffer_index]); + // Move forward to next record + fifo_buffer_index += 6; + } + break; + + case (uint8_t)FIFO::Header::skip_frame: + // Skip Frame + // Frame length: 2 bytes (1 byte header + 1 byte payload) + PX4_DEBUG("Skip Frame"); + fifo_buffer_index += 2; + break; + + case (uint8_t)FIFO::Header::sensor_time_frame: + // Sensortime Frame + // Frame length: 4 bytes (1 byte header + 3 bytes payload) + PX4_DEBUG("Sensortime Frame"); + fifo_buffer_index += 4; + break; + + case (uint8_t)FIFO::Header::FIFO_input_config_frame: + // FIFO input config Frame + // Frame length: 2 bytes (1 byte header + 1 byte payload) + PX4_DEBUG("FIFO input config Frame"); + fifo_buffer_index += 2; + break; + + case (uint8_t)FIFO::Header::sample_drop_frame: + // Sample drop Frame + // Frame length: 2 bytes (1 byte header + 1 byte payload) + PX4_DEBUG("Sample drop Frame"); + fifo_buffer_index += 2; + break; + + default: + fifo_buffer_index++; + break; + } + } + + _px4_accel.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + + perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf)); + + + if ((accel_buffer.samples == 0) && (gyro_buffer.samples == 0)) { + + return false; + + } else { + if (accel_buffer.samples > 0) { + _px4_accel.updateFIFO(accel_buffer); + } + + if (gyro_buffer.samples > 0) { + _px4_gyro.updateFIFO(gyro_buffer); + } + + return true; + } + +} + + + +void BMI270::FIFOReset() +{ + PX4_DEBUG("Resetting FIFO..."); + perf_count(_fifo_reset_perf); + + // ACC_SOFTRESET: trigger a FIFO reset by writing 0xB0 to ACC_SOFTRESET (register 0x7E). + RegisterWrite(Register::CMD, 0xB0); + + // reset while FIFO is disabled + _drdy_timestamp_sample.store(0); +} + +void BMI270::UpdateTemperature() +{ + uint8_t temperature_buf[4] {}; + temperature_buf[0] = static_cast(Register::TEMP_MSB) | DIR_READ; + + if (transfer(&temperature_buf[0], &temperature_buf[0], sizeof(temperature_buf)) != PX4_OK) { + perf_count(_bad_transfer_perf); + return; + } + + const uint16_t temp = temperature_buf[2] | (temperature_buf[3] << 8); + + float temperature; + + if (temp == 0x8000) { + // invalid + temperature = NAN; + + } else { + constexpr float lsb = 0.001953125f; // 1/2^9 + temperature = 23.0f + (int16_t)temp * lsb; + } + + _px4_accel.set_temperature(temperature); + _px4_gyro.set_temperature(temperature); +} diff --git a/src/drivers/imu/bosch/bmi270/BMI270.hpp b/src/drivers/imu/bosch/bmi270/BMI270.hpp new file mode 100644 index 0000000000..5f3add8078 --- /dev/null +++ b/src/drivers/imu/bosch/bmi270/BMI270.hpp @@ -0,0 +1,193 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file BMI270.hpp + * + * Driver for the Bosch BMI270 connected via SPI. + * + */ + +#pragma once + +#include "Bosch_BMI270_registers.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace Bosch_BMI270; + +class BMI270 : public device::SPI, public I2CSPIDriver +{ +public: + BMI270(const I2CSPIDriverConfig &config); + ~BMI270() override; + + static void print_usage(); + + void RunImpl(); + + int init() override; + void print_status() override; + +private: + void exit_and_cleanup() override; + + // Sensor Configuration + static constexpr uint32_t RATE{1600}; // 1600 Hz + static constexpr float FIFO_SAMPLE_DT{1e6f / RATE}; + + static constexpr uint8_t ID_088 = 0x1E; + static constexpr uint8_t ID_090L = 0x1A; + + static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::Data), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]))}; + + + hrt_abstime _temperature_update_timestamp{0}; + + struct FIFOLengthReadBuffer { + uint8_t cmd{static_cast(Register::FIFO_LENGTH_0) | DIR_READ}; + uint8_t dummy{0}; + uint8_t FIFO_LENGTH_0{0}; + uint8_t FIFO_LENGTH_1{0}; + }; + + struct FIFOReadBuffer { + uint8_t cmd{static_cast(Register::FIFO_DATA) | DIR_READ}; + uint8_t dummy{0}; + FIFO::Data f[FIFO_MAX_SAMPLES] {}; + }; + + // ensure no struct padding + static_assert(sizeof(FIFOReadBuffer) == (2 + FIFO_MAX_SAMPLES *sizeof(FIFO::Data))); + + struct register_config_t { + Register reg; + uint8_t set_bits{0}; + uint8_t clear_bits{0}; + }; + + int probe() override; + + bool Reset(); + + bool Configure(); + + void ProcessGyro(sensor_gyro_fifo_s *gyro, FIFO::Data *gyro_frame); + void ProcessAccel(sensor_accel_fifo_s *accel, FIFO::Data *accel_frame); + + bool readAccelFrame(FIFO::Data *accel_frame); + bool readGyroFrame(FIFO::Data *gyro_frame); + + void ConfigurePwr(); + void SetAccelScaleAndRange(); + void SetGyroScale(); + + void CheckErrorRegister(); + + void ConfigureFifo(); + void ConfigureSampleRate(int sample_rate = 0); + void ConfigureFIFOWatermark(uint8_t samples); + + static int DataReadyInterruptCallback(int irq, void *context, void *arg); + void DataReady(); + bool DataReadyInterruptConfigure(); + bool DataReadyInterruptDisable(); + + bool RegisterCheck(const register_config_t ®_cfg); + + uint8_t RegisterRead(Register reg); + void RegisterWrite(Register reg, uint8_t value); + void RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits); + + uint16_t FIFOReadCount(); + //bool FIFORead(const hrt_abstime ×tamp_sample, uint8_t samples); + bool FIFORead(const hrt_abstime ×tamp_sample, uint16_t fifo_bytes); + void FIFOReset(); + + void UpdateTemperature(); + + const spi_drdy_gpio_t _drdy_gpio; + + PX4Accelerometer _px4_accel; + PX4Gyroscope _px4_gyro; + + perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")}; + perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")}; + perf_counter_t _fifo_empty_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO empty")}; + perf_counter_t _fifo_overflow_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO overflow")}; + perf_counter_t _fifo_reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO reset")}; + + perf_counter_t _drdy_missed_perf{nullptr}; + + hrt_abstime _reset_timestamp{0}; + hrt_abstime _last_config_check_timestamp{0}; + int _failure_count{0}; + + px4::atomic _drdy_timestamp_sample{0}; + bool _data_ready_interrupt_enabled{false}; + + enum class STATE : uint8_t { + WAIT_FOR_RESET, + RESET, + MICROCODE_LOAD, + CONFIGURE, + FIFO_READ, + } _state{STATE::RESET}; + + uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval + int32_t _fifo_gyro_samples{static_cast(_fifo_empty_interval_us / (1000000 / RATE))}; // checks out to be 2 ... + + uint8_t _checked_register{0}; + static constexpr uint8_t size_register_cfg{11}; + register_config_t _register_cfg[size_register_cfg] { + // Register | Set bits, Clear bits + { Register::PWR_CONF, 0, ACC_PWR_CONF_BIT::acc_pwr_save }, + { Register::PWR_CTRL, PWR_CTRL_BIT::accel_en | PWR_CTRL_BIT::gyr_en | PWR_CTRL_BIT::temp_en, 0 }, + { Register::ACC_CONF, ACC_CONF_BIT::acc_bwp_Normal | ACC_CONF_BIT::acc_odr_1600, Bit1 | Bit0 }, + { Register::GYR_CONF, GYR_CONF_BIT::gyr_odr_1k6 | GYR_CONF_BIT::gyr_flt_mode_normal | GYR_CONF_BIT::gyr_noise_hp | GYR_CONF_BIT::gyr_flt_hp, Bit0 | Bit1 | Bit4}, + { Register::ACC_RANGE, ACC_RANGE_BIT::acc_range_16g, 0 }, + { Register::FIFO_WTM_0, 0, 0 }, + { Register::FIFO_WTM_1, 0, 0 }, + { Register::FIFO_CONFIG_0, FIFO_CONFIG_0_BIT::BIT1_ALWAYS | FIFO_CONFIG_0_BIT::FIFO_mode, 0 }, + { Register::FIFO_CONFIG_1, FIFO_CONFIG_1_BIT::BIT4_ALWAYS | FIFO_CONFIG_1_BIT::Acc_en | FIFO_CONFIG_1_BIT::Gyr_en, 0 }, + { Register::INT1_IO_CTRL, INT1_IO_CONF_BIT::int1_out, 0 }, + { Register::INT_MAP_DATA, INT1_INT2_MAP_DATA_BIT::int1_fwm, 0}, + }; +}; diff --git a/src/drivers/imu/bosch/bmi270/Bosch_BMI270_registers.hpp b/src/drivers/imu/bosch/bmi270/Bosch_BMI270_registers.hpp new file mode 100644 index 0000000000..29e008b627 --- /dev/null +++ b/src/drivers/imu/bosch/bmi270/Bosch_BMI270_registers.hpp @@ -0,0 +1,206 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file Bosch_BMI270_registers.hpp + * + * Bosch BMI270 registers. + * + */ + +#pragma once + +#include +#include + +// TODO: move to a central header +static constexpr uint8_t Bit0 = (1 << 0); +static constexpr uint8_t Bit1 = (1 << 1); +static constexpr uint8_t Bit2 = (1 << 2); +static constexpr uint8_t Bit3 = (1 << 3); +static constexpr uint8_t Bit4 = (1 << 4); +static constexpr uint8_t Bit5 = (1 << 5); +static constexpr uint8_t Bit6 = (1 << 6); +static constexpr uint8_t Bit7 = (1 << 7); + +namespace Bosch_BMI270 +{ +static constexpr uint32_t SPI_SPEED = 10 * 1000 * 1000; // 10MHz SPI serial interface +static constexpr uint8_t DIR_READ = 0x80; + +static constexpr uint8_t chip_id = 0x24; + +enum class Register : uint8_t { + CHIP_ID = 0x00, + + ERR_REG = 0x02, + STATUS = 0x03, + + EVENT = 0x1B, + + INTERNAL_STATUS = 0X21, + TEMP_MSB = 0x22, + TEMP_LSB = 0x23, + FIFO_LENGTH_0 = 0x24, + FIFO_LENGTH_1 = 0x25, + FIFO_DATA = 0x26, + + ACC_CONF = 0x40, + ACC_RANGE = 0x41, + GYR_CONF = 0x42, + + FIFO_DOWNS = 0x45, + FIFO_WTM_0 = 0x46, + FIFO_WTM_1 = 0x47, + FIFO_CONFIG_0 = 0x48, + FIFO_CONFIG_1 = 0x49, + + // controls interrupt pin behaviour + INT1_IO_CTRL = 0x53, + + INT_MAP_DATA = 0x58, + CONFIG1 = 0x59, + + CONFIG2 = 0x5E, + PWR_CONF = 0x7C, + PWR_CTRL = 0x7D, + CMD = 0x7E, + + + +}; + +enum GYR_CONF_BIT : uint8_t { + // 1.6 or 3.2kHz sample rate + gyr_odr_1k6 = Bit3 | Bit2, + gyr_odr_3k2 = Bit3 | Bit2 | Bit0, + // set LPF to normal mode + gyr_flt_mode_normal = Bit5, + // set noise profile to high performance mode + gyr_noise_hp = Bit6, + // set filter profile to high performance mode + gyr_flt_hp = Bit7 +}; + +enum PWR_CTRL_BIT : uint8_t { + gyr_en = Bit1, + accel_en = Bit2, + temp_en = Bit3, +}; + +// ACC_CONF +enum ACC_CONF_BIT : uint8_t { + // [7:4] acc_bwp + acc_bwp_Normal = Bit7 | Bit5, // Filter setting normal + + // [3:0] acc_odr + acc_odr_1600 = Bit3 | Bit2, // ODR 1600 Hz +}; + +// ACC_RANGE +enum ACC_RANGE_BIT : uint8_t { + acc_range_2g = 0, // ±2g + acc_range_4g = Bit0, // ±4g + acc_range_8g = Bit1, // ±8g + acc_range_16g = Bit1 | Bit0, // ±16g +}; + +// FIFO_CONFIG_0 +enum FIFO_CONFIG_0_BIT : uint8_t { + BIT1_ALWAYS = Bit1, // This bit must always be ‘1’. + FIFO_mode = Bit0, +}; + +// FIFO_CONFIG_1 +enum FIFO_CONFIG_1_BIT : uint8_t { + Acc_en = Bit6, + Gyr_en = Bit7, + BIT4_ALWAYS = Bit4, // This bit must always be ‘1’. +}; + +// INT1_IO_CONF +enum INT1_IO_CONF_BIT : uint8_t { + int1_in = Bit4, + int1_out = Bit3, + + int1_lvl = Bit1, +}; + +// INT1_INT2_MAP_DATA +enum INT1_INT2_MAP_DATA_BIT : uint8_t { + int2_drdy = Bit6, + int2_fwm = Bit5, + int2_ffull = Bit4, + + int1_drdy = Bit2, + int1_fwm = Bit1, + int1_ffull = Bit0, +}; + +// ACC_PWR_CONF +enum ACC_PWR_CONF_BIT : uint8_t { + acc_pwr_save = 0x03 +}; + +// ACC_PWR_CTRL +enum ACC_PWR_CTRL_BIT : uint8_t { + acc_enable = 0x04 +}; + + +namespace FIFO +{ +static constexpr size_t SIZE = 1024; + +struct Data { + uint8_t x_lsb; + uint8_t x_msb; + uint8_t y_lsb; + uint8_t y_msb; + uint8_t z_lsb; + uint8_t z_msb; +}; + + +enum class Header : uint8_t { + sensor_accel_frame = 0b10000100, + sensor_gyro_frame = 0b10001000, + skip_frame = 0b01000000, + sensor_time_frame = 0b01000100, + FIFO_input_config_frame = 0b01001000, + sample_drop_frame = 0b01010000, +}; + +} // namespace FIFO + +} // namespace Bosch_BMI270 diff --git a/src/drivers/imu/bosch/bmi270/CMakeLists.txt b/src/drivers/imu/bosch/bmi270/CMakeLists.txt new file mode 100644 index 0000000000..8769054263 --- /dev/null +++ b/src/drivers/imu/bosch/bmi270/CMakeLists.txt @@ -0,0 +1,48 @@ +############################################################################ +# +# Copyright (c) 2022 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_module( + MODULE drivers__imu__bosch__bmi270 + MAIN bmi270 + COMPILE_FLAGS + #-DDEBUG_BUILD + SRCS + bmi270_main.cpp + BMI270.cpp + BMI270.hpp + Bosch_BMI270_registers.hpp + DEPENDS + drivers_accelerometer + drivers_gyroscope + px4_work_queue + ) diff --git a/src/drivers/imu/bosch/bmi270/Kconfig b/src/drivers/imu/bosch/bmi270/Kconfig new file mode 100644 index 0000000000..3d8d82c5a4 --- /dev/null +++ b/src/drivers/imu/bosch/bmi270/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_IMU_BOSCH_BMI270 + bool "bosch bmi270" + default n + ---help--- + Enable support for bosch bmi270 diff --git a/src/drivers/imu/bosch/bmi270/bmi270_main.cpp b/src/drivers/imu/bosch/bmi270/bmi270_main.cpp new file mode 100644 index 0000000000..efdf50e5c0 --- /dev/null +++ b/src/drivers/imu/bosch/bmi270/bmi270_main.cpp @@ -0,0 +1,85 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "BMI270.hpp" + +#include +#include + +void BMI270::print_usage() +{ + PRINT_MODULE_USAGE_NAME("bmi270", "driver"); + PRINT_MODULE_USAGE_SUBCATEGORY("imu"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true); + PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); +} + +extern "C" int bmi270_main(int argc, char *argv[]) +{ + int ch; + using ThisDriver = BMI270; + BusCLIArguments cli{false, true}; + cli.default_spi_frequency = SPI_SPEED; + + while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) { + switch (ch) { + case 'R': + cli.rotation = (enum Rotation)atoi(cli.optArg()); + break; + } + } + + const char *verb = cli.optArg(); + + if (!verb) { + ThisDriver::print_usage(); + return -1; + } + + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_IMU_DEVTYPE_BMI270); + + if (!strcmp(verb, "start")) { + return ThisDriver::module_start(cli, iterator); + + } else if (!strcmp(verb, "stop")) { + return ThisDriver::module_stop(iterator); + + } else if (!strcmp(verb, "status")) { + return ThisDriver::module_status(iterator); + } + + ThisDriver::print_usage(); + return -1; +}