From 9d0e57230a032a537123866f6eb71a1827649921 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Fri, 20 May 2022 12:49:06 -0700 Subject: [PATCH] boards: new px4_fmu-v6c board support (#19544) --- .ci/Jenkinsfile-compile | 1 + .github/workflows/compile_nuttx.yml | 1 + Makefile | 1 + boards/px4/fmu-v6c/bootloader.px4board | 3 + boards/px4/fmu-v6c/default.px4board | 91 ++++ .../fmu-v6c/extras/px4_fmu-v6c_bootloader.bin | Bin 0 -> 45684 bytes .../px4/fmu-v6c/extras/px4_io-v2_default.bin | Bin 0 -> 39952 bytes boards/px4/fmu-v6c/firmware.prototype | 13 + boards/px4/fmu-v6c/init/rc.board_defaults | 20 + boards/px4/fmu-v6c/init/rc.board_sensors | 21 + boards/px4/fmu-v6c/nuttx-config/Kconfig | 17 + .../fmu-v6c/nuttx-config/bootloader/defconfig | 96 ++++ .../px4/fmu-v6c/nuttx-config/include/board.h | 505 ++++++++++++++++++ .../nuttx-config/include/board_dma_map.h | 79 +++ boards/px4/fmu-v6c/nuttx-config/nsh/defconfig | 241 +++++++++ .../nuttx-config/scripts/bootloader_script.ld | 215 ++++++++ .../fmu-v6c/nuttx-config/scripts/script.ld | 223 ++++++++ boards/px4/fmu-v6c/src/CMakeLists.txt | 75 +++ boards/px4/fmu-v6c/src/board_config.h | 321 +++++++++++ boards/px4/fmu-v6c/src/bootloader_main.c | 62 +++ boards/px4/fmu-v6c/src/can.c | 128 +++++ boards/px4/fmu-v6c/src/hw_config.h | 128 +++++ boards/px4/fmu-v6c/src/i2c.cpp | 40 ++ boards/px4/fmu-v6c/src/init.c | 259 +++++++++ boards/px4/fmu-v6c/src/led.c | 235 ++++++++ boards/px4/fmu-v6c/src/manifest.c | 147 +++++ boards/px4/fmu-v6c/src/mtd.cpp | 104 ++++ boards/px4/fmu-v6c/src/sdio.c | 177 ++++++ boards/px4/fmu-v6c/src/spi.cpp | 61 +++ boards/px4/fmu-v6c/src/timer_config.cpp | 73 +++ boards/px4/fmu-v6c/src/usb.c | 105 ++++ .../include/px4_arch/hw_description.h | 18 +- .../stm32h7/include/px4_arch/hw_description.h | 2 + .../px4_arch/io_timer_hw_description.h | 24 + 34 files changed, 3485 insertions(+), 1 deletion(-) create mode 100644 boards/px4/fmu-v6c/bootloader.px4board create mode 100644 boards/px4/fmu-v6c/default.px4board create mode 100755 boards/px4/fmu-v6c/extras/px4_fmu-v6c_bootloader.bin create mode 100755 boards/px4/fmu-v6c/extras/px4_io-v2_default.bin create mode 100644 boards/px4/fmu-v6c/firmware.prototype create mode 100644 boards/px4/fmu-v6c/init/rc.board_defaults create mode 100644 boards/px4/fmu-v6c/init/rc.board_sensors create mode 100644 boards/px4/fmu-v6c/nuttx-config/Kconfig create mode 100644 boards/px4/fmu-v6c/nuttx-config/bootloader/defconfig create mode 100644 boards/px4/fmu-v6c/nuttx-config/include/board.h create mode 100644 boards/px4/fmu-v6c/nuttx-config/include/board_dma_map.h create mode 100644 boards/px4/fmu-v6c/nuttx-config/nsh/defconfig create mode 100644 boards/px4/fmu-v6c/nuttx-config/scripts/bootloader_script.ld create mode 100644 boards/px4/fmu-v6c/nuttx-config/scripts/script.ld create mode 100644 boards/px4/fmu-v6c/src/CMakeLists.txt create mode 100644 boards/px4/fmu-v6c/src/board_config.h create mode 100644 boards/px4/fmu-v6c/src/bootloader_main.c create mode 100644 boards/px4/fmu-v6c/src/can.c create mode 100644 boards/px4/fmu-v6c/src/hw_config.h create mode 100644 boards/px4/fmu-v6c/src/i2c.cpp create mode 100644 boards/px4/fmu-v6c/src/init.c create mode 100644 boards/px4/fmu-v6c/src/led.c create mode 100644 boards/px4/fmu-v6c/src/manifest.c create mode 100644 boards/px4/fmu-v6c/src/mtd.cpp create mode 100644 boards/px4/fmu-v6c/src/sdio.c create mode 100644 boards/px4/fmu-v6c/src/spi.cpp create mode 100644 boards/px4/fmu-v6c/src/timer_config.cpp create mode 100644 boards/px4/fmu-v6c/src/usb.c diff --git a/.ci/Jenkinsfile-compile b/.ci/Jenkinsfile-compile index a3e67d3eed..4e61920c58 100644 --- a/.ci/Jenkinsfile-compile +++ b/.ci/Jenkinsfile-compile @@ -105,6 +105,7 @@ pipeline { "px4_fmu-v5_stackcheck", "px4_fmu-v5_uavcanv0periph", "px4_fmu-v5x_default", + "px4_fmu-v6c_default", "px4_fmu-v6u_default", "px4_fmu-v6x_default", "px4_io-v2_default", diff --git a/.github/workflows/compile_nuttx.yml b/.github/workflows/compile_nuttx.yml index a54ac9337b..f871cd60a6 100644 --- a/.github/workflows/compile_nuttx.yml +++ b/.github/workflows/compile_nuttx.yml @@ -61,6 +61,7 @@ jobs: px4_fmu-v4pro, px4_fmu-v5, px4_fmu-v5x, + px4_fmu-v6c, px4_fmu-v6u, px4_fmu-v6x, raspberrypi_pico, diff --git a/Makefile b/Makefile index d43c96123e..0378c834a4 100644 --- a/Makefile +++ b/Makefile @@ -325,6 +325,7 @@ px4io_update: px4_io-v2_default cubepilot_io-v2_default cp build/px4_io-v2_default/px4_io-v2_default.bin boards/px4/fmu-v5/extras/px4_io-v2_default.bin cp build/px4_io-v2_default/px4_io-v2_default.bin boards/px4/fmu-v5x/extras/px4_io-v2_default.bin cp build/px4_io-v2_default/px4_io-v2_default.bin boards/px4/fmu-v6x/extras/px4_io-v2_default.bin + cp build/px4_io-v2_default/px4_io-v2_default.bin boards/px4/fmu-v6c/extras/px4_io-v2_default.bin # cubepilot_io-v2_default cp build/cubepilot_io-v2_default/cubepilot_io-v2_default.bin boards/cubepilot/cubeorange/extras/cubepilot_io-v2_default.bin cp build/cubepilot_io-v2_default/cubepilot_io-v2_default.bin boards/cubepilot/cubeyellow/extras/cubepilot_io-v2_default.bin diff --git a/boards/px4/fmu-v6c/bootloader.px4board b/boards/px4/fmu-v6c/bootloader.px4board new file mode 100644 index 0000000000..19b6e662be --- /dev/null +++ b/boards/px4/fmu-v6c/bootloader.px4board @@ -0,0 +1,3 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_ROMFSROOT="" diff --git a/boards/px4/fmu-v6c/default.px4board b/boards/px4/fmu-v6c/default.px4board new file mode 100644 index 0000000000..6c491152a0 --- /dev/null +++ b/boards/px4/fmu-v6c/default.px4board @@ -0,0 +1,91 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS0" +CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS6" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS5" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS3" +CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS1" +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_HEATER=y +CONFIG_DRIVERS_IMU_BOSCH_BMI055=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y +CONFIG_COMMON_LIGHT=y +CONFIG_COMMON_MAGNETOMETER=y +CONFIG_COMMON_OPTICAL_FLOW=y +CONFIG_DRIVERS_POWER_MONITOR_INA226=y +CONFIG_DRIVERS_POWER_MONITOR_INA228=y +CONFIG_DRIVERS_POWER_MONITOR_INA238=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_DRIVERS_PWM_OUT_SIM=y +CONFIG_DRIVERS_PX4IO=y +CONFIG_COMMON_TELEMETRY=y +CONFIG_DRIVERS_TONE_ALARM=y +CONFIG_DRIVERS_UAVCAN=y +CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2 +CONFIG_MODULES_AIRSPEED_SELECTOR=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_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_MODULES_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_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_MTD=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_SERIAL_TEST=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_VER=y +CONFIG_SYSTEMCMDS_WORK_QUEUE=y diff --git a/boards/px4/fmu-v6c/extras/px4_fmu-v6c_bootloader.bin b/boards/px4/fmu-v6c/extras/px4_fmu-v6c_bootloader.bin new file mode 100755 index 0000000000000000000000000000000000000000..fa26776c8725f663d06c97a42cac24715dfde946 GIT binary patch literal 45684 zcmeFZd3;pW{WpH@GD{Yc$pTCk$YgGSY`~BNji7Zh6K3 z4B>k(M-oO(?TaL-Ul_tu`;_RG%j0Ext{9%=dR@q&%#*bK$jBs+&$7cSoX8d!A(DlLm5F~@N`A+x#VlQ~4Ptb+%f%S%_3pxvRGN>8Trj|(rib9oI2E7P%z|NmLLW5smL!# zkYeOi17$PwYeLiGe5QF;7iWOsFTWX3D<-OeP!*?HN4l;}G@{&S! zr8<8vG2)txl#Uc@wUWgwr<>0*OryS&L>99vjYvGw5Z-6=WF;|?Af0q9`g*dA_UpbcC;JHr zFxkkTL%M*}ixgmp;WDl@MS&}plh|iFiQqJUIcR8j@Z}xY84ZJmMhd%A9n(sa9!9!H)~7dBu41> zlDS^DfO{7ApL#8?*A$_Tm_xev(DDwnoEmDm{(8&W5(>Z0%@Gnw_Gd7Ma&S1a$(FQ&yiHj!A{g|X}dnIS)tk=y7L-T zPrU{QpB^D`p0Xh)`f}QJ>9q@zBG46G_p$*nz@u#^ z?GyR|+^SEe*Uv~Vg^izw5&Z*5acxe4d7(<=#ejcVz|S}-`?H_! zVAUJfM!qU{+qaYpwrxk)qzsW`(bgzWytKbJ=%n$J#!o)de)RCRGm&RL-tlp4HqnGf z$0y7b(n;s>LEN=kaBuE%v*O*xEo#Fx&dXZp{7qCWHYvi^M$f4dx^EK~W#R;1aWwnlOoDHXYV zs{GsUE`i_7}s?CS?p>}CD&h4&1m6qfZ*U2^YW%967FY2tl@ zDPmdw^dfmMrKqgmSX?!jQe4)bP;&oZN=aFNVrlhYN@-btl4HYQii7sQtUq}v>ii%y zXCIxX)VKOaxA}1Ba$?e$mE47gQJYHYgC`@_mH`KkJi+2Ip4}CKYl$Q zI8on5_ie!M4NObrio=w@jCNesTZy81g>=3MI3_0G#9~INewB2FMY4p_k`;F{0!dqO zM{Z5o(8&jvJ2b=3)i$&(D@zsA#apFqQnIvGy3_Hm|2u;D&_AymDiyq=u3vDfkL=`>hXOVS^Smt1kXWH$X>JXpXhyMr z8N3Z*i1e3v8P%8YPpQAn&m)xnPz_+7u~Co?ZNmGh;g_ z$ygdj^wyX$)r{*)e1s;ne`bLy7;Tew< zk3^8}jwkxxnuvb5lIUY*P+W)i$_7>AC>zROw?36T&0}2uTQ!Lk*OAVO!$e<>`e->? zhOC>sUgmFKm6;Mn^xs99-=^09iXz3tbHI0x0pER%f+x2FpH7%X-BNw=wN|UT&`Jj0 zAvWnTU}wNh$0lVe8N>Mgf%_D?g`+kmj|d5%TOS2@tY1o- z?iu!J?K2Vj8l3d4;G*dQ9e-V|B(@1w7PU9 zbmYI7AoDM{>C@kgP{;(ve4KPHj3asq@4(Y#k77nK`dF^%I_!OoZ~a8g--}fLzSs_$ zN=)Ze(!%5vmBqH+CLgs;>B*NVhWPQQtps>5{SMEyRtl9E4YBqznYYQWbTVe0b4`+yRcHURWHaB3;V^qI=m{InO1pFr_%_Sy`Ic9c&nl6aV#^lE`UUL3=?7Z|)1A?0M;(l1!3h5$vuK&BzcsR-Wi-b?WY zonMMKDE`QE%ojVQwAr*!L zs2$kJFiQNW78G)47)iIg$v{~6^FDpp36a083xwZ_9-g1zMOR?~s82cWKA5nph6_^nr&gzu^liPJr9PU)IpERR%c3x92!cACjsy2R(%q-aT>*P-%d z-&Oe(y~yn4ls`;3MdpA>)cY7cr~G=Nu+W1&vzm0%okLE0Yk>z0H)MjZ!3?9E9SKP@ zhK~F;1Mq3#dKi=H zWdPHhVjow%bpP`rB!{-oS~*9mXq1Wa7wl<9`Myfu8QOSprOPRsKtG;U4}&kx1YC4n zHdr}jRy*y323X@U%=Jy!8~Lt06Q?W~IV`2Oo3P`119U{1UdBvPIc4EEF}*0|ceIQ| zni^!toleHQ`hl9F{I;F4M$B_nZe_CgkD0}l4-nxP%F&VCGs62!gjb$HpU?*W1OB$a zKfSbH=#_<6cHtTA4^duz?O#uwUp+nc8WFUm+TsL}$^Bi{TcEnEPtU5JfL{lGfZ}M1 zn`=;C`3dxl^t!NHsjT-kX)M}7W+%zaV$M3HSR*e{b0s2^fm0k{zGnY*$*;LO&PC^L z9q3TN-xQ#I-iJL{({Y2|SzKIW63?849OR^Y38gPJMTWM3|7w8Z)b%z_IXuck!U*J< zL_>$#HeF0`XhFed$N|4H9_^~$uvj`v``AFtL29<+!I!B#x&x9hJ1@q~&5L(4^Wr`C zZ%CEH?Ii8~^*uqUlqfNf+%JxsM71MTzJCLi7{2tHKvH}JKl#nQnYB}$EIPi91eo7ux1<&>>s%mG?DPUS_5i5E{a)g|cIA&^R@=V}ALo1UH$j83%E!EJs(r9@* z@4G9QUp<=NjxsrK_r-wb5=9yZpR3$Gu6RRVLt0)DV0@mF5}ilo8EJ}m1$ z2<1dx^M3m{ZU_IxDX)zLdz#+uF_>6oe3GO$yRla`e}uVtU@W8dF!0kOV_MOA!YZeq zAj;!oB;DcOjP_gQ9nzqDNXwtisAYs*);N2ElqH`+zqpETZ;|&cWoVlfp*Cj+{CAF_ zWaBsII1$v3C{Ii_*J*)IzdpMWfgg-Es*Uu^`Fb~V9{Wja0Zvltf&H{+yv=o*EY8EH z(mIAmn(K5~UwX1?^;uUu^vRnem$4^qz}(yfqSv)Cdx^4PB;Cav;9Blh`GJ6c_>D~p zOM-V}l(*o;M_ZtU2m=I$6W?8W^t*xi4Oa8<2VZ^yJ$W3fd1M#!%THQkhscAx`*A!A zQEX7NeW1T1+(9k4imUEtcX9imeSF~Ah5|rik9t-uY-y7n{Tj_Z>*-gUWvwrg;SO3J z*+njPT z_QZ0)-PulJf~?p<1bu0&nro;B#$bVwSJrmfp?wElWpX33e4uzWMLR)L^V=SkS3@J1 zNK2D?TSBn3#+OXi%C84;$TwG>z6E7e!bYeDk)*}-(mMa<@lq)M7Ffs5Q2&LUpx;Bg zSTFlyklzee)r@PC4CW2i;^n>6jHy5aUJ!sn1=&)dByD6V1Dz zrK@aiFaTEqV<{%Q4bjVAyn->*e{;+YyWVS!Q+QVEwkrm%p@C8tI*0UX5NnFkEJP7r z)wQvpz|1~Yxg!9b(?KTHDDukQQE2@IEqD=7=27}wo5~{p=qNFTpf_-ZKsPv}2W?Xf z^fh@b+!0ZdB!>m*EHiM5KRLo%ciJL5mtn<-DWqS83}_n1p1DC&DPH~d)o7^W7jsz= z=RlW6c|nIADC{K4$Vg~~@;h{?L}?uJgPXwj-$EORGGpQf1^M>=HehdSJ|}wZz2l*_s7^zL zY@6=LxKlDggHV|6p#O8O|Njwrde15w#x-KGi})m6_Cx8Sap3}yRYoU|+x(s`V9&e9 zDep#nT^e@I^gHJ~WD<>ev_#+JZ&AXE5=*a_uxiL0!<}Q}l(0(k z8=?H%KGi-|yty<5lnTr!&cz3r)6|>5{ zle}mFPt+I)$K4Nbx6(zwGKq7xu*%Jo_eh)NX6b}{Nje(j>kkEaZ8~lu+}yT9W~fy7 z=-1)@uBz1E-a`l{v~P9g}JDRqzi}p#QbHCjD0epULqqVcRhq=b7{mg3rwF zNRx-Xfhz`R&b*^mmjQj6I@#MET^!E@gIt_GEBu@m+9tu20|_jeFhYWhgLQUK z1*8Vci1$K}zNmJToz9E;`Q)m{S>;%v&VeQXS_>x`vVxvOYflW5`9%4H^Te}!5nM{XB zxvJ^ak8*0{C^tB-Ums=fkK*jh%KAk;C#1_<1~;l7H1%_2Q$H8QAaCy{T$Z%S%@0{~ z(U03VxY4`YBRg5>Kn7$os#l`=InHRmEWPd{(FTt|+YI|gn_XdOFB>K8%Zg_*>5g-# zuM#zFDn@*amTG-lNUFR5kQt|D$y;&-iEB|hGUP1_g4z+t)FRM$|2wrRX%f+A~Bwun{+MxbkrI zephd0hImgUQJxxU-bGBX+OBAB$nUUMAG3YwCdvzHb2Z88_k4z#4_F{aT?t#NZxg75 z`7Jgs9y%10yaim$wh=mBVvZ=$w?#mI>lDIEnZ?wOH|7Zo>c-bhfcG0C(9_e|ViY^% z!!O_X!{6vws4v8_`IN7O-T?T5_||Kdwa0;PwO;+FaQNy!O_h~z_eBw+bPZ-G7b07Scvd;UwN(~G~busLGb`fIpKU` zXONFYyRZQZVI`!A9Nb}J&IB#KP0Yb3ZQRG?Ad^G(UG;(+%VG8nfk&ct+nTnBJF;a? z*&8T!h261LI= zYV$5wyD{^$hf%qemEb8)+BQatBs0>pA0F-%L*uydDzT1rc(K!&r3Jy)4@Es5>o9 z_d4V=_j=SuOWb{NjyLhdDLAMlb@Q8eMoUA#@rGMfO$GPvxOc~f!s|vOV~h-QG~j=C@`jzYa-&HM$uHmJ zcUefY8mW0BbJW~)X(QVtY!aG?Pxu}m%RcDF>94`yY z$*0wvm|6H~<+|$SN$V<&pv6QO0}R%*L2rYZ5BSfHQ*Qpi>M%!38x;HASbA1!(-1xtPLKg(F>w zno<+4#=-_0iybD5!(Mn_Wcj@mei;!skdRMmio!*jXM_2`6{XF-F~})=L$`EB-D3!c zUkXDo#~QtKM%{_DyXFBsGf>31#;*C+XB2$QEPxbfEzz4$4quoID6_}pk($&x!>m#! zgY745zQ;DywXR9sPQubzw?2KL=h+jsA?Na73$#3GQk_x(^@Uj$<$`Rwtl;|7da0g1 zePFihjQWg3-#H(smon}6#`0NfqC~kErQ^*(8=I;`GgkB@j1P;9vA28K41FisSa-^M z0zGjJ_inQ-1bJP1}-<*!i;`Yw1>>!%m+V``!9()WAGX@6+LV(OeCP+MsFDIX>#+ZUROe z%M~+4z9dxse~(e?3F)-*F(fR5=q+tAieB@0OS~BAj@{6_fg4!PHLvG=N1@r-g?2fm zUQO-&Ks(&sv>|pwT;aM6(C=ja(|tDB=GkDInte>uCA6xiEiNR=FUKl3m_=;~efxHw zVPO{d=tE;VaF_pT*U(nD@vPc{5&RCinz*}CJVk94@7pB!7B>< zjs0oWX)uUXXKzghYv3fF@7Kn4pHyp2d$rNsw8l>+>C;55mOiy4Y1!b@Ur$ooB5mW# zAOuC36W8@)Rf)N9<4$!Ow1YAA&RKDguBn#rhF=o;R{k97&VujLxy@8ZkcHLSQ+^Eo zEev-|`XH!p4(=pFlH-7lhtAYlkXSt7e{zD3i~9CWf!Eks^=SgTC*W;Au0O{08KMVW zdI-4}!2^Zn>@Lh1g-iG(0{U7kXVt^OcXYRjct@|}BlO*K7C>Dsu~L6AOB=KG@fM9( zxRGQOd(f&SK??TBWOp14^~hF$9u=TRv=?`yCfbWtxQ2Q`+udWLu(|@{{K<+i7xXAA zR=_?;pXX8!H!B?jE%Ortd=%lwNXKvQFQ{yi0{+$4$7)x|3cE4`<0S%S&B6dix6x6M zqb}@g4N+E%Q2Pk_59nl+BLT`OFiJax0pi3Py=B7x#W?1K7@_gJbp+2XWES&$S}x{; zFiqH@7PiSGXquw0@AzNSGQ?^I4`sn_EuZjzG|mWSQ4btI=kl2maD~DBe`CJ5Hv+jt z!;q0oqTk*YQ6d9omZ{(~?noA4xk|dl_5EZ^t}H&KhG8$g0UQR+RJPBRdr~fx1^I8f@w4{Y+tdD78#yN>E7<0!b|~>qjAd5jEZrDaxkkQH z9x2=9XQV>e0SW&uj_Z!B5(@Ekg{Jq$r-Yo_4$zkVqN0p$w z#u+>97>lFADr-tOIl;<1;;a#NPEvrAX@62k++=THh%vNv^|7%9vVN&vHrcH3)6@a` z89O5deP_H7XOED8iE*DST=No!Cw6(I>};(mg3hrhW+|B+Q&y%V>z@@{#jpV_Fd{i@ z77y3nUCZ300XL%`(+oe7uu7irR|A_pvdWORzbIafik$HGj2q_t12z$A)24IdMCux3 z&5=TC3BbASL*oZ)@1a(Non*Fi4)DWc557#b#g9S1m2l^b!e80g3*g0bb-gLfbXD6$ zCAkz2Gi&&}AsyG+$LyB3xbCj)YJIYn9q4B7kx#eoB}-H@bbidR z^T7(ayxb@%9Zc?NNK+%A**DhA(pFd-nHuZh;hW2hu{nF?3RhF@9Wpm?7TkGH>p|2a zSlMCc=2H2Y@}NBM4bFy`0Uy^`v&Th@hkAWGdS3J)4B7)HaqZ;u!ZW6HYq&023}NYXL08e#d*&>Yp&hiUvFoWGRCerR{+o1FDg-^>b!>9p#97v`lxv z|8{4<|I^NbI=1cuZCSr9c4?>H5$1S87gtC~)(khxUg^qo<_ zT;I?Df6eOqxN&mZdoFE;`5bg&ql|BX)#GAjy(mVGY6cT*zlQyDc7K%iv)0Kil=n;) zs_kZ%lxr+hIu?k1UT%2bW?0)7>gIm#3Bf%v`M?i!x!S3;PW9jLET*)TeoZZKj=C^F z0EsgeQXAHr+am0zw%pKqQ#r6}a7s`IpvPQr+bRYNIBp9a4Qhk#`Rlep9ei|eaR~|w z>T@b&1x8sr?tv{jdDNkAvnnQ{b!bbC@KApOv&k$P^>C&M|NEd3`VIrpTdh+#Vt|JL z^>EU{H~hVSEA_vErJx_XUM|0l15{5D?Jj#=54a6d=Y-VqVL+%#f|WDi?+J`}GeoLY zt(zReOaw8=H~PIv`62TeG$ZE%bY3Wr0?!HMi&qS7MiZm>paV9#;Ny|XDAXctfWEwj z*7R-mm~H2^QQhItT=eL|yTk6(nDXR8><%D&J{MGam6~^5e~4KG?xpie=SAz#mBRnt zC)rM`mlv_uE}IcwVULrj+-U&J7>Qm|+B9{hvL7-7tKey3Cz-kKKQ`VqGq3RE*tH?C zF3wK9lkEL(+nIUBJN2FR{=&@z%a+vfbsB}!tY?_H&8u{t*gNLRUuf)=D0`OqkpS@}DmPwYVjBlX(1Bv!sZHubX!CT<_ z?GbhJOV%)}SK1V^ZgqKO-Mxc5fKAkn!N#O^M;(oZ`uSy|QH27E?nOpgA_{B?&$85R_(>%FBt!ZWO>&k~y>x_<4=Jd*eeb#aeu z$$4V1acbcoT94Tl*1vSFvTn8Z*x6SoUcs1*NO>5SrHvCKmL|?fN<3gA7c3oud}>fP zi&gnsS(RIh{1T)}R%O?+DsyWb_8z`BU|!O9v}iBFDpSJehRgeH%)ZeJRAWNlg**ez zRjb>Dy2Ro&Ov1_ng;XS9uGR(Ts;0x!m=N@1NuL<-UTTcJbM?%aLUWyKY|yuyaoG2) zviHAfk1T36GO&c6*^_H`=Uw|J9Lx~PntZD0#cipg? ze+?I`sEb34%nVO_m80U;c6!Ha-|FLI!eF1^ddsT{DvaW?ijelZ23)*>j}bX)3z@Wt zgRqzXEI=ikXafI(PC}jtz+)S+FXTZQzY?n(@20R!VLiXiUdM|)+C|-5T^?X&&B_^k zPWQL(C$?@mvtp$H3Sn3F*jJQilyY^e7+) zNFx!Pm2r}}j+0rRfnm%~2HWGLKhm+&y@5pO2c3I85;0SGK1P(_H=HzCxs4C5PvsUW z*9ZLM4f;0VzZX3SH-(qb9$<&s?L#DPMfte@)_}H@K#mEuO4nKss0Jq%Tw6M)wmMEc zt!2&z{O3E_I&JZCCT1m#@rfxXrqy*eMbEB&n9e`#+keA?R7dyo>o!5)IDdCQ1;%87 zU(N0IAdK2+n~?d=ja7oO(fz$fH+(i#WP?^8(8M>o4DgXdUnxHrC+9Au5=tEK9yie| z8^J%Miap?%Cxy9XpQvv;$!zd!;oumgEZ`Qm*=e^N5?E9x)^}zBmyY zKPGmy6d@o6O@gNN*5zvat-{b`r43iBS$HxOAI5XS4ptBzggiTDr7{paWPddVezYjy zUx0b0b4*rR$Zyo%Qb%n4)88LwxH=D_MrYX*7;!ZhngUb`P z_2{N`KMlz;UECMK_~3X}m(%f3xTNEOU(w`^@&W(eaVm*^-xBg!)ha1kX5IofJ8z1c z%sb;D86MAv;6Oecv_?Mrqow`!$erqpuBpA8vK=zosdJ~`VG*u{#*i)RZ{U`8O1YHF z5enB(&XueD5iDju0xt@xziIqm`zql7-uUz0`+oHMA1PkbHUmcUYd6V!#zR|Y=S}mF z`P+xej)&T~{aWd9+RA%hUFul;hjy-+nFmc;z-4Ys6;VJ~ZvP0Dg_` z1z4iu;v|#9{A8S!N=^*o)OKtZcGP(XQ3HFt{qxjqWvFxFB?b4RFRg?XZ2Qo)ml%wD z@TC_%H*Z^pxBq;Jwm0xns6FpXH{kaZ_|9w}y7ZFyNh3z6cW}VW@tyV|b~Er9TIv2g zgqL7tvW_VMsO19z>SepDWVa&{)}vjeTt82etyGsAF2O_4Ig5+xe%$eR=?pnmo@$*N zNiB=8yr%9fO^QV9r*g*;g^^R>fNXS2g>AN3cQ)XFwnG>#cLMM4fL2IofFzQbrrbNYg6dmzFVd z=yD?L4P(CmCSlF@0~bGr+#}n5uYSKJ#O2UBOz_j8wzQWfk_B!|zkffvtT< zHli%45BTT6svWeS{p_5M={CqQ(i*)fXSJ8i{zUC^exjZ`3|!svrS9f#*jN)*srs7p z@bylys=B0ZMwq4qNQtVq98zN}S%7?IE!2Wg=@G~joQ_jo_i zk?v4`Na=(5D`kn zi9d8;)O(ZUX^3iBt%lXGqx!2kHnNXuP@G(fbviIL({!tw9D*0#cyj9Oeop%X$4rS4 zN};RzzJnQyTwoMeZ<7|#omA#w6@@L&jKU05jsB0LN!@CS#f)fiuNht8SPhv<7rS~( z!nQ_jZ1*SX(l<8^E{Qp~?Se{R_lS zKm@SgqESQeSw7?s=8&VE99mpV`V7GOMrkUPH~RDENq6W|HNL4N;9oiV^O32D6AACh+&)mZqMsEs?&@t6 ztLfsZx3qs=y^S@i-e!LLu;wKLYB+Ljh!utc&~NHEDmlER4&|R#)tf24cnepGwQ4{o zyH>x&evO=b&x?>wRO*^V=ep_DZQD2RyRBNCP={WYit-?UFOZHK-M3;mh@(B_A?< z);jKAJ4SV@0ly53+2x?Mh{}ASHq!67EK}|Cy8(RvEq=BbOKJ*yR0IAJ^&wOCgX8{% zW2vH9r2YPIA|ND`!pb25Yv;VB_AZE%-cm`9Dd+#}O1Y!(V1(J)br+ysop}zThl@NN#~Kyt=jf9xOuZe9)uMu$k8uDy!#$#wAIO<;eR;ze|7SwWtgMD zPZ1?G^dPlYeh0Rnw256MtjLMr^WX_#jkKv!5=pg5lV<0=Z*5^8V#O{sn~54@_~e}b zS#P+9_tk2^O;`&dVQi@Ru$5V`LtSfKuWaXa89UTs>xSA*gACprQA@inc@UpG$2!Z? zdP+^JIHg*cM5G;k%enCdP_ z+PJ@ZWF^Mz%!x-7?xg=-^}D4xlwVi6!2QB*5!A^$?OAxYQZ1t;5%JeN+*O${FHiO&KV>d;$&zZRxU(6v%~w?)lx~$DYe%`|33@x-^VRZwk$&!+5SFbF!s~;j=&dw{ftDh}3%BEbBeiF7X;6amJHAUK01Kt~xn{0|F z8y-giQ&tJ0JaS2Puv8&QLn)LnWTRB%E|f|unN=JPzB-VT1`Q1>3l6ig-DV!bCLY>m zGb;f$&R%ckq?wM$;s`{lN1@cD|MN-29E?0feOIjG{!8%Fpyvieyr?IUNZ~Jqk=@Yg z2loc9&w?jK_?}gd^p&oM;%=IvLNM72*rfZt7m4D1ivM{eivJDyk%U^BxndgI`8^f7 zEE+A>c@%XYjU{QUg9*l-biw}p@AvWOSLmL8O+A9E+W-Ci2iM=L@E@55?>M4-SA8Kt zKHS5Y5XY~ychV>Yy$l}= zWvXgR*1%4p8DL`@ssvYp9OE*{^)}6!r=_?1*c{GvLCWt)6c53Zzh&4Ntvk(yLBF0w z&cRdKvPM7RjfHkFq!oD{n6M?VpL#`qOPoo!YEA8FHB0ZP)&iL~y-tESE`oT+at9PxrCfxWX1m@A|9!M=4gu+ekxUA^um(tU_$8eYKP*(mFm! zT#0WCJ!kq@L%kH{Iw!viy(BHKFEmQaWCk{3#=cBSKvYJ+Uys?Oo-42(cB=5Uh^BPn zTl!1PZuGRLwF~{G{UzCpAQjx?*7>GuW+~B{<2K7V3+lT$(1v-*Jx%N07|>!acUJKy z%yP5*f-UpRr*acKNF!XI;`|ChUYQh~9{I6QSdkztyF;2K-57~QTn+r47pRo=dG_nG zLF>+dg+=X{1&P}tX5-!wcn$OYp*LCl2wECqKVYLWx>KAgvS+Aomh$+UJ`%p5Kb#Bi zXLQ3J!+ic6wM$b8_=!#yFu9dbh=u#DITSiy4XrF zuKeq3WW~r9%gu zdmrM)Xhf%SGE$BZ;n$K-C6kPV9^%B|Z}@bqM+yVy~9|=~ZxU2gNZrjmBH_ zh|*!qzzV79@b)4jz>y1y!27bvEO=%WI8+N zrin|?*@XI_M<3o$5ihp6vYv81h7QWvFp6tZBc2=yw=~$opkvS@c7#z@fD(@OFv30c zOz$NbQNZxhIt5>6_9C2rAvQZlK!aG&m z<#M_j5YLk+hfAgML0gyyb|#d_a#3&DCflfoC=svJ6mfEv3#ZyhY`aaOr+Way6@|;3 zyxiq*(eHpk*{PigaCv7W zzOXs%5mIh@sYC?o{aRBLC;7drk)oGs#D+FKv~gy+w0+8*Q=p5=hn$nY=M7lu89|Va z7LEH~9Qz*560k2BI^$S!^)#Y~nf*mHhW?88MbHl(=Mm9FK#=Iw;%XfxW!&l5R@O=F zBh(w1!o`0+*F&q;H#rUC0AyJFUcI;gC&REAd7Yb-|AD@Uev^{%8+evb+Ycv4*bjDc z5;1-A&947=LuZE0P)f$HI&q2%zGW2VdVO@wyWlZ-RVA~}su#*pMLy!Lo~B{kkxjT{Z4{sp_On z2WkV|>g0=68CB~ODZUw~dd4=}b5l}|m@3M^o+lyuPdWc{)dt-V%RP!KxZz1&UT>pc z{(15s@a9dDo)zYx{p<@PJW>xUS+ZpYhXD_y9|t_=l|@5#r(vd zbexzEs}ZL%NlfOPj|=*lS-;l(p*73RE)rCB4VP*Bc&%MIP^X1N~lpfUL0U``bB^0BllkNa+!ZU zr+Asn_s;pewwd%S3h4ZACt1{wloJyX=NVH=GDbay#F`?ukhz5u4Q)*}of{>~&`SqF z6kn5#cnlr%Zp0e}F3Zw(rjOhOoZ@+>q5;y36PB20*gosA+Ln&z;X(IiMXKoYGCBI( zl+4Sx1JAgu&)Y(Iq$;d*8z|}Xj4}i9XfJlM`)Rxm?8al1!c&@Ejpw61@CQ#HLAbbJhx-!AlPcT7DC}r30x_NM3C-y3(5HrS+lQ7P$6E2_!L=Fi z(sKV^{~|mu=L_ii@!!G9DFxK4Pve`Ww_@i0iqjHOdj@Jr)-9fS@Ed`CNqaLQj4gDG zkAL0boV7L3THT(tF&)$eP&89_o_dAQda|(s;9lzKyFd1aOe zIPGo?nVaWH%;kNiutHUk_<;tlSj??pl=_j%-6uz8V~;Vp2PvEgM|Q`y9<~ z!WiY%5ejQR9QpJq;itu7Y-spHsn^3tZVKW(!vVZ_O4J~5fX=<7o!lD z1ikc#X68&9Q}_5cy#Ox3Zz`Z6S@VGWfR7&&x4Ux!BBfL33$M0?%$A`h6Dj&MMp z4q9~%@jp{CG@eAA<}9UsizfQK;Ta>II3+Ebb59BCi7XT0sfrgTX_#Chh1(s9Rez}Z z-9e{ue-Xv5j3-YNdjGw?aMTxm))tgbz#Gmq^o02Ww_HZeL&2IEPk8E=UYi)IALnVz zymsETggO1YfiV95-N&xaWtclFh?DS~DA4pWT1nd&5gD&8IWGr-_O{iaGC_fGPO=HBI52#Jl#$fC+?kvES0e|K7GRY`& zH?(C`TfGHWD#6r@2^d$|P|t&G3Fff?p672IeSyIL6| zEO>?btnwm{_om6p>+g-E^7{K!yoaCr^<4={gq?5jB|!In-1+kGQ`;lXzdZcp_IW+O z8-DU(&Ad$5V6@7{JsBCl{LRb5w(VJN>(#K_U)$`s>Tpe@I0?M<*WtkY(@Rfnp@vsN8)7wD-LC z$ZiSGfEV!P-~Y>Z|Jo(m^Ub^0;XB$sv$(^%ZujA@zK%^CqThdu?r|ZkZ931Tr-a8a zL-)tIVgSkD7H(ta^5-gdn?&qr_%06yJ$!!Yg)ZZ&;en7`JkUYr3Oy%B;-T$LIse-o zjX~&X-m%k5(i=R~bNkn}2=~kI+ulC3?f65kEW|8U!=rHko{XEl3ti2Kn!JRlNug>$ zs>ZIJN@OqPV5d+?`&)Wi4>&wk^F+H5>zzJPSu4QD)kyBCfqWHcuFd)iIv`&KLg(4k zgyND@jgXFh0N+;%Jw#cJh%^et(B}km*~pa#fVBw&9~FAqM@c7z8%+?GPlL=vAV&$c zOqLcWhzK?Geh}Wx(C?;F%#&I}X-by72Im6+H&swA^W75|!?*Yh(egc@P{P)v_8>2Y ze2D9yUo{3}LVp|h2;4aQQ^on>y4^3e-oSze&%f51_Xr!wb$;rf;IBe@&DuAD^88e<5D2$QxkKNPJ_kcfg@;j~tkW=Ha60N~G5O6{h zrRYLckIvoArEA=Y=^uK?JbkBb_6cwJY?s%zyW#qNNpsilw(SmUZMdKEXghXCpXWw* z3^r8Sckj5q8@TmEI8|w1&uv_Df3?e4g!fl>RPNXv(+bYlv^=q^FFMDLvR~ljfF1WY z?5^Lyjy4SLs6J>joDBCg>_#aoGum*qb!=#mABz)`S{pzSy82>rx%COIP<+v=9iiAE zdS)CwUx0c*;LIm#Lxisz506nmOfbrh#<}a;N?j3KOF_LgT%6tUK%DDc=pWyOb&j4n z5W<6OF;Sd|XFUr%_%4zj`n*Tl16OFg=~Pi)Jgbl>3nyNL9IHXe7w~O<2jr-T3BNo- z?HhX9rt+P3wXFf?eMF19Y^Kvs5G(i=tEv@c*Kcc`&?Glbi0Uj-)6U%1T>C4V=DbCO zPJ06JVkErxdw4VH|G@+#qJ;&gEaG&V%BjMw9T zVw|7(ogQi-T_zq-S<5S(;XWPno?(}5ml$?|Sx^jU`6W)X;m#M+Zzj@v&-(}$4v*Q7 zRlbXQ)voMZYg?;knacWi!FPy{ZIVsQnRZxAoczECw3KPZO0Mt+5^|O`$o=}OT(Njj zB8oONil3bkd%R4ZAxEEm(G%fg!f9`Cs^5~U7b1KtL;JA;{dld1)>GF1KEFcEw46dK z^q%HJEzn%31)94kHPf9f(A9vpTzMI%ji|1M+LHsPqGz5aO5M0-=F}dNEr<``1g=Xn z*4d~pa9RH)j)f2MGF2ceJMXfQ0Y2V>lLU#>9`SL<3G%5I2H$8+1llpNms< zlXJw^Y}{MZ#A_Y{s6XAUIl=SF9vqkPUGB(d&_M>tcH0j?o`L7d(sL#G`Ddh}I zfU3$sP(KB|QnNFJXP3Ux0v?Up?TE4#5%MQTSZJzvPc!^7ncQY)=$xiO$hSwJC%MPj z>>LJM({FUeEhD@oq_b*phICd`W7Xg!%BltXi zPX>x1jYja={dz>(yWn@C4`RC8T-7pkS9Vz{qPB)vpL&~v_lG6;So;q^FMr9s@T)R7 z*;l&wRS7><-@wx|zNYdxSGSfESrO-*x#nl6o;Z=oNvNHlG3usTBejhKT}!N;pQ(4x zDUG+xfrsI?KsEUP0(X`)%aJZ8%UtdJtlK?!*2$-+Sgo{p5UP2;IiV~&GKxVn-3ec77K1|kXcQWv6Jfq|$Vjw44X`iW&nreh@qP^5 z^?)l`Di_~V_2Am?gCsr~^ocWZywA$sD+v{sDxOoV&bRwCLL}moY?j}+$gEpomk;@f zy{Ga8pLmNAsiA#3JMU9EW118AkI=MuZxaWC7)=LBbOz=H8dXR&u|3=i%mFGrObYn7 z2k=e$KireBYkvxeK3w!q@1*~pkshpph9AH`#VM38f2@VtK_Qt<03s1*${r=ZpdJAH z%*YbLb)>iUNKEs5pC`JsR)};O1x)YShW;ZJHOL0ZF2(cww{L#_wx5YaP~&kY7F31+ehc-!BuF$x>59u4!bJpiC8b3x-l}C40ap}P z5p`_}?6-x*vfkFq`fP%_Td1t{0uc)Aq!cnuFZsPclUCV%+~@WE_m|h}Y|nb$URrbW)@)?1t}txu6Eq|QUKpp{49c5EH2;;-zO?EYeJ+eI({8sEU={|>DD0ZBxE_=a2LmA=xxxGE1dwX_! z-+VlZ;HdNMb5(-}BS5oO&vflpsc?nGS-qmd`Th#D-&ug&1qYN`%CF$yRltP&<#SCd zVGm+{2(>~mf)j?|(Vk=`)3WH<27O>LEz@EbV5#YW4Ecd>oW4M-u{$o%_+lMu{`5v^ zfALvFxH{~kvwbA=Ax26JkPG`mg<*B13}^=r#k(8nYW**oF44Ngo)l_n)(Rf%st1sh zjI>)~idw3Iy#%$3%K67nBhnBpM@2|fF>kEniO&5C6d<9tAjfkd z06njC%;vUHStDA*p-zldO@>LTgbpT~|0K{npHz4gR}c6q=);6VGIl9ZjjvlY-Rg=! zdS9T!7aD+`%dFFV4%aB?zD^lLLjDgN!EcmhbuQ_0BPe#B{?PBjgl|B*M2QvLwy(dWXE)x zTx1pTVBYa{7C4vNE9awy6FxB!VJ>sT+4}=gJJ2VHim}dpv>W2kLU=qqF zt@=FvDc%jL@-NSrp3ockGM{hU?cMya9;iBb#{SR_6~&Nt%XwIHrgob*nLw8lIHI?N zLv2S$OIpqQ2w?_oemG`a9Ox{OjNQuZrga$}q;~pXj^5 zoS-m%;}|?;Aj!=x&3JJH_C>isdHv^e7~Am&h;d4g8YPHfuzld zImFSoVMi5X?g?G)wW9`EJ=>Jqjm$DCYtOPo$V>Exp6;&o;+~V`e&oARxg9mAKn;!t zV$eH1&>^LFAE$DLgqU;c`Y{vll-}!c7kIUw?wC$Hbv?dV+to2T6*A#duoIm8Untpy zc>g`2Q{hpwxk)RX39b(ny(b-KqyEq)C8ynnH^MB>xE3dKUk!ClY~JG-_VrL#R`U-j zNwd!djKG?f1GO%pD0-DJhb(bC&~0p|)h)688P{`;Jsq6zYKNeCxkJM}<2>)z0_mB_ zOS|84Mk(g1Tt*dPKe;h3Gk_*o52QMlUGI$fIuj@m7VJ8AVsF7t5+Zhg=td=qSyiQC zRu#G==TXp-IxrU~k0}Qefew=69SX|8#%|6;v)6#@wcSLQtCZ$l*1O*Z{T%b$&i$5? zGkdvo&579#9nvi6J{^dGe+Q&0zC(UTF|QJ^51$BkJKRn<@gOh1iPZ&MZqPf#IR=wh zCeBU)fB!aTIJBl4I&K*TQ;L(2ZMs9>y&GAmW5e26`%NoA(}mBM4a!{cO?#0)(E~_6 z#UPoRNfJew*B2^!H?g@dbgD18oez!>z%z}%t(bKl@w^-qHGQF{`)*Ohd-fOWTQ$dS z0|(j;>PV#jTGf9UyF21if{&A?>4*WLoy|Id@wp_0WtX~0){Vv9sKnGUD>Rpdw^~FX z1`9=tiy!*pO@x_sk*^eTCp4}X__sSe@)pn4DT3rb!@u_)u%76|!}~sU{Z-MMwmayZ zmK=|B9ZShCigQ{Zu>d(2$5JQC!-~P|$%UWdifUI5c@8li@-qBXsa)y~z1cs;1zB}p zwsfZy*Ac5dfxXi+&Y|8D$oErcdeIBTW1QK?-{=s8mq+ed0Qm){%SAZ~g?4F$vk$T( zj|QlYH9nTT+4n;6*T?pE#0o~=_0k&wi>oua0y2rLFvD3vb%D$^D@>xg*bWEEpqUQr zQryXhT+6b+ozl_()ll1)rS{*WIE&--)OGtiQ!(zlLpMVr$1W8FSmr_?PUn0p|DMe7Abd z*nJRxIvzREjB}S54ge$mP0%k=fr?!Z9LXMG$6OnD;PRS2m3!U+bb*qD1HwUJU{?y8 z4&Zg-EY?ALQ1`(nfRi!=lqt<#;aMinR5D;)N3)m7rAiDCkc>Eu^o36JXe6hXleir^ z#Eevg9a$1xT?5TeJ$Ah$`4J9OyZB1T2poIQ<-d`ct6lG5S6c1#-$?zTErWD}Q(ML_ zDRdG=k)Igvc9*c0s+CMU30JMAh3r8Ydbo_zRHKco<;Xxh(G;=H0kieVhI&v9o`FUc zQO|%xS$!y&X%Ky7X~2kaEx>xtO+JoOYtFaNDBSMH(B z&2ZLVDsv*g6I6jW`Fg6wm;QahV#xAuXpIAht+b1k6Q7@Q5nE!$9ttiubT``Ntsc9$ER{9li9o3=ivPgBQT#H{bct;`^u?jz1BP6bwazdO|92Zk`>kCb z*C^1XOS??2KL-v4#{tug@?Q*j9^Pj;`~CfY#E5;ph3cCw{mM})*NSf=-FQQ*n$9jS zuSVG$_Tb(v? z#cvM@YU2&F2HJL}xFlI`C_-;B!>|^OMX>C5=uafeqT#g;Ax891kt{9~zNp~%%#)<0t!{k8=Cz1 zBJPEeUW}cQwyZ2)YEWayZmTcOI&l~|P!I5+TI?(yeFC&t9k=tFRB$6M;=ED+$Cm7~ z9c#4~E}v@7d|dwP-Urf>iuOAWv|LVMW0K`qL#jVt8rIIeP@4YdM(&wUVR`D#^nV7} z*slZ0vJtWuTz()xy`ZVDE79!arf1+gdl|ivF8yb3P|xZ8jgqE4E*P0+Q+v+hqEwXr zW{L2~%TG{^c1S|K{>{ZjNz=9UZ+659+>VWoddG~GPeHHN?x3f0T5OgtZofNiYq93p z4A21KDPOfSdScFfRU+Mql8##El&S z*#OL{=}#)jHhN3L{SA_~T?|GFP^wz*lH$-8?>b)V&T}H7};q1_%0uJT_@)EGK1JDh_g^xtS^}f7%`*3%az9qS_yz$Q?>H^f>lz(flRgQ^26f8GT>!#ofu}V16 z7&~rSYixD__~?Q>Ys?hG%}*=79=Q6 ziI&ztle8#MD4md8_`fTV(&d?lag>kUXMS7bs=%zy7(V4JZ#^WK$SDfAUdWXoGia!R$Pg_K*U6!BQnO$_XwDweq zGzVp}JGDVxYjF*0DK~FW$KiKWvXFwc@1!pUE4DVssqxunlNov6ncR|VK8yNLX|zsk zFZncpah$K;XOb3z3wdX_8higl=y7Z>`Ean_Gq;yaKV_AM(5dWc)n!&mu%48s*(T_p3G z*c6-T)8ywIuo{$kcqD@)$Ze}64zVApPiVhl zVWGF-LP`AZJM>y^x;S+|*1`^do;UOCskK~wO3`yzW3Q6(fc^B5P=B61XuF_`f9ZVo zPx))w8ZT%bxwJxG$UN;+HoiZru;DEx{)lzl1)q67diCKa z1!g<=DENSokOY;M!~S(FPTSDjQnIjDCA#dXuomBlzL3yul?bhiGam&~Qy(zt=&5f( z1Hs+`bRYp798k>nI(J&arVhVLwS3FV@wn9(DLRy++ zl^a!w=9n-0KzXBk6qnqdR5`tw&Y^B_uWy27r!yxyahC53ErG-|$?r33&Iw9s!@ZRX z%Q=4HxdG|sN8ld|{QZnJZ)eJa^urEo%XBG{@?-K|M}j}8Xien~$NnYSR+4S%3z0p6 zwl+2);Lk}t9vF_1z=3ymCXgu+N{t3c-@TzPBLlSpeN(8s=%sqLS7ub!!N%{N%56%a zF*U^`eQ+iQ+8DQ{)F_1}a8>0Ys3GxU)YOp8=?kSop6p~G8N6$v*b}{;Y-2GiPwlX| zkB~gL5l@9cvCqSa#_2*{Hcp<>%K@qahe1!fUL%d$kSO;VV*T$y)*O;F;N1#sab-OCtD5#~$k0Ts zY!)2ZqYERx8S$6fprMsaJ~1Sw+!KaOfUp@-ylw0^S2MN~9sI~^vyOe)KG zFmbL0qdVP4wq&)_cQ{5KHvo-G2(k*GWkU{g=rLX%-wj(K(3pHI!pkwZ{uDEbm#+nF zgt!gB#3yb;3p98|4csuuT(>?jQ`)1Uc~=HrE(xG6I9yCz*bi@W?h{QD;gik$?fu= z3Q*2nuUy^d{G3eQ^cZ@Bm%md$5r7l}u=PRTHXc^*GhRNez%qbs zk0ZTR1D+zG>>#~eQ;$Bib30Qvq&7q6dq?VqB=@Y&M1eQstUvZ`>~|sm&@X{^#-TeplUdDKJD`Qs_c;{3vw;2(_ID4uuZJI3%h$x?gftwGOZDuj_2=eoQoJOZAZeViSbP_I9G$OPzPGTtt3ask5(=wjlfV_g z`q#v{*hhNNmVh5UV>Uv2f}8lDU6a+;NXX;WXGe_f@@pnjAHV7NYb)N|UfuJNP-Z{1 z1$jge4IQtGB{%2P~$zq zDm8uw;_vq9-WX0C%*jjGURqIQZH)zurWP%ekNHxV_vtHH%H^=^ie%t?~Cp@X{MzLsoAku zX(78K{{`}qocwA3BaZKV$FSc`%>Oy(Ti=+R(4H^Du9EnZhPlit0*#)Ne-|Dy z+vJ2ydstt_yvg*n&Wpdk(46cV=j;!GyI|F}S)C>zi<54g;ZVU5k`?&@~ z6?+iQ1c|v;g|lA3*;WU{E6Rh)pz;Rk*nOeNYToH(?t{@*)HZPPv~cR|7_>*iZmVE_ zGPu8Zw_)3U4yf%KsStAD{h>9Ake<|*czvamjTZkp%8h}`gW4``ZlZp-(nWC7U~0PzY+E8Cx4**6Rv1K#l@EC4no`6 z8s8ReA17C$bsDvGQT`b91h9pXmUdK9GRDDuvGWc<*N!Qt!;4e*3_p zkeFx%zXz~qGfZv)UjbTRp9j4zU|NrvVIaG$GuK-Qh@ zdcXEF56~Zggh>e7^}xbq*?Q>PVatsD3+Sf!r~AKQf^{2qI>ae=Ln4ZJWUAf>sJS0{ ztWt02yn_8PEDh#Er#1(&KqRwE@p^wrXy1#Fm zvIRY|9N5{_u;H=953ST#ApAPn__@9{pu6WkW^^?w;_l=k$mp8(bqH~g>CGoWJLaZ>a;Xf5Ja1oUw6d=_y|onm$n{LbIRkuE ztZ1JEPCbcl!clJG(iOSTCDR^Ffws$cTXvFcyeXRh{|W3X?r2Ss{swfvtBWRAt&&r< zgh8C`wMa2@J~rDiW0JyAj#*Tv1iCHS^MRR|9um2j|L|fA%Bw zeAt{nXpf@g|6iI0w|eXF{*szGiw8b1Wg<}-0^<>cjS7tc)pi?tpxQHrhn)fFhynkT zIOT+B;#0QQ(brOVu33q(IkO8|oEAcOKHwp`#W-K9qH)?tzGJJf6Hji5YL{)nlgxZT z$vFCS<10@X!cX)jHf}3U^)G>rLR2eGx0UD;G{rKxoymI%<#$$IlZhVEL(%odO>Uqa2HQd*)Dx({?i5~?t!2{5N{r0;WW0=rQYyzRv@0$ovhw{a?@=KwhxM*9-nS%|I@n zW*_y+L5vDZGwtNv_DJZ?7Wa`^Ui-?QCCfQqBy?j7NhDRfZU+{9Y(5YoeXfdAe=FhCYaeP~)ivMA6$x3PRw3&W=wi^Miy4~yfl_B;nfJD{ zaZEntxlws!W|DJHZM|pD#`@Kr0lIs_18i^`@NTe^#JGXRegW$DIdJcp{8!H&R}%Cl z7C~>~3+FcF@n1y2-wG6bK1)AmLKl^HrVvCss2!68D& z#*CFGwsCRFpBu?)+=_ttsZ)r>8e+?BoSoYFU-Ebv`Zdu!NWbP~Nc(>sc=?ItxJUXm zxl#R^UnqMCw9ul@k$%k*JV#}7(^~>dp7{MwPtQ*6!q;fHcC$JX$zIn2uL#Va9!Ojg zb`WWO&?v)~335&J4bez%0#o0kJcKt;(@#k8f@?9pGK3XBc(n^xbjn-|glN)b(o0)t ze*VG;$Nm8G&I#Meu!9(3mdlMRYTm4&F>Y}!23`l&XXv&>%f~t~tHw1%VmS+aw95TY zB+t|x;t8$ZI1ySpRu$=pa0#{%mb@{`>vECBS-P{^HU`q+HXYUx@$ZUpXcmz4cdl}l*5CtWyPH`mYW6_mSgs-| zkrR1Nk<%Gp4dfi6 zvF&Os<*^GfU;8wd9$Bxs^t%UWovm?tCA&G=KY#6UJDGKc?=?kih*_e)1}GUhSC`#f z`~@UIG{JK*(7TRvA}!eo_#@I5?2lP;)tt3J13}L{j@1}C*0i3Ijy0`jHuOe5wci9f z{zWd2)<#-m#cOUTKH<;CYL$jPiYo01sVbu6lFa#2flJj)JhO#~rygY@VSPi71hUJO zn8Dz2hb^vgxK33$bwp{SKD{_AI$9H;Kcew7oV%l%Ig!xLem4Fxv~^1%)dr*rfyVGq zaH{a##3_Gj8^R{u%3$yU9nk1Gg!^XP-!(A0zEMg&yPPz> z{tPJ`N<+Ik!t`=C(^p!54VV9}gTB<~{2Zv{uzG-5^lfhg*2_p}UjJL_Tzmt2VrnbK z1?1G8g?`9_jn8eIdzQ=bZ}b7r>{&&qlg@Hu>NX7BU_To>zE#;hf2i!#7{X8WZ}Kk! zfA^`bnCV)a>^}DpPeb^uaN6>@%|YHU#@8Ga3_1Acz+rjUlY<>8>>dEa+yayiO=_J3 zI0LEOe!e!*l;UY{fbuS6NqbvtQugW}JsW_1L9K!G38ZjWRKO@U2%nzZ<>O7Hy={WV;41l7Y&rC{RK4YSPdALtAijha^oO3%$$nso#%e6{ zKvyFE<-D#r^FLRNbusmypAipzekN{bPe_4udjV#Dye`H&$yp#7)Llwp=Q=AhvrGr( z*`fX+L1O|&IM7Nrx!?WKz&<#Wd(#JJCfxKEt|?6Z?|u~6ye>y!`kW_l^{VoSO~iUx zyW424L5t^gUA4AG5q4`j#JGw_utFeI)!>pUDjb*FE55JMTw1<%&r)OA2WOHe8OpXQ znmQmD*``ZGd1A6AOlLx%#e{W`(lX&h(i7SSJGb+t8*x>w=%n2k>#=VGn*=VIeCPpf zamCLunt!Wo!ncN`Ex+xie-9{^^o%z6DpNxD0_CVDv@%SVx9NEW-x!URbYVevHwkI! zY}oabqN*Pn>1p4lrAyV0VQqFMSa@57`)H%d9n&sYG^R3ltVMJ`?*7=V91*WS6%eog z9u7LIz|2l^b4rqv%cnOsoLvZPMmhfjw+?P294mE~)Whejpvd}o>nNY=u+E|#9q*p& zURYVWda1mL-|7}JK6TqO;#S9HY+bEyAB;a%S-X0xn;(c{8FN;XWt_p-XDdH;N1vuA zDVEFl2Is|dv{!~5Z0OloVdvQc&CoHU$6}N~$1V~IMm!eI%xBD#V-t3}A-$Lww!F5w z-91k>>R(%Lsf!Yig9(_7$L^4?j9jj?sU=hFNy6JAGe`l7g^K-@d*^ zN%vURE?N7^+V9qy*Hx?w0Q>mm`^K(cx_<2aOYUEVCrw8UMKy|<1@2!}S)*7y^W-@3 z*YYM^jgo1rQO0;`lnl3hY>hIuy+#@Bu2HPcm3V5LEdftuqb@l}k>#vWa*=vG+ypq= z(HdnUyMX-iks=SVlkntZ#9U*nQKlfafbFex;#^##Oof|fs!^sR#SEml)_5KEn{pLt z+-N}e8!lbvy{2+JT)MXbYu1nx?ju|-PUpFCzDzou!v@4sS5gtnR@y3@P8VTy+``VO znk>Q_+_Sx*N9er&lOD?#svZl;TwfTFbM7JDixV^lRnFOXKycLnQD67f?OqFXWp@5Q zb!DO@-1KiH{PPz|7=1+vq+ie*`T+U`pdw*Li~|k0H}rR){YSM5Y8!lBNY7iiI&N>w zHj;w?eYFVs7)8C%h1~7XwL#0lQ30KgMUcn(0NO{S%heP544eTG^drrVdMEfuXklx_ z<%LABeUkUR#~iYef3J!#(~U=fMhQwR>FjYV6o~32SfP~=l}8s3YRlriy`h&PeWC6! zaaVdmBS5?At^iKnflc6$ttOeXy?vqf-q?NO3*24?S+w3>ohrSi+U!HzX;_5^=Mnrf z)-;8*it5<2b^DQ4{$AmBvK?1%$j#+^*}33k!;+X;;b_>0jy&Uhk@FwrsFL%%oz& zU1!*|oQbyBV^z)N~f$H?+>jJ;9oOa>B$aM{3({NY;T5<0T!H(OF4fQ#dyJ;&G={d-7(ah(n57 zg!2JA!q*_oBRoYog%vDW`X2SvNLtr;a2;%@(=yO775^C22>57qJ)ylloGlEll}0kc z)_YHAJ7oBYx*0jg%>kbUScpG@eo5;fNeCcS^QYcnu0MG*@pS=@GW88FNofy9u9y8; z8z!KXsp=g)-SI(Z%soMh@lFAEl~xNqMl?X25eugV887RT?PtH zOdD)jU=D&$^wX*cS>1tEk)$Qu_NZ6tjCt$<2zdo+V@1CAuCVqCo%r&iIc`poM;?1aT5Hjp)Tw^ zn*`Xv@L-g=OCarz-Rmen?;7cKba|HY&WnKz_;}y03xP!JWqH}hTVx;f*afVIKj5Cd z(=ZWMSU`Cdg|De>;W++p5&r%ky>jS^UWrO97&C^DUK#do5$sd=H+n>hC?H`9CZxwvA<{v@v;=a#GQ>qf86Q3fmnd z&Bd})5wrA)nCSh5j*5;#!Ceyd8SI<={A@JE&vAuU#0@^TsCf?R7p)a)G%;GMx}R$$ zTd#PEcDwIA`_lRN6|>HbfZR&_;<1n-`O}4uRy19Z%3i%t{9x~ezdU%=1!KyH3w3ey zT_O1qrvBt>2mD!>`z7Lw8tDmOzS-q!&sN6>spNPgun`>I_kw`B4Kw)}`}21DSx&#r zKJV;^Qj7jiJ<8=#;Ya($o~!;`c$O=S=}8E_;K0`+IVKpfdE}8*FF5R-S5@)XO)pxU z@@nJ$9z_;=jQK@pg(B9I(EnO-?C}{R8Kjv!vbO5AVtXfF#TU#dvY(ByzuJ+YVPmfL zerZ>AdJ8Ag%JL}R9Qh~JiH)1@|11@?xB=xW+?f46{k>*=LQh&gY$MDPd-N9{cj%69 zOxB|w{4wIIp7GR3pQP+*^!D`22|dQ4sBe+j!<5~{x`8_I$96-b;5hhpuXczU`&qrc zCGa>}!*YT60gDyTt;9(?T95tV>dS2W9RK4E{)^4Xy~$IPvZO=cI1~CWcdrj==l(u` zeiKY#;qvpqi{NFd?Qy%UP|%|e)Ne{c&s9_p^qYu&(}el1fxesJk)QNX-);3o`>qOo zr`B6-AL|hZ+K0ZIu@>!HeVi|tTJ%{8dQRF9_$X-73$P@Y5!PQ!D>Q}mAub$qao*Ws zH<-iv;HbjsRDyb;TFi%IIvG-m$Co4nO~_VyY-_P}{EZ+M#iExx z;<%QddavXxSAtq7P3XMvchV1C4v20AVPsc+lBOd63XHF+eeWweHPaSLO;1epLA;T4X z6A-0i0jKn2ph{{E+$G%?utA>gtst+TT9g!^F*6$L8I5fnz7NKOu_ke@5_%mtoUCHH z$>LNX6*SGHgnkDNabb?dnWXanJh0!`3I~i16R<|0r$Ri=HdlEelV9p)^543n(kuso z{YyM((sB44_YcAr6_a=1KN|lSa5(g2^9El7vU#%*_Gx0FcQzE);_M80@4}2OBd{)G z=FlCim$;_7GL&tEV=ThusP#a7olIVdeF~>)%P`q>h)VsgCmM6Z6)~lUn9LuBUZ9iJ&ri~|GWp)H2{b>V9pbxi2%OI#O1Xq|@|TCJbz1+=Vf~-z zS#pE=qRhx{x0H>ZBdgl6BWmBj7|(8YrRs-2(B#PWJ^xq}^c^{qK2r;1r<+~FADGn% zHl@Slb98d>)8SJ%%{NO8i!HZhE=`Y@)}_Z4)jLKU1Kz@_^0bGWb4G7=q`A4{+tcoF z+f(Et%=A^@*32`3^2|Ws=FC&rO)!PK=M-d_{)P8UbsCNL!Id2?SNrkD;3&?@9GD)fVbkC^E2Q6$LO=?N-_peWM?AEc5FhUokHjK=MYjk0@jo}l zl+ruK*|c55AJ_VD~(?^wz%*uM?p)D6pInB>()x; z&9^{??5RG=@o(Y8vK-8`eB}8_m@_Ap_Jv0F@#aMM!=X#fM)!q2>+1u5PywgDb`m%C zy+AF-)_glF_n@C?9t^fy*Pv_IjSbPOaCiJh`yuh}U&o%J8BTx>WqF}cW|BAAX@t(e zbENadr)$f!rFvNF#Hm`=@FSfX^W{+EA&MtmFv17nN-u}rI0SoR2(t{%EC9>I`qO zqrY#2GaVQr*-(+L*3uTpQoUN#pxw1oMgg(g09nz4`7s2cmPMS4R^v411&@kC7+R)1la1gXlA zN{>`pHPwfpJ@tlexFQ{p!)MY87I%zY@-RC`TnC}jnrf z)`?s4l8ADLKxWT)`72$-AC?BZW(z zq#f!mr8l$)ZC&0*l<~xf2Py6HK95N@N0t>KesqcVl3yZ-S!_?4Tm1F)+Kg*qklY4LI8p!P( z0Y5_oLghQu8`=v*6Zk28U4NeI7SwcyBj3LUS82E!?{CCaGOn`xowyo?tI__v%3Dz5 z9gb1{HMr8_D#hQ3D-l=2{hhed;K~TpD$|r`E#v&AYkpPy7ucNsxxDgzLMzc|iT|h3 zzK#DO><NzG@IW``=(Y4Ew>HG5TJ*YEhu>vVYupGU(S(DT}#R<`kE1T&jP^-Oy0<2|=qy?du33{-e&5BBA|QbFdq*kkriA0c|O8OfY%~ z>*%#eFp%bCqb~%gZo`rZ?_n6$@=?6jb*t+cM~d(G-CNNwT!t0adN;E;Gi>Wlf?ZsG zmxB-TJP@Vk$M|%RBcd|vBPQ?pLX``gaa!fdtjBa^EtL1TR5p6antRtz$-Q?1{5xGs z)-1bg$$j^%8()D=(X*j&nQ$gJGaL`6ft$-|*u?E3v%y^rHymyxVhoI9!0u!7;2cQf zzyW6*{A~D%h&RH`UUtuN(~V2ku3(XK(en@1Z0P562qcpZ{}sLxgKFTHGY~%sK7~gQ z_?0Uw*Rfe{w_{D^%3t5)W|eDJPk_7gzLm>2xR+O~TfVGv?HaUY#>!PIm)yJjIu!XH zl`}L2vaticFyO}^;?hVa>JTPhMED~7xPkasgz35XfiO@A)bNl2A7iMTY2ZXSJ)9A4 z7+f-38k~tQpRjEC`U&gSZCtd0Mex{v`>AbQki*0$vzhq9I41rE?tikFSVhhXxnG2j zz%}D~Fq1vFehjW2?zaQ(H+ir_2bY=8#95P=IGKDndL|3^e=`O0!?uF2VGT$W3!gfd z`qPO3_2bb*ynFbbOvb1mselCXk)eV7Z&9uhKHeHnevQBc@{y4SzUgHh(^B|Tk;Ra2 zOkg_rq{A#yc-v4G3*Y%YF!bPiYSLLe{N0&jm>&Mok8;@%`0c-*%o5q{^JYK zS9p{DdFYQt(f;bYiHXY>Mz7WPxbnVx?zv(NKY1GypMgUz>hHG$*Pp;AM>qWge&*T< znQPhF+_&r5?#r$Bn8FGqEtLAJgk# ztSerlKPS@`UgZ7Fw<|Ks}7vE-ZX+}$|~K3o3z5!J8x$D^w6OI)P-cz3GL z?*7qE=~(UlsBhmC_1WwJf0%v83-oKA`QWfWlzjU@&CJ}ww2tqtNtrqA3EOv1Z7ja- zPmi@e-*{}-jQrR4er35ld+N2XcNO&+exLW^^H2OuJhV8oaMfGOtT7+cP{nVwUG>C?b0dfS zH2ZBoUE)1vE@P21k27&79QPE)%2p=cQNzUR;X;rRh=m^ycj{>-7S=|`(L0c_qdFB1 z_;h{cFLxlF0na(^yJrQPbsr6$RV&&2@<}EKzJu4_WLkes;rIRWj{{glx64yxl-IL}vdHakP$5-1vbw6UFJHacW}`M+4=jUuiT?bp=8k ztwsNvpO`FyF^e2Io}YvV%5Y7N@fuA5!q?-P90Ln-2EsSsnj97!qaESvtfK0;f%usS zlj8>B8Lr9k1Nn0lN8y3|hn_)y^!F{^?|I6=_T&(+WTgEeS5A^;h|S! zeDQWR_#2siRXOABgF)D(W9}=%S*DoUE5j4b#@$zjr_Q{p>B=x)%8Ym~Z15L1JH~ot zIB|~Aeq}hN?5grB!x?j#`^xaR8)Is(4Cmcs+m3Npoow^G&rDGI`#!JddH#S0 zUN`s7J=^De&gXpgb1Nqj^P+U3KbrUdU;kfmpzkEgyiuaeQi)O?Bg#`Mv7~y8)vd0S z!}M;fYH{fg6Xn}st#V+5QC5$*RX*=qd6~+m;wkl8nFP19#B7{E%!ClL_cfw_48NnD zq+qUR-VOQUDlyA8w{mXPyy_B>7mZ@3G-s(>VYp0jwr9F@t#hh4Nt&T-xqrr>chMyJ zJm{S{A!wwHo(lm8yfr-=I5}Ye+6PO5Z{9Qz^C4G&bN=1J zv{`Qw-H=wb_3QNqQ#i%Qx%&;=UZ=ZX&;1p(emwN+^9#Bv_`IXE9Wi%*r8Engw4x1$(CmyypANdTwf&~dv?KazPSJ78RGXO;q6aQ3XtHEk zFh9Z#5yf{S#Q|;qzv>s$Iy)uCM(deeS;1_9m{A)StNa&^&9)@hEFS$L{zYI5R^h+jFY&Vthoxm}IrPK=)0ys6FR{d< zpI%mb^ZL5Gvl^D`58wTnw0BrJzudqn(>bQ(GjXNf-A^)24b7ruu%qq?b+w5p%nyS` z>z`0>BL+Jg(%adv!S5dQ)}=RmBBnPyq1KbswYEWLogQyVUFzzL2EaV%+)xLI*+P8~ zb?Z;d&Fga;Zj|<_KQcS}SyzfktUZBt{A#Q)b$Zu-Kd%mTRq<7Rw_;}E(MP_6**3WS z9t*HBh<@@KvZbE$PE%t=^ZI6WOX8l_a0^!NQdtk)m3Hz%qTh#;_@e#Zf_}Zsqk(vI zMcgbf%4YSOz{9{`X}1mU4ZOh^k1mZ5%BIdO>yM~jLX^J`9Z`M7JrOf$b{RuQ)NSVJ zj?gMD!muepVtZfpnQ;}Hx`}y2F74!mHpzcs)5crZnuI;;uM7Dv?AWLcInObf&$x0! z4N5k_NDc4lZe3g7U^oh_g2HEvl`DFUt>Mb%X5jrfnG{_WdQx3o zyxY$>Y6rdd8iTciWCO8fx}Q|*&7>$R=(G}RCiqD)*DS8Sn=qW>ykA@|6T8 zo>V1ZV#YAB(x-Hx`zw=^Q5p^qQrh|OdTmva@D$!N${jv@feaD$Y5&57h3{PC9B9h;lK`*!F=E(zQDJ-khQ367TooJ$PY= zIQqiGT-u$fOyu%|brP}3m#xrP?9vKl=Vk{Pv#I+DbxW}cqq_Yj%&0$Z_L~zh6Xo3? zR?1S^sS&oUHw#--(G-ua9^1V>t2(ct)s-J&FEqO7{iO@{;jRe-)AJ`*5-U;u7PDek z*+Abm?Bc_s*btBI8Qs;;{Z%Q}>XmNH(85=qm2qFfRsI_?iBysV-=_SV(o;P8Ui?f$ zoj*xeF9K>xU+ZI(ioBqpQq-|k+Zr@NtLy4tFToTgI`-&k_z}O}%{Q>laD*wk#S9kp=UlNK-9#ajV`01l10<(~t{cV`ce$8ztvYC0^}w?h3Ic%+RJ|ZW$Y3krm#`CGQoQ$M6`?n(99oAnazeL znB&opNBY2d{xZ_7YE2wUvN12V0~DE4tpIP! ztZwWiwk+YMCZ9_iVoG!lHezC<;aVkLGL3ohR(^mpZ6#?IxAeI4QoE^Aq4O5;VQG%P z{nq!SHgeqgQQ%c7`7mY&;6#rzQfrJiFjivOl~0u4#)x&kAT}|ISEVb_8D|!WO~7wV zH;0dy`2KKXH*I%oF41c}rcQ2vWMK})jN*zB`<@_nGxlGyZ~As=S8->#v_~pX7=Djf zCRtW5k*Zg_q{HG8iEG=Mn6Gs(?pn!Pl}2`@G1J{57I*iYWiysYKFJB`<<;tr5Jz^< zXwwH%!1G$DjK=SYwu^E+dVA0<9h0a8`Cfi-lC7U%QrxU~CO~x*z(a~V4IV>f4md}y zpsz73efPZ1@{EfO>!3RocXOguWJ-#}3-b1FNZ$@zyKp^=>q%VSp4WJ%DSR3m`u4nG z(__M&0vj>27_WKG*^%0{!g`@V(Xmf~kM*e*!RIoBNMttS|4KdeaTBgsB zzqA))aMO9cZ_{~$kHUtzkTkog({##IT9(cYY~i?w=;ki0cBf)rf<_a%jY*_fo#5f^ z&=bLvyd{}{RRcK;{#2sM+QgIBc3w`ib79^?+kPX1E{e}!Zxvt;rasbF+DR!Tr#h!5 zQ>+tnJUP|v7x}t_DnF-I@h&*1X3epR2i5#Jc~VvtQ_@SA#ZwNE85y1;A)_kQJXIvh z1MwokRK=Nza!=fbwc7}p)91E{NT>17zKTZb)v zOQ?rr%v-mkg4koB`t^kZ<-Lv0wmY*c=A2b)%Ue2P(~9Q=m_@wc6CM|KyN(6P3{X%C zbn?{pmb}1jx&zGhE!I2p0&@ba%6V^Z?*2;sO;-d=4SOj4k*%QoWXkSAXTaOAj5W?q19l0uHibIwLws6pmsC0BgfQ}On6ofB_n?6%`|94F`MDevn_Ni=#K9U7| zR*xkxXb)ce*&Dzi|Da5NCwp*WwCxpl0i!qa^lu7(C-AB)X_jtXtBIKOPGF-5_{`~@ z+r(TkS&3`sg_yaH1jNZbH_#Lhtm|60-Y(=xSixz!6SQ~Q?gn5HINU_b+7e~!)^jK9 z!b&`UEw8*Y)o;g|uUpSpP`-9Y1#LIC@iRa%h5wfv?MxF$q()03t2FGn-k0zZ@UQHvD5vrBJe(D7~ZgE&F2A%y|K(WG7u? z+e!O8?LP(o+1E*mswWhZzb zg|Aui3TeV*;nuZ`a{pM9AJC0krn&*Get)?woYaaICOC-l2vLrWY!ZmQKdgylr`(8f z$42;RO7Ag@RwXnmPCca)y5s%Ahv&W7lwah87Yd8}c1u%KChwSzJ366I6VBCXj!;YG z)zQDoDTT3crc#i`g(iT8;?d_vQ!I*ndHodBPdVZeXz2^1{4`UqArikTgqDMemP|Os z@~!L=lI?$u8W&Ia%b7#JA29HDw(K6qEgwo!DTR(g{@KWriCywZlSVKK4*w^T8>{|_ zsIj$(RF?uRk$6-YNj?|gxqtlGgy%0th!tz6OBYH3)v3gp4uQwU>LL;zM z1Xj@w5lb=d?2rxfz)CHt9qhfAfrh)Siv-s{PxVwD+za~9SbNiAKc(<2(%wkzG@`V; zP5lGBz@z*JWA)?Z8Ki!^{O2fd?7XyJ>euICCz>KTX-;L6*LhA?@;tcSn-^Rgbtm4u zuyx}PVa-m;eBQM#)Yx5uX9LP(lpAbHQs(#cc=d_)mfIi?G6HS6%;GfYQJ3miA3L)x zNwX;@&up_kmaOw}fbwPsBwARuKGwHwU#zG&BR~Qfxr+RK+m~AE$Jc~AS>N&!`tOrp zY7yyfP1Kg<+gf&^1_>nIWkNMSjB@XR(%B)&)=v^~ts;g8j~n1%M{c=Yc0 z#lWvHeo#)zJJ&hWJ@M$`cZhv!nA-2pyHbFKO&goT_xl}Ey|_+l7YC#>;?Lzp{(8wF zwoB{8Gtz*#0lNe`0PIs95-bCDMhaGut_M*xBTRxah(Z!i1$Z|+3=TyYOLDCS-=Zs_ z0q26Ytt|$nJw0q|VjSAz%wk(3{r&QALVLH=ueUU|t)(*40q+%m1bcTiB;GXZvBVtKULRmy+fng`>#NY2 zZJ$HO2;J-moY_LDg;|vBi@D+$cIWveI`Gcz!c<_*q4Lu*dY53oF|UK);gKk z(Sv9C+pS7k!cTi*6H$s`_aw(Nb?Ryus~M{pkERaSUW{+3_YzyXH@Q+Z`xk-_mO`hp zji%ISgnco#xY@LPjhu%%)n*j=3gFW#(mc z5o>PaIeA{uPzd^?V^G$2Qrq%haq7>6<{9K)Ci-hIF0oO$o+(Zi*d|{GDJlJP;5ygHn@ki;d z;r}6XLQYe*kSj1H^ZoM*)v!t6nwSOi97CbT3bm8N&d6<=SELWksK?m8u1q*jZ%0#o~6mVMWFX;Pmk4}cw&{o7-!r4fZ(a9{@^TMqP6XAF85poX>jl? zDcG~=1GNX(_}SPJk0eYLSsU|ONh71Y9D`lOFv^?5yzQDAn`o05$Bn1VQV)D}@4=e~ z%KOC>;R?`+S;~>NyUI^mG2;nho>W-*thaC@-Td;ailsZD~l3>)j4 zVaB>!%Du<#$@g-$l15fJADih(^&^%uG6Q!;xiwaRJGJj@iT8DJMzIa?wv5UWXc@{d ztGt6~&kj|C)|AeOvP)%@U&Kaao3xU=A}>)FH0DUmzLe^_M`O<7)_(06mb*6tiGF1=OO}oFZoPnoAyfVl%JNz$Ub9VU)+zeK9j3b|vI= z{fF*3GLYz#N?j^5!f|fuP>AY1@#rUUewy|K=ulnb;u97)JVNl+bAqX}axcc;lC%md zF2?Vn5Nw6tH>tTcC|e047(ONeO%gi8Nkg?sD)zJMce?b@;<+^x*U9?CEXzbndd@$W zDSciVUxV?lzcl_rvp_X2N?Y&&#=oa7hcA~m9C7&a_=!i8+DJS)Ii8UBv(KNh3(Pg~ z=*#hfK)*cO`yW}?Df#u)#L_W*?mRo339l=3fDOyt%ytQU`|A-dVbg=(xYEd+v+*Uo z@V?mxeSJFO9*1KZ;h5jpwoA?TvaP9Y7J+F^XY^LT zoJHZksl-WbpV!gnvt@hWKe!lpX%ponY}<2csoA!g!<{$PuWoN)5lOZw2f0jldov#? zV=WJ?et1rHoMY#Z^7I$YOj{`^Vxwz_jU~2mX_MPoUXt6p-OJtESG~9&sG=o zF$=y#AM<1UUvEx{AyeKZ>7zS;4Ks zPQfeOC`>}{PmBrqi{6?ounV;VTNrAI4#Zy}@ch`zLS#T^=uye`8&U6}F-m} z-Zl9Uv+!+~5^hkmM3y}&UyI1VbiKel&L}lwn+4_>Gw6*;8H-*!c5~U^LFX&TVM^^J z1fKX!g7+r)MZ(`nl#cJBlkhZ}F>clb@XNz$k9nDDHdS@4TH7|x7xm$EVOIrF@>HgH zm+RXwV^(DboQhFCA42>j!EZI5Y^l`W{1E40!yPdr{D=(-i@%gt@`ApT_k0n^mIz`g zL|KC98e&Idw2bn+<$kYZ1}{FW?t&-euZV3XwbZ{L-bJHbqM0biiIk4$5Oh`&lyb4?%PEUA^OVpcWNQp1deQT{n*7g*d6 zCp<)@;49=U<`kn$06ddPX*Z3-(0L`>M?lxf`Fw+zBeCTOnrdw&Q?k_Y9`qYFqG#3c zSoAkso$7Ni*w*~niFGD`{=l|N=OT}bBZw8qlLtn zD#fE?!)g9?>lceJQ@EOw(uYMvFH0yoCjQRAq%x}I-;uG_# zwfFh04{mj7!(1ebDel5vR7O?4&ny&3@MbGa?2^4&3R%wmd?nRswv4YSINe_LQsj+w zXsH)syo|3>WX5qmoTQqvcaG?Y`LDR>USJ9+#EjA$Gj}ly&9f5`G+?KUE_m)2yni;`Tnb;lefS&1JZB6mi1^bM#zd_bReM(!`eBr3)$A@> zuXN}f?4Nez6Hv=*f?#Okg_yq#JEAzb0}r9pmt6HpRg|_ijI=+bb?C$Y zCx+ya9}oG?Co+@f=|c=629w}Lq22@VIqh+|{UMFxVwi^(NtE+yM7~59-B%Jll~BBZ zn1LQNz%i{PaTmUVPWo_>wy_BFKrGGT*M{Bv!@$n2;_3Mj;Tb<4Q zsoYfO!~LmfLzMdXkB7Dn(O3y$$Ya#^^L6Y^@KwO+PJ?bm>{oMpNj_q!n-*l+=;+dD+@ zo0^Cqe+Dl>Ji2VGy#*Nu_3h0`UnG2CC{6Cp{QU*+J7gb7@_VAZK1OU-cr&xHlZr;I z0x&XPeUX<~nJp}E{mF2Wk6_mlc*6?pp28boS(n*8qM6=jdH#&Q^l7Gt{w0ca zL?cxCNkr7mzTXaAH+vD4$8Wfz;U4UY%n>S+*;&~)kVuba*8OH$Vv&1}s*^kG)R;`^qZuc=eC^xq}Ncs6^V+GPVdGkZsq7-;K<5uLMneVfDk&i>H z>>Wh8_N~SWN^@60{!D;IpHDgX_-@pJLm{srUg~7FW9LOX{LHJ!j~k`BO+2a}o#ST( zL|U$Qh;{IfY9EY8^G9ivr&vhrpXL&&eGT>VmxPGrxXKq#s{|GH^<nJea-a3jAL}KJUQm` z{5f>9x(X1~1A-n|o7nqz#l8eJr)a;$UrzgVLN;9wJBiv#Z(R-j;p3f)OFJjPo8^QD zLr}kvvi!DR>~?XGX5gQY6$KLfdBg<&Uo__QPpBlyi=!P~l%iAd+(v10ToxgRMQI&P z(wDs}8!O8X!+&Pu?l*N`T0h9PU;ejjL!^4V4J^?A2if+I1DE=sGR6q|ZbyPQEC_Ag zOA6;*7Bg~7)H}0AonCdfI8pjQHF}wa)4J&1H;vBjD;8k=*&dfxW-~|ArTJa$uB?;k zQejtC)fc&6%O)X3P-LIPl_j4Fg^AUe$Ec}~YAzXlk4sOx+6Nf!Ex*h($`a71<&7 zv!P1A;RYTUoi7!FYpjh^jFyOv-4vgVPKwX|{YiY%b_sd6&g^e*>E)TD;P+~K3(f3k zZ$?Jo*@-3f^^leOTz4As8v>yzbMfe1bN9?x*WM@%X`YMTl?^@bF)aal+^8 zipoB(zir*JyxZ!t!C#4O7mr}yY~(igN6^s{u{@C8vEZkF1-xLh^DQhQY%Za z`I(K3!ZGovJ3hM%8C(47JiGAcvlD-C52%5I52g|O_+UQGw!6u%FEb<8q$ijD<*T#H znvr4CSRn})WSlr~MCEVa`NO{$0Wo+&PB&Jh2=tjvJQI=vTb&U&@RjB2r5Mab_zu1R zA7MR=0P0Z=0>;aKXJelwf78Bbjr+PTFYg^MSFz6~(-@2LL-q9>O(cz5oy?&_1S^!w zl)ZnJ$>e9`Am5f$SdiiHdr4pVQ|}J1Y5G!cpJ-M2Rc*e{rG|dDyP?0%Z5HahM7cSk ziK$Lx3+!jmBh4&o?0hNmGM_EHY&Dh<%T(cuTrTpmmMH#F4tpw}C6Qgsiq$VgjKsKL zt84jfT*Sy-1ji?e4qi7abeVYcwYXhq?53WD(dYw-2sF*L)l(kbJr?yLDirG-i~eoo zM%c=TN5p#EJ!uhnG`f3~18?I`nc?r54(sAVFJu0}C;ISit$8g@+;+xh9I-fgCN}A~1++9AJ;b>JhA`_n#++Nz#C%JnQZ8Lih zpJhx-M1y~aM^Z@dN{WH5FOy;>mx=5yG2F`%I;kp)j5hB|0PGG}%i1xQ3ZnSa3L@Zmo zaIe)@FTL)%;_ypS9W

|4XnOt-iXvREO#KtAVz_>YVyIYgwh&d>Le%yUt7dEQ#0q z_=T6KisFa9of$9izYcAyXgbdHzzR^iXGoSrjzd{|w(v*bKPcx5??Unt_cJa?tqL!^ zg6!eMnUY7|jgXZ2{V7~ZKcl-4HqGOYK_*qCqu9eM%l$};@ z?%5G+&kN+-BRaQ_u7VU=hb>{Y6cM-&zIjQ~jc=UIpQ5p7>m?~v<>BUOu zKOQMt9FkL^5pi9Wq8@KJK@Z8FA!Ych5&dDX3ST0ZlNITSnpBAuTKryrGkg=dQi@1x z7!Wz}pv>W?T3D-VC=7dXKkbLWl4K=hfOf4A>8RkY5`DYOUx~gqyJ{7}P6{%TN6ZmX z3+2S-25g*|+4b&ONQ{c*35Y^x$$|x4sbWc2HTpqxx&#^2VpLv%ix5Og?%j^KNvep* z3CC8t-Na1cq56MgCo--qWfCjtJpZ0@W|%F925*BaySjXUfoJ3|YO$D*7O)X6*$k$d zIcI&9Qv@sgq!Wb|U8mKBUR>L8?ZVaZVL7Z*n@zk5^#5wbSaizha+P0PSAOpB%5vq< zjtclogI_^DG~w(Ty=cxXz1#g3cnS&f(5T)?byVbW3iJJy(^&h$V1x2Ql6*J4@C9TU za6?RllTzI~gB&3I-z6>oRRXM<>H17Bd@e|&55fPL@!lrx<2t$5VYlVcY)TFC2$3B+ zz;!fMC^%IyL1^jF3GA!1>{`S^3-L@8LFJ!kuAuw=DOk3mpMgE0!TW5rP+)e_r$y>D zVk)xpXb$X<+$^zcTV1+C?@8gCiKSV35ApG4F?@4lg~r2N&50ThyV{*tId*<(Bh|Sb zRYdO!aQj}&2(|#${u_0UknV;^JiQ|>vG=akWBUhE;GrW#^bJLC`>N%BPZmJseKDZ0- z)~9Lkjv#jxy{!%7jg*#3Yc(X_6k?3zSl+chls2ZZ!NNiFcjGt9x>@>&PH$Iq`qS#k z7xfXnK1b1~<%Ru1Y9BM}G1W*WM2snevYAGAu_81>ueLmT_)o6i59qijgSi5+9*hvq zhdj(lWz))15F7fqTo=B!>`H;mb1Ifii^^^hQjoc2lX8(Mh}`A5{|PbIeGO>Vq#590 zV}X~oY5x;m+(qH$Lzqv4F-QW=05DZY>^8;3y+hBb)wKnjt`i#e=#Bk4Lux3+N?HM% zp$h8&A4A)&0fXVk!0QRkr;r2p9L34Jl&8>26(&s^3b?%Iws`}&?0`XcF0d@`P+)c6 ze?#jH^aKiFj~s|BPiqU6$JxO-sKqQoR>1f; zDUtZqpURZ7w7cmFasvjI(o2SF%->P*;divhTz~XSM2izqQxc>xAia<=NbboGG8QYm zyZeUEU0{bNxebn)!fZivvbZGRBFNW2cf7IN;J>mYD*v$Lf=t_W_{mJl^=@A`1F<$l z1Jq*Z!;VMWbh@KV!t&IsSM-~;_MldnBYiDvabl11E>2*}T5%=-PoU+P%po5ty@@Fy zg+pOeZes|OGD{ta zK0SIKYivim(dfNztDVh=Wwf}`1oI)M^e7?9H6vO<^8c>MYetSjOi*W##;*1z=Gy06 zniEtiY>v^$UP|Bppd=psc?|KHMcpp>@EzhSjW~CauoFh2KZIV(3lF26?vRLB)K3sk zM9#ExEPBU*#+h%Bdg<6L#bu`Jh%!GmIbuq?7^ZLH(Yn~b5o1wnX|91!n1Y-?XjRI- zp-q_41!S*Z*L#yddGP*VN<_yzqq4;xnOa;eg5hMPz|4pl3`QNzp+e8Sf)=gNL8QV-@zJU##2 z(6@8Q$z71ANvsVlm*o7tvW2nK~(&A!z4%O*9d|qV@*- zCPQSdMp)F_-1H`TpMQePo7X!Ubpyv~y}O%g`(^!~@ZN|_4}FBeZ2D26K`zN?v?758 zoCc5-QytDV)Tw5oQ*?}0(P`NMCY=muP4JK-j!0`4k7e~2@TI|PgfkQdzE4&0H{S5_mU9qp4$PPc+axY{K2h-Z$wM zz{0*o(wA`Wz2%xLM8hW7sMKHW!n{YL=SL-s!l?hoF8&qtxrh7<*XH}zNJjsz3Lg3! zV8oeCvI)`X#S)XnJt;UB@$`fAR)8l zO6at$i`TC6pOd&J@+6O6qNiSfAzF7HsGY(|ZL@dd>jZvJuE2f#lFKfx6*C1*M6bCR zCIw$7@NriDSO-pVlwJD-bmMe*9}J9%?GshIgV?_fjp6+E*Rb5T#J>W}8a|P!#h;Oj zvBxI}k90uiAGQcM2@2oR9#Flp_Y_Y4A$xrXY1OnL_X)Jao}i^yBZjU+#Nu+zG#>4Z zf3KNN+&86DGp@7Zd}bnwJ`#QSvM4%@IGCJNi%n!GP4j`@O&6+qkwL561e#*!PC5l2 z(`)hQEf|;YW!iQ~_AcbB@juy1Y>UOhz9qm$S={6YUim?poqJT(Z=Z3>WFCt;;u@iJ zuOU)^a}A@>CvdKTo^u(C>f(L!ahyBc-LN!I8Vg->G`qs)8apoK0>HJGf1IXGRMTYUF(ddbo9k!$f z)JNmmqtX4KwG`~U(db&_SiKm$Q))%cR@D3eltm-xyzn7H5ej&g zcSyvaU!3$KnmW@whz&}6afXHzrJTxrh(77aNsO{Wx~&Tq-vVk0X7=SFxiIq#qn_`v=nvl_!5rka zPK0-OKaCiS$3>B4f|dLz$oc4rIW7`MboBio=cPF>9|rYn627g9q@_svr5Pxv#`lgq ze0c^6)e+KaW=NDbEsWDVCz{*)lXwB%oN3`LGz*nYWSu=4XDrASgies6a(IIb)x~Gv z?Wg_IHKq17ARNy&7>yQ=eG}k?UH;7v89j97@u)?m-(;XXc>(s&C#q#5rOies_1}(0 zOWvWSiQK4eiWk;%$!E&$r#@4v2T)JaM8VQUzg3{tjzy(+GP=g1`myXjHj+s_oT)3y z$+?4G&Z_eU2&8qw2>SEy`0KVV2XfINT8 zAX&0HmpJ@*#`(!v3splSap1|C;?qHc%`974-lJ^x?#44IS`j+nVvdt}w}f}Pyh@Ar zx!g=4e3t3yfp$SN(Akhr`m3rZJ6$w-cEj-$Ut8c%t_R-36tB&z_nlV1?s38^C3+92 ztl25B{YH2aUx)TE7JXopkMODUh2_*6#g0V(F=Y0UIj)dH_>ph|d5o}>j5mqt{u8R+ zyj6TIP?Y^d;02e}JJVBmd}CfP@XE3mv#mI*SM7ZpeP=qSyZ5MU<5;wMbfZk>T|fBC zH;Ci3TZ${6mPEef^ zVr%dLd5}AjB4vzwpR&baoN*Y5K08EcIr(|lyVYK;apa-Id09#F{W4emfSYTq zNZui(|5uIg%Pg0bU20d?RSo!(P*Iyc3Dw=-Lxo(M%b}s3Q2p~X_WWysz&FtB-Rhbm zR2&ZsZ9ebuAB>n8%@8+chOg%Z=aT-36Q6a~_D`vNXILGXKJ-XTKb-%$aoC_(j7#v9 zmF=#S;|H>hk@$SClD%oW3um`B?TN6)tIINS1H-DKABOHTY>2!ZDC=j-UUV6bUz0`ZeXRx^%~XnFA;mu!mlCk#lFgPYQE#QBRpTY(hPv((yHA zyUKOr**$S*j7H#ST$b%P({0#ho+PA?c@&pqcP!}^gT_atL^ zKU2P^oFC!_4S*MT0M>rv-Ba#_WzLpfTlTvFqXtk%CJv{sVutmxrvl~u#`4zy^)GW6 z%oV|+HG-#IelO;{^yAiXl#c3VVjnwpW#u-Dp!P@@(8}<#4#ikE!0}MH3 z<{{mA!?1qnG3Qjb!|3kk%U&vHhnRC@h&gXKoNqB*GTIL?-?DRcH4oV$kEM`@dy@;?hpg`C5aLqGEG9hk_j z8PM?jkS=P)NYmYar7MeYFvaS8=($FDn=tUDNr->xF8}h-{sDI4TFjp*?UV6!5jkDh zCT|kDWWBHfSCveu>e{kQnbJ#vOyv~kW5LOu2&B4OU8(fk_jfX>Es*18#&acM<@%so zpytZ~e7AvXcQqh8F;TzSYuN-&&-i)+j|Nhltd~kF6Li3m&Iz8CTiXIVTon{LeC^s>!obu3!t%=xl)wagusT+Ept2m+g6~lg2AR}h$iTP{XsaC@%m2oy_ z32Z*L3VMKz%3C!dN0i#Q`I!zzxo(JB2rpD34@+egSYy`bYMx(uGkj^hB?A#pd^>4m zkBb#9grBKt4xHIcY^>4R%(ed3L;cJwBB4)Jeu|!1MOL-u)+$bU9OwVs7hfR5(d|Pu z=ib2mPXT(3;bB{-td2Xpy2%=(b%p)lt(ZNj%ZavO)E<{@Op-dn$|Ezs(k9CDpY!!vD9e|wG%NA2JB zVWWL8Od%Iul9)9&^pt1ex_=tN`j64) zBnJmp-djH(%$JUs0q&dT}ZvjCYOa(*QvU0b+?))lqV z^tn$4=L>(1T*p+oD}%cEADIe8lw6UHNQYMZy)1!d6W`JxjGoeQI0rc>cxQ&+Z8ldEm6Y2XGb%fIKjd-rXH5+Yi#=W3=hR1~SUOt={r}qpz<%+2x^HPhas7Yt$ z23#4_L8I3XJFtr!jf}E?v;&%q9w$U{t5<`!8WMQdGf9jE2mkfQ+qY)C`VO)r+3ovO zoQ_a_tWIj9KH)zE-V971Y8fsP7&ENjy{T0t0`;6C4-B-nZFG~6^7uPnk|5oScEESdG%k!L`m+Tn3td_RjDSi-HCi3FVSw!ZRt;m506 zDTIUG4Xxg<Zx*pUc0;n}$z#6q6QKoaI=)79i`{f=y0_8C%=ZSI@N+bJ*^&Wy zftpEn;fn+o+?kRNc^5J|^S#;Lew3sWeOI=Z?YqU!9h>H5=CdX5p!~w|e6K2hk{x$6 z)^51L7koDo*S`d5L(k#94Db1Zw5wFMHWyDD5lQLx9*8hFA^yIXos$~k=J4T|@6mvz zpXW!s%%X1s`bfO=Gx;rFo;G^qZJd0aJcpe7qxbYz8pUW`aCPOLwR`sb&yV)}d{5(^ zEjxBRihm?a;qYx7#zMxq8Lwt345tfcVI}#nsH+L`Hh6Zp67S#S4L%+*P!2fFcW>~m z(7J*fe4Jv&ua^V&dEb#Ez6Fmy3duviw$wNFro8XMhZ~Xejqj#?cu!7YX)wpqff`Q) zyn+A7*Z65^qU^q$*(fXM@I`!9(95^-eVgyyiuJF00{y%{@&x9yQuCW5?u$a^v#yq~ z1M=YX$RqOm=ecwqUl?SSJ4d{7`MK2kXI)o@-}2c8z4du~p7Aa3{o`+E<85MX>k;b^ z7H(mc2Zw+1e*=L<_+13&xgeKJT{G25&L7Cb<5WBBt(D1~je|?y_`*%)2ahk}4Z%-a zmdKn6fB7@)AIc{v-HzaH9H-v~?4w=3Vu&t5$JCQI38zfhA_qjdHzW1_pQbKa zd8PelA?>jR_Ip6z8$wi5t?s+aJ`}#nBlhuTqP#-6`Gy^?{GjXHp8DnP)9Ro%KiG3V zt@u?$aWts$^mvWwLFdIU+{@j`vWesa_%QXh%)Zm=t6nFhDYXl>j?g}TIrOSZ5^bCC z#3pK*5i6bQnbtVfGriF!z2NFPM9dC}8>rwMqDA^B&mz)YbXr}to%S`UaxA)Lgv!W| zVNVd{!!h_C_4_0Gv_h3ly%5IOyN=36j{`i{=o!Y9DdyNdl~Zw_V*5x=@kDUZ?}$>I zJ&3Cb*W0)r0^ah0!>xFxe1PJY;*{dIw6pr$Br+8J%b0Dct)`&5bnkA+tAZo-zT-N} z(M&1DVL86rhX_nVy>DhH6Y+`ib2rj+BKPQnIYLeimE<-lXURwI75O;nz*xnh=%Zsc zDa`?ifceuLs7#!cW=c{Vnc{K!-KW*|sql3VAC?hAz&PdSesEJJqDR?5&hig@v+eaI zhcO2V8->+}yND~*^NcTj(0dOTrW7+2{oy-pzD&i8R>Y$5@WF(vW^x)Ku^F6eNVMOE zcKAvVe3d$nu9|8vx`YNp^%-Kr*@=(ge!_o91JN`}_svUII;mjZWr^f0s+i>)s@cpD#<>Jk7 zbRcHKcQ>49%i;@eS!Kk@<2r)Ak)iE8hWlfd%^XHQ~I@h{^9oIq5SuBK94}`K+dbN=CV!Vdu}d!9#9q`q8uuGLizz0^CbAX#EDfSQAg|wwEZf^*c_tkTt28F`Ax>ZBM+X=la5Qrr1OWpl%Nqk?tY1$ zG0Uq-G1G6EF=)pqh>1ukX8J7``l}kh82r*Fe!1~0PsbjmT+!5Rlh&YwPn2AtnkAFM zlSo+aF;pWfC30-p3K6t9NKlS*ed2O)hxi5Xpf6yYMqIsb-bH5PPQO${Y?O~>)_kOr zV#Fgo8P(bNLNSFQ6EF;^RQEwvd~usD6o&MECGq(Ye!GNe=#-D_}m`TTj0`#OBsk@cuq~`+S{GHO?Ix{6nz);0KQYRq@y)`1 zzsXVx{&-M*PQ3=F-LDQUu-5x19&wu1L<&noR4c6amiSicm?KtCuJ?N{B!m2w)9RBw zCt+Q53~+kL5)KxIA-~!K%fbb3o_uJ2-fjhAg>tA7`w0rV=XS}J?kbP{^wI(y!b#06NyybalBZ6A* zqxq}#-nW3eEY7JEZF)?dU7TU#Thn%P$Yhwyv8@w#Gp()Fsk>V%DF!D*=5Q2;EY7oe zh3vi0sdLQrK2k)K$Gw!kTdLEZF-hs-CYLstoE60=omaj0IdyLFbFsPRH4fde5{%4q zRNpD_ZUk(%`2)`0zU7^$Gj&k{$Ddar-voko#Od<3a1rIBTiG z-H%=v3(XFx$Bb$!ZQikHYdk%f9bmf_>&s((*UPWLHd&MU9B}O^pLBd;+hgjyj!e}+ zGHgui^&aEvH1}d2GhsLAEM|x>24dF-%N0fg-ZL-5{|>+J?6XC%h2OU#U}@(<)I6XTv%pWRN!e#z@VsTYFZAPj*2>p7Mfkh z)WotP$x=zPteZ;~pq6MlX5Cw6-3D9&m!x%a=+=Pfynry@|I9$y{l4yZ=l45vmiL_V zp7%NXbDrmabVcT&HZpvI?u0G{?YRli{XqnC_m?+xlcx zcjR2FYrMr~Qhv@1yO7(S=Z%%|PN4oW%F66sr>{#H@dBvd-W!Jbb`2yFYgz@8XR-Gb zdU^Vz?V=KWz1R6=FG^iNU7z)C%5FdpISfw48=6Nio~oE5di3(Ciru2Qm)Gmp=WuOu zCLi)v&uW0c?cLcxqE1yjk&D$?naGOXg0*_MrKsZL+iLEyDJdp*NSUGEDC z!K)!nH#2+6f5c3|yz7!`>vv-0r>?O2U0CT-t-+!SeI7wRvAJGfU0-Rnx)SFhGUz<{{a_8h-&n9pUB^5yqxsbD_m2;=)T6^S_XdTW# z!kE#JtKcOG&IVP*UP!XU>Y}rg^@uSk7&;Kool;daiGRAB0Mt+#7LiRu*;6Q2qM{xv zV^Py(5~Z3FmIs}=3gUQyXGSB}G)cm6{C6PFt775h!AKqIPWfY(3VfS0mnD${vM=C_ z;E;71umcGAXiS7CYYV=%*Lkx?kzI|p*uU@Dh53rMNy`6lz@}? zqC~*YI;cCOI{+Q+9%luAmDG+lo zf*18lvKrO4?2D}&*A0U@QVuqss0N0UFoh%8)Mlo;* z0-Izx4CIE0T)VccHe4lG>!F>tHXroULSm!;d-`)gTShr3?YyFQTUdPxFL!?}!qH|K z)kZZ@igK#!lF=VSAgjeBN~3x3uv|EO_r!7Q&D?j&O79Bb1~WJIYanX}Ibo5*OCMSh zX(w!`IfV~Y%C#yU2z5%&ih|-8dA@rZ!;)%q7cq$$kjLPna|`bD>A8E*Y3REty*z!z z<+Nc}HLoF2AFWH_gA6XG3KIAfn+5o#emOMnEgo(1JV&@h(k~cQa z?a_~OD$d7eGr*SAgNJB=TqS zi!0TT>;*^rWp#?Sy3V`s2--mZSU~q*6f^3znt{+yD2?O}^nfcs5&|vIJ)=7oi71}5 z9|xCJ;1CE527%yFvSAnvpM?=5P)c3cJqo)UE;3keCfJLcBym=(eS132;Ct@8Y0&4#&zs)aTh@xLtltt-E_Wq9qOjb6Qg+D z>RKkAwKBWeSg$4y$S#8qaTzS|fxiLoUdzvpm$fl6-!{2#5p5CVOpyma@1Fst-qdu7yO|h zTd0>B*^ZV%+}>ia+|GNU*CvC$uz++4N#s*CHFwso_i7@M#nA!ymqv%+Uvdguuu05z zruyieuI22teRtNH>^7#FNnxrwY&87SIOaxi>JNQkcimwMgh^AuWpu~$gke;)jz@xM~nWZH+rb4%gcVq4qd;h(BC=5kl?6KDxaNWCsu3 z4t9fXIP(;`k!mRII-OfiQ8_fsQGJNIGH~c*;E7ghWLe^7=rv^q-Z78KY$IYwt}P5T z(2xsmoS%Kc2!*0IF#j=MC^5duVR*=NeeKF^pPIZW82z91(Wkev!Z2f5?M2AH$lH=d zIQtgAhJDk^8fl|v|3n%CbprjHb>U`#KHR`V!qsUDMqa~UQjs?goWU?-TCMdKyAZi# zzK7I*hvc0%_;I)IQHgEvVJxpg0x*ugrSJLOMh0^nuQuqIoO{k6f!9Ryq!Ql)b%XC) ztsqyN%(7M~a%;JF3jhe@r zZ}M+`wBLx^u#{DrN38R*=VeKuot9^XWVMPjfd67(RI0KlbD3D(w%h!Wetbn(#mwx+ zP@>viM|W{l0%fCldN%loN+vlS|DZUK@rn18$u4K0D;{X|kfM%XW4(lL+y$ytYRWEB zl<<4bCHD}vb^juK-(m0C7NFo>!rA*&$V7bWj_s)-;l&L@a&7Bg=TN_8)jCu#pVg`Y$mG6995aZUOJ2a#bRKe7Onw4P<<~w|Kwkw z?Q`&zw$gB@ySy4bu#{cfY{dJWUm9plf?*8??}L0vKSmB#FRJ-FCjNPEF3%kr5{>X1pT`r@_=_#^>2*2pkPIn+dXWwEXL*?9!!_i*?UH zM*K8CbgD4U4{I*Y!AS$cR7o?V)sA1xj55(gcdRpUzX%JwRtEbeqUy3~qPsEknDjeM zCVi-hQ?=Qa>X+s;nH11hQJ4g(Teij7#vJc*Yw&i2T&rWoS7|;qd7Hjz5UQ@(iu47! zA?2HR%p6JQ>-L3*;Dlk9>x4$_IAXSyGb3kP*^$I?&`fo_4w(0?VQ^32ETDd!=>{fX zk1PSa2a7E78Jr$|zecjB0xka_dTyR$-5j=6!$R-!^*no_WKyP}jS~pl1VRqF#RR3_ zWh+|4jSXX>pyj)7jn*!ggGbRb4H5+}c4t$0pnHsCN)Nd+)r1(bmNij4E8D*&jO98^ zMZsDIm?kvt6YjXlh@09Mmz8~HqourlF9D@of>Ory1Sin)#p=Fa6K#R0CCca-5!TJ7 zWtHsllyqC=Ecwab%Xzr5XpOXulm5AcwfAV*)WGv#98TojbMLQ9SnZA*>yG;^;@bM+ zva$;{fpVQqs9bAfmE|^0sjz*9_I*wUen0H+o!J6%miS8hId3H5c z^i$PLnz`;S^pf1|CU5DKEj5Cw6zBoHsFS~Tn$OtBy(o?whPI=C96}tKLROG{0QNgZ z&XJzy8HBHygGfmgW*N2;Cq<l2wvB@a$8|QPqs^w{>ET+ehx>O(Syx%For_M%Q@eL z+9fB;7log%8>+8u)%b;#qweL>>m+Uma*WMl$6dBX{hpg3G8q!Y6;_I}?9;i)7NXyX zgP{Q^K}u~XFe5@Mlvx2%2DfPiBFwv1sM=){sa~=9AzlWdCvb7KavHCEAaeMH7pu0~ zBzTrdz%c;@egaybKc3v=zqNgidbYyw7+-0m;rAP$#|GYU^c=W zfN6y3g^AryBoFK7W(Sv7=%nQjPrk`l)U7oE>N5M%s;cu*3$G$QG z8GE*h!MR;REfbm7v2z9G0lw3V)50i;eh@e(C%eS%l>^YW;P-+K4)(gEsH_B)Cvo6C zO)xDm6zf16anCuvHwLoHRD+3NKS24CF5E2V88f!lR6fGUINx_Qu*5O;MiL;)$Q+_g#_Meh(J(=eS z@bZU*488MJu7j2wc;5=0?6uTQPi*q3-p`ZD)+aGrvA~K{`c?;<{9thBWCV$P&W2lb zIq*sdYxS)THMQA%cPmVOFu!7DyIV8yuHikg2DB~O|2Cn0(XoN{!9?`=kKqO@cOuqy+8s4it5$Lbq;RV1$6dYWIAh_NtJSS8Vm4+*NmtD#-EQI8(JwKs_Nf{pi=la{gO z&dY9jyO&ML(W#bXU?SlLS_ef+^5AdK{&M#A$0{JtKw?MUboRECc$*kU5=Jjb-$1%~ zcds0-)woaK30`Sdw8-MvI%xxmRkz<;dG>a?>K%(LMV`Xi&Ez|qUmVUyV!5j>74m?F z7e#(2mM13$O(bUvGANd?caAJeVUXpxjrl$sZYCu#P6;kMJ=y1hHBHyxCqQ2#%!e>% zVWz?q!z_TQgmKH1gUfJ)RMha!ecdE}Fh*ZGuI}&2>-aW_j=(|Cwj)F7D6Ddi!jQ0X z?CL>HW5i{6k$c2-IcuO}C#jM5A!ZnmcNPp$B_0n!Y7*szGWQuXY?GI}yud`x^xxpQ z8!{1lV_LokU{Yb!Fg)sj-nFSh{c%_6h+WXF#CtE|mOCobA*qKni~F73whGim>hVE( zAME5w+^B1U=w!FNASYFXj3883AXQDjly}CTqMQ}#8s6=ZznKPJ&@+U(dFRoSWavAZ zz^mmrZG5elAm%fO8PHAruk_NC)GzI3Mzz}L9n0U_Qcu%&Gpf%e40MDkvdPAI-nhG& zQPNtVywI&RI9X7g&JpQoOa7$4B@63wJUk3P-HBYY<9RdvPPL=LOz(qZq{s9(`Ie`` z%5U=I=yQB<;t@VXO>aIq!VgkIGI<|~+{v@@ojh0hoFp$s(8dU|?M%!Oet_~hK4>bp zBc|#6Z7%{Ox866<2G>B&KELO6xJIB|j&jB55R-@wxSVf55{#=R*gZXOPUC~X4T!_P zSqR;7mQhuygLp~2LMsH4dKPXA9l-ju1wmtaLvKs%9=ywy5NwUniILYj%%r8j z&92Bv=7SQcoJmlk$KjlC(tmkk$AJF2}b`EV+3f?Xwc#aaZY(#p^Y;z zqXOgG8>;%^>T~qJ$E9@o`Tr5uY*XhjQGvis$M~1@JICZ-rE3L9iG6}mS)F@bcls^vkRt*%GRnaz}E+>&{>3Gb89CxWyyJZUM*)L$p@ z*$sl2NzGIfFvIE{8LSU6OR@j&b@ucOaE}9tHc34+9Fui;#i%ch9ILMd7pJ)A`cLasw|_e8?g=H>Ipd7C z8+Rmk(>ePfv`gOWL9LTL`~bNtW}H=gfV@cWvnIQY&>79_asA{C8RnjMMTgMp(c&dQ zdHcDc<0mH4r%fr7wKmAYdBHE}_Z-{B-PYQDIYo>Nd-awkwtAQ5Mk5n5#LC7ft^W0_ zinb`cM;V~!T3H1o+ZLYYgO#BhJpEvON8P(`wRv-oXdX!ihg&(w;l%8~ar1Yg@3@|P*f8vgrDVew%f^FrbGc(oJ6k~84MX*XSq+yS$$GzNx ztQf0CJ8W{GOMAk{yXV(qeX?XrAWN3sUXkpPUb)qX`;W`hOeuyWa816mB^!63HROkn z$o7j%F)%#a($pLPZYQaV)z8s|RmgX{oKN@Ad`a|$hE&sen?KeAL-qdd^+3hZ2tA1! zTtPX!kORzqS0is`T7}_|Wm{(pV_#X)Did?BK-aNc3t6_4njsgN(ZASE^SOzBy948* zErF_|SR1G46lV01$yGR&9kF#pt?w@I@eE9|d^9wybzL_1oTL|e*0t_^37wDVd_vn2 zZEuu*w7pUKQNGgCHM?|Afv4mLN$*$g`9pU{rc>f}VJg3IisuFg-SFKVt)pwI2;Bg~ zX+8@YBBWfnUA|keU6g}W#P(#YA|7yw=sUMSW&`zfwf`MJW}p}X1C3-vx;15yw*IYl z?<7IYgP5Hdhirl}dL)^Po1t!7#{-*y-e&i!l|Rj5w==&s4X6tQ_jRZpYSs%Olp_uN zQ=kCIJCz8-SqVZRI)WGsD~b4bpP#`~hP`XMz14F=l*Bp?G7O&7RVOIM#Fsb`3MQZ1 z{)|#^@J}cOj~^Ew!L1WZuus_0?HmfE0nxQs;3YCUfphNGyyl&;|AAUyQd86d`7zG8 z$#_jHx}009T9gK)TWid>7*ladhT9Ucm9*jRifG{GyPUsZzPo+36MBXmX4Y#w zy?K(N^WrYswiA%Ej#n6=qygX%y@je%Ij=4Y;?2v+l>faX<7DcxQ7rXB5Oa z=-uz+4;(h!+nr>Z^ySMQoHmL{AwzDjbr%ed3&x#mxT`MIL=h|KJ#}|+e~}2hDK4EN zA>efK5D@SA{yU4dVeR+lUi9;Qw3cFr#rV6Nv(K17^#N#6Fh(ax{oz7@v$4 zH^mId>)5aYn6GEX9+&E#h}f zPG@qLvOe0VuH}FP&9y|k@tx)ZUvuXqH##}!=fUI{>Xr%eU`Cw1{b~OCv;lF9&KG=0 zRwuvaofVwUXoS|h4l@&y?w=6>xe-52vaC<}kcC44GV3u1$Gr%vFso7x5e0zGtU}z7KBCUO@Tx*}^zptA~k~tuA ze~0)zXvrq&_KPd#61ANng>CEEp4dT6N85l(+InuP#DYXlAMtZpi<*GP?Z=Gov{Eia6~Y;JcsB8;;c1;#%Kv%6 zChPlJ$g$virPf5m3brKSzo`LVjy(Z=J1zG>3;y#Yx)g;+L#{u(lbQkB~dw6*p2$kO$8Rq0|YH0}!XTK00b#?Y!JK+g43T&!s`uNPfZOY-bVxNV1Fdt;|H*8Zu!u;xdJ zvxΝDjmKLeu@b5x2(Odk5GHb*(bggUoTrLQuogHNn~sEmn1~)>8``P%4qdjIuJx zOxkf<9=?-)W3YvOWdvx)`3bDFz9gU-C~Rl^!Xd$EniSwzpC@QglW2?DP5Xd0vho27 zxkKqlze4*vRgque0Hj8aMT}0viYPTzG$(b38L??Q!pWKiRDv&m9 zv7HGCZ^20{OT%bfDdP6~L0QmCt)Una^U(uX%O;EvkQH&fua#K#w*k@n4&grR+SV=^ zzrXcdzWf@4Hd&NGTPAIxSo!BEfkU8eWl4YQXAw?g(dSd`xwJL+KXRF2`X!Wyk>v|Mf`m~M8H^kz zkWr5;E_rNZ(eh9xIjmY$+HHd_rJ%Mfwk-|2> z;|=?403qN`5_1$GKCtNve5s8L{iyvd-YbWF4!lBYZ{i3?ZDb^XP34e<)c?7J0O5=f zxi1mJru#Pu^}pm#ykHx?2R^~ET5A2qOqlqtU;x&Qz!TuCA zs2xTm%ZCujtw;~u>W31^mIxw=DjOM9MzV`bOO!JT%9bFJ=ST5(D2?cRKxxr$%i*Un z9_V|X(dS>ZY|$#RXyuBLF!NV0D=9aX6s;;LUR1UcgnwY!@?`}}OCBUMmzFFgu?tF< z7p#gU(aTnsmc}5j+9^ar4b8W{&z4S|Jt{kIys~Un!OB&OmMxT$$(fVKD;KORDN#nP zj#riztXx=9whC@3i7Jzl;_*?dm1PqW%cIJb1s-dG$67ieYBebmUduea_2&#~#| zm4;6~{`||zSN87O_Uu!e54`vH*N+`NdHU0{=l^x>`sGF!-*)%=AO3Cspr-!J;dkD8 zdCcRpWkBMvAz1$ z+V?FdjvU(mW@oR{@#A-`zkKx1uRgnR_2MNEz_5Dd6YHiuG=KKw9BoF(;DK@>>n)2I zHZpp`_>@sovelEO&zv{+u@&X(7cVU?dT?Cg*vP2(I1wZ93mp`wc=zDGx9g67c`8KxQL2FyQT&cmQO)!}khP1_L}vy6$ScK=V}c}Dvi zeFh%kA3uTT8SR_&S;8Mr+%6hU^Q3maq5YS-X@1nu=kB~`!?BMBuDr_*bCcZJDV55(QRR>Pf8r+pPjNx1tobuCb2BGr&X_*uzZMonnEU5U|62)C=hLj+ zgz76UVj~M>f-+A}$u?>ED&=DZrHhK)wWXX|P_(3E)qjR({SiKWwukV=$WCi4SXPF- ziyu)+rKHG639WR$3w1k}Rxo~1#Y##e&pQ_sEGorYAuE73Eq0HQ*rlRXO{BvzVMUlj zh>H~vMk-*|qpkVTsfq{%ewe$~ySso$jFiO1NC@X8VJBkl8bYTv;w2OlDRI%RgZaya zxyzHk4Eg)?&|5u)r|?(MRUZg~a%*hi1;mpFS;b%4B^`_9R_~CVk?Bv~gocC67#q9j;P_2P=Hs zpG-gFzy3z{J$*S*{CIqU@2@APdF=}MWU2Utj>V?YZx!9zzoU3>a>X^@H&0w&Gx*l! zmWt-aSK@Nt_d30<_HW{Z6SfxzSt^U=9}H?&)-29iQGffJ^fQ~DymU3R^@kfvryal4 z@#*G^8z0(PsyKUKN%7jw!k%BwmpqdCNR}4A+zUZwVEU%?p zI`Q)x=f<I*H%&2fSVm-s$`8DWApb6DBN`yi!{HA~Ub(r;ce$mJ}A0hBqaaY@R|2 z+yA{oRQkkQOxQ;qlBZ^U;&DZ&y5FG2&W!OXNSw>-e;ax%Fag}H_ke`dAZW-(t`Teekk~t zZ|66U?mXWd6S>n`bL_y^pAMXGbj}8!VB+OojVJ#7{!z~@AyBlj) zjhl4Y{@&w_dxC~t6KouCy)3}H==)7+#Vt}W(;M`jf|FDxuEuNYGrViPS{_E^}Z-j=wFTOVF~RsKcJjfKl^ z)^~RPcIJ8QnGXhDe!j-=^ZqMW{#MoS`X}d_Qm41y%xhbCs$t=*5C48cbbO+IOkLL1 zIG?86FzI`DbGR{vwSx4Ie>2>id2```-=1HVwC2nr-L|6z7eY5K_;6q2@~AQ28K>`f zT=oSw$@^6O05)>uFyYKIX(#^??R~m#`@_fL1dET(d)xK(xumbG$DiHX96#vYZ|2sV zE3O{aUi8bW`xmd4RxM=@io9N(IL5bi^Fr}T#VjIxD#x-TaP5ij-i@n$A~o!NS@rXc z+kMVl{_%s~uCI%@dCmJ$aj z?DgkmABWu(KK$HoY{C~cuJ!Kq2&VYI9xRl&c8@4M&(#o4=y(4)#n3(OHBMi=KR?5w z-o4HrO@pz9A*qbo3u^=EzP@$IebKFdk^di2)MP5xUj}T$es}*7gSvi!9)2b1kN4+i z{pA@$gK2p(z%xL3xPAW_?|<>DiR1S&fIh#w|D2Z6M~VI4pF{h79{P->URs_kq7FrQ z!uDZK>yOfBTWS7b)WfRr=l(I_1oI55c6;#04k8dNW7TlFf81c~Q3$Jcd+;Y1LdaCY zs^N71gi1_EgjJKipD2Qma>ALBL}$?uis@xX*}ucz>Mx?JVMQ-S2n( V{<}Za + * + * 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. + * + ************************************************************************************/ +#ifndef __NUTTX_CONFIG_PX4_FMU_V6C_INCLUDE_BOARD_H +#define __NUTTX_CONFIG_PX4_FMU_V6C_INCLUDE_BOARD_H + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include "board_dma_map.h" + +#include + +#ifndef __ASSEMBLY__ +# include +#endif + +#include "stm32_rcc.h" +#include "stm32_sdmmc.h" + +/************************************************************************************ + * Pre-processor Definitions + ************************************************************************************/ + +/* Clocking *************************************************************************/ +/* The px4_fmu-v6C board provides the following clock sources: + * + * X1: 16 MHz crystal for HSE + * + * So we have these clock source available within the STM32 + * + * HSI: 16 MHz RC factory-trimmed + * HSE: 16 MHz crystal for HSE + */ + +#define STM32_BOARD_XTAL 16000000ul + +#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 = 16,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 = (16,000,000 / 1) * 60 = 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(60) +#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) * 60) +#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(4) +#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 / 4) * 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(4) +#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 / 4) * 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 PX4 FMUV6C 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() */ + +/* LED definitions ******************************************************************/ +/* The px4_fmu-v6c 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_LED2 1 +#define BOARD_LED3 2 +#define BOARD_NLEDS 3 + +#define BOARD_LED_RED BOARD_LED1 +#define BOARD_LED_GREEN BOARD_LED2 +#define BOARD_LED_BLUE BOARD_LED3 + +/* LED bits for use with board_userled_all() */ + +#define BOARD_LED1_BIT (1 << BOARD_LED1) +#define BOARD_LED2_BIT (1 << BOARD_LED2) +#define BOARD_LED3_BIT (1 << BOARD_LED3) + +/* 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_3 /* PB6 */ + +#define GPIO_USART2_RX GPIO_USART2_RX_1 /* PA3 */ +#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_UART5_RX GPIO_UART5_RX_3 /* PD2 */ +#define GPIO_UART5_TX GPIO_UART5_TX_3 /* PC12 */ +// GPIO_UART5_RTS no remap /* PC8 */ +// GPIO_UART5_CTS No remap /* PC9 */ + +#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 */ +#define GPIO_UART7_RTS GPIO_UART7_RTS_1 /* PE9 */ +#define GPIO_UART7_CTS GPIO_UART7_CTS_1 /* PE10 */ + +#define GPIO_UART8_RX GPIO_UART8_RX_1 /* PE0 */ +#define GPIO_UART8_TX GPIO_UART8_TX_1 /* PE1 */ + + +/* CAN + * + * CAN1 is routed to transceiver. + * CAN2 is routed to transceiver. + */ +#define GPIO_CAN1_RX GPIO_CAN1_RX_3 /* PD0 */ +#define GPIO_CAN1_TX GPIO_CAN1_TX_3 /* PD1 */ +#define GPIO_CAN2_RX GPIO_CAN2_RX_2 /* PB5 */ +#define GPIO_CAN2_TX GPIO_CAN2_TX_1 /* PB13 */ + +/* SPI + * SPI1 is sensors + * SPI2 is FRAM + * + */ + +#define ADJ_SLEW_RATE(p) (((p) & ~GPIO_SPEED_MASK) | (GPIO_SPEED_2MHz)) + +#define GPIO_SPI1_MISO GPIO_SPI1_MISO_1 /* PA6 */ +#define GPIO_SPI1_MOSI GPIO_SPI1_MOSI_1 /* PA7 */ +#define GPIO_SPI1_SCK ADJ_SLEW_RATE(GPIO_SPI1_SCK_1) /* PA5 */ + +#define GPIO_SPI2_MISO GPIO_SPI2_MISO_2 /* PC2 */ +#define GPIO_SPI2_MOSI GPIO_SPI2_MOSI_3 /* PC3 */ +#define GPIO_SPI2_SCK ADJ_SLEW_RATE(GPIO_SPI2_SCK_5) /* PD3 */ + +/* I2C + * + * The optional _GPIO configurations allow the I2C driver to manually + * reset the bus to clear stuck slaves. They match the pin configuration, + * but are normally-high GPIOs. + * + */ + +#define GPIO_I2C1_SCL GPIO_I2C1_SCL_2 /* PB8 */ +#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_PIN8) +#define GPIO_I2C1_SDA_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTB | GPIO_PIN7) + +#define GPIO_I2C2_SCL GPIO_I2C2_SCL_1 /* PB10 */ +#define GPIO_I2C2_SDA GPIO_I2C2_SDA_1 /* PB11*/ + +#define GPIO_I2C2_SCL_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTB | GPIO_PIN10) +#define GPIO_I2C2_SDA_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTB | GPIO_PIN11) + +#define GPIO_I2C4_SCL GPIO_I2C4_SCL_1 /* PD12 */ +#define GPIO_I2C4_SDA GPIO_I2C4_SDA_1 /* PD13 */ + +#define GPIO_I2C4_SCL_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN | GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTD | GPIO_PIN12) +#define GPIO_I2C4_SDA_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN | GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTD | GPIO_PIN13) + +/* SDMMC2 + * + * VDD 3.3 + * GND + * SDMMC2_CK PD6 + * SDMMC2_CMD PD7 + * SDMMC2_D0 PB14 + * SDMMC2_D1 PB15 + * SDMMC2_D2 PB3 + * SDMMC2_D3 PB4 + */ + +#define GPIO_SDMMC2_CK GPIO_SDMMC2_CK_1 /* PD6 */ +#define GPIO_SDMMC2_CMD GPIO_SDMMC2_CMD_1 /* PD7 */ +// GPIO_SDMMC2_D0 No Remap /* PB14 */ +// GPIO_SDMMC2_D1 No Remap /* PB15 */ +#define GPIO_SDMMC2_D2 GPIO_SDMMC2_D2_2 /* PB3 */ +// GPIO_SDMMC2_D3 No Remap /* PB4 */ + +/* USB + * + * OTG_FS_DM PA11 + * OTG_FS_DP PA12 + * VBUS PA9 + */ + + +/* Board provides GPIO or other Hardware for signaling to timing analyzer */ + +#if defined(CONFIG_BOARD_USE_PROBES) +# include "stm32_gpio.h" +# define PROBE_N(n) (1<<((n)-1)) +# define PROBE_1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN8) /* PA8 AUX1 */ +# define PROBE_2 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN11) /* PE11 AUX2 */ +# define PROBE_3 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN13) /* PE13 AUX3 */ +# define PROBE_4 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN14) /* PE14 AUX4 */ +# define PROBE_5 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN14) /* PD14 AUX5 */ +# define PROBE_6 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN15) /* PD15 AUX6 */ +# define PROBE_7 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0) /* PA0 AUX7 */ +# define PROBE_8 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN2) /* PA1 AUX8 */ + +# define PROBE_INIT(mask) \ + do { \ + if ((mask)& PROBE_N(1)) { stm32_configgpio(PROBE_1); } \ + if ((mask)& PROBE_N(2)) { stm32_configgpio(PROBE_2); } \ + if ((mask)& PROBE_N(3)) { stm32_configgpio(PROBE_3); } \ + if ((mask)& PROBE_N(4)) { stm32_configgpio(PROBE_4); } \ + if ((mask)& PROBE_N(5)) { stm32_configgpio(PROBE_5); } \ + if ((mask)& PROBE_N(6)) { stm32_configgpio(PROBE_6); } \ + if ((mask)& PROBE_N(7)) { stm32_configgpio(PROBE_7); } \ + if ((mask)& PROBE_N(8)) { stm32_configgpio(PROBE_8); } \ + } while(0) + +# define PROBE(n,s) do {stm32_gpiowrite(PROBE_##n,(s));}while(0) +# define PROBE_MARK(n) PROBE(n,false);PROBE(n,true) +#else +# define PROBE_INIT(mask) +# define PROBE(n,s) +# define PROBE_MARK(n) +#endif + +#endif /*__NUTTX_CONFIG_PX4_FMU_V6C_INCLUDE_BOARD_H */ diff --git a/boards/px4/fmu-v6c/nuttx-config/include/board_dma_map.h b/boards/px4/fmu-v6c/nuttx-config/include/board_dma_map.h new file mode 100644 index 0000000000..fb688dc517 --- /dev/null +++ b/boards/px4/fmu-v6c/nuttx-config/include/board_dma_map.h @@ -0,0 +1,79 @@ +/**************************************************************************** + * + * 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 + +// DMAMUX1 Using at most 8 Channels on DMA1 -------- Assigned +// V + +#define DMAMAP_SPI1_RX DMAMAP_DMA12_SPI1RX_0 /* 1 DMA1:37 BMI055, ICM-42688-P */ +#define DMAMAP_SPI1_TX DMAMAP_DMA12_SPI1TX_0 /* 2 DMA1:38 BMI055, ICM-42688-P */ + +#define DMAMAP_SPI2_RX DMAMAP_DMA12_SPI2RX_0 /* 3 DMA1:39 FRAM */ +#define DMAMAP_SPI2_TX DMAMAP_DMA12_SPI2TX_0 /* 4 DMA1:40 FRAM */ + +//#define DMAMAP_USART1_RX DMAMAP_DMA12_USART1RX_0 /* DMA1:41 GPS1 */ +//#define DMAMAP_USART1_TX DMAMAP_DMA12_USART1TX_0 /* DMA1:42 GPS1 */ + +//#define DMAMAP_USART2_RX DMAMAP_DMA12_USART2RX_0 /* DMA1:43 Telem3 */ +//#define DMAMAP_USART2_TX DMAMAP_DMA12_USART2TX_0 /* DMA1:44 Telem3 */ + +//#define DMAMAP_USART3_RX DMAMAP_DMA12_USART3RX_0 /* DMA1:45 DEBUG */ +//#define DMAMAP_USART3_TX DMAMAP_DMA12_USART3TX_0 /* DMA1:46 DEBUG */ + +//#define DMAMAP_UART4_RX DMAMAP_DMA12_UART4RX_0 /* DMA1:63 EXT2 */ +//#define DMAMAP_UART4_TX DMAMAP_DMA12_UART4TX_0 /* DMA1:64 EXT2 */ + +#define DMAMAP_USART6_RX DMAMAP_DMA12_USART6RX_0 /* 5 DMA1:71 PX4IO */ +#define DMAMAP_USART6_TX DMAMAP_DMA12_USART6TX_0 /* 6 DMA1:72 PX4IO */ + +// Assigned in timer_config.cpp + +// Timer 4 /* 7 DMA1:32 TIM4UP */ +// Timer 5 /* 8 DMA1:50 TIM5UP */ + +// DMAMUX2 Using at most 8 Channels on DMA2 -------- Assigned +// V + + +#define DMAMAP_USART3_RX DMAMAP_DMA12_USART3RX_1 /* 3 DMA2:45 DEBUG */ +#define DMAMAP_USART3_TX DMAMAP_DMA12_USART3TX_1 /* 4 DMA2:46 DEBUG */ + +#define DMAMAP_UART5_RX DMAMAP_DMA12_UART5RX_1 /* 5 DMA2:65 TELEM2 */ +#define DMAMAP_UART5_TX DMAMAP_DMA12_UART5TX_1 /* 6 DMA2:66 TELEM2 */ + +#define DMAMAP_UART7_RX DMAMAP_DMA12_UART7RX_1 /* 7 DMA1:79 TELEM1 */ +#define DMAMAP_UART7_TX DMAMAP_DMA12_UART7TX_1 /* 8 DMA1:80 TELEM1 */ + +//#define DMAMAP_UART8_RX DMAMAP_DMA12_UART8RX_1 /* DMA1:81 GPS2 */ +//#define DMAMAP_UART8_TX DMAMAP_DMA12_UART8TX_1 /* DMA1:82 GPS2 */ diff --git a/boards/px4/fmu-v6c/nuttx-config/nsh/defconfig b/boards/px4/fmu-v6c/nuttx-config/nsh/defconfig new file mode 100644 index 0000000000..4ba3218abf --- /dev/null +++ b/boards/px4/fmu-v6c/nuttx-config/nsh/defconfig @@ -0,0 +1,241 @@ +# +# 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_MMCSD_HAVE_CARDDETECT is not set +# CONFIG_MMCSD_HAVE_WRITEPROTECT is not set +# CONFIG_MMCSD_MMCSUPPORT is not set +# CONFIG_MMCSD_SPI is not set +# CONFIG_NSH_DISABLEBG is not set +# CONFIG_NSH_DISABLESCRIPT is not set +# CONFIG_NSH_DISABLE_DF is not set +# CONFIG_NSH_DISABLE_EXEC is not set +# CONFIG_NSH_DISABLE_EXIT is not set +# CONFIG_NSH_DISABLE_GET is not set +# CONFIG_NSH_DISABLE_ITEF is not set +# CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set +# CONFIG_NSH_DISABLE_SEMICOLON is not set +# CONFIG_NSH_DISABLE_TIME is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/px4/fmu-v6c/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="stm32h7" +CONFIG_ARCH_CHIP_STM32H743VI=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_BOARD_CRASHDUMP=y +CONFIG_BOARD_LOOPSPERMSEC=95751 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_BUILTIN=y +CONFIG_C99_BOOL8=y +CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y +CONFIG_CDCACM_PRODUCTID=0x0038 +CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v6C.x" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_CDCACM_VENDORID=0x3185 +CONFIG_CDCACM_VENDORSTR="Auterion" +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_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_IOB_NBUFFERS=24 +CONFIG_IOB_NCHAINS=24 +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_MMCSD_SDIOWAIT_WRCOMPLETE=y +CONFIG_MM_IOB=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_LINELEN=128 +CONFIG_NSH_MAXARGUMENTS=15 +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_SCHED_ATEXIT=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_HPWORKPRIORITY=249 +CONFIG_SCHED_HPWORKSTACKSIZE=1280 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKSTACKSIZE=1632 +CONFIG_SCHED_WAITPID=y +CONFIG_SDCLONE_DISABLE=y +CONFIG_SEM_NNESTPRIO=8 +CONFIG_SEM_PREALLOCHOLDERS=0 +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_I2C2=y +CONFIG_STM32H7_I2C4=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_HSECLOCK=y +CONFIG_STM32H7_RTC_MAGIC_REG=1 +CONFIG_STM32H7_SAVE_CRASHDUMP=y +CONFIG_STM32H7_SDMMC2=y +CONFIG_STM32H7_SERIALBRK_BSDCOMPAT=y +CONFIG_STM32H7_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32H7_SPI1=y +CONFIG_STM32H7_SPI1_DMA=y +CONFIG_STM32H7_SPI1_DMA_BUFFER=1024 +CONFIG_STM32H7_SPI2=y +CONFIG_STM32H7_SPI2_DMA=y +CONFIG_STM32H7_SPI2_DMA_BUFFER=4096 +CONFIG_STM32H7_SPI_DMA=y +CONFIG_STM32H7_TIM12=y +CONFIG_STM32H7_TIM1=y +CONFIG_STM32H7_TIM4=y +CONFIG_STM32H7_TIM5=y +CONFIG_STM32H7_UART5=y +CONFIG_STM32H7_UART7=y +CONFIG_STM32H7_UART8=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_UART5_IFLOWCONTROL=y +CONFIG_UART5_OFLOWCONTROL=y +CONFIG_UART5_RXDMA=y +CONFIG_UART5_TXBUFSIZE=3000 +CONFIG_UART5_TXDMA=y +CONFIG_UART7_BAUD=57600 +CONFIG_UART7_IFLOWCONTROL=y +CONFIG_UART7_OFLOWCONTROL=y +CONFIG_UART7_RXBUFSIZE=600 +CONFIG_UART7_RXDMA=y +CONFIG_UART7_TXBUFSIZE=3000 +CONFIG_UART7_TXDMA=y +CONFIG_UART8_BAUD=57600 +CONFIG_UART8_RXBUFSIZE=600 +CONFIG_UART8_TXBUFSIZE=1500 +CONFIG_USART1_BAUD=57600 +CONFIG_USART1_RXBUFSIZE=600 +CONFIG_USART1_TXBUFSIZE=1500 +CONFIG_USART2_BAUD=57600 +CONFIG_USART2_RXBUFSIZE=600 +CONFIG_USART2_TXBUFSIZE=3000 +CONFIG_USART3_BAUD=57600 +CONFIG_USART3_RXBUFSIZE=180 +CONFIG_USART3_RXDMA=y +CONFIG_USART3_SERIAL_CONSOLE=y +CONFIG_USART3_TXBUFSIZE=1500 +CONFIG_USART3_TXDMA=y +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_USERMAIN_STACKSIZE=2944 +CONFIG_USER_ENTRYPOINT="nsh_main" +CONFIG_WATCHDOG=y +CONFIG_WQUEUE_NOTIFIER=y diff --git a/boards/px4/fmu-v6c/nuttx-config/scripts/bootloader_script.ld b/boards/px4/fmu-v6c/nuttx-config/scripts/bootloader_script.ld new file mode 100644 index 0000000000..c10e790824 --- /dev/null +++ b/boards/px4/fmu-v6c/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 PX4 FMUV6C uses an STM32H753II 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 STM32H743II, 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 PX4 FMUV6C has a test point on board, the BOOT0 pin is at ground so by + * default, the STM32 will boot to address 0x0800:0000 in FLASH unless the test + * point is pulled to 3.3V.then the boot will be from 0x1FF0:0000 + * + * The STM32H743II 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/px4/fmu-v6c/nuttx-config/scripts/script.ld b/boards/px4/fmu-v6c/nuttx-config/scripts/script.ld new file mode 100644 index 0000000000..f14f0377f1 --- /dev/null +++ b/boards/px4/fmu-v6c/nuttx-config/scripts/script.ld @@ -0,0 +1,223 @@ +/**************************************************************************** + * 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 PX4 FMUV6C uses an STM32H753II 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 STM32H743II, 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 PX4 FMUV6C has a test point on board, the BOOT0 pin is at ground so by + * default, the STM32 will boot to address 0x0800:0000 in FLASH unless the test + * point is pulled to 3.3V.then the boot will be from 0x1FF0:0000 + * + * The STM32H743II 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 (rwx) : ORIGIN = 0x00000000, LENGTH = 64K + flash (rx) : ORIGIN = 0x08020000, LENGTH = 1920K + 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) +EXTERN(board_get_manifest) + +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 + + /* 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/px4/fmu-v6c/src/CMakeLists.txt b/boards/px4/fmu-v6c/src/CMakeLists.txt new file mode 100644 index 0000000000..d7b08bb9c3 --- /dev/null +++ b/boards/px4/fmu-v6c/src/CMakeLists.txt @@ -0,0 +1,75 @@ +############################################################################ +# +# 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_compile_definitions(BOOTLOADER) + add_library(drivers_board + bootloader_main.c + init.c + usb.c + timer_config.cpp + ) + target_link_libraries(drivers_board + PRIVATE + nuttx_arch # sdio + nuttx_drivers # sdio + px4_layer #gpio + arch_io_pins # iotimer + bootloader + ) + target_include_directories(drivers_board PRIVATE ${PX4_SOURCE_DIR}/platforms/nuttx/src/bootloader/common) + +else() + add_library(drivers_board + can.c + i2c.cpp + init.c + led.c + mtd.cpp + manifest.c + sdio.c + spi.cpp + timer_config.cpp + usb.c + ) + add_dependencies(drivers_board arch_board_hw_info) + + target_link_libraries(drivers_board + PRIVATE + arch_spi + arch_board_hw_info + drivers__led # drv_led_start + nuttx_arch # sdio + nuttx_drivers # sdio + px4_layer + ) +endif() diff --git a/boards/px4/fmu-v6c/src/board_config.h b/boards/px4/fmu-v6c/src/board_config.h new file mode 100644 index 0000000000..d92fa394a0 --- /dev/null +++ b/boards/px4/fmu-v6c/src/board_config.h @@ -0,0 +1,321 @@ +/**************************************************************************** + * + * 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 board_config.h + * + * PX4FMU-v6c internal definitions + */ + +#pragma once + +/**************************************************************************************************** + * Included Files + ****************************************************************************************************/ + +#include +#include +#include + + +#include + +/**************************************************************************************************** + * Definitions + ****************************************************************************************************/ + +#undef TRACE_PINS + +/* PX4IO connection configuration */ + +#define BOARD_USES_PX4IO_VERSION 2 +#define PX4IO_SERIAL_DEVICE "/dev/ttyS4" +#define PX4IO_SERIAL_TX_GPIO GPIO_USART6_TX +#define PX4IO_SERIAL_RX_GPIO GPIO_USART6_RX +#define PX4IO_SERIAL_BASE STM32_USART6_BASE +#define PX4IO_SERIAL_VECTOR STM32_IRQ_USART6 +#define PX4IO_SERIAL_TX_DMAMAP DMAMAP_USART6_TX +#define PX4IO_SERIAL_RX_DMAMAP DMAMAP_USART6_RX +#define PX4IO_SERIAL_RCC_REG STM32_RCC_APB2ENR +#define PX4IO_SERIAL_RCC_EN RCC_APB2ENR_USART6EN +#define PX4IO_SERIAL_CLOCK STM32_PCLK2_FREQUENCY +#define PX4IO_SERIAL_BITRATE 1500000 /* 1.5Mbps -> max rate for IO */ + +/* PX4FMU GPIOs ***********************************************************************************/ + + +/* LEDs are driven with push pull Anodes to 3.3V */ + +#define GPIO_nLED_RED /* PD10 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10) +#define GPIO_nLED_BLUE /* PD11 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN11) + +#define BOARD_HAS_CONTROL_STATUS_LEDS 1 +#define BOARD_OVERLOAD_LED LED_RED +#define BOARD_ARMED_STATE_LED LED_BLUE + +/* + * 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) + +/* N.B. there is no offset mapping needed for ADC3 because */ +#define ADC3_CH(n) (n) + +/* We are only use ADC3 for REV/VER. */ + +/* Define GPIO pins used as ADC N.B. Channel numbers must match below */ + +#define PX4_ADC_GPIO \ + /* PC4 */ GPIO_ADC12_INP4, \ + /* PB1 */ GPIO_ADC12_INP5, \ + /* PC5 */ GPIO_ADC12_INP8, \ + /* PC0 */ GPIO_ADC123_INP10, \ + /* PC1 */ GPIO_ADC123_INP11, \ + /* PA2 */ GPIO_ADC12_INP14, \ + /* PA4 */ GPIO_ADC12_INP18 \ + +/* Define Channel numbers must match above GPIO pin IN(n)*/ +#define ADC_BATTERY1_CURRENT_CHANNEL /* PC4 */ ADC1_CH(4) +#define ADC_BATTERY2_VOLTAGE_CHANNEL /* PB1 */ ADC1_CH(5) +#define ADC_BATTERY1_VOLTAGE_CHANNEL /* PC5 */ ADC1_CH(8) +#define ADC_HW_REV_SENSE_CHANNEL /* PC0 */ ADC3_CH(10) +#define ADC_HW_VER_SENSE_CHANNEL /* PC1 */ ADC3_CH(11) +#define ADC_BATTERY2_CURRENT_CHANNEL /* PA2 */ ADC1_CH(14) +#define ADC_SCALED_V5_CHANNEL /* PA4 */ ADC1_CH(18) + +#define ADC_CHANNELS \ + ((1 << ADC_BATTERY1_CURRENT_CHANNEL) | \ + (1 << ADC_BATTERY2_VOLTAGE_CHANNEL) | \ + (1 << ADC_BATTERY1_VOLTAGE_CHANNEL) | \ + (1 << ADC_BATTERY2_CURRENT_CHANNEL) | \ + (1 << ADC_SCALED_V5_CHANNEL )) + +#define HW_REV_VER_ADC_BASE STM32_ADC3_BASE + +#define SYSTEM_ADC_BASE STM32_ADC1_BASE + +/* HW has to large of R termination on ADC todo:change when HW value is chosen */ +#define BOARD_ADC_OPEN_CIRCUIT_V (5.6f) + +/* HW Version and Revision drive signals Default to 1 to detect */ +#define BOARD_HAS_HW_VERSIONING + +#define GPIO_HW_VER_REV_DRIVE /* PE12 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN12) +#define GPIO_HW_REV_SENSE /* PC0 */ GPIO_ADC123_INP10 +#define GPIO_HW_VER_SENSE /* PC1 */ GPIO_ADC123_INP11 +#define HW_INFO_INIT {'V','6','C','x', 'x',0} +#define HW_INFO_INIT_VER 3 /* Offset in above string of the VER */ +#define HW_INFO_INIT_REV 4 /* Offset in above string of the REV */ + +#define BOARD_NUM_SPI_CFG_HW_VERSIONS 2 // Rev 0, 10 Sensor sets +// Base/FMUM +#define V6C00 HW_VER_REV(0x0,0x0) // FMUV6C, Rev 0 +#define V6C10 HW_VER_REV(0x1,0x0) // NO PX4IO, Rev 0 + + +/* HEATER + * PWM in future + */ +#define GPIO_HEATER_OUTPUT /* PB9 T17CH1 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN9) +#define HEATER_OUTPUT_EN(on_true) px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, (on_true)) + +/* PWM + */ +#define DIRECT_PWM_OUTPUT_CHANNELS 8 + + +/* Power supply control and monitoring GPIOs */ + +#define GPIO_nPOWER_IN_A /* PA15 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN15) +#define GPIO_nPOWER_IN_B /* PB12 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN12) +#define GPIO_nPOWER_IN_C /* PE15 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN15) + +#define GPIO_nVDD_BRICK1_VALID GPIO_nPOWER_IN_A /* Brick 1 Is Chosen */ +#define GPIO_nVDD_BRICK2_VALID GPIO_nPOWER_IN_B /* Brick 2 Is Chosen */ +#define BOARD_NUMBER_BRICKS 2 +#define GPIO_nVDD_USB_VALID GPIO_nPOWER_IN_C /* USB Is Chosen */ + +#define GPIO_VDD_5V_PERIPH_nEN /* PE2 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN2) +#define GPIO_VDD_5V_PERIPH_nOC /* PE3 */ (GPIO_INPUT |GPIO_FLOAT|GPIO_PORTE|GPIO_PIN3) +#define GPIO_VDD_5V_HIPOWER_nEN /* PC10 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN10) +#define GPIO_VDD_5V_HIPOWER_nOC /* PC11 */ (GPIO_INPUT |GPIO_FLOAT|GPIO_PORTC|GPIO_PIN11) +#define GPIO_VDD_3V3_SENSORS_EN /* PB2 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN2) + +/* Define True logic Power Control in arch agnostic form */ + +#define VDD_5V_PERIPH_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_5V_PERIPH_nEN, !(on_true)) +#define VDD_5V_HIPOWER_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_5V_HIPOWER_nEN, !(on_true)) +#define VDD_3V3_SENSORS_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_3V3_SENSORS4_EN, (on_true)) + +/* Tone alarm output */ + +#define TONE_ALARM_TIMER 3 /* Timer 3 */ +#define TONE_ALARM_CHANNEL 3 /* PB0 GPIO_TIM3_CH3OUT_1 */ + +#define GPIO_BUZZER_1 /* PPB0 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN0) + +#define GPIO_TONE_ALARM_IDLE GPIO_BUZZER_1 +#define GPIO_TONE_ALARM GPIO_TIM3_CH3OUT_1 + +/* USB OTG FS + * + * PA9 OTG_FS_VBUS VBUS sensing + */ +#define GPIO_OTGFS_VBUS /* PA9 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_100MHz|GPIO_PORTA|GPIO_PIN9) + +/* High-resolution timer */ +#define HRT_TIMER 8 /* use timer8 for the HRT */ +#define HRT_TIMER_CHANNEL 3 /* use capture/compare channel 3 */ + +/* PWM input driver. Use FMU AUX5 pins attached to timer4 channel 3 */ +#define PWMIN_TIMER 4 +#define PWMIN_TIMER_CHANNEL /* T4C3 */ 3 +#define GPIO_PWM_IN /* PD14 */ GPIO_TIM4_CH3IN_2 + +#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)) +#define BOARD_ADC_USB_VALID (!px4_arch_gpioread(GPIO_nVDD_USB_VALID)) + +/* FMUv6C never powers off the Servo rail */ + +#define BOARD_ADC_SERVO_VALID (1) + +#define BOARD_ADC_BRICK1_VALID (!px4_arch_gpioread(GPIO_nVDD_BRICK1_VALID)) +#define BOARD_ADC_BRICK2_VALID (!px4_arch_gpioread(GPIO_nVDD_BRICK2_VALID)) + +#define BOARD_ADC_PERIPH_5V_OC (!px4_arch_gpioread(GPIO_VDD_5V_PERIPH_nOC)) +#define BOARD_ADC_HIPOWER_5V_OC (!px4_arch_gpioread(GPIO_VDD_5V_HIPOWER_nOC)) + + +/* 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_HW_VER_REV_DRIVE, \ + GPIO_CAN1_TX, \ + GPIO_CAN1_RX, \ + GPIO_CAN2_TX, \ + GPIO_CAN2_RX, \ + GPIO_HEATER_OUTPUT, \ + GPIO_nPOWER_IN_A, \ + GPIO_nPOWER_IN_B, \ + GPIO_nPOWER_IN_C, \ + GPIO_VDD_5V_PERIPH_nEN, \ + GPIO_VDD_5V_PERIPH_nOC, \ + GPIO_VDD_5V_HIPOWER_nEN, \ + GPIO_VDD_5V_HIPOWER_nOC, \ + GPIO_VDD_3V3_SENSORS_EN, \ + GPIO_TONE_ALARM_IDLE, \ + } + +#define BOARD_ENABLE_CONSOLE_BUFFER + +#define PX4_I2C_BUS_MTD 4,5 + + +#define BOARD_NUM_IO_TIMERS 5 + +__BEGIN_DECLS + +/**************************************************************************************************** + * Public Types + ****************************************************************************************************/ + +/**************************************************************************************************** + * Public data + ****************************************************************************************************/ + +#ifndef __ASSEMBLY__ + +/**************************************************************************************************** + * Public Functions + ****************************************************************************************************/ + +/**************************************************************************** + * Name: stm32_sdio_initialize + * + * Description: + * Initialize SDIO-based MMC/SD card support + * + ****************************************************************************/ + +int stm32_sdio_initialize(void); + +/**************************************************************************************************** + * Name: stm32_spiinitialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the PX4FMU board. + * + ****************************************************************************************************/ + +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/px4/fmu-v6c/src/bootloader_main.c b/boards/px4/fmu-v6c/src/bootloader_main.c new file mode 100644 index 0000000000..c6189fbd04 --- /dev/null +++ b/boards/px4/fmu-v6c/src/bootloader_main.c @@ -0,0 +1,62 @@ +/**************************************************************************** + * + * 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); + +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/px4/fmu-v6c/src/can.c b/boards/px4/fmu-v6c/src/can.c new file mode 100644 index 0000000000..3834074f37 --- /dev/null +++ b/boards/px4/fmu-v6c/src/can.c @@ -0,0 +1,128 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file px4fmu_can.c + * + * Board-specific CAN functions. + */ + +#ifdef CONFIG_CAN + +#include +#include + +#include +#include + +#include "chip.h" +#include "arm_arch.h" + +#include "chip.h" +#include "stm32_can.h" +#include "board_config.h" + +#ifdef CONFIG_CAN + +/************************************************************************************ + * Pre-processor Definitions + ************************************************************************************/ +/* Configuration ********************************************************************/ + +#if defined(CONFIG_STM32_CAN1) && defined(CONFIG_STM32_CAN2) +# warning "Both CAN1 and CAN2 are enabled. Assuming only CAN1." +# undef CONFIG_STM32_CAN2 +#endif + +#ifdef CONFIG_STM32_CAN1 +# define CAN_PORT 1 +#else +# define CAN_PORT 2 +#endif + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +/************************************************************************************ + * Public Functions + ************************************************************************************/ +int can_devinit(void); + +/************************************************************************************ + * Name: can_devinit + * + * Description: + * All STM32 architectures must provide the following interface to work with + * examples/can. + * + ************************************************************************************/ + +int can_devinit(void) +{ + static bool initialized = false; + struct can_dev_s *can; + int ret; + + /* Check if we have already initialized */ + + if (!initialized) { + /* Call stm32_caninitialize() to get an instance of the CAN interface */ + + can = stm32_caninitialize(CAN_PORT); + + if (can == NULL) { + canerr("ERROR: Failed to get CAN interface\n"); + return -ENODEV; + } + + /* Register the CAN driver at "/dev/can0" */ + + ret = can_register("/dev/can0", can); + + if (ret < 0) { + canerr("ERROR: can_register failed: %d\n", ret); + return ret; + } + + /* Now we are initialized */ + + initialized = true; + } + + return OK; +} + +#endif + +#endif /* CONFIG_CAN */ diff --git a/boards/px4/fmu-v6c/src/hw_config.h b/boards/px4/fmu-v6c/src/hw_config.h new file mode 100644 index 0000000000..07bf6f40aa --- /dev/null +++ b/boards/px4/fmu-v6c/src/hw_config.h @@ -0,0 +1,128 @@ +/* + * 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 5000 +#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 INTERFACE_USART 1 +#define INTERFACE_USART_CONFIG "/dev/ttyS0,115200" +#define BOOT_DELAY_ADDRESS 0x000001a0 +#define BOARD_TYPE 56 +#define _FLASH_KBYTES (*(uint32_t *)0x1FF1E880) +#define BOARD_FLASH_SECTORS (15) +#define BOARD_FLASH_SIZE (_FLASH_KBYTES * 1024) + +#define OSC_FREQ 16 + +#define BOARD_PIN_LED_ACTIVITY GPIO_nLED_BLUE // BLUE +#define BOARD_PIN_LED_BOOTLOADER GPIO_nLED_RED // 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/px4/fmu-v6c/src/i2c.cpp b/boards/px4/fmu-v6c/src/i2c.cpp new file mode 100644 index 0000000000..61785c89bc --- /dev/null +++ b/boards/px4/fmu-v6c/src/i2c.cpp @@ -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. + * + ****************************************************************************/ + +#include + +constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { + initI2CBusExternal(1), + initI2CBusExternal(2), + initI2CBusInternal(4), +}; diff --git a/boards/px4/fmu-v6c/src/init.c b/boards/px4/fmu-v6c/src/init.c new file mode 100644 index 0000000000..5a853b31b1 --- /dev/null +++ b/boards/px4/fmu-v6c/src/init.c @@ -0,0 +1,259 @@ +/**************************************************************************** + * + * Copyright (c) 2012-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 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 initialization. + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include "board_config.h" + +#include +#include +#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 + +/**************************************************************************** + * 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) +{ + /* set the peripheral rails off */ + + VDD_5V_PERIPH_EN(false); + board_control_spi_sensors_power(false, 0xffff); + + /* wait for the peripheral rail to reach GND */ + usleep(ms * 1000); + syslog(LOG_DEBUG, "reset done, %d ms\n", ms); + + /* re-enable power */ + + /* switch the peripheral rail back on */ + board_control_spi_sensors_power(true, 0xffff); + VDD_5V_PERIPH_EN(true); + +} + +/************************************************************************************ + * 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(io_timer_channel_get_gpio_output(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)); + + /* 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) +{ +#if !defined(BOOTLOADER) + + /* Power on Interfaces */ + VDD_5V_PERIPH_EN(true); + VDD_5V_HIPOWER_EN(true); + + /* Need hrt running before using the ADC */ + + px4_platform_init(); + + + if (OK == board_determine_hw_info()) { + syslog(LOG_INFO, "[boot] Rev 0x%1x : Ver 0x%1x %s\n", board_get_hw_revision(), board_get_hw_version(), + board_get_hw_type_name()); + + } else { + syslog(LOG_ERR, "[boot] Failed to read HW revision and version\n"); + } + + stm32_spiinitialize(); + + board_spi_reset(10, 0xffff); + + /* 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); + led_off(LED_BLUE); + + if (board_hardfault_init(2, true) != 0) { + led_on(LED_RED); + } + +# ifdef CONFIG_MMCSD + int ret = stm32_sdio_initialize(); + + if (ret != OK) { + led_on(LED_RED); + } + +# endif /* CONFIG_MMCSD */ + + /* Configure the HW based on the manifest */ + + px4_platform_configure(); +#endif + return OK; +} diff --git a/boards/px4/fmu-v6c/src/led.c b/boards/px4/fmu-v6c/src/led.c new file mode 100644 index 0000000000..442e7636f1 --- /dev/null +++ b/boards/px4/fmu-v6c/src/led.c @@ -0,0 +1,235 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file px4fmu2_led.c + * + * PX4FMU LED backend. + */ + +#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_GREEN, // Indexed by BOARD_LED_GREEN + GPIO_nLED_BLUE, // Indexed by BOARD_LED_BLUE + GPIO_nLED_RED, // Indexed by BOARD_LED_RED + GPIO_nSAFETY_SWITCH_LED_OUT, // Indexed by LED_SAFETY by xlatpx4 +}; + +#else + +# define xlat(p) (p) +static uint32_t g_ledmap[] = { + GPIO_nLED_BLUE, // Indexed by LED_BLUE + GPIO_nLED_RED, // Indexed by LED_RED, LED_AMBER + 0, // Indexed by LED_SAFETY (defaulted to an input) + 0, // Indexed by LED_GREEN +}; + +#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/px4/fmu-v6c/src/manifest.c b/boards/px4/fmu-v6c/src/manifest.c new file mode 100644 index 0000000000..04d2386163 --- /dev/null +++ b/boards/px4/fmu-v6c/src/manifest.c @@ -0,0 +1,147 @@ +/**************************************************************************** + * + * Copyright (c) 2018, 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 manifest.c + * + * This module supplies the interface to the manifest of hardware that is + * optional and dependent on the HW REV and HW VER IDs + * + * The manifest allows the system to know whether a hardware option + * say for example the PX4IO is an no-pop option vs it is broken. + * + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include + +#include +#include +#include + +#include "systemlib/px4_macros.h" + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ + +typedef struct { + uint32_t hw_ver_rev; /* the version and revision */ + const px4_hw_mft_item_t *mft; /* The first entry */ + uint32_t entries; /* the lenght of the list */ +} px4_hw_mft_list_entry_t; + +typedef px4_hw_mft_list_entry_t *px4_hw_mft_list_entry; +#define px4_hw_mft_list_uninitialized (px4_hw_mft_list_entry) -1 + +static const px4_hw_mft_item_t device_unsupported = {0, 0, 0}; + +// List of components on a specific board configuration +// The index of those components is given by the enum (px4_hw_mft_item_id_t) +// declared in board_common.h +static const px4_hw_mft_item_t hw_mft_list_v0600[] = { + { + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, +}; + +static const px4_hw_mft_item_t hw_mft_list_v0610[] = { + { + .present = 0, + .mandatory = 0, + .connection = px4_hw_con_unknown, + }, + { + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, +}; + + +static px4_hw_mft_list_entry_t mft_lists[] = { +// ver_rev + {V6C00, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, + {V6C10, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // No PX4IO +}; + +/************************************************************************************ + * Name: board_query_manifest + * + * Description: + * Optional returns manifest item. + * + * Input Parameters: + * manifest_id - the ID for the manifest item to retrieve + * + * Returned Value: + * 0 - item is not in manifest => assume legacy operations + * pointer to a manifest item + * + ************************************************************************************/ + +__EXPORT px4_hw_mft_item board_query_manifest(px4_hw_mft_item_id_t id) +{ + static px4_hw_mft_list_entry boards_manifest = px4_hw_mft_list_uninitialized; + + if (boards_manifest == px4_hw_mft_list_uninitialized) { + uint32_t ver_rev = board_get_hw_version() << 8; + ver_rev |= board_get_hw_revision(); + + for (unsigned i = 0; i < arraySize(mft_lists); i++) { + if (mft_lists[i].hw_ver_rev == ver_rev) { + boards_manifest = &mft_lists[i]; + break; + } + } + + if (boards_manifest == px4_hw_mft_list_uninitialized) { + syslog(LOG_ERR, "[boot] Board %4" PRIx32 " is not supported!\n", ver_rev); + } + } + + px4_hw_mft_item rv = &device_unsupported; + + if (boards_manifest != px4_hw_mft_list_uninitialized && + id < boards_manifest->entries) { + rv = &boards_manifest->mft[id]; + } + + return rv; +} diff --git a/boards/px4/fmu-v6c/src/mtd.cpp b/boards/px4/fmu-v6c/src/mtd.cpp new file mode 100644 index 0000000000..530c7a3bef --- /dev/null +++ b/boards/px4/fmu-v6c/src/mtd.cpp @@ -0,0 +1,104 @@ +/**************************************************************************** + * + * 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 +#include +// KiB BS nB +static const px4_mft_device_t spi2 = { // FM25V02A on FMUM 32K 512 X 64 + .bus_type = px4_mft_device_t::SPI, + .devid = SPIDEV_FLASH(0) +}; + +static const px4_mft_device_t i2c4 = { // 24LC64T on IMU 8K 32 X 256 + .bus_type = px4_mft_device_t::I2C, + .devid = PX4_MK_I2C_DEVID(4, 0x50) +}; + + +static const px4_mtd_entry_t fmum_fram = { + .device = &spi2, + .npart = 2, + .partd = { + { + .type = MTD_PARAMETERS, + .path = "/fs/mtd_params", + .nblocks = 32 + }, + { + .type = MTD_WAYPOINTS, + .path = "/fs/mtd_waypoints", + .nblocks = 32 + + } + }, +}; + +static const px4_mtd_entry_t imu_eeprom = { + .device = &i2c4, + .npart = 2, + .partd = { + { + .type = MTD_CALDATA, + .path = "/fs/mtd_caldata", + .nblocks = 248 + }, + { + .type = MTD_ID, + .path = "/fs/mtd_id", + .nblocks = 8 // 256 = 32 * 8 + } + }, +}; + +static const px4_mtd_manifest_t board_mtd_config = { + .nconfigs = 2, + .entries = { + &fmum_fram, + &imu_eeprom + } +}; + +static const px4_mft_entry_s mtd_mft = { + .type = MTD, + .pmft = (void *) &board_mtd_config, +}; + +static const px4_mft_s mft = { + .nmft = 1, + .mfts = &mtd_mft +}; + +const px4_mft_s *board_get_manifest(void) +{ + return &mft; +} diff --git a/boards/px4/fmu-v6c/src/sdio.c b/boards/px4/fmu-v6c/src/sdio.c new file mode 100644 index 0000000000..869d757756 --- /dev/null +++ b/boards/px4/fmu-v6c/src/sdio.c @@ -0,0 +1,177 @@ +/**************************************************************************** + * + * Copyright (C) 2014, 2016 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include + +#include +#include +#include +#include + +#include +#include + +#include "chip.h" +#include "board_config.h" +#include "stm32_gpio.h" +#include "stm32_sdmmc.h" + +#ifdef CONFIG_MMCSD + + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/* Card detections requires card support and a card detection GPIO */ + +#define HAVE_NCD 1 +#if !defined(GPIO_SDMMC1_NCD) +# undef HAVE_NCD +#endif + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +static FAR struct sdio_dev_s *sdio_dev; +#ifdef HAVE_NCD +static bool g_sd_inserted = 0xff; /* Impossible value */ +#endif + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: stm32_ncd_interrupt + * + * Description: + * Card detect interrupt handler. + * + ****************************************************************************/ + +#ifdef HAVE_NCD +static int stm32_ncd_interrupt(int irq, FAR void *context) +{ + bool present; + + present = !stm32_gpioread(GPIO_SDMMC1_NCD); + + if (sdio_dev && present != g_sd_inserted) { + sdio_mediachange(sdio_dev, present); + g_sd_inserted = present; + } + + return OK; +} +#endif + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: stm32_sdio_initialize + * + * Description: + * Initialize SDIO-based MMC/SD card support + * + ****************************************************************************/ + +int stm32_sdio_initialize(void) +{ + int ret; + +#ifdef HAVE_NCD + /* Card detect */ + + bool cd_status; + + /* Configure the card detect GPIO */ + + stm32_configgpio(GPIO_SDMMC1_NCD); + + /* Register an interrupt handler for the card detect pin */ + + stm32_gpiosetevent(GPIO_SDMMC1_NCD, true, true, true, stm32_ncd_interrupt); +#endif + + /* Mount the SDIO-based MMC/SD block driver */ + /* First, get an instance of the SDIO interface */ + + finfo("Initializing SDIO slot %d\n", SDIO_SLOTNO); + + sdio_dev = sdio_initialize(SDIO_SLOTNO); + + if (!sdio_dev) { + syslog(LOG_ERR, "[boot] Failed to initialize SDIO slot %d\n", SDIO_SLOTNO); + return -ENODEV; + } + + /* Now bind the SDIO interface to the MMC/SD driver */ + + finfo("Bind SDIO to the MMC/SD driver, minor=%d\n", SDIO_MINOR); + + ret = mmcsd_slotinitialize(SDIO_MINOR, sdio_dev); + + if (ret != OK) { + syslog(LOG_ERR, "[boot] Failed to bind SDIO to the MMC/SD driver: %d\n", ret); + return ret; + } + + finfo("Successfully bound SDIO to the MMC/SD driver\n"); + +#ifdef HAVE_NCD + /* Use SD card detect pin to check if a card is g_sd_inserted */ + + cd_status = !stm32_gpioread(GPIO_SDMMC1_NCD); + finfo("Card detect : %d\n", cd_status); + + sdio_mediachange(sdio_dev, cd_status); +#else + /* Assume that the SD card is inserted. What choice do we have? */ + + sdio_mediachange(sdio_dev, true); +#endif + + return OK; +} + +#endif /* CONFIG_MMCSD */ diff --git a/boards/px4/fmu-v6c/src/spi.cpp b/boards/px4/fmu-v6c/src/spi.cpp new file mode 100644 index 0000000000..9e636e0ac4 --- /dev/null +++ b/boards/px4/fmu-v6c/src/spi.cpp @@ -0,0 +1,61 @@ +/**************************************************************************** + * + * Copyright (C) 2020, 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 +#include +#include + +constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSIONS] = { + initSPIHWVersion(V6C00, { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_GYR_DEVTYPE_BMI055, SPI::CS{GPIO::PortC, GPIO::Pin14}, SPI::DRDY{GPIO::PortE, GPIO::Pin5}), + initSPIDevice(DRV_ACC_DEVTYPE_BMI055, SPI::CS{GPIO::PortC, GPIO::Pin15}, SPI::DRDY{GPIO::PortE, GPIO::Pin4}), + initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortC, GPIO::Pin13}, SPI::DRDY{GPIO::PortE, GPIO::Pin6}), + }, {GPIO::PortB, GPIO::Pin2}), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortD, GPIO::Pin4}) + }), + }), + initSPIHWVersion(V6C10, { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_GYR_DEVTYPE_BMI055, SPI::CS{GPIO::PortC, GPIO::Pin14}, SPI::DRDY{GPIO::PortE, GPIO::Pin5}), + initSPIDevice(DRV_ACC_DEVTYPE_BMI055, SPI::CS{GPIO::PortC, GPIO::Pin15}, SPI::DRDY{GPIO::PortE, GPIO::Pin4}), + initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortC, GPIO::Pin13}, SPI::DRDY{GPIO::PortE, GPIO::Pin6}), + }, {GPIO::PortB, GPIO::Pin2}), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortD, GPIO::Pin4}) + }), + }), +}; + +static constexpr bool unused = validateSPIConfig(px4_spi_buses_all_hw); diff --git a/boards/px4/fmu-v6c/src/timer_config.cpp b/boards/px4/fmu-v6c/src/timer_config.cpp new file mode 100644 index 0000000000..e26a8c5d1e --- /dev/null +++ b/boards/px4/fmu-v6c/src/timer_config.cpp @@ -0,0 +1,73 @@ +/**************************************************************************** + * + * 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 + +/* Timer allocation + * + * TIM1_CH1 T FMU_CH1 + * TIM1_CH2 T FMU_CH2 + * TIM1_CH3 T FMU_CH3 + * TIM1_CH4 T FMU_CH4 + * + * TIM4_CH3 T FMU_CH5 + * TIM4_CH4 T FMU_CH6 + * + * TIM5_CH1 T FMU_CH7 + * TIM5_CH2 T FMU_CH8 + * + * TIM17_CH1 T HEATER > PWM OUT or GPIO + * + * TIM3_CH3 T BUZZER_1 - Driven by other driver + */ + +constexpr io_timers_t io_timers[MAX_IO_TIMERS] = { + initIOTimer(Timer::Timer1, DMA{DMA::Index1}), + initIOTimer(Timer::Timer4, DMA{DMA::Index1}), + initIOTimer(Timer::Timer5), + initIOTimer(Timer::Timer17), +}; + +constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = { + initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel1}, {GPIO::PortA, GPIO::Pin8}), + initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel2}, {GPIO::PortE, GPIO::Pin11}), + initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel3}, {GPIO::PortE, GPIO::Pin13}), + initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel4}, {GPIO::PortE, GPIO::Pin14}), + initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel3}, {GPIO::PortD, GPIO::Pin14}), + initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel4}, {GPIO::PortD, GPIO::Pin15}), + initIOTimerChannel(io_timers, {Timer::Timer5, Timer::Channel1}, {GPIO::PortA, GPIO::Pin0}), + initIOTimerChannel(io_timers, {Timer::Timer5, Timer::Channel2}, {GPIO::PortA, GPIO::Pin1}), +}; + +constexpr io_timers_channel_mapping_t io_timers_channel_mapping = + initIOTimerChannelMapping(io_timers, timer_io_channels); diff --git a/boards/px4/fmu-v6c/src/usb.c b/boards/px4/fmu-v6c/src/usb.c new file mode 100644 index 0000000000..9d5915c0e6 --- /dev/null +++ b/boards/px4/fmu-v6c/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/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/hw_description.h b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/hw_description.h index ba2292b97e..c28fa7e86c 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/hw_description.h +++ b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/hw_description.h @@ -92,7 +92,13 @@ enum Timer { Timer13, Timer14, #ifdef STM32_TIM15_BASE - Timer15 + Timer15, +#endif +#ifdef STM32_TIM16_BASE + Timer16, +#endif +#ifdef STM32_TIM17_BASE + Timer17 #endif }; enum Channel { @@ -161,6 +167,16 @@ static inline constexpr uint32_t timerBaseRegister(Timer::Timer timer) case Timer::Timer15: return STM32_TIM15_BASE; #endif +#ifdef STM32_TIM16_BASE + + case Timer::Timer16: return STM32_TIM16_BASE; +#endif + +#ifdef STM32_TIM17_BASE + + case Timer::Timer17: return STM32_TIM17_BASE; +#endif + default: break; } diff --git a/platforms/nuttx/src/px4/stm/stm32h7/include/px4_arch/hw_description.h b/platforms/nuttx/src/px4/stm/stm32h7/include/px4_arch/hw_description.h index 9627d9a4cd..a93b9538be 100644 --- a/platforms/nuttx/src/px4/stm/stm32h7/include/px4_arch/hw_description.h +++ b/platforms/nuttx/src/px4/stm/stm32h7/include/px4_arch/hw_description.h @@ -86,6 +86,8 @@ static inline constexpr uint32_t getTimerUpdateDMAMap(Timer::Timer timer, const case Timer::Timer13: case Timer::Timer14: case Timer::Timer15: + case Timer::Timer16: + case Timer::Timer17: break; } diff --git a/platforms/nuttx/src/px4/stm/stm32h7/include/px4_arch/io_timer_hw_description.h b/platforms/nuttx/src/px4/stm/stm32h7/include/px4_arch/io_timer_hw_description.h index 06e7db74ea..b1299b04e3 100644 --- a/platforms/nuttx/src/px4/stm/stm32h7/include/px4_arch/io_timer_hw_description.h +++ b/platforms/nuttx/src/px4/stm/stm32h7/include/px4_arch/io_timer_hw_description.h @@ -49,6 +49,8 @@ static inline constexpr timer_io_channels_t initIOTimerGPIOInOut(Timer::TimerCha switch (timer.timer) { case Timer::Timer1: case Timer::Timer2: + case Timer::Timer16: + case Timer::Timer17: gpio_af = GPIO_AF1; break; @@ -274,6 +276,28 @@ static inline constexpr io_timers_t initIOTimer(Timer::Timer timer, DMA dshot_dm ret.vectorno = STM32_IRQ_TIM15; #ifdef CONFIG_STM32_TIM15 nuttx_config_timer_enabled = true; +#endif + break; + + case Timer::Timer16: + ret.base = STM32_TIM16_BASE; + ret.clock_register = STM32_RCC_APB2ENR; + ret.clock_bit = RCC_APB2ENR_TIM16EN; + ret.clock_freq = STM32_APB2_TIM16_CLKIN; + ret.vectorno = STM32_IRQ_TIM16; +#ifdef CONFIG_STM32_TIM16 + nuttx_config_timer_enabled = true; +#endif + break; + + case Timer::Timer17: + ret.base = STM32_TIM17_BASE; + ret.clock_register = STM32_RCC_APB2ENR; + ret.clock_bit = RCC_APB2ENR_TIM17EN; + ret.clock_freq = STM32_APB2_TIM17_CLKIN; + ret.vectorno = STM32_IRQ_TIM17; +#ifdef CONFIG_STM32_TIM17 + nuttx_config_timer_enabled = true; #endif break; }