From 0262a699c13a0cb1fb9c64a66d8d06ff22f061d3 Mon Sep 17 00:00:00 2001 From: Hyon Lim Date: Sat, 24 Aug 2019 06:02:25 +0900 Subject: [PATCH] UVify Core board support and airframes (Draco, Draco-R, IFO) (#12337) * also includes minor changes to make it easy to keep in sync with px4_fmu-v4 --- .ci/Jenkinsfile-compile | 2 +- .../px4fmu_common/init.d/airframes/4013_bebop | 3 +- .../init.d/airframes/4020_hk_micro_pcb | 3 +- .../init.d/airframes/4030_3dr_solo | 4 +- .../init.d/airframes/4041_beta75x | 2 + .../init.d/airframes/4070_aerofc | 1 + ROMFS/px4fmu_common/init.d/airframes/4071_ifo | 123 ++++++ .../px4fmu_common/init.d/airframes/4072_draco | 119 ++++++ .../init.d/airframes/4080_zmr250 | 1 + .../init.d/airframes/4090_nanomind | 4 +- .../init.d/airframes/4900_crazyflie | 3 +- .../init.d/airframes/6002_draco_r | 177 ++++++++ .../init.d/airframes/CMakeLists.txt | 3 + Tools/upload.sh | 2 +- .../px4/fmu-v4/nuttx-config/include/board.h | 2 +- boards/px4/fmu-v4/src/board_config.h | 2 +- boards/px4/fmu-v4/src/can.c | 2 +- boards/px4/fmu-v4/src/init.c | 2 +- boards/px4/fmu-v4/src/led.c | 2 +- boards/px4/fmu-v4/src/timer_config.c | 2 +- boards/px4/fmu-v4/src/usb.c | 2 +- boards/uvify/core/default.cmake | 122 ++++++ boards/uvify/core/firmware.prototype | 13 + boards/uvify/core/init/rc.board_defaults | 13 + boards/uvify/core/init/rc.board_extras | 18 + boards/uvify/core/init/rc.board_sensors | 38 ++ .../uvify/core/nuttx-config/include/board.h | 346 +++++++++++++++ boards/uvify/core/nuttx-config/nsh/defconfig | 247 +++++++++++ .../uvify/core/nuttx-config/scripts/script.ld | 159 +++++++ boards/uvify/core/src/CMakeLists.txt | 49 +++ boards/uvify/core/src/board_config.h | 389 +++++++++++++++++ boards/uvify/core/src/can.c | 130 ++++++ boards/uvify/core/src/init.c | 403 ++++++++++++++++++ boards/uvify/core/src/led.c | 106 +++++ boards/uvify/core/src/spi.c | 320 ++++++++++++++ boards/uvify/core/src/timer_config.c | 126 ++++++ boards/uvify/core/src/usb.c | 108 +++++ platforms/nuttx/CMakeLists.txt | 1 + 38 files changed, 3034 insertions(+), 15 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d/airframes/4071_ifo create mode 100644 ROMFS/px4fmu_common/init.d/airframes/4072_draco create mode 100644 ROMFS/px4fmu_common/init.d/airframes/6002_draco_r create mode 100644 boards/uvify/core/default.cmake create mode 100644 boards/uvify/core/firmware.prototype create mode 100644 boards/uvify/core/init/rc.board_defaults create mode 100644 boards/uvify/core/init/rc.board_extras create mode 100644 boards/uvify/core/init/rc.board_sensors create mode 100644 boards/uvify/core/nuttx-config/include/board.h create mode 100644 boards/uvify/core/nuttx-config/nsh/defconfig create mode 100644 boards/uvify/core/nuttx-config/scripts/script.ld create mode 100644 boards/uvify/core/src/CMakeLists.txt create mode 100644 boards/uvify/core/src/board_config.h create mode 100644 boards/uvify/core/src/can.c create mode 100644 boards/uvify/core/src/init.c create mode 100644 boards/uvify/core/src/led.c create mode 100644 boards/uvify/core/src/spi.c create mode 100644 boards/uvify/core/src/timer_config.c create mode 100644 boards/uvify/core/src/usb.c diff --git a/.ci/Jenkinsfile-compile b/.ci/Jenkinsfile-compile index 91c264ae1b..6a9ebcf664 100644 --- a/.ci/Jenkinsfile-compile +++ b/.ci/Jenkinsfile-compile @@ -37,7 +37,7 @@ pipeline { "px4_fmu-v5_default", "px4_fmu-v5_fixedwing", "px4_fmu-v5_multicopter", "px4_fmu-v5_rover", "px4_fmu-v5_rtps", "px4_fmu-v5_stackcheck", "px4_fmu-v5x_default", "px4_fmu-v5x_fixedwing", "px4_fmu-v5x_multicopter", "px4_fmu-v5x_rover", "px4_fmu-v5x_rtps", "px4_fmu-v5x_stackcheck", "intel_aerofc-v1_default", "auav_x21_default", "av_x-v1_default", "bitcraze_crazyflie_default", "airmind_mindpx-v2_default", - "holybro_kakutef7", "mro_ctrl-zero-f7_default", "nxp_fmuk66-v3_default", "omnibus_f4sd_default"], + "holybro_kakutef7", "mro_ctrl-zero-f7_default", "nxp_fmuk66-v3_default", "omnibus_f4sd_default", "uvify_core_default"], image: docker_images.nuttx, archive: true ] diff --git a/ROMFS/px4fmu_common/init.d/airframes/4013_bebop b/ROMFS/px4fmu_common/init.d/airframes/4013_bebop index 6be8d107e8..b97ce76dc6 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4013_bebop +++ b/ROMFS/px4fmu_common/init.d/airframes/4013_bebop @@ -10,7 +10,8 @@ # @board px4_fmu-v4 exclude # @board px4_fmu-v4pro exclude # @board px4_fmu-v5 exclude -# @board aerofc-v1 exclude +# @board px4_fmu-v5x exclude +# @board intel_aerofc-v1 exclude # # @maintainer Michael Schaeuble # diff --git a/ROMFS/px4fmu_common/init.d/airframes/4020_hk_micro_pcb b/ROMFS/px4fmu_common/init.d/airframes/4020_hk_micro_pcb index 36d4666ab1..9bc37f9f5f 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4020_hk_micro_pcb +++ b/ROMFS/px4fmu_common/init.d/airframes/4020_hk_micro_pcb @@ -10,7 +10,8 @@ # @board px4_fmu-v4 exclude # @board px4_fmu-v4pro exclude # @board px4_fmu-v5 exclude -# @board aerofc-v1 exclude +# @board px4_fmu-v5x exclude +# @board intel_aerofc-v1 exclude # # @maintainer Thomas Gubler # diff --git a/ROMFS/px4fmu_common/init.d/airframes/4030_3dr_solo b/ROMFS/px4fmu_common/init.d/airframes/4030_3dr_solo index 87bfcabb5e..cc36bb8d34 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4030_3dr_solo +++ b/ROMFS/px4fmu_common/init.d/airframes/4030_3dr_solo @@ -6,10 +6,12 @@ # @class Copter # # @board px4_fmu-v2 exclude +# @board px4_fmu-v3 exclude # @board px4_fmu-v4 exclude # @board px4_fmu-v4pro exclude # @board px4_fmu-v5 exclude -# @board aerofc-v1 exclude +# @board px4_fmu-v5x exclude +# @board intel_aerofc-v1 exclude # # @maintainer Andreas Antener # diff --git a/ROMFS/px4fmu_common/init.d/airframes/4041_beta75x b/ROMFS/px4fmu_common/init.d/airframes/4041_beta75x index 2f4949b49a..6b1f8abbf8 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4041_beta75x +++ b/ROMFS/px4fmu_common/init.d/airframes/4041_beta75x @@ -10,6 +10,8 @@ # @board px4_fmu-v4 exclude # @board px4_fmu-v4pro exclude # @board px4_fmu-v5 exclude +# @board px4_fmu-v5x exclude +# @board intel_aerofc-v1 exclude # # @output MAIN1 motor 1 # @output MAIN2 motor 2 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4070_aerofc b/ROMFS/px4fmu_common/init.d/airframes/4070_aerofc index 341dd809f5..d7b0c1ab95 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4070_aerofc +++ b/ROMFS/px4fmu_common/init.d/airframes/4070_aerofc @@ -7,6 +7,7 @@ # @board px4_fmu-v4 exclude # @board px4_fmu-v4pro exclude # @board px4_fmu-v5 exclude +# @board px4_fmu-v5x exclude # # @type Quadrotor x # @class Copter diff --git a/ROMFS/px4fmu_common/init.d/airframes/4071_ifo b/ROMFS/px4fmu_common/init.d/airframes/4071_ifo new file mode 100644 index 0000000000..994613ed51 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/airframes/4071_ifo @@ -0,0 +1,123 @@ +#!/bin/sh +# +# @name UVify IFO +# +# @type Quadrotor x +# @class Copter +# +# @output MAIN1 motor 1 +# @output MAIN2 motor 2 +# @output MAIN3 motor 3 +# @output MAIN4 motor 4 +# +# @board px4_fmu-v2 exclude +# @board px4_fmu-v3 exclude +# @board px4_fmu-v4pro exclude +# @board px4_fmu-v5 exclude +# @board px4_fmu-v5x exclude +# @board intel_aerofc-v1 exclude +# +# @maintainer Hyon Lim +# + +set VEHICLE_TYPE mc +set MIXER quad_x +set PWM_OUT 1234 + +if [ $AUTOCNF = yes ] +then + # Attitude & rate gains + #param set MC_ROLL_P 7.00000 + param set MC_ROLLRATE_P 0.15000 + #param set MC_ROLLRATE_I 0.90000 + param set MC_ROLLRATE_D 0.00130 + + #param set MC_PITCH_P 7.00000 + param set MC_PITCHRATE_P 0.15000 + #param set MC_PITCHRATE_I 1.10000 + param set MC_PITCHRATE_D 0.00160 + + param set MC_YAW_P 2.8 + param set MC_YAWRATE_P 0.2 + #param set MC_YAWRATE_I 0.15 + param set MC_YAWRATE_D 0.0 + param set MC_YAW_FF 0.5 + + #param set MC_ROLL_TC 0.19 + #param set MC_PITCH_TC 0.16 + + # Manual mode settings: Unleash Draco R's power :) + #param set MPC_MAN_TILT_MAX 70.00000 + #param set MC_PITCHRATE_MAX 1600.00000 + #param set MC_ROLLRATE_MAX 1600.00000 + #param set MC_YAWRATE_MAX 700.00000 + param set MPC_MANTHR_MAX 0.90000 + param set MPC_MANTHR_MIN 0.08000 + #param set MPC_MAN_TILT_MAX 35.0000 + #param set MPC_TILTMAX_AIR 20.0000 + + # Disable RC filtering + param set RC_FLT_CUTOFF 0.00000 + + # Filter settings + param set MC_DTERM_CUTOFF 90.00000 + param set IMU_GYRO_CUTOFF 100.00000 + + # Thrust curve (avoids the need for TPA) + #param set THR_MDL_FAC 0.25 + + # System + param set PWM_MAX 1950 + param set PWM_MIN 1100 + param set PWM_RATE 0 + + #param set SYS_FMU_TASK 1 + param set SENS_BOARD_ROT 10 + + # Position control + param set MPC_Z_P 1.00000 + param set MPC_Z_VEL_P 0.20000 + param set MPC_Z_VEL_I 0.02000 + param set MPC_Z_VEL_D 0.00000 + + param set MPC_THR_MIN 0.06000 + param set MPC_THR_MAX 0.40000 + param set MPC_THR_HOVER 0.3000 + + param set MIS_TAKEOFF_ALT 1.1000 + param set MPC_XY_P 1.7000 + param set MPC_XY_VEL_P 0.1300 + param set MPC_XY_VEL_I 0.0600 + param set MPC_XY_VEL_D 0.0100 + param set MPC_TKO_RAMP_T 1.0000 + param set MPC_TKO_SPEED 1.1000 + param set MPC_VEL_MANUAL 3.0000 + + param set BAT_SOURCE 0 + param set BAT_N_CELLS 4 + param set BAT_V_DIV 10.14 + param set BAT_A_PER_V 18.18 + #param set CBRK_IO_SAFETY 22027 + param set COM_ARM_EKF_AB 0.00500 + param set COM_DISARM_LAND 2 + + # Filter settings + param set IMU_GYRO_CUTOFF 90.00000 + param set MC_DTERM_CUTOFF 70.00000 + + # Don't try to be intelligent on RC loss: just cut the motors + param set NAV_RCL_ACT 6 + + # enable to use high-rate logging for better rate tracking analysis + # param set SDLOG_PROFILE 19 + + # TELEM1 ttyS1 - Wifi module + param set MAV_0_CONFIG 101 + param set MAV_0_MODE 2 # onboard + param set SER_TEL1_BAUD 921600 + + # TELEM2 ttyS2 - Sub 1-Ghz + param set MAV_1_CONFIG 102 + param set MAV_1_MODE 0 # normal + param set SER_TEL2_BAUD 57600 +fi diff --git a/ROMFS/px4fmu_common/init.d/airframes/4072_draco b/ROMFS/px4fmu_common/init.d/airframes/4072_draco new file mode 100644 index 0000000000..661572074d --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/airframes/4072_draco @@ -0,0 +1,119 @@ +#!/bin/sh +# +# @name UVify Draco +# +# @type Quadrotor x +# @class Copter +# +# @output MAIN1 motor 1 +# @output MAIN2 motor 2 +# @output MAIN3 motor 3 +# @output MAIN4 motor 4 +# +# @board px4_fmu-v2 exclude +# @board px4_fmu-v3 exclude +# @board px4_fmu-v4pro exclude +# @board px4_fmu-v5 exclude +# @board px4_fmu-v5x exclude +# @board intel_aerofc-v1 exclude +# +# @maintainer Hyon Lim +# + +set VEHICLE_TYPE mc +set MIXER quad_x +set PWM_OUT 1234 + +if [ $AUTOCNF = yes ] +then + # Attitude & rate gains + param set MC_ROLL_P 7.00000 + param set MC_ROLLRATE_P 0.15000 + param set MC_ROLLRATE_I 0.90000 + param set MC_ROLLRATE_D 0.00130 + + param set MC_PITCH_P 7.00000 + param set MC_PITCHRATE_P 0.15000 + param set MC_PITCHRATE_I 1.10000 + param set MC_PITCHRATE_D 0.00160 + + param set MC_YAW_P 2.8 + param set MC_YAWRATE_P 0.2 + param set MC_YAWRATE_I 0.15 + param set MC_YAWRATE_D 0.0 + param set MC_YAW_FF 0.5 + + param set MC_ROLL_TC 0.19 + param set MC_PITCH_TC 0.16 + + # Manual mode settings: Unleash Draco R's power :) + param set MPC_MAN_TILT_MAX 70.00000 + param set MC_PITCHRATE_MAX 1600.00000 + param set MC_ROLLRATE_MAX 1600.00000 + param set MC_YAWRATE_MAX 700.00000 + param set MPC_MANTHR_MAX 0.90000 + param set MPC_MANTHR_MIN 0.08000 + param set MPC_MAN_TILT_MAX 35.0000 + param set MPC_TILTMAX_AIR 20.0000 + + # Disable RC filtering + param set RC_FLT_CUTOFF 0.00000 + + # Filter settings + param set MC_DTERM_CUTOFF 90.00000 + param set IMU_GYRO_CUTOFF 100.00000 + + # Thrust curve (avoids the need for TPA) + param set THR_MDL_FAC 0.25 + + # System + param set PWM_MAX 1950 + param set PWM_MIN 1100 + param set PWM_RATE 0 + + param set SYS_FMU_TASK 1 + param set SENS_BOARD_ROT 2 + + # Position control + param set MPC_Z_P 1.00000 + param set MPC_Z_VEL_P 0.20000 + param set MPC_Z_VEL_I 0.02000 + param set MPC_Z_VEL_D 0.00000 + + param set MPC_THR_MIN 0.06000 + param set MPC_THR_MAX 0.40000 + param set MPC_THR_HOVER 0.3000 + + param set MIS_TAKEOFF_ALT 1.1000 + param set MPC_XY_P 1.7000 + param set MPC_XY_VEL_P 0.1300 + param set MPC_XY_VEL_I 0.0600 + param set MPC_XY_VEL_D 0.0100 + param set MPC_TKO_RAMP_T 1.0000 + param set MPC_TKO_SPEED 1.1000 + param set MPC_VEL_MANUAL 3.0000 + + param set BAT_SOURCE 0 + param set CBRK_IO_SAFETY 22027 + param set COM_ARM_EKF_AB 0.00500 + param set COM_DISARM_LAND 3 + + # Filter settings + param set IMU_GYRO_CUTOFF 90.00000 + param set MC_DTERM_CUTOFF 70.00000 + + # Don't try to be intelligent on RC loss: just cut the motors + param set NAV_RCL_ACT 6 + + # enable to use high-rate logging for better rate tracking analysis + # param set SDLOG_PROFILE 19 + + # TELEM1 ttyS1 + param set MAV_0_CONFIG 101 + param set MAV_0_MODE 2 # onboard + param set MAV_0_RATE 20000 + param set SER_TEL1_BAUD 921600 + + # TELEM2 ttyS2 + param set MAV_1_CONFIG 0 +fi diff --git a/ROMFS/px4fmu_common/init.d/airframes/4080_zmr250 b/ROMFS/px4fmu_common/init.d/airframes/4080_zmr250 index dd8d8a89f9..2f84a26953 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4080_zmr250 +++ b/ROMFS/px4fmu_common/init.d/airframes/4080_zmr250 @@ -6,6 +6,7 @@ # @class Copter # # @board px4_fmu-v2 exclude +# @board intel_aerofc-v1 exclude # # @maintainer Anton Matosov # diff --git a/ROMFS/px4fmu_common/init.d/airframes/4090_nanomind b/ROMFS/px4fmu_common/init.d/airframes/4090_nanomind index 3ec3080d56..d5d4599a1b 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4090_nanomind +++ b/ROMFS/px4fmu_common/init.d/airframes/4090_nanomind @@ -7,10 +7,10 @@ # # @board px4_fmu-v2 exclude # @board px4_fmu-v3 exclude -# @board px4_fmu-v4 exclude # @board px4_fmu-v4pro exclude # @board px4_fmu-v5 exclude -# @board aerofc-v1 exclude +# @board px4_fmu-v5x exclude +# @board intel_aerofc-v1 exclude # # @maintainer Henry Zhang # diff --git a/ROMFS/px4fmu_common/init.d/airframes/4900_crazyflie b/ROMFS/px4fmu_common/init.d/airframes/4900_crazyflie index 24f911c584..3593ed9037 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4900_crazyflie +++ b/ROMFS/px4fmu_common/init.d/airframes/4900_crazyflie @@ -7,7 +7,8 @@ # @board px4_fmu-v4 exclude # @board px4_fmu-v4pro exclude # @board px4_fmu-v5 exclude -# @board aerofc-v1 exclude +# @board px4_fmu-v5x exclude +# @board intel_aerofc-v1 exclude # # @type Quadrotor x # @class Copter diff --git a/ROMFS/px4fmu_common/init.d/airframes/6002_draco_r b/ROMFS/px4fmu_common/init.d/airframes/6002_draco_r new file mode 100644 index 0000000000..95bad2029c --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/airframes/6002_draco_r @@ -0,0 +1,177 @@ +#!/bin/sh +# +# @name UVify Draco-R +# +# @type Hexarotor x +# @class Copter +# +# @output MAIN1 motor 1 +# @output MAIN2 motor 2 +# @output MAIN3 motor 3 +# @output MAIN4 motor 4 +# @output MAIN5 motor 5 +# @output MAIN6 motor 6 +# +# @board px4_fmu-v2 exclude +# @board px4_fmu-v3 exclude +# @board px4_fmu-v4pro exclude +# @board px4_fmu-v5 exclude +# @board px4_fmu-v5x exclude +# @board intel_aerofc-v1 exclude +# +# @output AUX1 feed-through of RC AUX1 channel +# @output AUX2 feed-through of RC AUX2 channel +# +# @maintainer Hyon Lim +# + +set VEHICLE_TYPE mc +set MIXER hexa_x +set PWM_OUT 123456 + +if [ $AUTOCNF = yes ] +then + # Attitude & rate gains + param set MC_ROLL_P 6.50000 + param set MC_ROLLRATE_P 0.15000 + param set MC_ROLLRATE_I 0.05000 + param set MC_ROLLRATE_D 0.00130 + + param set MC_PITCH_P 6.50000 + param set MC_PITCHRATE_P 0.15000 + param set MC_PITCHRATE_I 0.05000 + param set MC_PITCHRATE_D 0.00160 + + param set MC_YAW_P 2.80000 + param set MC_YAWRATE_P 0.20000 + param set MC_YAWRATE_I 0.10000 + param set MC_YAWRATE_D 0.00000 + param set MC_YAW_FF 0.00000 + + #param set MC_ROLL_TC 0.19 + #param set MC_PITCH_TC 0.16 + + # Manual mode settings: Unleash Draco R's power :) + param set MPC_MAN_TILT_MAX 70.00000 + param set MPC_MANTHR_MAX 0.90000 + param set MPC_MANTHR_MIN 0.08000 + param set MPC_MAN_TILT_MAX 35.0000 + param set MPC_TILTMAX_AIR 45.0000 + param set MPC_POS_MODE 2 + param set MPC_AUTO_MODE 1 + param set MPC_ACC_HOR 8.0000 + + param set MC_PITCHRATE_MAX 800.00000 + param set MC_ROLLRATE_MAX 800.00000 + param set MC_YAWRATE_MAX 700.00000 + + # Disable RC filtering + param set RC_FLT_CUTOFF 0.00000 + + # Filter settings + param set MC_DTERM_CUTOFF 90.00000 + param set IMU_GYRO_CUTOFF 100.00000 + + # Thrust curve (avoids the need for TPA) + param set THR_MDL_FAC 0.25 + + # System + param set PWM_MAX 1950 + param set PWM_MIN 1100 + param set PWM_MAIN_DIS5 980 + param set PWM_MAIN_DIS6 980 + + param set SYS_FMU_TASK 1 + param set SENS_BOARD_ROT 2 + + param set COM_ARM_MAG 0.2000 + + # Sensors + param set SENS_EN_LL40LS 2 + param set SENS_FLOW_ROT 2 + + # Position control + param set MPC_Z_P 1.00000 + param set MPC_Z_VEL_P 0.20000 + param set MPC_Z_VEL_I 0.02000 + param set MPC_Z_VEL_D 0.00000 + + param set MPC_THR_MIN 0.06000 + param set MPC_THR_MAX 0.40000 + param set MPC_THR_HOVER 0.3000 + + param set MIS_TAKEOFF_ALT 1.1000 + + param set MPC_XY_P 0.9500 + param set MPC_XY_VEL_P 0.0900 + param set MPC_XY_VEL_I 0.0200 + param set MPC_XY_VEL_D 0.0100 + + param set MPC_TKO_RAMP_T 0.4000 + param set MPC_TKO_SPEED 1.5000 + param set MPC_VEL_MANUAL 10.0000 + + # EKF + # Set baro first + param set EKF2_HGT_MODE 1 + # Enable optical flow and GPS + param set EKF2_AID_MASK 1 + param set EKF2_RNG_AID 0 + param set EKF2_MAG_TYPE 1 + param set EKF2_OF_QMIN 80.0000 + + # + param set CBRK_IO_SAFETY 22027 + param set SYS_COMPANION 921600 + param set COM_DISARM_LAND 3 + + #PWM + # ONESHOT + param set PWM_RATE 0 + + # gimbal + #param set MNT_DO_STAB 1 + #param set MNT_MAN_PITCH 1 + #param set MNT_MAN_ROLL 2 + #param set MNT_MODE_IN 1 + param set BAT_SOURCE 0 + param set BAT_N_CELLS 4 + param set BAT_V_DIV 10.133 + + # TELEM1 ttyS1 + param set MAV_0_CONFIG 101 + param set MAV_0_MODE 1 # onboard + param set MAV_0_FORWARD 1 + param set SER_TEL1_BAUD 57600 + + # TELEM2 ttyS2 + param set MAV_1_CONFIG 102 + param set MAV_1_MODE 2 + param set MAV_1_RATE 800000 + param set MAV_1_FORWARD 1 + param set SER_TEL2_BAUD 921600 +fi + +#set PWM_OUT 12345678 + +#set MIXER_AUX mount_2axes +#set PWM_AUX_OUT 78 +#set PWM_AUX_RATE 50 +#set OUTPUT_AUX_DEV /dev/pwm_output0 +#set OUTPUT_AUX_TO_MAIN yes +#set MIXER_APPEND yes + +#if mixer append /dev/pwm_output0 /etc/mixers/mount_2axes.aux.mix +#then +# echo "INFO [6002] Mixer append success" +#else +# echo "ERROR [6002] Mixer append failed" +#fi + +#if pwm rate -c 78 -r 50 -d /dev/pwm_output0 +#then +# echo "INFO [6002] PWM RATE CHANGE SUCCESS" +#else +# echo "INFO [6002] PWM RATE CHANGE FAILED" +#fi + diff --git a/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt index f09ee37f6e..0dc26be261 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt @@ -81,6 +81,8 @@ px4_add_romfs_files( 4053_holybro_kopis2 4060_dji_matrice_100 4070_aerofc + 4071_ifo + 4072_draco 4080_zmr250 4090_nanomind 4100_tiltquadrotor @@ -92,6 +94,7 @@ px4_add_romfs_files( # [6000, 6999] Hexarotor x" 6001_hexa_x + 6002_draco_r # [7000, 7999] Hexarotor +" 7001_hexa_+ diff --git a/Tools/upload.sh b/Tools/upload.sh index d12a3c7e8b..383f1a9b9e 100755 --- a/Tools/upload.sh +++ b/Tools/upload.sh @@ -15,7 +15,7 @@ SERIAL_PORTS="/dev/tty.usbmodemPX*,/dev/tty.usbmodem*" fi if [ $SYSTYPE = "Linux" ]; then -SERIAL_PORTS="/dev/serial/by-id/*_PX4_*,/dev/serial/by-id/usb-3D_Robotics*,/dev/serial/by-id/usb-The_Autopilot*,/dev/serial/by-id/usb-Bitcraze*,/dev/serial/by-id/pci-Bitcraze*,/dev/serial/by-id/usb-Gumstix*," +SERIAL_PORTS="/dev/serial/by-id/*_PX4_*,/dev/serial/by-id/usb-3D_Robotics*,/dev/serial/by-id/usb-The_Autopilot*,/dev/serial/by-id/usb-Bitcraze*,/dev/serial/by-id/pci-Bitcraze*,/dev/serial/by-id/usb-Gumstix*,/dev/serial/by-id/usb-UVify_FMU_BL*," fi if [[ $SYSTYPE = *"CYGWIN"* ]]; then diff --git a/boards/px4/fmu-v4/nuttx-config/include/board.h b/boards/px4/fmu-v4/nuttx-config/include/board.h index 98d0ba6274..3c1a239dc4 100644 --- a/boards/px4/fmu-v4/nuttx-config/include/board.h +++ b/boards/px4/fmu-v4/nuttx-config/include/board.h @@ -53,7 +53,7 @@ ************************************************************************************/ /* Clocking *************************************************************************/ -/* The PX4FMUV2 uses a 24MHz crystal connected to the HSE. +/* The PX4FMUV4 uses a 24MHz crystal connected to the HSE. * * This is the "standard" configuration as set up by arch/arm/src/stm32f40xx_rcc.c: * System Clock source : PLL (HSE) diff --git a/boards/px4/fmu-v4/src/board_config.h b/boards/px4/fmu-v4/src/board_config.h index 86faaeca8c..a6fe9ee29c 100644 --- a/boards/px4/fmu-v4/src/board_config.h +++ b/boards/px4/fmu-v4/src/board_config.h @@ -34,7 +34,7 @@ /** * @file board_config.h * - * PX4FMUv2 internal definitions + * PX4FMUv4 internal definitions */ #pragma once diff --git a/boards/px4/fmu-v4/src/can.c b/boards/px4/fmu-v4/src/can.c index 7234c4eb8d..bb44863ea8 100644 --- a/boards/px4/fmu-v4/src/can.c +++ b/boards/px4/fmu-v4/src/can.c @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file px4fmu_can.c + * @file can.c * * Board-specific CAN functions. */ diff --git a/boards/px4/fmu-v4/src/init.c b/boards/px4/fmu-v4/src/init.c index 984932d663..61aa8f9ba0 100644 --- a/boards/px4/fmu-v4/src/init.c +++ b/boards/px4/fmu-v4/src/init.c @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file px4fmu_init.c + * @file init.c * * PX4FMU-specific early startup code. This file implements the * board_app_initialize() function that is called early by nsh during startup. diff --git a/boards/px4/fmu-v4/src/led.c b/boards/px4/fmu-v4/src/led.c index ddb34e8a56..12b5a51bed 100644 --- a/boards/px4/fmu-v4/src/led.c +++ b/boards/px4/fmu-v4/src/led.c @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file px4fmu2_led.c + * @file led.c * * PX4FMU LED backend. */ diff --git a/boards/px4/fmu-v4/src/timer_config.c b/boards/px4/fmu-v4/src/timer_config.c index c2a778b793..25363fd723 100644 --- a/boards/px4/fmu-v4/src/timer_config.c +++ b/boards/px4/fmu-v4/src/timer_config.c @@ -32,7 +32,7 @@ ****************************************************************************/ /* - * @file px4fmu_timer_config.c + * @file timer_config.c * * Configuration data for the stm32 pwm_servo, input capture and pwm input driver. * diff --git a/boards/px4/fmu-v4/src/usb.c b/boards/px4/fmu-v4/src/usb.c index ea49d0b521..ff68631ad6 100644 --- a/boards/px4/fmu-v4/src/usb.c +++ b/boards/px4/fmu-v4/src/usb.c @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file px4fmu_usb.c + * @file usb.c * * Board-specific USB functions. */ diff --git a/boards/uvify/core/default.cmake b/boards/uvify/core/default.cmake new file mode 100644 index 0000000000..6b50d55c88 --- /dev/null +++ b/boards/uvify/core/default.cmake @@ -0,0 +1,122 @@ + +px4_add_board( + PLATFORM nuttx + VENDOR uvify + MODEL core + LABEL default + TOOLCHAIN arm-none-eabi + ARCHITECTURE cortex-m4 + ROMFSROOT px4fmu_common + #TESTING + UAVCAN_INTERFACES 1 + + SERIAL_PORTS + GPS1:/dev/ttyS3 + TEL1:/dev/ttyS1 + TEL2:/dev/ttyS2 + + DRIVERS + #barometer # all available barometer drivers + barometer/ms5611 + batt_smbus + camera_capture + camera_trigger + #differential_pressure # all available differential pressure drivers + distance_sensor # all available distance sensor drivers + gps + #heater + #imu # all available imu drivers + imu/mpu6000 + imu/mpu9250 + irlock + #lights/blinkm + #lights/oreoled + #lights/rgbled + lights/rgbled_ncp5623c + #magnetometer # all available magnetometer drivers + magnetometer/bmm150 + magnetometer/lis3mdl + magnetometer/ist8310 + #mkblctrl + #optical_flow # all available optical flow drivers + optical_flow/px4flow + pca9685 + pwm_input + pwm_out_sim + px4fmu + rc_input + stm32 + stm32/adc + stm32/tone_alarm + #tap_esc + telemetry # all available telemetry drivers + #test_ppm + tone_alarm + uavcan + + MODULES + attitude_estimator_q + camera_feedback + commander + dataman + ekf2 + events + #fw_att_control + #fw_pos_control_l1 + #rover_pos_control + land_detector + landing_target_estimator + load_mon + local_position_estimator + logger + mavlink + mc_att_control + mc_pos_control + navigator + sensors + sih + vmount + #vtol_att_control + #airspeed_selector + + SYSTEMCMDS + bl_update + config + dmesg + dumpfile + esc_calib + hardfault_log + i2cdetect + led_control + mixer + motor_ramp + motor_test + mtd + nshterm + param + perf + pwm + reboot + reflect + sd_bench + shutdown + #tests # tests and test runner + top + topic_listener + tune_control + usb_connected + ver + + EXAMPLES + #bottle_drop # OBC challenge + #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control + #hello + #hwtest # Hardware test + #matlab_csv_serial + #px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html + #px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html + #rover_steering_control # Rover example app + #segway + #uuv_example_app + + ) diff --git a/boards/uvify/core/firmware.prototype b/boards/uvify/core/firmware.prototype new file mode 100644 index 0000000000..88971ff33e --- /dev/null +++ b/boards/uvify/core/firmware.prototype @@ -0,0 +1,13 @@ +{ + "board_id": 20, + "magic": "UVFYCORE", + "description": "Firmware for the UVify Core flight controller", + "image": "", + "build_time": 0, + "summary": "UVify Core", + "version": "0.1", + "image_size": 0, + "image_maxsize": 2080768, + "git_identity": "", + "board_revision": 0 +} diff --git a/boards/uvify/core/init/rc.board_defaults b/boards/uvify/core/init/rc.board_defaults new file mode 100644 index 0000000000..0d7410d11a --- /dev/null +++ b/boards/uvify/core/init/rc.board_defaults @@ -0,0 +1,13 @@ +#!/bin/sh +# +# UVify Core specific board defaults +#------------------------------------------------------------------------------ + + +if [ $AUTOCNF = yes ] +then + # Disable safety switch by default + param set CBRK_IO_SAFETY 22027 + +fi + diff --git a/boards/uvify/core/init/rc.board_extras b/boards/uvify/core/init/rc.board_extras new file mode 100644 index 0000000000..4625aa9e3a --- /dev/null +++ b/boards/uvify/core/init/rc.board_extras @@ -0,0 +1,18 @@ +#!/bin/sh +# +# UVify UVF4 specific board extras init +#------------------------------------------------------------------------------ + + +# IFO +if param compare SYS_AUTOSTART 4071 +then + # Change rate to 400 Khz for fast barometer + fmu i2c 1 400000 + + # IFO has only external i2c barometer. + # It does not start EKF2 in the beginning which is strange behaviour. but 3 seconds hack. + # We intentionally put this initialization to here for delayed initialization. + sleep 4 + ms5611 -T 0 -X start +fi diff --git a/boards/uvify/core/init/rc.board_sensors b/boards/uvify/core/init/rc.board_sensors new file mode 100644 index 0000000000..28440fe2fb --- /dev/null +++ b/boards/uvify/core/init/rc.board_sensors @@ -0,0 +1,38 @@ +#!/bin/sh +# +# UVify UVF4 specific board sensors init +#------------------------------------------------------------------------------ + +# Draco-R +if param compare SYS_AUTOSTART 6002 +then + # Barometer + # Internal SPI + ms5611 -T 0 -s start + + # PX4flow + px4flow start + ist8310 start + mpu6000 -R 2 -T 20608 start + mpu9250 -R 2 start + lis3mdl -R 2 -X start +fi + +# Draco +if param compare SYS_AUTOSTART 4072 +then + mpu9250 -R 2 start +fi + +# IFO +if param compare SYS_AUTOSTART 4071 +then + # IFO GPS LED + rgbled_ncp5623c start -a 0x38 + + # IFO rgb LED + pca9685 start + + mpu9250 -R 2 start + lis3mdl -R 2 -X start +fi diff --git a/boards/uvify/core/nuttx-config/include/board.h b/boards/uvify/core/nuttx-config/include/board.h new file mode 100644 index 0000000000..aa158ba6a2 --- /dev/null +++ b/boards/uvify/core/nuttx-config/include/board.h @@ -0,0 +1,346 @@ +/************************************************************************************ + * configs/px4fmu/include/board.h + * include/arch/board/board.h + * + * Copyright (C) 2009 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. + * + ************************************************************************************/ + +#ifndef __ARCH_BOARD_BOARD_H +#define __ARCH_BOARD_BOARD_H + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include +#ifndef __ASSEMBLY__ +# include +#endif + +#include + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/* Clocking *************************************************************************/ +/* The UVify Core uses a 24MHz crystal connected to the HSE. + * + * This is the "standard" configuration as set up by arch/arm/src/stm32f40xx_rcc.c: + * System Clock source : PLL (HSE) + * SYSCLK(Hz) : 168000000 Determined by PLL configuration + * HCLK(Hz) : 168000000 (STM32_RCC_CFGR_HPRE) + * AHB Prescaler : 1 (STM32_RCC_CFGR_HPRE) + * APB1 Prescaler : 4 (STM32_RCC_CFGR_PPRE1) + * APB2 Prescaler : 2 (STM32_RCC_CFGR_PPRE2) + * HSE Frequency(Hz) : 24000000 (STM32_BOARD_XTAL) + * PLLM : 24 (STM32_PLLCFG_PLLM) + * PLLN : 336 (STM32_PLLCFG_PLLN) + * PLLP : 2 (STM32_PLLCFG_PLLP) + * PLLQ : 7 (STM32_PLLCFG_PPQ) + * Main regulator output voltage : Scale1 mode Needed for high speed SYSCLK + * Flash Latency(WS) : 5 + * Prefetch Buffer : OFF + * Instruction cache : ON + * Data cache : ON + * Require 48MHz for USB OTG FS, : Enabled + * SDIO and RNG clock + */ + +/* HSI - 16 MHz RC factory-trimmed + * LSI - 32 KHz RC + * HSE - On-board crystal frequency is 24MHz + * LSE - not installed + */ + +#define STM32_BOARD_XTAL 24000000ul + +#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 + * PLL_VCO = (STM32_HSE_FREQUENCY / PLLM) * PLLN + * = (25,000,000 / 25) * 336 + * = 336,000,000 + * SYSCLK = PLL_VCO / PLLP + * = 336,000,000 / 2 = 168,000,000 + * USB OTG FS, SDIO and RNG Clock + * = PLL_VCO / PLLQ + * = 48,000,000 + */ + +#define STM32_PLLCFG_PLLM RCC_PLLCFG_PLLM(24) +#define STM32_PLLCFG_PLLN RCC_PLLCFG_PLLN(336) +#define STM32_PLLCFG_PLLP RCC_PLLCFG_PLLP_2 +#define STM32_PLLCFG_PLLQ RCC_PLLCFG_PLLQ(7) + +#define STM32_SYSCLK_FREQUENCY 168000000ul + +/* AHB clock (HCLK) is SYSCLK (168MHz) */ + +#define STM32_RCC_CFGR_HPRE RCC_CFGR_HPRE_SYSCLK /* HCLK = SYSCLK / 1 */ +#define STM32_HCLK_FREQUENCY STM32_SYSCLK_FREQUENCY +#define STM32_BOARD_HCLK STM32_HCLK_FREQUENCY /* same as above, to satisfy compiler */ + +/* APB1 clock (PCLK1) is HCLK/4 (42MHz) */ + +#define STM32_RCC_CFGR_PPRE1 RCC_CFGR_PPRE1_HCLKd4 /* PCLK1 = HCLK / 4 */ +#define STM32_PCLK1_FREQUENCY (STM32_HCLK_FREQUENCY/4) + +/* 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) + +/* APB2 clock (PCLK2) is HCLK/2 (84MHz) */ + +#define STM32_RCC_CFGR_PPRE2 RCC_CFGR_PPRE2_HCLKd2 /* PCLK2 = HCLK / 2 */ +#define STM32_PCLK2_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* 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_TIM9_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM10_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM11_CLKIN (2*STM32_PCLK2_FREQUENCY) + +/* Timer Frequencies, if APBx is set to 1, frequency is same to APBx + * otherwise frequency is 2xAPBx. + * Note: TIM1,8-11 are on APB2, others on APB1 + */ + +#define BOARD_TIM1_FREQUENCY STM32_APB2_TIM1_CLKIN +#define BOARD_TIM2_FREQUENCY STM32_APB1_TIM2_CLKIN +#define BOARD_TIM3_FREQUENCY STM32_APB1_TIM3_CLKIN +#define BOARD_TIM4_FREQUENCY STM32_APB1_TIM4_CLKIN +#define BOARD_TIM5_FREQUENCY STM32_APB1_TIM5_CLKIN +#define BOARD_TIM6_FREQUENCY STM32_APB1_TIM6_CLKIN +#define BOARD_TIM7_FREQUENCY STM32_APB1_TIM7_CLKIN +#define BOARD_TIM8_FREQUENCY STM32_APB2_TIM8_CLKIN +#define BOARD_TIM9_FREQUENCY STM32_APB2_TIM9_CLKIN +#define BOARD_TIM10_FREQUENCY STM32_APB2_TIM10_CLKIN +#define BOARD_TIM11_FREQUENCY STM32_APB2_TIM11_CLKIN +#define BOARD_TIM12_FREQUENCY STM32_APB1_TIM12_CLKIN +#define BOARD_TIM13_FREQUENCY STM32_APB1_TIM13_CLKIN +#define BOARD_TIM14_FREQUENCY STM32_APB1_TIM14_CLKIN + +/* SDIO dividers. Note that slower clocking is required when DMA is disabled + * in order to avoid RX overrun/TX underrun errors due to delayed responses + * to service FIFOs in interrupt driven mode. These values have not been + * tuned!!! + * + * SDIOCLK=48MHz, SDIO_CK=SDIOCLK/(118+2)=400 KHz + */ + +#define SDIO_INIT_CLKDIV (118 << SDIO_CLKCR_CLKDIV_SHIFT) + +/* DMA ON: SDIOCLK=48MHz, SDIO_CK=SDIOCLK/(1+2)=16 MHz + * DMA OFF: SDIOCLK=48MHz, SDIO_CK=SDIOCLK/(2+2)=12 MHz + */ + +#ifdef CONFIG_STM32_SDIO_DMA +# define SDIO_MMCXFR_CLKDIV (1 << SDIO_CLKCR_CLKDIV_SHIFT) +#else +# define SDIO_MMCXFR_CLKDIV (2 << SDIO_CLKCR_CLKDIV_SHIFT) +#endif + +/* DMA ON: SDIOCLK=48MHz, SDIO_CK=SDIOCLK/(1+2)=16 MHz + * DMA OFF: SDIOCLK=48MHz, SDIO_CK=SDIOCLK/(2+2)=12 MHz + */ + +#ifdef CONFIG_STM32_SDIO_DMA +# define SDIO_SDXFR_CLKDIV (1 << SDIO_CLKCR_CLKDIV_SHIFT) +#else +# define SDIO_SDXFR_CLKDIV (2 << SDIO_CLKCR_CLKDIV_SHIFT) +#endif + +/* DMA Channl/Stream Selections *****************************************************/ +/* Stream selections are arbitrary for now but might become important in the future + * is we set aside more DMA channels/streams. + * + * SDIO DMA + *   DMAMAP_SDIO_1 = Channel 4, Stream 3 <- may later be used by SPI DMA + *   DMAMAP_SDIO_2 = Channel 4, Stream 6 + */ + +#define DMAMAP_SDIO DMAMAP_SDIO_1 + +/* Alternate function pin selections ************************************************/ + +/* + * UARTs. + */ +#define GPIO_USART1_RX GPIO_USART1_RX_2 /* ESP8266 */ +#define GPIO_USART1_TX GPIO_USART1_TX_2 + +#define GPIO_USART2_RX GPIO_USART2_RX_2 +#define GPIO_USART2_TX GPIO_USART2_TX_2 +#define GPIO_USART2_RTS GPIO_USART2_RTS_2 +#define GPIO_USART2_CTS GPIO_USART2_CTS_2 + +#define GPIO_USART3_RX GPIO_USART3_RX_3 +#define GPIO_USART3_TX GPIO_USART3_TX_3 +#define GPIO_USART3_RTS GPIO_USART3_RTS_2 +#define GPIO_USART3_CTS GPIO_USART3_CTS_2 + +#define GPIO_UART4_RX GPIO_UART4_RX_1 +#define GPIO_UART4_TX GPIO_UART4_TX_1 + +#define GPIO_USART6_RX GPIO_USART6_RX_1 /* RC_INPUT */ +#define GPIO_USART6_TX GPIO_USART6_TX_1 + +#define GPIO_UART7_RX GPIO_UART7_RX_1 +#define GPIO_UART7_TX GPIO_UART7_TX_1 + +/* UART8 has no alternate pin config */ + +/* UART RX DMA configurations */ +#define DMAMAP_USART1_RX DMAMAP_USART1_RX_2 +#define DMAMAP_USART6_RX DMAMAP_USART6_RX_2 + +/* + * CAN + * + * CAN1 is routed to the onboard transceiver. + */ +#define GPIO_CAN1_RX GPIO_CAN1_RX_3 +#define GPIO_CAN1_TX GPIO_CAN1_TX_3 + +/* + * 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 +#define GPIO_I2C1_SDA GPIO_I2C1_SDA_2 +#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_PIN9) + + +/* + * SPI + * + * There are sensors on SPI1, and SPI2 is connected to the FRAM. + */ +#define GPIO_SPI1_MISO (GPIO_SPI1_MISO_1|GPIO_SPEED_50MHz) +#define GPIO_SPI1_MOSI (GPIO_SPI1_MOSI_1|GPIO_SPEED_50MHz) +#define GPIO_SPI1_SCK (GPIO_SPI1_SCK_1|GPIO_SPEED_50MHz) + +#define GPIO_SPI2_MISO (GPIO_SPI2_MISO_1|GPIO_SPEED_50MHz) +#define GPIO_SPI2_MOSI (GPIO_SPI2_MOSI_1|GPIO_SPEED_50MHz) +#define GPIO_SPI2_SCK (GPIO_SPI2_SCK_1|GPIO_SPEED_50MHz) + +#if defined(CONFIG_STM32_SPI4) +# define GPIO_SPI4_MISO (GPIO_SPI4_MISO_1|GPIO_SPEED_50MHz) +# define GPIO_SPI4_MOSI (GPIO_SPI4_MOSI_1|GPIO_SPEED_50MHz) +# define GPIO_SPI4_SCK (GPIO_SPI4_SCK_1|GPIO_SPEED_50MHz) +#endif + +/* Board provides GPIO or other Hardware for signaling to timing analyzer */ + +#if defined(CONFIG_BOARD_USE_PROBES) +# define PROBE_N(n) (1<<((n)-1)) +# define PROBE_1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN14) +# define PROBE_2 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN13) +# define PROBE_3 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN11) +# define PROBE_4 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN9) +# define PROBE_5 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN13) +# define PROBE_6 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN14) + +# 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); } \ + } 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 + +/************************************************************************************ + * Public Data + ************************************************************************************/ + +#ifndef __ASSEMBLY__ + +#undef EXTERN +#if defined(__cplusplus) +#define EXTERN extern "C" +extern "C" { +#else +#define EXTERN extern +#endif + +/************************************************************************************ + * Public Function Prototypes + ************************************************************************************/ +/************************************************************************************ + * 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. + * + ************************************************************************************/ + +EXTERN void stm32_boardinitialize(void); + +#undef EXTERN +#if defined(__cplusplus) +} +#endif + +#endif /* __ASSEMBLY__ */ +#endif /* __ARCH_BOARD_BOARD_H */ diff --git a/boards/uvify/core/nuttx-config/nsh/defconfig b/boards/uvify/core/nuttx-config/nsh/defconfig new file mode 100644 index 0000000000..39a594e1f3 --- /dev/null +++ b/boards/uvify/core/nuttx-config/nsh/defconfig @@ -0,0 +1,247 @@ +# +# 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_OS_API 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_SEMICOLON is not set +# CONFIG_NSH_DISABLE_TIME is not set +# CONFIG_STM32_CCMEXCLUDE is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP_STM32=y +CONFIG_ARCH_CHIP_STM32F427V=y +CONFIG_ARCH_INTERRUPTSTACK=512 +CONFIG_ARCH_MATH_H=y +CONFIG_ARCH_STACKDUMP=y +CONFIG_ARMV7M_MEMCPY=y +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_BOARDCTL_RESET=y +CONFIG_BOARD_CRASHDUMP=y +CONFIG_BOARD_LOOPSPERMSEC=16717 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_BUILTIN=y +CONFIG_C99_BOOL8=y +CONFIG_CDCACM=y +CONFIG_CDCACM_PRODUCTID=0x0012 +CONFIG_CDCACM_PRODUCTSTR="Core" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=2000 +CONFIG_CDCACM_VENDORID=0x26ac +CONFIG_CDCACM_VENDORSTR="UVify" +CONFIG_CLOCK_MONOTONIC=y +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_HARDFAULT_ALERT=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_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_EXCLUDE_BLOCKS=y +CONFIG_FS_PROCFS_EXCLUDE_MOUNT=y +CONFIG_FS_PROCFS_EXCLUDE_MOUNTS=y +CONFIG_FS_PROCFS_EXCLUDE_PARTITIONS=y +CONFIG_FS_PROCFS_EXCLUDE_USAGE=y +CONFIG_FS_PROCFS_REGISTER=y +CONFIG_FS_ROMFS=y +CONFIG_GRAN=y +CONFIG_GRAN_INTR=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +CONFIG_I2C=y +CONFIG_I2C_RESET=y +CONFIG_IDLETHREAD_STACKSIZE=750 +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_STRERROR=y +CONFIG_MAX_TASKS=64 +CONFIG_MAX_WDOGPARMS=2 +CONFIG_MEMSET_64BIT=y +CONFIG_MEMSET_OPTSPEED=y +CONFIG_MMCSD=y +CONFIG_MMCSD_MULTIBLOCK_DISABLE=y +CONFIG_MMCSD_SDIO=y +CONFIG_MMCSD_SDIOWAIT_WRCOMPLETE=y +CONFIG_MM_REGIONS=2 +CONFIG_MTD=y +CONFIG_MTD_BYTE_WRITE=y +CONFIG_MTD_PARTITION=y +CONFIG_MTD_RAMTRON=y +CONFIG_NFILE_DESCRIPTORS=20 +CONFIG_NFILE_STREAMS=8 +CONFIG_NSH_ARCHINIT=y +CONFIG_NSH_ARCHROMFS=y +CONFIG_NSH_ARGCAT=y +CONFIG_NSH_BUILTIN_APPS=y +CONFIG_NSH_CMDPARMS=y +CONFIG_NSH_CROMFSETC=y +CONFIG_NSH_DISABLE_IFCONFIG=y +CONFIG_NSH_DISABLE_IFUPDOWN=y +CONFIG_NSH_DISABLE_MB=y +CONFIG_NSH_DISABLE_MH=y +CONFIG_NSH_DISABLE_TELNETD=y +CONFIG_NSH_LINELEN=128 +CONFIG_NSH_MAXARGUMENTS=12 +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_NXFONTS_DISABLE_16BPP=y +CONFIG_NXFONTS_DISABLE_1BPP=y +CONFIG_NXFONTS_DISABLE_24BPP=y +CONFIG_NXFONTS_DISABLE_2BPP=y +CONFIG_NXFONTS_DISABLE_32BPP=y +CONFIG_NXFONTS_DISABLE_4BPP=y +CONFIG_NXFONTS_DISABLE_8BPP=y +CONFIG_PIPES=y +CONFIG_PREALLOC_MQ_MSGS=4 +CONFIG_PREALLOC_TIMERS=50 +CONFIG_PREALLOC_WDOGS=50 +CONFIG_PRIORITY_INHERITANCE=y +CONFIG_PTHREAD_MUTEX_ROBUST=y +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_SETSPEED=y +CONFIG_RAMTRON_WRITEWAIT=y +CONFIG_RAM_SIZE=262144 +CONFIG_RAM_START=0x20000000 +CONFIG_RAW_BINARY=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_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKSTACKSIZE=1536 +CONFIG_SCHED_WAITPID=y +CONFIG_SDCLONE_DISABLE=y +CONFIG_SDIO_BLOCKSETUP=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=32 +CONFIG_STM32_ADC1=y +CONFIG_STM32_BBSRAM=y +CONFIG_STM32_BBSRAM_FILES=5 +CONFIG_STM32_BKPSRAM=y +CONFIG_STM32_CCMDATARAM=y +CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y +CONFIG_STM32_DMA1=y +CONFIG_STM32_DMA2=y +CONFIG_STM32_FLASH_CONFIG_I=y +CONFIG_STM32_FLOWCONTROL_BROKEN=y +CONFIG_STM32_I2C1=y +CONFIG_STM32_I2CTIMEOMS=10 +CONFIG_STM32_I2CTIMEOTICKS=10 +CONFIG_STM32_JTAG_SW_ENABLE=y +CONFIG_STM32_OTGFS=y +CONFIG_STM32_PWR=y +CONFIG_STM32_RTC=y +CONFIG_STM32_RTC_HSECLOCK=y +CONFIG_STM32_RTC_MAGIC=0xfacefeee +CONFIG_STM32_RTC_MAGIC_REG=1 +CONFIG_STM32_RTC_MAGIC_TIME_SET=0xfacefeef +CONFIG_STM32_SAVE_CRASHDUMP=y +CONFIG_STM32_SDIO=y +CONFIG_STM32_SDIO_CARD=y +CONFIG_STM32_SERIALBRK_BSDCOMPAT=y +CONFIG_STM32_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32_SPI1=y +CONFIG_STM32_SPI2=y +CONFIG_STM32_SPI4=y +CONFIG_STM32_TIM10=y +CONFIG_STM32_TIM11=y +CONFIG_STM32_TIM1=y +CONFIG_STM32_TIM4=y +CONFIG_STM32_TIM8=y +CONFIG_STM32_TIM9=y +CONFIG_STM32_UART4=y +CONFIG_STM32_UART7=y +CONFIG_STM32_UART8=y +CONFIG_STM32_USART1=y +CONFIG_STM32_USART2=y +CONFIG_STM32_USART3=y +CONFIG_STM32_USART6=y +CONFIG_STM32_USART_BREAKS=y +CONFIG_STM32_USART_SINGLEWIRE=y +CONFIG_STM32_WWDG=y +CONFIG_SYSTEM_CDCACM=y +CONFIG_SYSTEM_NSH=y +CONFIG_TASK_NAME_SIZE=24 +CONFIG_TIME_EXTENDED=y +CONFIG_UART4_BAUD=57600 +CONFIG_UART4_RXBUFSIZE=300 +CONFIG_UART4_RXDMA=y +CONFIG_UART4_TXBUFSIZE=300 +CONFIG_UART7_BAUD=57600 +CONFIG_UART7_RXBUFSIZE=300 +CONFIG_UART7_RXDMA=y +CONFIG_UART7_SERIAL_CONSOLE=y +CONFIG_UART7_TXBUFSIZE=300 +CONFIG_UART8_BAUD=57600 +CONFIG_UART8_RXBUFSIZE=300 +CONFIG_UART8_RXDMA=y +CONFIG_UART8_TXBUFSIZE=300 +CONFIG_USART1_RXBUFSIZE=600 +CONFIG_USART1_RXDMA=y +CONFIG_USART1_TXBUFSIZE=2500 +CONFIG_USART2_BAUD=57600 +CONFIG_USART2_IFLOWCONTROL=y +CONFIG_USART2_OFLOWCONTROL=y +CONFIG_USART2_RXBUFSIZE=600 +CONFIG_USART2_RXDMA=y +CONFIG_USART2_TXBUFSIZE=1100 +CONFIG_USART3_BAUD=57600 +CONFIG_USART3_IFLOWCONTROL=y +CONFIG_USART3_OFLOWCONTROL=y +CONFIG_USART3_RXBUFSIZE=1200 +CONFIG_USART3_RXDMA=y +CONFIG_USART3_TXBUFSIZE=900 +CONFIG_USART6_BAUD=57600 +CONFIG_USART6_RXBUFSIZE=300 +CONFIG_USART6_RXDMA=y +CONFIG_USART6_TXBUFSIZE=300 +CONFIG_USBDEV=y +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_MAXPOWER=500 +CONFIG_USEC_PER_TICK=1000 +CONFIG_USERMAIN_STACKSIZE=2624 +CONFIG_USER_ENTRYPOINT="nsh_main" diff --git a/boards/uvify/core/nuttx-config/scripts/script.ld b/boards/uvify/core/nuttx-config/scripts/script.ld new file mode 100644 index 0000000000..0d19229c01 --- /dev/null +++ b/boards/uvify/core/nuttx-config/scripts/script.ld @@ -0,0 +1,159 @@ +/**************************************************************************** + * scripts/ld.script + * + * Copyright (C) 2011 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 STM32F427 has 2048Kb of FLASH beginning at address 0x0800:0000 and + * 256Kb of SRAM. SRAM is split up into three blocks: + * + * 1) 112Kb of SRAM beginning at address 0x2000:0000 + * 2) 16Kb of SRAM beginning at address 0x2001:c000 + * 3) 64Kb of SRAM beginning at address 0x2002:0000 + * 4) 64Kb of TCM SRAM beginning at address 0x1000: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 first 0x4000 of flash is reserved for the bootloader. + */ + +MEMORY +{ + flash (rx) : ORIGIN = 0x08004000, LENGTH = 2032K + sram (rwx) : ORIGIN = 0x20000000, LENGTH = 192K + ccsram (rwx) : ORIGIN = 0x10000000, LENGTH = 64K +} + +OUTPUT_ARCH(arm) + +ENTRY(__start) /* treat __start as the anchor for dead code stripping */ +EXTERN(_vectors) /* force the vectors to be included in the output */ + +/* + * 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.*) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + + /* + * This is a hack to make the newlib libm __errno() call + * use the NuttX get_errno_ptr() function. + */ + __errno = get_errno_ptr; + } > flash + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > flash + + /* + * Construction data for parameters. + */ + __param ALIGN(4): { + __param_start = ABSOLUTE(.); + KEEP(*(__param*)) + __param_end = 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/uvify/core/src/CMakeLists.txt b/boards/uvify/core/src/CMakeLists.txt new file mode 100644 index 0000000000..481e75cdcd --- /dev/null +++ b/boards/uvify/core/src/CMakeLists.txt @@ -0,0 +1,49 @@ +############################################################################ +# +# Copyright (c) 2019 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +add_library(drivers_board + can.c + init.c + led.c + spi.c + timer_config.c + usb.c +) + +target_link_libraries(drivers_board + PRIVATE + drivers__led # drv_led_start + nuttx_arch # sdio + nuttx_drivers # sdio + px4_layer +) diff --git a/boards/uvify/core/src/board_config.h b/boards/uvify/core/src/board_config.h new file mode 100644 index 0000000000..cb358e1310 --- /dev/null +++ b/boards/uvify/core/src/board_config.h @@ -0,0 +1,389 @@ +/**************************************************************************** + * + * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file board_config.h + * + * UVify Core internal definitions + */ + +#pragma once + +/**************************************************************************************************** + * Included Files + ****************************************************************************************************/ + +#include +#include +#include + +/**************************************************************************************************** + * Definitions + ****************************************************************************************************/ +/* Configuration ************************************************************************************/ + +/* PX4FMU GPIOs ***********************************************************************************/ +/* LEDs */ +#define GPIO_LED1 (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN11) +#define GPIO_LED2 (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN1) +#define GPIO_LED3 (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN3) + +#define GPIO_LED_RED GPIO_LED1 +#define GPIO_LED_GREEN GPIO_LED2 +#define GPIO_LED_BLUE GPIO_LED3 + +#define BOARD_HAS_CONTROL_STATUS_LEDS 1 +#define BOARD_OVERLOAD_LED LED_RED +#define BOARD_ARMED_LED LED_BLUE +#define BOARD_ARMED_STATE_LED LED_GREEN + +/** + * The MPU9250 is default. The wrong driver will fail during start because of an incorrect WHO_AM_I register. + */ +#define GPIO_SPI1_CS_PORTC_PIN2 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN2) + +/** + * The ICM20608G is default. The wrong driver will fail during start because of an incorrect WHO_AM_I register. + */ +#define GPIO_SPI1_CS_PORTC_PIN15 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15) + +/** + * Reserved. The wrong driver will fail during start because of an incorrect WHO_AM_I register. + */ +#define GPIO_SPI1_CS_PORTE_PIN15 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN15) + +/* Define the Data Ready interrupts On SPI 1. */ +#define GPIO_DRDY_PORTD_PIN15 (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN15) +#define GPIO_DRDY_PORTC_PIN14 (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTC|GPIO_PIN14) +#define GPIO_DRDY_PORTE_PIN12 (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTE|GPIO_PIN12) + + +/* Define the Chip Selects for SPI2. */ +#define GPIO_SPI2_CS_MS5611 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN7) +#define GPIO_SPI2_CS_FRAM (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10) + +/* Define the Chip Selects for SPI4. */ + +#ifdef CONFIG_STM32_SPI4 +# define BOARD_HAS_BUS_MANIFEST 1 // We support a bus manifest because spi 4 is optional +# define GPIO_SPI4_CS_1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN8) //ESP_RTS_PIN +#endif /* CONFIG_STM32_SPI4 */ +/** + * Define the ability to shut off off the sensor signals + * by changing the signals to inputs. + */ +#define _PIN_OFF(def) (((def) & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz)) + +/* SPI 1 bus off. */ +#define GPIO_SPI1_SCK_OFF _PIN_OFF(GPIO_SPI1_SCK) +#define GPIO_SPI1_MISO_OFF _PIN_OFF(GPIO_SPI1_MISO) +#define GPIO_SPI1_MOSI_OFF _PIN_OFF(GPIO_SPI1_MOSI) + +/* SPI 1 CS's off. */ +#define GPIO_SPI1_CS_OFF_PORTC_PIN2 _PIN_OFF(GPIO_SPI1_CS_PORTC_PIN2) +#define GPIO_SPI1_CS_OFF_PORTC_PIN15 _PIN_OFF(GPIO_SPI1_CS_PORTC_PIN15) +#define GPIO_SPI1_CS_OFF_PORTE_PIN15 _PIN_OFF(GPIO_SPI1_CS_PORTE_PIN15) + +/* SPI 1 DRDY's off. */ +#define GPIO_DRDY_OFF_PORTD_PIN15 _PIN_OFF(GPIO_DRDY_PORTD_PIN15) +#define GPIO_DRDY_OFF_PORTC_PIN14 _PIN_OFF(GPIO_DRDY_PORTC_PIN14) +#define GPIO_DRDY_OFF_PORTE_PIN12 _PIN_OFF(GPIO_DRDY_PORTE_PIN12) + +/* SPI 4 bus off. */ +#ifdef CONFIG_STM32_SPI4 +# define GPIO_SPI4_SCK_OFF _PIN_OFF(GPIO_SPI4_SCK) +# define GPIO_SPI4_MISO_OFF _PIN_OFF(GPIO_SPI4_MISO) +# define GPIO_SPI4_MOSI_OFF _PIN_OFF(GPIO_SPI4_MOSI) +#endif /* CONFIG_STM32_SPI4 */ + +/** + * N.B we do not have control over the SPI 2 buss powered devices + * so the the ms5611 is not resetable. + * + */ + +#define PX4_SPI_BUS_SENSORS 1 +#define PX4_SPI_BUS_RAMTRON 2 +#define PX4_SPI_BUS_BARO PX4_SPI_BUS_RAMTRON + +#ifdef CONFIG_STM32_SPI4 +# define PX4_SPI_BUS_EXTERNAL 4 +/* The mask passes to init the SPI bus pins + * N.B This works ONLY with buss numbers that are powers of 2 + * Adding SPI3 would break this! + */ +# define SPI_BUS_INIT_MASK_EXT PX4_SPI_BUS_EXTERNAL +#endif /* CONFIG_STM32_SPI4 */ + +#define SPI_BUS_INIT_MASK (PX4_SPI_BUS_RAMTRON | PX4_SPI_BUS_SENSORS) + +/* Use these in place of the uint32_t enumeration to select a specific SPI device on SPI1 */ +#define PX4_SPIDEV_GYRO PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 1) +#define PX4_SPIDEV_ACCEL_MAG PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 2) +#define PX4_SPIDEV_MPU PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 4) +#define PX4_SPIDEV_HMC PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 5) +#define PX4_SPIDEV_ICM PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 6) +#define PX4_SPIDEV_LIS PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 7) +#define PX4_SPIDEV_BMI PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 8) +#define PX4_SPIDEV_BMA PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 9) +#define PX4_SPIDEV_ICM_20608 PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 10) +#define PX4_SPIDEV_ICM_20602 PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 11) +#define PX4_SPIDEV_BMI055_ACC PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 12) +#define PX4_SPIDEV_BMI055_GYR PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 13) +#define PX4_SPIDEV_MPU2 PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 14) + +/** + * Onboard MS5611 and FRAM are both on bus SPI2. + * spi_dev_e:SPIDEV_FLASH has the value 2 and is used in the NuttX ramtron driver. + * PX4_MK_SPI_SEL differentiate by adding in PX4_SPI_DEVICE_ID. + */ +#define PX4_SPIDEV_BARO PX4_MK_SPI_SEL(PX4_SPI_BUS_BARO, 3) + +#ifdef CONFIG_STM32_SPI4 +# define PX4_SPIDEV_EXTERNAL PX4_MK_SPI_SEL(PX4_SPI_BUS_EXTERNAL, 1) +#endif /* CONFIG_STM32_SPI4 */ + +/* I2C busses. */ +#define PX4_I2C_BUS_EXPANSION 1 +#define PX4_I2C_BUS_LED PX4_I2C_BUS_EXPANSION + +/** + * Devices on the external bus. + * Note that these are unshifted addresses. + */ +#define PX4_I2C_OBDEV_BMP280 0x76 + +/** + * 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. + */ +#define ADC_CHANNELS (1 << 2) | (1 << 3) | (1 << 4) | (1 << 10) | (1 << 11) | (1 << 12) | (1 << 13) | (1 << 14) + +/* ADC defines to be used in sensors.cpp to read from a particular channel. */ +#define ADC_BATTERY_VOLTAGE_CHANNEL 2 +#define ADC_BATTERY_CURRENT_CHANNEL 3 +#define ADC_5V_RAIL_SENSE 4 +#define ADC_RC_RSSI_CHANNEL 11 + +/* Define Battery 1 Voltage Divider and A per V. */ +#define BOARD_BATTERY1_V_DIV (10.14f) +#define BOARD_BATTERY1_A_PER_V (18.18f) + + +/** + * User GPIOs: + * GPIO0-5 are the PWM servo outputs. + */ +#define GPIO_GPIO0_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN14) +#define GPIO_GPIO1_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN13) +#define GPIO_GPIO2_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN11) +#define GPIO_GPIO3_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN9) +#define GPIO_GPIO4_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN13) +#define GPIO_GPIO5_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN14) + +#define GPIO_GPIO0_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN14) +#define GPIO_GPIO1_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN13) +#define GPIO_GPIO2_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN11) +#define GPIO_GPIO3_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN9) +#define GPIO_GPIO4_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN13) +#define GPIO_GPIO5_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN14) + +/* Power supply control and monitoring GPIOs. */ +#define GPIO_VDD_BRICK_VALID (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5) +#define GPIO_VDD_USB_VALID (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN0) +#define GPIO_VDD_3V3_SENSORS_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN3) + +/* Tone alarm output. */ +#define TONE_ALARM_TIMER 2 /* timer 2 */ +#define TONE_ALARM_CHANNEL 1 /* channel 1 */ +#define GPIO_TONE_ALARM_IDLE (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN15) +#define GPIO_TONE_ALARM (GPIO_ALT|GPIO_AF1|GPIO_SPEED_2MHz|GPIO_PUSHPULL|GPIO_PORTA|GPIO_PIN15) + +/** + * PWM: + * + * Six PWM outputs are configured. + * + * Pins: + * + * CH1 : PE14 : TIM1_CH4 + * CH2 : PE13 : TIM1_CH3 + * CH3 : PE11 : TIM1_CH2 + * CH4 : PE9 : TIM1_CH1 + * CH5 : PD13 : TIM4_CH2 + * CH6 : PD14 : TIM4_CH3 + */ + +/** + * N.B. the added pull down, on the timer being disabled the PD + * will keep the channel low + */ +#define GPIO_TIM1_CH1OUT (GPIO_ALT|GPIO_AF1|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PUSHPULL|GPIO_PULLDOWN|GPIO_PORTE|GPIO_PIN9) +#define GPIO_TIM1_CH2OUT (GPIO_ALT|GPIO_AF1|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PUSHPULL|GPIO_PULLDOWN|GPIO_PORTE|GPIO_PIN11) +#define GPIO_TIM1_CH3OUT (GPIO_ALT|GPIO_AF1|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PUSHPULL|GPIO_PULLDOWN|GPIO_PORTE|GPIO_PIN13) +#define GPIO_TIM1_CH4OUT (GPIO_ALT|GPIO_AF1|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PUSHPULL|GPIO_PULLDOWN|GPIO_PORTE|GPIO_PIN14) +#define GPIO_TIM4_CH2OUT (GPIO_ALT|GPIO_AF2|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PUSHPULL|GPIO_PULLDOWN|GPIO_PORTD|GPIO_PIN13) +#define GPIO_TIM4_CH3OUT (GPIO_ALT|GPIO_AF2|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PUSHPULL|GPIO_PULLDOWN|GPIO_PORTD|GPIO_PIN14) +#define DIRECT_PWM_OUTPUT_CHANNELS 6 + +#define GPIO_TIM1_CH1IN GPIO_TIM1_CH1IN_2 +#define GPIO_TIM1_CH2IN GPIO_TIM1_CH2IN_2 +#define GPIO_TIM1_CH3IN GPIO_TIM1_CH3IN_2 +#define GPIO_TIM1_CH4IN GPIO_TIM1_CH4IN_2 +#define GPIO_TIM4_CH2IN GPIO_TIM4_CH2IN_2 +#define GPIO_TIM4_CH3IN GPIO_TIM4_CH3IN_2 +#define DIRECT_INPUT_TIMER_CHANNELS 6 + +/** + * USB OTG FS: + * PA9 OTG_FS_VBUS VBUS sensing. + */ +#define GPIO_OTGFS_VBUS (GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTA|GPIO_PIN9) + +/* High-resolution timer */ +#define HRT_TIMER 3 /* use timer 3 for the HRT */ +#define HRT_TIMER_CHANNEL 4 /* use capture/compare channel 4 */ + +#define HRT_PPM_CHANNEL 3 /* use capture/compare channel 3 */ +#define GPIO_PPM_IN (GPIO_ALT|GPIO_AF2|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN0) + +/* RC Serial port */ + +#define RC_SERIAL_PORT "/dev/ttyS4" + +/* PWM input driver. Use FMU AUX5 pins attached to timer4 channel 2. */ +#define PWMIN_TIMER 4 +#define PWMIN_TIMER_CHANNEL 2 +#define GPIO_PWM_IN GPIO_TIM4_CH2IN_2 + +#define GPIO_RSSI_IN (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN1) +#define GPIO_LED_SAFETY (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN3) +#define GPIO_BTN_SAFETY (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN4) +#define GPIO_PERIPH_3V3_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN5) + +/* For,this signal is active high. */ +#define GPIO_SBUS_INV (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) +#define RC_INVERT_INPUT(_invert_true) px4_arch_gpiowrite(GPIO_SBUS_INV, _invert_true) + +#define GPIO_SPEKTRUM_PWR_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN4) + +/* GPIOs */ +#define GPIO_PE2 (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN2) +#define GPIO_PB4 (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN4) +#define GPIO_PE5 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN5) +#define GPIO_PE6 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN6) + +/* Heater pins (reserved) */ +#define GPIO_HEATER_INPUT (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTC|GPIO_PIN6) +#define GPIO_HEATER_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN6) + +/* Power switch controls */ + +#define SPEKTRUM_POWER(_on_true) px4_arch_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (!_on_true)) + +/** + * UVify Core has separate RC_IN + * + * GPIO PPM_IN on PB0 T3C3 + * SPEKTRUM_RX (it's TX or RX in Bind) on UART6 PC7 + * Inversion is possible via the 74LVC2G86 controlled by the FMU + * The FMU can drive GPIO PPM_IN as an output + */ + +#define GPIO_PPM_IN_AS_OUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN0) +#define SPEKTRUM_RX_AS_GPIO_OUTPUT() px4_arch_configgpio(GPIO_PPM_IN_AS_OUT) +#define SPEKTRUM_RX_AS_UART() /* Can be left as uart */ +#define SPEKTRUM_OUT(_one_true) px4_arch_gpiowrite(GPIO_PPM_IN_AS_OUT, (_one_true)) + +/** + * 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_BRICK_VALID (px4_arch_gpioread(GPIO_VDD_BRICK_VALID)) +#define BOARD_ADC_USB_VALID (px4_arch_gpioread(GPIO_VDD_USB_VALID)) +#define BOARD_ADC_SERVO_VALID (1) +#define BOARD_ADC_PERIPH_5V_OC (0) +#define BOARD_ADC_HIPOWER_5V_OC (0) + +#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS + +/* This board provides a DMA pool and APIs. */ +#define BOARD_DMA_ALLOC_POOL_SIZE 5120 + +#define BOARD_HAS_ON_RESET 1 + +#define BOARD_ENABLE_CONSOLE_BUFFER +#define BOARD_CONSOLE_BUFFER_SIZE (1024*3) + +__BEGIN_DECLS + +/**************************************************************************************************** + * Public Types + ****************************************************************************************************/ + +/**************************************************************************************************** + * Public data + ****************************************************************************************************/ + +#ifndef __ASSEMBLY__ + +/**************************************************************************************************** + * Public Functions + ****************************************************************************************************/ + +/**************************************************************************************************** + * Name: stm32_spiinitialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the PX4FMU board. + * + * mask - is bus selection + * 1 - 1 << 0 + * 2 - 1 << 1 + * + ****************************************************************************************************/ + +extern void stm32_spiinitialize(int mask); +void board_spi_reset(int ms); + +extern void stm32_usbinitialize(void); + +extern void board_peripheral_reset(int ms); + +#include + +#endif /* __ASSEMBLY__ */ + +__END_DECLS diff --git a/boards/uvify/core/src/can.c b/boards/uvify/core/src/can.c new file mode 100644 index 0000000000..2835d99a98 --- /dev/null +++ b/boards/uvify/core/src/can.c @@ -0,0 +1,130 @@ +/**************************************************************************** + * + * Copyright (C) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file can.c + * + * Board-specific CAN functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include + +#include +#include + +#include "chip.h" +#include "up_arch.h" + +#include "stm32.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 diff --git a/boards/uvify/core/src/init.c b/boards/uvify/core/src/init.c new file mode 100644 index 0000000000..131f883ad3 --- /dev/null +++ b/boards/uvify/core/src/init.c @@ -0,0 +1,403 @@ +/**************************************************************************** + * + * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file init.c + * + * UVify Core 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 +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include "board_config.h" +#include + +#include + +#include +#include + +#include + +#include +#include + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ + +/** + * Ideally we'd be able to get these from up_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 + +/**************************************************************************** + * Protected Functions + ****************************************************************************/ +/**************************************************************************** + * Public Functions + ****************************************************************************/ +/************************************************************************************ + * Name: board_peripheral_reset + * + * Description: + * + ************************************************************************************/ +__EXPORT void board_peripheral_reset(int ms) +{ + // Set the peripheral rails off. + stm32_configgpio(GPIO_PERIPH_3V3_EN); + + stm32_gpiowrite(GPIO_PERIPH_3V3_EN, 0); + + bool last = stm32_gpioread(GPIO_SPEKTRUM_PWR_EN); + // Keep Spektum on to discharge rail. + stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, 1); + + // 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. + stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, last); + stm32_gpiowrite(GPIO_PERIPH_3V3_EN, 1); + +} + +/************************************************************************************ + * 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) +{ + // Configure the GPIO pins to outputs and keep them low. + stm32_configgpio(GPIO_GPIO0_OUTPUT); + stm32_configgpio(GPIO_GPIO1_OUTPUT); + stm32_configgpio(GPIO_GPIO2_OUTPUT); + stm32_configgpio(GPIO_GPIO3_OUTPUT); + stm32_configgpio(GPIO_GPIO4_OUTPUT); + stm32_configgpio(GPIO_GPIO5_OUTPUT); + + /** + * On resets invoked from system (not boot) insure we establish a low + * output state (discharge the pins) on PWM pins before they become inputs. + */ + + if (status >= 0) { + up_mdelay(400); + } +} + +/************************************************************************************ + * 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) +{ + // Reset all PWM to Low outputs. + board_on_reset(-1); + + // Configure LEDs. + board_autoled_initialize(); + + + // Configure ADC pins. + stm32_configgpio(GPIO_ADC1_IN2); /* BATT_VOLTAGE_SENS */ + stm32_configgpio(GPIO_ADC1_IN3); /* BATT_CURRENT_SENS */ + stm32_configgpio(GPIO_ADC1_IN4); /* VDD_5V_SENS */ + stm32_configgpio(GPIO_ADC1_IN11); /* RSSI analog in */ + + // Configure CAN interface + stm32_configgpio(GPIO_CAN1_RX); + stm32_configgpio(GPIO_CAN1_TX); + + // Configure power supply control/sense pins. + stm32_configgpio(GPIO_PERIPH_3V3_EN); + stm32_configgpio(GPIO_VDD_BRICK_VALID); + stm32_configgpio(GPIO_VDD_USB_VALID); + + /** + * Start with Sensor voltage off We will enable it + * in board_app_initialize. + */ + stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN); + + stm32_configgpio(GPIO_SBUS_INV); + stm32_configgpio(GPIO_SPEKTRUM_PWR_EN); + + stm32_configgpio(GPIO_PB4); + stm32_configgpio(GPIO_PE2); + + // Safety - led on in led driver. + stm32_configgpio(GPIO_BTN_SAFETY); + stm32_configgpio(GPIO_PPM_IN); + + int spi_init_mask = SPI_BUS_INIT_MASK; + +#if defined(CONFIG_STM32_SPI4) + + /* We have SPI4 is GPIO_PB4 pin 3 Low */ + if (stm32_gpioread(GPIO_PB4) == 0) { + spi_init_mask |= SPI_BUS_INIT_MASK_EXT; + + } else { +#endif /* CONFIG_STM32_SPI4 */ + + stm32_configgpio(GPIO_PE5); + stm32_configgpio(GPIO_PE6); + +#if defined(CONFIG_STM32_SPI4) + } + +#endif /* CONFIG_STM32_SPI4 */ + +// Configure SPI all interfaces GPIO. + stm32_spiinitialize(spi_init_mask); + + // Configure heater GPIO. + stm32_configgpio(GPIO_HEATER_INPUT); + stm32_configgpio(GPIO_HEATER_OUTPUT); +} + +/**************************************************************************** + * 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. + * + ****************************************************************************/ + +static struct spi_dev_s *spi1; +static struct spi_dev_s *spi2; +static struct sdio_dev_s *sdio; +#if defined(CONFIG_STM32_SPI4) +static struct spi_dev_s *spi4; +#endif + +__EXPORT int board_app_initialize(uintptr_t arg) +{ + px4_platform_init(); + + // Configure the DMA allocator. + if (board_dma_alloc_init() < 0) { + syslog(LOG_ERR, "DMA alloc FAILED\n"); + } + + // Set up the serial DMA polling. + static struct hrt_call serial_dma_call; + struct timespec ts; + + /** + * Poll at 1ms intervals for received bytes that have not triggered + * a DMA event. + */ + ts.tv_sec = 0; + ts.tv_nsec = 1000000; + + hrt_call_every(&serial_dma_call, + ts_to_abstime(&ts), + ts_to_abstime(&ts), + (hrt_callout)stm32_serial_dma_poll, + NULL); + + // Initial LED state. + drv_led_start(); + led_off(LED_RED); + led_off(LED_GREEN); + led_off(LED_BLUE); + + if (board_hardfault_init(2, true) != 0) { + led_on(LED_RED); + } + + // Power up the sensors. + stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 1); + + // Power down the heater. + stm32_gpiowrite(GPIO_HEATER_OUTPUT, 0); + + // Configure SPI-based devices. + spi1 = stm32_spibus_initialize(1); + + if (!spi1) { + syslog(LOG_ERR, "[boot] FAILED to initialize SPI port 1\n"); + led_on(LED_RED); + return -ENODEV; + } + + + // Default SPI1 to 1MHz and de-assert the known chip selects. + SPI_SETFREQUENCY(spi1, 10000000); + SPI_SETBITS(spi1, 8); + SPI_SETMODE(spi1, SPIDEV_MODE3); + SPI_SELECT(spi1, PX4_SPIDEV_GYRO, false); + SPI_SELECT(spi1, PX4_SPIDEV_HMC, false); + SPI_SELECT(spi1, PX4_SPIDEV_MPU, false); + up_udelay(20); + + // Get the SPI port for the FRAM. + spi2 = stm32_spibus_initialize(2); + + if (!spi2) { + syslog(LOG_ERR, "[boot] FAILED to initialize SPI port 2\n"); + led_on(LED_RED); + return -ENODEV; + } + + /** + * Default SPI2 to 12MHz and de-assert the known chip selects. + * MS5611 has max SPI clock speed of 20MHz. + */ + + // XXX start with 10.4 MHz and go up to 20 once validated. + SPI_SETFREQUENCY(spi2, 20 * 1000 * 1000); + SPI_SETBITS(spi2, 8); + SPI_SETMODE(spi2, SPIDEV_MODE3); + SPI_SELECT(spi2, SPIDEV_FLASH(0), false); + SPI_SELECT(spi2, PX4_SPIDEV_BARO, false); + +#if defined(CONFIG_STM32_SPI4) + + if (stm32_gpioread(GPIO_PB4) == 0) { + syslog(LOG_INFO, "[boot] GPIO_PB4 - Low Initialize SPI port 4 \n"); + + // Configure SPI-based devices. + spi4 = stm32_spibus_initialize(4); + + if (!spi4) { + syslog(LOG_ERR, "[boot] FAILED to initialize SPI port 4\n"); + + } else { + // Default SPI4 to 20 MHz and de-assert the known chip selects. + SPI_SETFREQUENCY(spi4, 20 * 1000 * 1000); + SPI_SETBITS(spi4, 8); + SPI_SETMODE(spi4, SPIDEV_MODE3); + SPI_SELECT(spi4, PX4_SPIDEV_EXTERNAL, false); + } + } + +#endif /* defined(CONFIG_STM32_SPI4) */ + + +#ifdef CONFIG_MMCSD + + // First, get an instance of the SDIO interface. + sdio = sdio_initialize(CONFIG_NSH_MMCSDSLOTNO); + + if (!sdio) { + led_on(LED_RED); + syslog(LOG_ERR, "[boot] Failed to initialize SDIO slot %d\n", + CONFIG_NSH_MMCSDSLOTNO); + return -ENODEV; + } + + // Now bind the SDIO interface to the MMC/SD driver. + int ret = mmcsd_slotinitialize(CONFIG_NSH_MMCSDMINOR, sdio); + + if (ret != OK) { + led_on(LED_RED); + syslog(LOG_ERR, "[boot] Failed to bind SDIO to the MMC/SD driver: %d\n", ret); + return ret; + } + + // Then let's guess and say that there is a card in the slot. There is no card detect GPIO. + sdio_mediachange(sdio, true); + +#endif + + return OK; +} diff --git a/boards/uvify/core/src/led.c b/boards/uvify/core/src/led.c new file mode 100644 index 0000000000..d088d75c4e --- /dev/null +++ b/boards/uvify/core/src/led.c @@ -0,0 +1,106 @@ +/**************************************************************************** + * + * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file led.c + * + * UVify Core LED backend. + */ + +#include + +#include + +#include "stm32.h" +#include "board_config.h" + +#include + +/* + * Ideally we'd be able to get these from up_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 + + + +static uint32_t g_ledmap[] = { + GPIO_LED_BLUE, // Indexed by LED_BLUE + GPIO_LED_RED, // Indexed by LED_RED, LED_AMBER + GPIO_LED_SAFETY, // Indexed by LED_SAFETY + GPIO_LED_GREEN, // Indexed by LED_GREEN +}; + +__EXPORT void led_init(void) +{ + /* Configure LED GPIOs for output */ + for (size_t l = 0; l < (sizeof(g_ledmap) / sizeof(g_ledmap[0])); l++) { + stm32_configgpio(g_ledmap[l]); + } +} + +static void phy_set_led(int led, bool state) +{ + /* Pull Down to switch on */ + stm32_gpiowrite(g_ledmap[led], !state); +} + +static bool phy_get_led(int led) +{ + + return !stm32_gpioread(g_ledmap[led]); +} + +__EXPORT void led_on(int led) +{ + phy_set_led(led, true); +} + +__EXPORT void led_off(int led) +{ + phy_set_led(led, false); +} + +__EXPORT void led_toggle(int led) +{ + + phy_set_led(led, !phy_get_led(led)); +} diff --git a/boards/uvify/core/src/spi.c b/boards/uvify/core/src/spi.c new file mode 100644 index 0000000000..dfac2a2412 --- /dev/null +++ b/boards/uvify/core/src/spi.c @@ -0,0 +1,320 @@ +/**************************************************************************** + * + * Copyright (C) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file spi.c + * + * UVify Core specific SPI functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include "board_config.h" + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +__EXPORT bool board_has_bus(enum board_bus_types type, uint32_t bus) +{ + bool rv = true; + + switch (type) { + case BOARD_SPI_BUS: +#ifdef CONFIG_STM32_SPI4 + rv = bus != PX4_SPI_BUS_EXTERNAL || (stm32_gpioread(GPIO_PB4) == 0); +#endif /* CONFIG_STM32_SPI4 */ + break; + + case BOARD_I2C_BUS: + break; + } + + return rv; +} + +/************************************************************************************ + * Name: stm32_spiinitialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the PX4FMU board. + * mask - is bus selection + * 1 - 1 << 0 + * 2 - 1 << 1 + * + ************************************************************************************/ + +__EXPORT void stm32_spiinitialize(int mask) +{ +#ifdef CONFIG_STM32_SPI1 + + if (mask & PX4_SPI_BUS_SENSORS) { + stm32_configgpio(GPIO_SPI1_CS_PORTC_PIN2); + stm32_configgpio(GPIO_SPI1_CS_PORTC_PIN15); + stm32_configgpio(GPIO_SPI1_CS_PORTE_PIN15); + + stm32_configgpio(GPIO_DRDY_PORTD_PIN15); + stm32_configgpio(GPIO_DRDY_PORTC_PIN14); + stm32_configgpio(GPIO_DRDY_PORTE_PIN12); + } + +#endif + +#ifdef CONFIG_STM32_SPI2 + + if (mask & (PX4_SPI_BUS_RAMTRON | PX4_SPI_BUS_BARO)) { + stm32_configgpio(GPIO_SPI2_CS_MS5611); + stm32_configgpio(GPIO_SPI2_CS_FRAM); + } + +#endif + +#ifdef CONFIG_STM32_SPI4 + + if (mask & PX4_SPI_BUS_EXTERNAL) { + stm32_configgpio(GPIO_SPI4_CS_1); //add cs + } + +#endif /* CONFIG_STM32_SPI4 */ +} + +__EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) +{ + /* SPI select is active low, so write !selected to select the device */ + + switch (devid) { + + /* Shared PC2 CS devices */ + + case PX4_SPIDEV_BMI: + case PX4_SPIDEV_MPU: + /* Making sure the other peripherals are not selected */ + px4_arch_gpiowrite(GPIO_SPI1_CS_PORTC_PIN2, !selected); + px4_arch_gpiowrite(GPIO_SPI1_CS_PORTC_PIN15, 1); + px4_arch_gpiowrite(GPIO_SPI1_CS_PORTE_PIN15, 1); + break; + + /* Shared PC15 CS devices */ + + case PX4_SPIDEV_ICM: + case PX4_SPIDEV_ICM_20602: + case PX4_SPIDEV_ICM_20608: + case PX4_SPIDEV_BMI055_ACC: + case PX4_SPIDEV_MPU2: + /* Making sure the other peripherals are not selected */ + px4_arch_gpiowrite(GPIO_SPI1_CS_PORTC_PIN2, 1); + px4_arch_gpiowrite(GPIO_SPI1_CS_PORTC_PIN15, !selected); + px4_arch_gpiowrite(GPIO_SPI1_CS_PORTE_PIN15, 1); + break; + + /* Shared PE15 CS devices */ + + case PX4_SPIDEV_HMC: + case PX4_SPIDEV_LIS: + case PX4_SPIDEV_BMI055_GYR: + /* Making sure the other peripherals are not selected */ + px4_arch_gpiowrite(GPIO_SPI1_CS_PORTC_PIN2, 1); + px4_arch_gpiowrite(GPIO_SPI1_CS_PORTC_PIN15, 1); + px4_arch_gpiowrite(GPIO_SPI1_CS_PORTE_PIN15, !selected); + break; + + default: + break; + } +} + +__EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, uint32_t devid) +{ + return SPI_STATUS_PRESENT; +} + + +#ifdef CONFIG_STM32_SPI2 +__EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) +{ + /* SPI select is active low, so write !selected to select the device */ + + switch (devid) { + case SPIDEV_FLASH(0): + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI2_CS_MS5611, 1); + stm32_gpiowrite(GPIO_SPI2_CS_FRAM, !selected); + break; + + case PX4_SPIDEV_BARO: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI2_CS_FRAM, 1); + stm32_gpiowrite(GPIO_SPI2_CS_MS5611, !selected); + break; + + default: + break; + } +} + +__EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, uint32_t devid) +{ + /* FRAM is always present */ + return SPI_STATUS_PRESENT; +} +#endif + +#ifdef CONFIG_STM32_SPI4 +__EXPORT void stm32_spi4select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) +{ + if (devid == PX4_SPIDEV_EXTERNAL && stm32_gpioread(GPIO_PB4) == 0) { + stm32_gpiowrite(GPIO_SPI4_CS_1, !selected); // add cs + } +} + +__EXPORT uint8_t stm32_spi4status(FAR struct spi_dev_s *dev, uint32_t devid) +{ + return SPI_STATUS_PRESENT; +} +#endif /* CONFIG_STM32_SPI4 */ + +__EXPORT void board_spi_reset(int ms) +{ + /* disable SPI bus 1 DRDY */ + + stm32_configgpio(GPIO_DRDY_OFF_PORTD_PIN15); + stm32_configgpio(GPIO_DRDY_OFF_PORTC_PIN14); + stm32_configgpio(GPIO_DRDY_OFF_PORTE_PIN12); + + stm32_gpiowrite(GPIO_DRDY_OFF_PORTD_PIN15, 0); + stm32_gpiowrite(GPIO_DRDY_OFF_PORTC_PIN14, 0); + stm32_gpiowrite(GPIO_DRDY_OFF_PORTE_PIN12, 0); + + /* disable SPI bus 1 CS */ + + stm32_configgpio(GPIO_SPI1_CS_OFF_PORTC_PIN2); + stm32_configgpio(GPIO_SPI1_CS_OFF_PORTC_PIN15); + stm32_configgpio(GPIO_SPI1_CS_OFF_PORTE_PIN15); + + stm32_gpiowrite(GPIO_SPI1_CS_OFF_PORTC_PIN2, 0); + stm32_gpiowrite(GPIO_SPI1_CS_OFF_PORTC_PIN15, 0); + stm32_gpiowrite(GPIO_SPI1_CS_OFF_PORTE_PIN15, 0); + + /* disable SPI bus 1*/ + + stm32_configgpio(GPIO_SPI1_SCK_OFF); + stm32_configgpio(GPIO_SPI1_MISO_OFF); + stm32_configgpio(GPIO_SPI1_MOSI_OFF); + + stm32_gpiowrite(GPIO_SPI1_SCK_OFF, 0); + stm32_gpiowrite(GPIO_SPI1_MISO_OFF, 0); + stm32_gpiowrite(GPIO_SPI1_MOSI_OFF, 0); + +#ifdef CONFIG_STM32_SPI4 + + /* disable SPI bus 4*/ + if (stm32_gpioread(GPIO_PB4) == 0) { + stm32_configgpio(GPIO_SPI4_SCK_OFF); + stm32_configgpio(GPIO_SPI4_MISO_OFF); + stm32_configgpio(GPIO_SPI4_MOSI_OFF); + + stm32_gpiowrite(GPIO_SPI4_SCK_OFF, 0); + stm32_gpiowrite(GPIO_SPI4_MISO_OFF, 0); + stm32_gpiowrite(GPIO_SPI4_MOSI_OFF, 0); + } + +#endif /* CONFIG_STM32_SPI4 */ + + /* N.B we do not have control over the SPI 2 buss powered devices + * so the the ms5611 is not resetable. + */ + + /* set the sensor rail off (default) */ + stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN); + +#ifdef CONFIG_STM32_SPI4 + + if (stm32_gpioread(GPIO_PB4) == 0) { + /* set the periph rail off (default) for SPI4 */ + stm32_configgpio(GPIO_PERIPH_3V3_EN); + } + +#endif /* CONFIG_STM32_SPI4 */ + + /* wait for the sensor rail to reach GND */ + usleep(ms * 1000); + syslog(LOG_DEBUG, "reset done, %d ms\n", ms); + + /* re-enable power */ + +#ifdef CONFIG_STM32_SPI4 + + if (stm32_gpioread(GPIO_PB4) == 0) { + /* switch the periph rail back on */ + stm32_gpiowrite(GPIO_PERIPH_3V3_EN, 1); + } + +#endif /* CONFIG_STM32_SPI4 */ + + /* switch the sensor rail back on */ + stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 1); + + /* wait a bit before starting SPI, different times didn't influence results */ + usleep(100); + + stm32_spiinitialize(PX4_SPI_BUS_SENSORS); + stm32_configgpio(GPIO_SPI1_SCK); + stm32_configgpio(GPIO_SPI1_MISO); + stm32_configgpio(GPIO_SPI1_MOSI); + +#ifdef CONFIG_STM32_SPI4 + + if (stm32_gpioread(GPIO_PB4) == 0) { + stm32_spiinitialize(PX4_SPI_BUS_EXTERNAL); + stm32_configgpio(GPIO_SPI4_SCK); + stm32_configgpio(GPIO_SPI4_MISO); + stm32_configgpio(GPIO_SPI4_MOSI); + } + +#endif /* CONFIG_STM32_SPI4 */ +} diff --git a/boards/uvify/core/src/timer_config.c b/boards/uvify/core/src/timer_config.c new file mode 100644 index 0000000000..0d9ff0898c --- /dev/null +++ b/boards/uvify/core/src/timer_config.c @@ -0,0 +1,126 @@ +/**************************************************************************** + * + * Copyright (C) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file timer_config.c + * + * Configuration data for the stm32 pwm_servo, input capture and pwm input driver. + * + * Note that these arrays must always be fully-sized. + */ + +#include + +#include +#include +#include + +#include +#include + +#include "board_config.h" + +__EXPORT const io_timers_t io_timers[MAX_IO_TIMERS] = { + { + .base = STM32_TIM1_BASE, + .clock_register = STM32_RCC_APB2ENR, + .clock_bit = RCC_APB2ENR_TIM1EN, + .clock_freq = STM32_APB2_TIM1_CLKIN, + .first_channel_index = 0, + .last_channel_index = 3, + .handler = io_timer_handler0, + .vectorno = STM32_IRQ_TIM1CC, + + }, + { + .base = STM32_TIM4_BASE, + .clock_register = STM32_RCC_APB1ENR, + .clock_bit = RCC_APB1ENR_TIM4EN, + .clock_freq = STM32_APB1_TIM4_CLKIN, + .first_channel_index = 4, + .last_channel_index = 5, + .handler = io_timer_handler1, + .vectorno = STM32_IRQ_TIM4, + } +}; + +__EXPORT const timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = { + { + .gpio_out = GPIO_TIM1_CH4OUT, + .gpio_in = GPIO_TIM1_CH4IN, + .timer_index = 0, + .timer_channel = 4, + .ccr_offset = STM32_GTIM_CCR4_OFFSET, + .masks = GTIM_SR_CC4IF | GTIM_SR_CC4OF + }, + { + .gpio_out = GPIO_TIM1_CH3OUT, + .gpio_in = GPIO_TIM1_CH3IN, + .timer_index = 0, + .timer_channel = 3, + .ccr_offset = STM32_GTIM_CCR3_OFFSET, + .masks = GTIM_SR_CC3IF | GTIM_SR_CC3OF + }, + { + .gpio_out = GPIO_TIM1_CH2OUT, + .gpio_in = GPIO_TIM1_CH2IN, + .timer_index = 0, + .timer_channel = 2, + .ccr_offset = STM32_GTIM_CCR2_OFFSET, + .masks = GTIM_SR_CC2IF | GTIM_SR_CC2OF + }, + { + .gpio_out = GPIO_TIM1_CH1OUT, + .gpio_in = GPIO_TIM1_CH1IN, + .timer_index = 0, + .timer_channel = 1, + .ccr_offset = STM32_GTIM_CCR1_OFFSET, + .masks = GTIM_SR_CC1IF | GTIM_SR_CC1OF + }, + { + .gpio_out = GPIO_TIM4_CH2OUT, + .gpio_in = GPIO_TIM4_CH2IN, + .timer_index = 1, + .timer_channel = 2, + .ccr_offset = STM32_GTIM_CCR2_OFFSET, + .masks = GTIM_SR_CC2IF | GTIM_SR_CC2OF + }, + { + .gpio_out = GPIO_TIM4_CH3OUT, + .gpio_in = GPIO_TIM4_CH3IN, + .timer_index = 1, + .timer_channel = 3, + .ccr_offset = STM32_GTIM_CCR3_OFFSET, + .masks = GTIM_SR_CC3IF | GTIM_SR_CC3OF + } +}; diff --git a/boards/uvify/core/src/usb.c b/boards/uvify/core/src/usb.c new file mode 100644 index 0000000000..fe85fd61c6 --- /dev/null +++ b/boards/uvify/core/src/usb.c @@ -0,0 +1,108 @@ +/**************************************************************************** + * + * Copyright (C) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file usb.c + * + * UVify Core specific USB functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#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_STM32_OTGFS + stm32_configgpio(GPIO_OTGFS_VBUS); + /* XXX We only support device mode + stm32_configgpio(GPIO_OTGFS_PWRON); + stm32_configgpio(GPIO_OTGFS_OVER); + */ +#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/CMakeLists.txt b/platforms/nuttx/CMakeLists.txt index 3dcff137c9..fb6b630d24 100644 --- a/platforms/nuttx/CMakeLists.txt +++ b/platforms/nuttx/CMakeLists.txt @@ -173,6 +173,7 @@ if (TARGET parameters_xml AND TARGET airframes_xml) /dev/serial/by-id/pci-Bitcraze* /dev/serial/by-id/usb-Gumstix* /dev/serial/by-id/usb-Hex_ProfiCNC* + /dev/serial/by-id/usb-UVify_FMU_BL* ) elseif(${CMAKE_HOST_SYSTEM_NAME} STREQUAL "Darwin")