fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-21 15:37:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'beta' into stable

Browse Source
This commit is contained in:
Lorenz Meier
2016-02-21 01:37:36 +01:00
parent cc79299375 73751922de
commit f5efe0afb0
165 changed files with 7116 additions and 1728 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.gitignore
+2
View File
@@ -22,6 +22,8 @@ build/*
build_*/
core
cscope.out
cscope.in.out
cscope.po.out
Firmware.sublime-workspace
user.sublime*
Images/*.bin
.travis.yml
+14 -17
View File
@@ -60,9 +60,8 @@ before_install:
elif [ "${TRAVIS_OS_NAME}" = "osx" ]; then
brew tap PX4/homebrew-px4
&& brew update; brew update
&& brew install cmake ninja astyle gcc-arm-none-eabi
&& brew install cmake ninja
&& brew install genromfs
&& brew install kconfig-frontends
&& sudo easy_install pip
&& sudo pip install pyserial empy
;
@@ -93,24 +92,22 @@ env:
- PX4_AWS_BUCKET=px4-travis
script:
- git submodule update --init --recursive
- make check_format
- arm-none-eabi-gcc --version
- git submodule update --quiet --init --recursive
- echo 'Building POSIX Firmware..' && make posix_sitl_default
- echo 'Running Tests..' && make posix_sitl_default test
- echo 'Running Unittests..' && cd unittests && ./run_tests.sh
- cd ..
- echo 'Building NuttX px4fmu-v1 Firmware..' && make px4fmu-v1_default
- echo 'Building NuttX px4fmu-v2 Firmware..' && make px4fmu-v2_default
# Only provide UAVCAN firmware binaries for Pixracer and Pixhawk 3
- echo 'Building UAVCAN node firmware..' && git clone https://github.com/thiemar/vectorcontrol
- cd vectorcontrol
- BOARD=s2740vc_1_0 make && BOARD=px4esc_1_6 make
- ../Tools/uavcan_copy.sh
- cd ..
- echo 'Building NuttX px4fmu-v4 Firmware..' && make px4fmu-v4_default
- echo 'Building NuttX px4-stm32f4discovery Firmware..' && make px4-stm32f4discovery_default
- echo 'Running Tests..' && make px4fmu-v2_default test
- if [ "${TRAVIS_OS_NAME}" = "linux" ]; then
cd ${TRAVIS_BUILD_DIR}
&& make check_format
&& arm-none-eabi-gcc --version
&& echo 'Building NuttX px4fmu-v1 Firmware..' && make px4fmu-v1_default
&& echo 'Building NuttX px4fmu-v2 Firmware..' && make px4fmu-v2_default
&& echo 'Building UAVCAN node firmware..' && (git clone https://github.com/thiemar/vectorcontrol && cd vectorcontrol && BOARD=s2740vc_1_0 make -s && BOARD=px4esc_1_6 make -s && ../Tools/uavcan_copy.sh)
&& echo 'Building NuttX px4fmu-v4 Firmware..' && make px4fmu-v4_default
&& echo 'Building NuttX px4-stm32f4discovery Firmware..' && make px4-stm32f4discovery_default
&& echo 'Running Tests..' && make px4fmu-v2_default test
;
fi
after_success:
- if [ "${TRAVIS_OS_NAME}" = "linux" ]; then
Makefile
+3 -5
View File
@@ -109,9 +109,8 @@ endif
# describe how to build a cmake config
define cmake-build
+@if [ $(PX4_CMAKE_GENERATOR) = "Ninja" ] && [ -e $(PWD)/build_$@/Makefile ]; then rm -rf $(PWD)/build_$@; fi
+git submodule init
+Tools/check_submodules.sh
+@if [ ! -e $(PWD)/build_$@/CMakeCache.txt ]; then git submodule sync && git submodule init && mkdir -p $(PWD)/build_$@ && cd $(PWD)/build_$@ && cmake .. -G$(PX4_CMAKE_GENERATOR) -DCONFIG=$(1); fi
+@if [ ! -e $(PWD)/build_$@/CMakeCache.txt ]; then git submodule sync && git submodule update --init --recursive && mkdir -p $(PWD)/build_$@ && cd $(PWD)/build_$@ && cmake .. -G$(PX4_CMAKE_GENERATOR) -DCONFIG=$(1); fi
+Tools/check_submodules.sh
+$(PX4_MAKE) -C $(PWD)/build_$@ $(PX4_MAKE_ARGS) $(ARGS)
endef
@@ -155,7 +154,6 @@ posix_sitl_ekf2:
ros_sitl_default:
@echo "This target is deprecated. Use make 'posix_sitl_default gazebo' instead."
# $(call cmake-build,$@)
qurt_eagle_travis:
$(call cmake-build,$@)
@@ -202,7 +200,7 @@ clean:
# targets handled by cmake
cmake_targets = test upload package package_source debug debug_tui debug_ddd debug_io debug_io_tui debug_io_ddd check_weak \
run_cmake_config config gazebo gazebo_gdb gazebo_lldb jmavsim \
jmavsim_gdb jmavsim_lldb gazebo_gdb_iris gazebo_lldb_tailsitter gazebo_iris gazebo_tailsitter \
jmavsim_gdb jmavsim_lldb gazebo_gdb_iris gazebo_lldb_tailsitter gazebo_iris gazebo_iris_opt_flow gazebo_tailsitter \
gazebo_gdb_standard_vtol gazebo_lldb_standard_vtol gazebo_standard_vtol
$(foreach targ,$(cmake_targets),$(eval $(call cmake-targ,$(targ))))
ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil
+1 -2
View File
@@ -37,9 +37,8 @@ then
param set FW_RR_I 0.1
param set FW_RR_IMAX 0.2
param set FW_RR_P 0.3
param set RWTO_TKOFF 1
fi
param set RWTO_TKOFF 1
set HIL yes
set MIXER AERT
ROMFS/px4fmu_common/init.d/12002_steadidrone_mavrik
+41
View File
@@ -0,0 +1,41 @@
#!nsh
#
# @name Steadidrone MAVRIK
#
# @type Octo Coax Wide
#
# @maintainer Simon Wilks <simon@uaventure.com>
#
sh /etc/init.d/rc.mc_defaults
if [ $AUTOCNF == yes ]
then
param set MC_PITCH_P 4.0
param set MC_PITCHRATE_P 0.24
param set MC_PITCHRATE_I 0.09
param set MC_PITCHRATE_D 0.013
param set MC_PITCHRATE_MAX 180.0
param set MC_ROLL_P 4.0
param set MC_ROLLRATE_P 0.16
param set MC_ROLLRATE_I 0.07
param set MC_ROLLRATE_D 0.009
param set MC_ROLLRATE_MAX 180.0
param set MC_YAW_P 3.0
param set MC_YAWRATE_P 0.2
param set MC_YAWRATE_I 0.1
param set MC_YAWRATE_D 0.0
param set MC_YAW_FF 0.5
param set MPC_HOLD_MAX_XY 0.25
param set MPC_THR_MIN 0.15
param set MPC_Z_VEL_MAX 2.0
param set BAT_N_CELLS 4
fi
set MIXER octo_cox_w
set PWM_OUT 12345678
ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol
+27 -1
View File
@@ -1,14 +1,40 @@
#!nsh
#
# @name Duorotor Tailsitter
# @name Caipiroshka Duo Tailsitter
#
# @type VTOL Duo Tailsitter
#
# @output MAIN1 motor left
# @output MAIN2 motor right
# @output MAIN5 elevon left
# @output MAIN6 elevon right
#
# @maintainer Roman Bapst <roman@px4.io>
#
sh /etc/init.d/rc.vtol_defaults
if [ $AUTOCNF == yes ]
then
param set MC_ROLL_P 6.0
param set MC_ROLLRATE_P 0.12
param set MC_ROLLRATE_I 0.002
param set MC_ROLLRATE_D 0.003
param set MC_ROLLRATE_FF 0.0
param set MC_PITCH_P 4.5
param set MC_PITCHRATE_P 0.3
param set MC_PITCHRATE_I 0.002
param set MC_PITCHRATE_D 0.003
param set MC_PITCHRATE_FF 0.0
param set MC_YAW_P 3.8
param set MC_YAW_FF 0.5
param set MC_YAWRATE_P 0.22
param set MC_YAWRATE_I 0.02
param set MC_YAWRATE_D 0.0
param set MC_YAWRATE_FF 0.0
fi
set MIXER caipirinha_vtol
set PWM_OUT 12
ROMFS/px4fmu_common/init.d/13002_firefly6
+3 -7
View File
@@ -14,12 +14,12 @@ then
param set MC_ROLL_P 7.0
param set MC_ROLLRATE_P 0.17
param set MC_ROLLRATE_I 0.002
param set MC_ROLLRATE_D 0.004
param set MC_ROLLRATE_D 0.003
param set MC_ROLLRATE_FF 0.0
param set MC_PITCH_P 7.0
param set MC_PITCHRATE_P 0.14
param set MC_PITCHRATE_I 0.002
param set MC_PITCHRATE_D 0.004
param set MC_PITCHRATE_D 0.003
param set MC_PITCHRATE_FF 0.0
param set MC_YAW_P 3.8
param set MC_YAW_FF 0.5
@@ -35,13 +35,9 @@ fi
set MIXER firefly6
set PWM_OUT 12345678
set PWM_RATE 400
set MIXER_AUX firefly6
set PWM_AUX_RATE 50
set PWM_AUX_OUT 1234
set PWM_AUX_DISARMED 1000
set PWM_AUX_MIN 1000
set PWM_AUX_MAX 2000
set MAV_TYPE 21
ROMFS/px4fmu_common/init.d/13005_vtol_AAERT_quad
+22 -12
View File
@@ -14,16 +14,18 @@ then
param set VT_TYPE 2
param set VT_MOT_COUNT 4
param set VT_TRANS_THR 0.75
param set VT_ARSP_TRANS 12
param set VT_ARSP_BLEND 6
param set MC_ROLL_P 7.0
param set MC_ROLLRATE_P 0.15
param set MC_ROLL_P 6.0
param set MC_ROLLRATE_P 0.17
param set MC_ROLLRATE_I 0.002
param set MC_ROLLRATE_D 0.003
param set MC_ROLLRATE_D 0.004
param set MC_ROLLRATE_FF 0.0
param set MC_PITCH_P 7.0
param set MC_PITCHRATE_P 0.12
param set MC_PITCH_P 6.0
param set MC_PITCHRATE_P 0.19
param set MC_PITCHRATE_I 0.002
param set MC_PITCHRATE_D 0.003
param set MC_PITCHRATE_D 0.004
param set MC_PITCHRATE_FF 0.0
param set MC_YAW_P 2.8
param set MC_YAW_FF 0.5
@@ -31,17 +33,25 @@ then
param set MC_YAWRATE_I 0.02
param set MC_YAWRATE_D 0.0
param set MC_YAWRATE_FF 0.0
param set FW_PR_FF 0.5
param set FW_PR_I 0.02
param set FW_PR_IMAX 0.4
param set FW_PR_P 0.08
param set FW_RR_FF 0.6
param set FW_RR_I 0.01
param set FW_RR_IMAX 0.2
param set FW_RR_P 0.05
param set FW_THR_CRUISE 0.75
fi
set MIXER vtol_quad_x
set PWM_OUT 12345678
set PWM_OUT 1234
set PWM_RATE 400
set MIXER_AUX vtol_AAERT
set PWM_AUX_RATE 50
set PWM_AUX_OUT 1234
set PWM_AUX_DISARMED 1000
set PWM_AUX_MIN 1000
set PWM_AUX_MAX 2000
set PWM_ACHDIS 5
set PWM_AUX_DISARMED 950
set MAV_TYPE 22
ROMFS/px4fmu_common/init.d/13006_vtol_standard_delta
+4 -6
View File
@@ -34,14 +34,12 @@ then
fi
set MIXER vtol_quad_x
set PWM_OUT 12345678
set PWM_OUT 1234
set PWM_RATE 400
set MIXER_AUX vtol_delta
set PWM_AUX_RATE 50
set PWM_AUX_OUT 1234
set PWM_AUX_DISARMED 1000
set PWM_AUX_MIN 1000
set PWM_AUX_MAX 2000
set PWM_ACHDIS 3
set PWM_AUX_DISARMED 950
set MAV_TYPE 22
ROMFS/px4fmu_common/init.d/13007_vtol_AAVVT_quad
+4 -7
View File
@@ -31,18 +31,15 @@ then
param set MC_YAWRATE_I 0.02
param set MC_YAWRATE_D 0.0
param set MC_YAWRATE_FF 0.0
fi
set MIXER vtol_quad_x
set PWM_OUT 12345678
set PWM_OUT 1234
set PWM_RATE 400
set MIXER_AUX vtol_AAVVT
set PWM_AUX_RATE 50
set PWM_AUX_OUT 1234
set PWM_AUX_DISARMED 1000
set PWM_AUX_MIN 1000
set PWM_AUX_MAX 2000
set PWM_ACHDIS 5
set PWM_AUX_DISARMED 950
set MAV_TYPE 22
ROMFS/px4fmu_common/init.d/13008_QuadRanger
+20 -16
View File
@@ -19,32 +19,36 @@ then
param set PWM_AUX_REV2 1
param set MC_ROLL_P 7.0
param set MC_ROLLRATE_P 0.25
param set MC_ROLLRATE_I 0.002
param set MC_ROLLRATE_D 0.003
param set MC_ROLLRATE_P 0.15
param set MC_ROLLRATE_I 0.1
param set MC_ROLLRATE_D 0.004
param set MC_ROLLRATE_FF 0.0
param set MC_PITCH_P 7.0
param set MC_PITCHRATE_P 0.23
param set MC_PITCHRATE_I 0.002
param set MC_PITCHRATE_D 0.003
param set MC_PITCHRATE_P 0.15
param set MC_PITCHRATE_I 0.1
param set MC_PITCHRATE_D 0.004
param set MC_PITCHRATE_FF 0.0
param set MC_YAW_P 2.8
param set MC_YAW_FF 0.5
param set MC_YAWRATE_P 0.22
param set MC_YAWRATE_I 0.02
param set MC_YAW_P 3.5
param set MC_YAW_FF 0.7
param set MC_YAWRATE_P 0.6
param set MC_YAWRATE_I 0.04
param set MC_YAWRATE_D 0.0
param set MC_YAWRATE_FF 0.0
param set MPC_ACC_HOR_MAX 2.0
param set VT_ARSP_TRANS 15.0
param set VT_ARSP_BLEND 10.0
param set VT_B_TRANS_DUR 4.0
fi
set MIXER vtol_quad_x
set PWM_OUT 12345678
set PWM_OUT 1234
set PWM_RATE 400
set MIXER_AUX vtol_AAERT
set PWM_AUX_RATE 50
set PWM_AUX_OUT 1234
set PWM_AUX_DISARMED 1000
set PWM_AUX_MIN 1000
set PWM_AUX_MAX 2000
set PWM_ACHDIS 5
set PWM_AUX_DISARMED 950
set MAV_TYPE 22
ROMFS/px4fmu_common/init.d/4010_dji_f330
-6
View File
@@ -30,9 +30,3 @@ then
# DJI ESCs do not support calibration and need a higher min
param set PWM_MIN 1230
fi
# Transitional support: ensure suitable PWM min/max param values
if param compare PWM_MIN 1075
then
param set PWM_MIN 1230
fi
ROMFS/px4fmu_common/init.d/4011_dji_f450
+2 -8
View File
@@ -18,11 +18,11 @@ then
param set MC_ROLL_P 7.0
param set MC_ROLLRATE_P 0.15
param set MC_ROLLRATE_I 0.05
param set MC_ROLLRATE_D 0.01
param set MC_ROLLRATE_D 0.003
param set MC_PITCH_P 7.0
param set MC_PITCHRATE_P 0.15
param set MC_PITCHRATE_I 0.05
param set MC_PITCHRATE_D 0.01
param set MC_PITCHRATE_D 0.003
param set MC_YAW_P 2.8
param set MC_YAWRATE_P 0.3
param set MC_YAWRATE_I 0.1
@@ -30,9 +30,3 @@ then
# DJI ESCs do not support calibration and need a higher min
param set PWM_MIN 1230
fi
# Transitional support: ensure suitable PWM min/max param values
if param compare PWM_MIN 1075
then
param set PWM_MIN 1230
fi
ROMFS/px4fmu_common/init.d/4012_quad_x_can
+2 -2
View File
@@ -14,11 +14,11 @@ then
param set MC_ROLL_P 7.0
param set MC_ROLLRATE_P 0.16
param set MC_ROLLRATE_I 0.05
param set MC_ROLLRATE_D 0.01
param set MC_ROLLRATE_D 0.003
param set MC_PITCH_P 7.0
param set MC_PITCHRATE_P 0.16
param set MC_PITCHRATE_I 0.05
param set MC_PITCHRATE_D 0.01
param set MC_PITCHRATE_D 0.003
param set MC_YAW_P 2.8
param set MC_YAWRATE_P 0.3
param set MC_YAWRATE_I 0.1
ROMFS/px4fmu_common/init.d/4040_reaper
+42
View File
@@ -0,0 +1,42 @@
#!nsh
#
# @name Reaper 500 Quad
#
# @type Quadrotor x
#
# @maintainer Blankered
#
sh /etc/init.d/rc.mc_defaults
if [ $AUTOCNF == yes ]
then
param set MC_ROLL_P 6.5
param set MC_ROLLRATE_P 0.14
param set MC_ROLLRATE_I 0.1
param set MC_ROLLRATE_D 0.004
param set MC_PITCH_P 6.0
param set MC_PITCHRATE_P 0.14
param set MC_PITCHRATE_I 0.09
param set MC_PITCHRATE_D 0.004
param set MC_YAW_P 4
param set NAV_ACC_RAD 2.0
param set RTL_RETURN_ALT 30.0
param set RTL_DESCEND_ALT 10.0
fi
set MIXER quad_h
set PWM_RATE 50
set PWM_OUT 1234
set PWM_MIN 1100
set PWM_MAX 1900
set PWM_DISARMED 1500
set MIXER_AUX pass
set PWM_AUX_RATE 50
set PWM_AUX_OUT 1234
set PWM_AUX_MIN 1000
set PWM_AUX_MAX 2000
set PWM_AUX_DISARMED 950
ROMFS/px4fmu_common/init.d/4050_generic_250
+37
View File
@@ -0,0 +1,37 @@
#!nsh
#
# @name generic_250
#
# @type Quadrotor x
#
# @output AUX1 feed-through of RC AUX1 channel
# @output AUX2 feed-through of RC AUX2 channel
# @output AUX3 feed-through of RC AUX3 channel
#
# @maintainer Mark Whitehorn <kd0aij@gmail.com>
#
sh /etc/init.d/4001_quad_x
if [ $AUTOCNF == yes ]
then
param set MC_ROLL_P 7.0
param set MC_ROLLRATE_P 0.1
param set MC_ROLLRATE_I 0.1
param set MC_ROLLRATE_D 0.005
param set MC_PITCH_P 7.0
param set MC_PITCHRATE_P 0.1
param set MC_PITCHRATE_I 0.2
param set MC_PITCHRATE_D 0.005
param set MC_YAW_P 2.8
param set MC_YAWRATE_P 0.2
param set MC_YAWRATE_I 0.1
param set MC_YAWRATE_D 0.0
param set PWM_MIN 980
param set MPC_THR_MIN 0.06
param set MPC_MANTHR_MIN 0.06
param set MC_ROLLRATE_MAX 360.0
param set MC_PITCHRATE_MAX 360.0
param set MC_YAWRATE_MAX 200.0
param set ATT_BIAS_MAX 0.0
fi
ROMFS/px4fmu_common/init.d/rc.interface
+18 -7
View File
@@ -147,6 +147,7 @@ then
set FAILSAFE_AUX none
fi
# Set min / max for aux out and rates
if [ $PWM_AUX_OUT != none ]
then
#
@@ -157,13 +158,6 @@ then
pwm rate -c $PWM_AUX_OUT -r $PWM_AUX_RATE -d $OUTPUT_AUX_DEV
fi
#
# Set disarmed, min and max PWM_AUX values
#
if [ $PWM_AUX_DISARMED != none ]
then
pwm disarmed -c $PWM_AUX_OUT -p $PWM_AUX_DISARMED -d $OUTPUT_AUX_DEV
fi
if [ $PWM_AUX_MIN != none ]
then
pwm min -c $PWM_AUX_OUT -p $PWM_AUX_MIN -d $OUTPUT_AUX_DEV
@@ -174,6 +168,23 @@ then
fi
fi
# Set disarmed values for aux out
# Transitional support until all configs
# are updated
if [ $PWM_ACHDIS == none ]
then
set PWM_ACHDIS ${PWM_AUX_OUT}
fi
#
# Set disarmed, min and max PWM_AUX values
#
if [ $PWM_AUX_DISARMED != none -a $PWM_ACHDIS != none ]
then
pwm disarmed -c $PWM_ACHDIS -p $PWM_AUX_DISARMED -d $OUTPUT_AUX_DEV
fi
if [ $FAILSAFE_AUX != none ]
then
pwm failsafe -d $OUTPUT_AUX_DEV $FAILSAFE
ROMFS/px4fmu_common/init.d/rc.sensors
+4
View File
@@ -145,6 +145,10 @@ else
fi
fi
if sf10a start
then
fi
# Wait 20 ms for sensors (because we need to wait for the HRT and work queue callbacks to fire)
usleep 20000
if sensors start
ROMFS/px4fmu_common/init.d/rc.vtol_defaults
+10 -25
View File
@@ -4,40 +4,25 @@ set VEHICLE_TYPE vtol
if [ $AUTOCNF == yes ]
then
#
#Default controller parameters for MC
#
param set MC_ROLL_P 6.0
param set MC_ROLLRATE_P 0.1
param set MC_ROLLRATE_I 0.0
param set MC_ROLLRATE_D 0.003
param set MC_PITCH_P 6.0
param set MC_PITCHRATE_P 0.2
param set MC_PITCHRATE_I 0.0
param set MC_PITCHRATE_D 0.003
param set MC_YAW_P 4
param set MC_YAWRATE_P 0.2
param set MC_YAWRATE_I 0.0
param set MC_YAWRATE_D 0.0
param set MC_YAW_FF 0.3
#
# Default parameters for FW
#
param set FW_PR_FF 0.3
param set FW_PR_I 0.000
param set FW_PR_IMAX 0.2
param set FW_PR_P 0.02
param set FW_RR_FF 0.3
param set FW_RR_I 0.00
param set FW_RR_IMAX 0.2
param set FW_RR_P 0.02
param set PE_VELNE_NOISE 0.5
param set PE_VELD_NOISE 0.3
param set PE_POSNE_NOISE 0.5
param set PE_POSD_NOISE 1.25
param set PE_ABIAS_PNOISE 0.0001
#
# Default parameters for mission and position handling
#
param set NAV_ACC_RAD 3
param set MPC_TKO_SPEED 1.0
param set MPC_LAND_SPEED 0.7
param set MPC_Z_VEL_MAX 1.5
param set MPC_XY_VEL_MAX 4.0
param set MIS_YAW_TMT 10
fi
set PWM_DISARMED 900
ROMFS/px4fmu_common/init.d/rcS
+13 -73
View File
@@ -86,70 +86,6 @@ then
fi
fi
# Compare existing params and save defaults
# this only needs to be in for 1-2 releases
if param compare RC_MAP_THROTTLE 0
then
# So this is a default setup,
# now lets find out if channel 3
# is calibrated
if param compare RC3_MIN 1000
then
# This is default, do nothing
else
# Set old default
param set RC_MAP_THROTTLE 3
fi
fi
# Compare existing params and save defaults
# this only needs to be in for 1-2 releases
if param compare RC_MAP_ROLL 0
then
# So this is a default setup,
# now lets find out if channel 1
# is calibrated
if param compare RC1_MIN 1000
then
# This is default, do nothing
else
# Set old default
param set RC_MAP_ROLL 1
fi
fi
# Compare existing params and save defaults
# this only needs to be in for 1-2 releases
if param compare RC_MAP_PITCH 0
then
# So this is a default setup,
# now lets find out if channel 2
# is calibrated
if param compare RC2_MIN 1000
then
# This is default, do nothing
else
# Set old default
param set RC_MAP_PITCH 2
fi
fi
# Compare existing params and save defaults
# this only needs to be in for 1-2 releases
if param compare RC_MAP_YAW 0
then
# So this is a default setup,
# now lets find out if channel 4
# is calibrated
if param compare RC4_MIN 1000
then
# This is default, do nothing
else
# Set old default
param set RC_MAP_YAW 4
fi
fi
#
# Start system state indicator
#
@@ -182,6 +118,7 @@ then
set PWM_MAX none
set PWM_AUX_OUT none
set PWM_AUX_RATE none
set PWM_ACHDIS none
set PWM_AUX_DISARMED none
set PWM_AUX_MIN none
set PWM_AUX_MAX none
@@ -552,7 +489,7 @@ then
then
set MAVLINK_F "-r 1200 -d /dev/ttyS1"
# Start MAVLink on Wifi (ESP8266 port)
mavlink start -r 800000 -b 921600 -d /dev/ttyS0
mavlink start -r 20000 -m config -b 921600 -d /dev/ttyS0
fi
fi
fi
@@ -560,17 +497,12 @@ then
mavlink start $MAVLINK_F
unset MAVLINK_F
if ver hwcmp PX4FMU_V4
then
# Start MAVLink on OSD (TELEM2 port)
mavlink start -r 1200 -d /dev/ttyS2 -b 57600 -m osd
fi
#
# MAVLink onboard / TELEM2
#
if ver hwcmp PX4FMU_V2
if ver hwcmp PX4FMU_V1
then
else
# XXX We need a better way for runtime eval of shell variables,
# but this works for now
if param compare SYS_COMPANION 921600
@@ -589,6 +521,10 @@ then
then
mavlink start -d /dev/ttyS2 -b 57600 -m magic -r 5000 -x
fi
if param compare SYS_COMPANION 357600
then
mavlink start -d /dev/ttyS2 -b 57600 -r 1000
fi
# Sensors on the PWM interface bank
# clear pins 5 and 6
if param compare SENS_EN_LL40LS 1
@@ -676,6 +612,10 @@ then
then
set MAV_TYPE 2
fi
if [ $MIXER == quad_h ]
then
set MAV_TYPE 2
fi
if [ $MIXER == tri_y_yaw- -o $MIXER == tri_y_yaw+ ]
then
set MAV_TYPE 15
@@ -692,7 +632,7 @@ then
then
set MAV_TYPE 14
fi
if [ $MIXER == octo_cox ]
if [ $MIXER == octo_cox -o $MIXER == octo_cox_w ]
then
set MAV_TYPE 14
fi
ROMFS/px4fmu_common/mixers/caipirinha_vtol.main.mix
+22 -5
View File
@@ -1,10 +1,27 @@
#!nsh
# Caipirinha vtol mixer for PX4FMU
#
#===========================
Caipirinha tailsitter mixer
============================
This file defines a mixer for the TBS Caipirinha tailsitter edition. This vehicle
has two motors in total, one attached to each wing. It also has two elevons which
are located in the slipstream of the propellers.
Motor mixer
------------
When standing in front of the vehicle the first motor should be on the left side
while the second motor should be on the right side.
R: 2- 10000 10000 10000 0
#mixer for the elevons
Zero mixer (2x)
---------------
Z:
Z:
Elevons mixer
--------------
M: 2
O: 10000 10000 0 -10000 10000
S: 1 0 10000 10000 0 -10000 10000
ROMFS/px4fmu_common/mixers/octo_cox_w.main.mix
+3
View File
@@ -0,0 +1,3 @@
# Octo coaxial with wide arms
R: 8cw 10000 10000 10000 0
Tools/check_submodules.sh
+4 -4
View File
@@ -20,13 +20,13 @@ function check_git_submodule {
if [ -d $1 ];
then
SUBMODULE_STATUS=$(git submodule summary "$1")
STATUSRETVAL=$(echo $SUBMODULE_STATUS | grep -A20 -i "$1" | grep "<")
STATUSRETVAL=$(echo $SUBMODULE_STATUS | grep -A20 -i "$1")
if [ -z "$STATUSRETVAL" ]; then
echo "Checked $1 submodule, correct version found"
else
echo -e "\033[31mChecked $1 submodule, ACTION REQUIRED:\033[0m"
echo ""
echo -e "New commits required:"
echo -e "Different commits:"
echo -e "$SUBMODULE_STATUS"
echo ""
echo ""
@@ -59,7 +59,6 @@ if [ -d $1 ];
fi
else
git submodule update --init --recursive;
git submodule update;
fi
}
@@ -71,7 +70,8 @@ check_git_submodule Tools/jMAVSim
check_git_submodule Tools/sitl_gazebo
check_git_submodule cmake/cmake_hexagon
check_git_submodule mavlink/include/mavlink/v1.0
check_git_submodule src/lib/DriverFramework
check_git_submodule src/lib/DriverFramework/cmake_hexagon
check_git_submodule src/lib/DriverFramework/dspal
check_git_submodule src/lib/dspal
check_git_submodule src/lib/ecl
check_git_submodule src/lib/matrix
Tools/jMAVSim
+1 -1
Submodule Tools/jMAVSim updated: 0876a46d53...218ffbb8cf
Tools/px4airframes/xmlout.py
+2
View File
@@ -45,6 +45,8 @@ class XMLOutput():
xml_group.attrib["image"] = "OctoRotorX"
elif (group.GetName() == "Octorotor Coaxial"):
xml_group.attrib["image"] = "OctoRotorXCoaxial"
elif (group.GetName() == "Octo Coax Wide"):
xml_group.attrib["image"] = "OctoRotorXCoaxial"
elif (group.GetName() == "Quadrotor Wide"):
xml_group.attrib["image"] = "QuadRotorWide"
elif (group.GetName() == "Quadrotor H"):
Tools/px4params/srcparser.py
+45 -3
View File
@@ -51,6 +51,7 @@ class Parameter(object):
def __init__(self, name, type, default = ""):
self.fields = {}
self.values = {}
self.name = name
self.type = type
self.default = default
@@ -70,6 +71,12 @@ class Parameter(object):
"""
self.fields[code] = value
def SetEnumValue(self, code, value):
"""
Set named enum value
"""
self.values[code] = value
def GetFieldCodes(self):
"""
Return list of existing field codes in convenient order
@@ -87,7 +94,26 @@ class Parameter(object):
if not fv:
# required because python 3 sorted does not accept None
return ""
return self.fields.get(code)
return fv
def GetEnumCodes(self):
"""
Return list of existing value codes in convenient order
"""
keys = self.values.keys()
#keys = sorted(keys)
#keys = sorted(keys, key=lambda x: self.priority.get(x, 0), reverse=True)
return keys
def GetEnumValue(self, code):
"""
Return value of the given enum code or None if not found.
"""
fv = self.values.get(code)
if not fv:
# required because python 3 sorted does not accept None
return ""
return fv
class SourceParser(object):
"""
@@ -107,7 +133,7 @@ class SourceParser(object):
re_remove_dots = re.compile(r'\.+$')
re_remove_carriage_return = re.compile('\n+')
valid_tags = set(["group", "board", "min", "max", "unit", "decimal", "reboot_required"])
valid_tags = set(["group", "board", "min", "max", "unit", "decimal", "reboot_required", "value"])
# Order of parameter groups
priority = {
@@ -137,6 +163,7 @@ class SourceParser(object):
short_desc = None
long_desc = None
tags = {}
def_values = {}
elif state is not None and state != "comment-processed":
m = self.re_comment_end.search(line)
if m:
@@ -156,7 +183,12 @@ class SourceParser(object):
m = self.re_comment_tag.match(comment_content)
if m:
tag, desc = m.group(1, 2)
tags[tag] = desc
if (tag == "value"):
# Take the meta info string and split the code and description
metainfo = desc.split(" ", 1)
def_values[metainfo[0]] = metainfo[1]
else:
tags[tag] = desc
current_tag = tag
state = "wait-tag-end"
elif state == "wait-short":
@@ -191,6 +223,7 @@ class SourceParser(object):
defval = ""
# Non-empty line outside the comment
m = self.re_px4_param_default_definition.match(line)
# Default value handling
if m:
name_m, defval_m = m.group(1,2)
default_var[name_m] = defval_m
@@ -226,6 +259,8 @@ class SourceParser(object):
return False
else:
param.SetField(tag, tags[tag])
for def_value in def_values:
param.SetEnumValue(def_value, def_values[def_value])
# Store the parameter
if group not in self.param_groups:
self.param_groups[group] = ParameterGroup(group)
@@ -278,6 +313,13 @@ class SourceParser(object):
if default != "" and float(default) > float(max):
sys.stderr.write("Default value is larger than max: {0} default:{1} max:{2}\n".format(name, default, max))
return False
for code in param.GetEnumCodes():
if not self.IsNumber(code):
sys.stderr.write("Min value not number: {0} {1}\n".format(name, code))
return False
if param.GetEnumValue(code) == "":
sys.stderr.write("Description for enum value is empty: {0} {1}\n".format(name, code))
return False
return True
def GetParamGroups(self):
Tools/px4params/xmlout.py
+7
View File
@@ -52,6 +52,13 @@ class XMLOutput():
xml_field.text = value
if last_param_name != param.GetName():
board_specific_param_set = False
if len(param.GetEnumCodes()) > 0:
xml_values = ET.SubElement(xml_param, "values")
for code in param.GetEnumCodes():
xml_value = ET.SubElement(xml_values, "value")
xml_value.attrib["code"] = code;
xml_value.text = param.GetEnumValue(code)
indent(xml_parameters)
self.xml_document = ET.ElementTree(xml_parameters)
Tools/sitl_gazebo
+1 -1
Submodule Tools/sitl_gazebo updated: 16f7fb3761...b28258f1ac
cmake/common/px4_base.cmake
+25 -10
View File
@@ -138,13 +138,15 @@ function(px4_add_git_submodule)
string(REPLACE "/" "_" NAME ${PATH})
add_custom_command(OUTPUT ${CMAKE_BINARY_DIR}/git_init_${NAME}.stamp
WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
COMMAND git submodule init ${PATH}
COMMAND git submodule update --init --recursive ${PATH}
COMMAND touch ${CMAKE_BINARY_DIR}/git_init_${NAME}.stamp
DEPENDS ${CMAKE_SOURCE_DIR}/.gitmodules
)
add_custom_target(${TARGET}
WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
COMMAND git submodule update --recursive ${PATH}
# This is NOT a good approach as it overwrites checked out branches
# behind the back of a developer
#COMMAND git submodule update --recursive ${PATH}
DEPENDS ${CMAKE_BINARY_DIR}/git_init_${NAME}.stamp
)
endfunction()
@@ -534,15 +536,28 @@ function(px4_add_common_flags)
)
endif()
set(max_optimization -Os)
if ($ENV{MEMORY_DEBUG} MATCHES "1")
set(max_optimization -O0)
set(optimization_flags
-fno-strict-aliasing
-fomit-frame-pointer
-funsafe-math-optimizations
-ffunction-sections
-fdata-sections
)
set(optimization_flags
-fno-strict-aliasing
-fno-omit-frame-pointer
-funsafe-math-optimizations
-ffunction-sections
-fdata-sections
-g -fsanitize=address
)
else()
set(max_optimization -Os)
set(optimization_flags
-fno-strict-aliasing
-fomit-frame-pointer
-funsafe-math-optimizations
-ffunction-sections
-fdata-sections
)
endif()
if (NOT ${CMAKE_C_COMPILER_ID} MATCHES ".*Clang.*")
list(APPEND optimization_flags
msg/airspeed.msg
+1
View File
@@ -3,3 +3,4 @@ float32 indicated_airspeed_m_s # indicated airspeed in meters per second, -1 if
float32 true_airspeed_m_s # true filtered airspeed in meters per second, -1 if unknown
float32 true_airspeed_unfiltered_m_s # true airspeed in meters per second, -1 if unknown
float32 air_temperature_celsius # air temperature in degrees celsius, -1000 if unknown
float32 confidence # confidence value from 0 to 1 for this sensor
msg/battery_status.msg
+1
View File
@@ -3,3 +3,4 @@ float32 voltage_v # Battery voltage in volts, 0 if unknown
float32 voltage_filtered_v # Battery voltage in volts, filtered, 0 if unknown
float32 current_a # Battery current in amperes, -1 if unknown
float32 discharged_mah # Discharged amount in mAh, -1 if unknown
bool is_powering_off # Power off event imminent indication, false if unknown
msg/estimator_status.msg
+11
View File
@@ -5,3 +5,14 @@ uint8 nan_flags # Bitmask to indicate NaN states
uint8 health_flags # Bitmask to indicate sensor health states (vel, pos, hgt)
uint8 timeout_flags # Bitmask to indicate timeout flags (vel, pos, hgt)
float32[28] covariances # Diagonal Elements of Covariance Matrix
uint16 gps_check_fail_flags # Bitmask to indicate status of GPS checks - see definition below
# bits are true when corresponding test has failed
# 0 : minimum required sat count fail
# 1 : minimum required GDoP fail
# 2 : maximum allowed horizontal position error fail
# 3 : maximum allowed vertical position error fail
# 4 : maximum allowed speed error fail
# 5 : maximum allowed horizontal position drift fail
# 6 : maximum allowed vertical position drift fail
# 7 : maximum allowed horizontal speed fail
# 8 : maximum allowed vertical velocity discrepancy fail
msg/fw_virtual_attitude_setpoint.msg
+14
View File
@@ -1,3 +1,12 @@
###############################################################################################
# The vehicle_attitude_setpoint.msg needs to be in sync with the virtual setpoint messages
#
# Please keep the following messages identical;
# vehicle_attitude_setpoint.msg
# mc_virtual_attitude_setpoint.msg
# fw_virtual_attitude_setpoint.msg
#
###############################################################################################
uint64 timestamp # in microseconds since system start, is set whenever the writing thread stores new data
@@ -21,3 +30,8 @@ float32 thrust # Thrust in Newton the power system should generate
bool roll_reset_integral # Reset roll integral part (navigation logic change)
bool pitch_reset_integral # Reset pitch integral part (navigation logic change)
bool yaw_reset_integral # Reset yaw integral part (navigation logic change)
bool fw_control_yaw # control heading with rudder (used for auto takeoff on runway)
bool disable_mc_yaw_control # control yaw for mc (used for vtol weather-vane mode)
bool apply_flaps
msg/fw_virtual_rates_setpoint.msg
+11 -1
View File
@@ -1,6 +1,16 @@
###############################################################################################
# The vehicle_rates_setpoint.msg needs to be in sync with the virtual setpoint messages
#
# Please keep the following messages identical;
# vehicle_rates_setpoint.msg
# mc_virtual_rates_setpoint.msg
# fw_virtual_rates_setpoint.msg
#
###############################################################################################
uint64 timestamp # in microseconds since system start
float32 roll # body angular rates in NED frame
float32 pitch # body angular rates in NED frame
float32 yaw # body angular rates in NED frame
float32 yaw # body angular rates in NED frame
float32 thrust # thrust normalized to 0..1
msg/mc_virtual_attitude_setpoint.msg
+14
View File
@@ -1,3 +1,12 @@
###############################################################################################
# The vehicle_attitude_setpoint.msg needs to be in sync with the virtual setpoint messages
#
# Please keep the following messages identical;
# vehicle_attitude_setpoint.msg
# mc_virtual_attitude_setpoint.msg
# fw_virtual_attitude_setpoint.msg
#
###############################################################################################
uint64 timestamp # in microseconds since system start, is set whenever the writing thread stores new data
@@ -21,3 +30,8 @@ float32 thrust # Thrust in Newton the power system should generate
bool roll_reset_integral # Reset roll integral part (navigation logic change)
bool pitch_reset_integral # Reset pitch integral part (navigation logic change)
bool yaw_reset_integral # Reset yaw integral part (navigation logic change)
bool fw_control_yaw # control heading with rudder (used for auto takeoff on runway)
bool disable_mc_yaw_control # control yaw for mc (used for vtol weather-vane mode)
bool apply_flaps
msg/mc_virtual_rates_setpoint.msg
+11 -1
View File
@@ -1,6 +1,16 @@
###############################################################################################
# The vehicle_rates_setpoint.msg needs to be in sync with the virtual setpoint messages
#
# Please keep the following messages identical;
# vehicle_rates_setpoint.msg
# mc_virtual_rates_setpoint.msg
# fw_virtual_rates_setpoint.msg
#
###############################################################################################
uint64 timestamp # in microseconds since system start
float32 roll # body angular rates in NED frame
float32 pitch # body angular rates in NED frame
float32 yaw # body angular rates in NED frame
float32 yaw # body angular rates in NED frame
float32 thrust # thrust normalized to 0..1
msg/mission_result.msg
+2 -1
View File
@@ -6,7 +6,8 @@ bool warning # true if mission is valid, but has potentially problematic items
bool reached # true if mission has been reached
bool finished # true if mission has been completed
bool stay_in_failsafe # true if the commander should not switch out of the failsafe mode
bool flight_termination # true if the navigator demands a flight termination from the commander app
bool flight_termination # true if the navigator demands a flight termination from the commander app
bool item_do_jump_changed # true if the number of do jumps remaining has changed
uint32 item_changed_index # indicate which item has changed
uint32 item_do_jump_remaining # set to the number of do jumps remaining for that item
bool mission_failure # true if the mission cannot continue or be completed for some reason
msg/position_setpoint.msg
+1
View File
@@ -23,6 +23,7 @@ float64 lon # longitude, in deg
float32 alt # altitude AMSL, in m
float32 yaw # yaw (only for multirotors), in rad [-PI..PI), NaN = hold current yaw
bool yaw_valid # true if yaw setpoint valid
bool disable_mc_yaw_control # control yaw for mc (used for vtol weather-vane mode)
float32 yawspeed # yawspeed (only for multirotors, in rad/s)
bool yawspeed_valid # true if yawspeed setpoint valid
float32 loiter_radius # loiter radius (only for fixed wing), in m
msg/vehicle_attitude_setpoint.msg
+10
View File
@@ -1,3 +1,12 @@
###############################################################################################
# The vehicle_attitude_setpoint.msg needs to be in sync with the virtual setpoint messages
#
# Please keep the following messages identical;
# vehicle_attitude_setpoint.msg
# mc_virtual_attitude_setpoint.msg
# fw_virtual_attitude_setpoint.msg
#
###############################################################################################
uint64 timestamp # in microseconds since system start, is set whenever the writing thread stores new data
@@ -23,5 +32,6 @@ bool pitch_reset_integral # Reset pitch integral part (navigation logic change
bool yaw_reset_integral # Reset yaw integral part (navigation logic change)
bool fw_control_yaw # control heading with rudder (used for auto takeoff on runway)
bool disable_mc_yaw_control # control yaw for mc (used for vtol weather-vane mode)
bool apply_flaps
msg/vehicle_rates_setpoint.msg
+11 -1
View File
@@ -1,6 +1,16 @@
###############################################################################################
# The vehicle_rates_setpoint.msg needs to be in sync with the virtual setpoint messages
#
# Please keep the following messages identical;
# vehicle_rates_setpoint.msg
# mc_virtual_rates_setpoint.msg
# fw_virtual_rates_setpoint.msg
#
###############################################################################################
uint64 timestamp # in microseconds since system start
float32 roll # body angular rates in NED frame
float32 pitch # body angular rates in NED frame
float32 yaw # body angular rates in NED frame
float32 yaw # body angular rates in NED frame
float32 thrust # thrust normalized to 0..1
msg/vehicle_status.msg
+4 -3
View File
@@ -117,10 +117,9 @@ uint32 system_id # system id, inspired by MAVLink's system ID field
uint32 component_id # subsystem / component id, inspired by MAVLink's component ID field
bool is_rotary_wing # True if system is in rotary wing configuration, so for a VTOL this is only true while flying as a multicopter
bool vtol_in_transition # True if VTOL is doing a transition
bool is_vtol # True if the system is VTOL capable
bool vtol_fw_permanent_stab # True if vtol should stabilize attitude for fw in manual mode
bool in_transition_mode
bool in_transition_mode # True if VTOL is doing a transition
bool condition_battery_voltage_valid
bool condition_system_in_air_restore # true if we can restore in mid air
@@ -151,9 +150,11 @@ bool data_link_lost_cmd # datalink to GCS lost mode commanded
uint8 data_link_lost_counter # counts unique data link lost events
bool engine_failure # Set to true if an engine failure is detected
bool engine_failure_cmd # Set to true if an engine failure mode is commanded
bool vtol_transition_failure # Set to true if vtol transition failed
bool vtol_transition_failure_cmd # Set to true if vtol transition failure mode is commanded
bool gps_failure # Set to true if a gps failure is detected
bool gps_failure_cmd # Set to true if a gps failure mode is commanded
bool mission_failure # Set to true if mission could not continue/finish
bool barometer_failure # Set to true if a barometer failure is detected
bool offboard_control_signal_found_once
msg/vtol_vehicle_status.msg
+5 -4
View File
@@ -1,5 +1,6 @@
uint64 timestamp # Microseconds since system boot
bool vtol_in_rw_mode # true: vtol vehicle is in rotating wing mode
uint64 timestamp # Microseconds since system boot
bool vtol_in_rw_mode # true: vtol vehicle is in rotating wing mode
bool vtol_in_trans_mode
bool fw_permanent_stab # In fw mode stabilize attitude even if in manual mode
float32 airspeed_tot # Estimated airspeed over control surfaces
bool vtol_transition_failsafe # vtol in transition failsafe mode
bool fw_permanent_stab # In fw mode stabilize attitude even if in manual mode
float32 airspeed_tot # Estimated airspeed over control surfaces
nuttx-configs/px4fmu-v2/nsh/defconfig
+1 -1
View File
@@ -667,7 +667,7 @@ CONFIG_CDCACM_NWRREQS=4
CONFIG_CDCACM_NRDREQS=4
CONFIG_CDCACM_BULKIN_REQLEN=96
CONFIG_CDCACM_RXBUFSIZE=600
CONFIG_CDCACM_TXBUFSIZE=4000
CONFIG_CDCACM_TXBUFSIZE=8000
CONFIG_CDCACM_VENDORID=0x26ac
CONFIG_CDCACM_PRODUCTID=0x0011
CONFIG_CDCACM_VENDORSTR="3D Robotics"
nuttx-configs/px4fmu-v4/nsh/defconfig
+2 -2
View File
@@ -552,7 +552,7 @@ CONFIG_UART7_SERIAL_CONSOLE=y
# USART1 Configuration
#
CONFIG_USART1_RXBUFSIZE=600
CONFIG_USART1_TXBUFSIZE=2000
CONFIG_USART1_TXBUFSIZE=4000
CONFIG_USART1_BAUD=115200
CONFIG_USART1_BITS=8
CONFIG_USART1_PARITY=0
@@ -667,7 +667,7 @@ CONFIG_CDCACM_NWRREQS=4
CONFIG_CDCACM_NRDREQS=4
CONFIG_CDCACM_BULKIN_REQLEN=96
CONFIG_CDCACM_RXBUFSIZE=600
CONFIG_CDCACM_TXBUFSIZE=4000
CONFIG_CDCACM_TXBUFSIZE=8000
CONFIG_CDCACM_VENDORID=0x26ac
CONFIG_CDCACM_PRODUCTID=0x0012
CONFIG_CDCACM_VENDORSTR="3D Robotics"
posix-configs/SITL/init/rcS_gazebo_iris
+1 -1
View File
@@ -50,7 +50,7 @@ mc_pos_control start
mc_att_control start
mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_w.main.mix
mavlink start -u 14556 -r 2000000
mavlink start -u 14557 -r 2000000 -m onboard
mavlink start -u 14557 -r 2000000 -m onboard -o 14540
mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556
mavlink stream -r 80 -s LOCAL_POSITION_NED -u 14556
mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556
posix-configs/SITL/init/rcS_gazebo_iris_opt_flow
+63
View File
@@ -0,0 +1,63 @@
uorb start
simulator start -s
param load
param set MAV_TYPE 2
param set MC_PITCHRATE_P 0.3
param set MC_ROLLRATE_P 0.3
param set MC_YAW_P 2.0
param set MC_YAWRATE_P 0.35
param set SYS_AUTOSTART 4010
param set SYS_RESTART_TYPE 2
dataman start
param set CAL_GYRO0_ID 2293768
param set CAL_ACC0_ID 1376264
param set CAL_ACC1_ID 1310728
param set CAL_MAG0_ID 196616
param set CAL_GYRO0_XOFF 0.01
param set CAL_ACC0_XOFF 0.01
param set CAL_ACC0_YOFF -0.01
param set CAL_ACC0_ZOFF 0.01
param set CAL_ACC0_XSCALE 1.01
param set CAL_ACC0_YSCALE 1.01
param set CAL_ACC0_ZSCALE 1.01
param set CAL_ACC1_XOFF 0.01
param set CAL_MAG0_XOFF 0.01
param set MPC_XY_P 0.15
param set MPC_XY_VEL_P 0.05
param set MPC_XY_VEL_D 0.005
param set MPC_XY_FF 0.1
param set SENS_BOARD_ROT 8
param set COM_RC_IN_MODE 1
param set NAV_ACC_RAD 2.0
param set RTL_RETURN_ALT 30.0
param set RTL_DESCEND_ALT 10.0
rgbledsim start
tone_alarm start
gyrosim start
accelsim start
barosim start
adcsim start
gpssim start
pwm_out_sim mode_pwm
sleep 1
sensors start
commander start
land_detector start multicopter
navigator start
attitude_estimator_q start
position_estimator_inav start
mc_pos_control start
mc_att_control start
mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_w.main.mix
mavlink start -u 14556 -r 2000000
mavlink start -u 14557 -r 2000000 -m onboard -o 14540
mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556
mavlink stream -r 80 -s LOCAL_POSITION_NED -u 14556
mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556
mavlink stream -r 80 -s ATTITUDE -u 14556
mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556
mavlink stream -r 20 -s RC_CHANNELS -u 14556
mavlink stream -r 250 -s HIGHRES_IMU -u 14556
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556
mavlink boot_complete
sdlog2 start -r 100 -e -t -a
posix-configs/SITL/init/rcS_gazebo_standard_vtol
+5 -2
View File
@@ -27,9 +27,12 @@ param set MPC_XY_P 0.15
param set MPC_XY_VEL_P 0.05
param set MPC_XY_VEL_D 0.005
param set MPC_XY_FF 0.1
param set MPC_Z_VEL_MAX 1.0
param set MPC_Z_VEL_MAX 1.5
param set SENS_BOARD_ROT 8
param set COM_RC_IN_MODE 1
param set NAV_ACC_RAD 3.0
param set MPC_TKO_SPEED 1.0
param set MIS_YAW_TMT 10
rgbledsim start
tone_alarm start
gyrosim start
@@ -53,7 +56,7 @@ fw_pos_control_l1 start
fw_att_control start
mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/standard_vtol_sitl.main.mix
mavlink start -u 14556 -r 2000000
mavlink start -u 14557 -r 2000000 -m onboard
mavlink start -u 14557 -r 2000000 -m onboard -o 14540
mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556
mavlink stream -r 80 -s LOCAL_POSITION_NED -u 14556
mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556
posix-configs/SITL/init/rcS_gazebo_tailsitter
+1 -1
View File
@@ -53,7 +53,7 @@ fw_pos_control_l1 start
fw_att_control start
mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_x_vtol.main.mix
mavlink start -u 14556 -r 2000000
mavlink start -u 14557 -r 2000000 -m onboard
mavlink start -u 14557 -r 2000000 -m onboard -o 14540
mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556
mavlink stream -r 80 -s LOCAL_POSITION_NED -u 14556
mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556
posix-configs/SITL/init/rcS_jmavsim_iris
+1 -1
View File
@@ -56,7 +56,7 @@ mc_pos_control start
mc_att_control start
mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_x.main.mix
mavlink start -u 14556 -r 2000000
mavlink start -u 14557 -r 2000000 -m onboard
mavlink start -u 14557 -r 2000000 -m onboard -o 14540
mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556
mavlink stream -r 80 -s LOCAL_POSITION_NED -u 14556
mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556
posix-configs/SITL/init/rcS_lpe_gazebo_iris
+1
View File
@@ -50,6 +50,7 @@ mc_pos_control start
mc_att_control start
mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_w.main.mix
mavlink start -u 14556 -r 2000000
mavlink start -u 14557 -r 2000000 -m onboard -o 14540
mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556
mavlink stream -r 80 -s LOCAL_POSITION_NED_COV -u 14556
mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556
posix-configs/SITL/init/rcS_lpe_gazebo_iris_opt_flow
+71
View File
@@ -0,0 +1,71 @@
uorb start
simulator start -s
param load
param set MAV_TYPE 2
param set MC_PITCHRATE_P 0.3
param set MC_ROLLRATE_P 0.3
param set MC_YAW_P 2.0
param set MC_YAWRATE_P 0.35
param set SYS_AUTOSTART 4010
param set SYS_RESTART_TYPE 2
dataman start
param set CAL_GYRO0_ID 2293768
param set CAL_ACC0_ID 1376264
param set CAL_ACC1_ID 1310728
param set CAL_MAG0_ID 196616
param set CAL_GYRO0_XOFF 0.01
param set CAL_ACC0_XOFF 0.01
param set CAL_ACC0_YOFF -0.01
param set CAL_ACC0_ZOFF 0.01
param set CAL_ACC0_XSCALE 1.01
param set CAL_ACC0_YSCALE 1.01
param set CAL_ACC0_ZSCALE 1.01
param set CAL_ACC1_XOFF 0.01
param set CAL_MAG0_XOFF 0.01
param set MPC_XY_P 0.15
param set MPC_XY_VEL_P 0.05
param set MPC_XY_VEL_D 0.005
param set MPC_XY_FF 0.1
param set SENS_BOARD_ROT 8
param set COM_RC_IN_MODE 1
param set NAV_ACC_RAD 2.0
param set RTL_RETURN_ALT 30.0
param set RTL_DESCEND_ALT 10.0
# changes for LPE
param set COM_RC_IN_MODE 1
param set LPE_BETA_MAX 10000
rgbledsim start
tone_alarm start
gyrosim start
accelsim start
barosim start
adcsim start
gpssim start
pwm_out_sim mode_pwm
sleep 1
sensors start
commander start
land_detector start multicopter
navigator start
attitude_estimator_q start
mc_pos_control start
mc_att_control start
mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_w.main.mix
mavlink start -u 14556 -r 2000000
mavlink start -u 14557 -r 2000000 -m onboard -o 14540
mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556
mavlink stream -r 80 -s LOCAL_POSITION_NED_COV -u 14556
mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556
mavlink stream -r 80 -s ATTITUDE -u 14556
mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556
mavlink stream -r 20 -s RC_CHANNELS -u 14556
mavlink stream -r 250 -s HIGHRES_IMU -u 14556
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556
mavlink boot_complete
sdlog2 start -r 100 -e -t -a
# start LPE at end, when we know it is ok to init sensors
sleep 5
local_position_estimator start
posix-configs/SITL/init/rcS_lpe_jmavsim_iris
+1 -1
View File
@@ -61,7 +61,7 @@ mc_pos_control start
mc_att_control start
mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_x.main.mix
mavlink start -u 14556 -r 2000000
mavlink start -u 14557 -r 2000000 -m onboard
mavlink start -u 14557 -r 2000000 -m onboard -o 14540
mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556
mavlink stream -r 80 -s LOCAL_POSITION_NED_COV -u 14556
mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556
src/drivers/batt_smbus/batt_smbus.cpp
+526 -30
View File
@@ -77,30 +77,48 @@
#include <drivers/drv_batt_smbus.h>
#include <drivers/device/ringbuffer.h>
#define BATT_SMBUS_ADDR_MIN 0x08 ///< lowest possible address
#define BATT_SMBUS_ADDR_MAX 0x7F ///< highest possible address
#define BATT_SMBUS_ADDR_MIN 0x08 ///< lowest possible address
#define BATT_SMBUS_ADDR_MAX 0x7F ///< highest possible address
#define BATT_SMBUS_I2C_BUS PX4_I2C_BUS_EXPANSION
#define BATT_SMBUS_ADDR 0x0B ///< I2C address
#define BATT_SMBUS_TEMP 0x08 ///< temperature register
#define BATT_SMBUS_VOLTAGE 0x09 ///< voltage register
#define BATT_SMBUS_I2C_BUS PX4_I2C_BUS_EXPANSION
#define BATT_SMBUS_ADDR 0x0B ///< I2C address
#define BATT_SMBUS_TEMP 0x08 ///< temperature register
#define BATT_SMBUS_VOLTAGE 0x09 ///< voltage register
#define BATT_SMBUS_REMAINING_CAPACITY 0x0f ///< predicted remaining battery capacity as a percentage
#define BATT_SMBUS_FULL_CHARGE_CAPACITY 0x10 ///< capacity when fully charged
#define BATT_SMBUS_DESIGN_CAPACITY 0x18 ///< design capacity register
#define BATT_SMBUS_DESIGN_VOLTAGE 0x19 ///< design voltage register
#define BATT_SMBUS_SERIALNUM 0x1c ///< serial number register
#define BATT_SMBUS_MANUFACTURE_NAME 0x20 ///< manufacturer name
#define BATT_SMBUS_MANUFACTURE_INFO 0x25 ///< cell voltage register
#define BATT_SMBUS_CURRENT 0x2a ///< current register
#define BATT_SMBUS_MEASUREMENT_INTERVAL_MS (1000000 / 10) ///< time in microseconds, measure at 10hz
#define BATT_SMBUS_TIMEOUT_MS 10000000 ///< timeout looking for battery 10seconds after startup
#define BATT_SMBUS_DESIGN_CAPACITY 0x18 ///< design capacity register
#define BATT_SMBUS_DESIGN_VOLTAGE 0x19 ///< design voltage register
#define BATT_SMBUS_MANUFACTURE_DATE 0x1B ///< manufacture date register
#define BATT_SMBUS_SERIAL_NUMBER 0x1C ///< serial number register
#define BATT_SMBUS_MANUFACTURER_NAME 0x20 ///< manufacturer name
#define BATT_SMBUS_DEVICE_NAME 0x21 ///< device name register
#define BATT_SMBUS_DEVICE_CHEMISTRY 0x22 ///< device chemistry register
#define BATT_SMBUS_MANUFACTURER_DATA 0x23 ///< manufacturer data
#define BATT_SMBUS_MANUFACTURE_INFO 0x25 ///< cell voltage register
#define BATT_SMBUS_CURRENT 0x2a ///< current register
#define BATT_SMBUS_MEASUREMENT_INTERVAL_US (1000000 / 10) ///< time in microseconds, measure at 10Hz
#define BATT_SMBUS_TIMEOUT_US 10000000 ///< timeout looking for battery 10seconds after startup
#define BATT_SMBUS_PEC_POLYNOMIAL 0x07 ///< Polynomial for calculating PEC
#define BATT_SMBUS_BUTTON_DEBOUNCE_MS 300 ///< button holds longer than this time will cause a power off event
#define BATT_SMBUS_MANUFACTURER_ACCESS 0x00
#define BATT_SMBUS_MANUFACTURER_BLOCK_ACCESS 0x44
#define BATT_SMBUS_PEC_POLYNOMIAL 0x07 ///< Polynomial for calculating PEC
#ifndef CONFIG_SCHED_WORKQUEUE
# error This requires CONFIG_SCHED_WORKQUEUE.
#endif
struct battery_type {
char *ManufacturerName;
char *DeviceName;
char *DeviceChemistry;
};
// Declaration of the solo battery data, as determined by reading out the data from multiple 3DR Solo batteries
const struct battery_type solo_battery = {(char *)"BMTPOW", (char *)"MA03", (char *)"LIon"};
class BATT_SMBUS : public device::I2C
{
public:
@@ -133,6 +151,62 @@ public:
*/
int search();
/**
* Get the SBS manufacturer name of the battery device
*
* @param manufacturer_name pointer a buffer into which the manufacturer name is to be written
* @param max_length the maximum number of bytes to attempt to read from the manufacturer name register, including the null character that is appended to the end
*
* @return the number of bytes read
*/
uint8_t manufacturer_name(uint8_t *man_name, uint8_t max_length);
/**
* Return the SBS manufacture date of the battery device
*
* @return the date in the following format:
* see Smart Battery Data Specification, Revision 1.1
* http://sbs-forum.org/specs/sbdat110.pdf for more details
* Date as uint16_t = (year-1980) * 512 + month * 32 + day
* | Field | Bits | Format | Allowable Values |
* | ----- | ---- | ------------------ | ------------------------------------------ |
* | Day 0-4 5-bit binary value 1-31 (corresponds to day) |
* | Month 5-8 4-bit binary value 1-12 (corresponds to month number) |
* | Year 9-15 7-bit binary value 0-127 (corresponds to year biased by 1980) |
* otherwise, return 0 on failure
*/
uint16_t manufacture_date();
/**
* Get the SBS device name of the battery device
*
* @param dev_name pointer a buffer into which the device name is to be written
* @param max_length the maximum number of bytes to attempt to read from the device name register, including the null character that is appended to the end
*
* @return the number of bytes read
*/
uint8_t device_name(uint8_t *dev_name, uint8_t max_length);
/**
* Return the SBS serial number of the battery device
*/
uint16_t serial_number();
/**
* Get the SBS device chemistry of the battery device
*
* @param dev_chem pointer a buffer into which the device chemistry is to be written
* @param max_length the maximum number of bytes to attempt to read from the device chemistry register, including the null character that is appended to the end
*
* @return the number of bytes read
*/
uint8_t device_chemistry(uint8_t *dev_chem, uint8_t max_length);
/**
* Checks whether the current SBS battery data corresponds to a 3DR Solo battery
*/
bool is_solo_battery();
protected:
/**
* Check if the device can be contacted
@@ -166,18 +240,41 @@ private:
*/
int read_reg(uint8_t reg, uint16_t &val);
/**
* Write a word to specified register
*/
int write_reg(uint8_t reg, uint16_t val);
/**
* Read block from bus
* @return returns number of characters read if successful, zero if unsuccessful
*/
uint8_t read_block(uint8_t reg, uint8_t *data, uint8_t max_len, bool append_zero);
/**
* Write block to the bus
* @return the number of characters sent if successful, zero if unsuccessful
*/
uint8_t write_block(uint8_t reg, uint8_t *data, uint8_t len);
/**
* Calculate PEC for a read or write from the battery
* @param buff is the data that was read or will be written
*/
uint8_t get_PEC(uint8_t cmd, bool reading, const uint8_t buff[], uint8_t len) const;
/**
* Write a word to Manufacturer Access register (0x00)
* @param cmd the word to be written to Manufacturer Access
*/
uint8_t ManufacturerAccess(uint16_t cmd);
/**
* Checks if the battery that has been detected is a 3DR Solo Battery. If it is, it sets
* the private variable _is_solo_battery to be true
*/
void check_if_solo_battery();
// internal variables
bool _enabled; ///< true if we have successfully connected to battery
work_s _work; ///< work queue for scheduling reads
@@ -187,6 +284,11 @@ private:
orb_id_t _batt_orb_id; ///< uORB battery topic ID
uint64_t _start_time; ///< system time we first attempt to communicate with battery
uint16_t _batt_capacity; ///< battery's design capacity in mAh (0 means unknown)
char *_manufacturer_name; ///< The name of the battery manufacturer
char *_device_name; ///< The name of the battery device
char *_device_chemistry; ///< The battery chemistry
bool _is_solo_battery; ///< Boolean as to whether the battery detected is a 3DR Solo Battery or not
uint8_t _button_press_counts; ///< count of button presses detected on 3DR Solo Battery
};
namespace
@@ -198,15 +300,27 @@ void batt_smbus_usage();
extern "C" __EXPORT int batt_smbus_main(int argc, char *argv[]);
int manufacturer_name();
int manufacture_date();
int device_name();
int serial_number();
int device_chemistry();
int solo_battery_check();
BATT_SMBUS::BATT_SMBUS(int bus, uint16_t batt_smbus_addr) :
I2C("batt_smbus", BATT_SMBUS_DEVICE_PATH, bus, batt_smbus_addr, 400000),
I2C("batt_smbus", BATT_SMBUS0_DEVICE_PATH, bus, batt_smbus_addr, 100000),
_enabled(false),
_work{},
_reports(nullptr),
_batt_topic(nullptr),
_batt_topic(-1),
_batt_orb_id(nullptr),
_start_time(0),
_batt_capacity(0)
_batt_capacity(0),
_manufacturer_name(nullptr),
_device_name(nullptr),
_device_chemistry(nullptr),
_is_solo_battery(false),
_button_press_counts(0)
{
// work_cancel in the dtor will explode if we don't do this...
memset(&_work, 0, sizeof(_work));
@@ -223,6 +337,18 @@ BATT_SMBUS::~BATT_SMBUS()
if (_reports != nullptr) {
delete _reports;
}
if (_manufacturer_name != nullptr) {
delete _manufacturer_name;
}
if (_device_name != nullptr) {
delete _device_name;
}
if (_device_chemistry != nullptr) {
delete _device_chemistry;
}
}
int
@@ -297,8 +423,8 @@ BATT_SMBUS::test()
if (updated) {
if (orb_copy(ORB_ID(battery_status), sub, &status) == OK) {
warnx("V=%4.2f C=%4.2f DismAh=%4.2f Cap:%d", (float)status.voltage_v, (float)status.current_a,
(float)status.discharged_mah, (int)_batt_capacity);
warnx("V=%4.2f C=%4.2f DismAh=%4.2f Cap:%d Shutdown:%d", (double)status.voltage_v, (double)status.current_a,
(double)status.discharged_mah, (int)_batt_capacity, (int)status.is_powering_off);
}
}
@@ -343,6 +469,89 @@ BATT_SMBUS::search()
return OK;
}
uint8_t
BATT_SMBUS::manufacturer_name(uint8_t *man_name, uint8_t max_length)
{
uint8_t len = read_block(BATT_SMBUS_MANUFACTURER_NAME, man_name, max_length, false);
if (len > 0) {
if (len >= max_length - 1) {
man_name[max_length - 1] = 0;
} else {
man_name[len] = 0;
}
}
return len;
}
uint16_t
BATT_SMBUS::manufacture_date()
{
uint16_t man_date;
if (read_reg(BATT_SMBUS_MANUFACTURE_DATE, man_date) == OK) {
return man_date;
}
// Return 0 if could not read the date correctly
return 0;
}
uint8_t
BATT_SMBUS::device_name(uint8_t *dev_name, uint8_t max_length)
{
uint8_t len = read_block(BATT_SMBUS_DEVICE_NAME, dev_name, max_length, false);
if (len > 0) {
if (len >= max_length - 1) {
dev_name[max_length - 1] = 0;
} else {
dev_name[len] = 0;
}
}
return len;
}
uint16_t
BATT_SMBUS::serial_number()
{
uint16_t serial_num;
if (read_reg(BATT_SMBUS_SERIAL_NUMBER, serial_num) == OK) {
return serial_num;
}
return -1;
}
uint8_t
BATT_SMBUS::device_chemistry(uint8_t *dev_chem, uint8_t max_length)
{
uint8_t len = read_block(BATT_SMBUS_DEVICE_CHEMISTRY, dev_chem, max_length, false);
if (len > 0) {
if (len >= max_length - 1) {
dev_chem[max_length - 1] = 0;
} else {
dev_chem[len] = 0;
}
}
return len;
}
bool
BATT_SMBUS::is_solo_battery()
{
check_if_solo_battery();
return _is_solo_battery;
}
int
BATT_SMBUS::probe()
{
@@ -381,11 +590,53 @@ BATT_SMBUS::cycle()
uint64_t now = hrt_absolute_time();
// exit without rescheduling if we have failed to find a battery after 10 seconds
if (!_enabled && (now - _start_time > BATT_SMBUS_TIMEOUT_MS)) {
if (!_enabled && (now - _start_time > BATT_SMBUS_TIMEOUT_US)) {
warnx("did not find smart battery");
return;
}
bool perform_solo_battry_check = false; // Only check if it is a solo battery if changes have been made to the SBS data
// Try and get battery SBS info
if (_manufacturer_name == nullptr) {
char man_name[21];
uint8_t len = manufacturer_name((uint8_t *)man_name, sizeof(man_name));
if (len > 0) {
_manufacturer_name = new char[len];
strcpy(_manufacturer_name, man_name);
perform_solo_battry_check = true;
}
}
if (_device_name == nullptr) {
char dev_name[21];
uint8_t len = device_name((uint8_t *)dev_name, sizeof(dev_name));
if (len > 0) {
_device_name = new char[len];
strcpy(_device_name, dev_name);
perform_solo_battry_check = true;
}
}
if (_device_chemistry == nullptr) {
char dev_chem[21];
uint8_t len = device_chemistry((uint8_t *)dev_chem, sizeof(dev_chem));
if (len > 0) {
_device_chemistry = new char[len];
strcpy(_device_chemistry, dev_chem);
perform_solo_battry_check = true;
}
}
// If necessary, check if the battery is a 3DR Solo Battery
if (perform_solo_battry_check) {
warnx("Checking solo battery");
check_if_solo_battery();
}
// read data from sensor
struct battery_status_s new_report;
@@ -403,7 +654,7 @@ BATT_SMBUS::cycle()
new_report.voltage_v = ((float)tmp) / 1000.0f;
// read current
uint8_t buff[4];
uint8_t buff[6];
if (read_block(BATT_SMBUS_CURRENT, buff, 4, false) == 4) {
new_report.current_a = -(float)((int32_t)((uint32_t)buff[3] << 24 | (uint32_t)buff[2] << 16 | (uint32_t)buff[1] << 8 |
@@ -426,14 +677,42 @@ BATT_SMBUS::cycle()
}
}
// if it is a solo battery, check for shutdown on button press
if (_is_solo_battery) {
// read the button press indicator
if (read_block(BATT_SMBUS_MANUFACTURER_DATA, buff, 6, false) == 6) {
bool pressed = (buff[1] >> 3) & 0x01;
if (_button_press_counts >= ((BATT_SMBUS_BUTTON_DEBOUNCE_MS * 1000) / BATT_SMBUS_MEASUREMENT_INTERVAL_US)) {
// battery will power off
new_report.is_powering_off = true;
// warn only once
if (_button_press_counts++ == ((BATT_SMBUS_BUTTON_DEBOUNCE_MS * 1000) / BATT_SMBUS_MEASUREMENT_INTERVAL_US)) {
warnx("system is shutting down NOW...");
}
} else if (pressed) {
// battery will power off if the button is held
_button_press_counts++;
} else {
// button released early, reset counters
_button_press_counts = 0;
new_report.is_powering_off = false;
}
}
}
// publish to orb
if (_batt_topic != nullptr) {
if (_batt_topic != -1) {
orb_publish(_batt_orb_id, _batt_topic, &new_report);
} else {
_batt_topic = orb_advertise(_batt_orb_id, &new_report);
if (_batt_topic == nullptr) {
if (_batt_topic < 0) {
errx(1, "ADVERT FAIL");
}
}
@@ -453,7 +732,7 @@ BATT_SMBUS::cycle()
// schedule a fresh cycle call when the measurement is done
work_queue(HPWORK, &_work, (worker_t)&BATT_SMBUS::cycle_trampoline, this,
USEC2TICK(BATT_SMBUS_MEASUREMENT_INTERVAL_MS));
USEC2TICK(BATT_SMBUS_MEASUREMENT_INTERVAL_US));
}
int
@@ -480,6 +759,27 @@ BATT_SMBUS::read_reg(uint8_t reg, uint16_t &val)
return ret;
}
int
BATT_SMBUS::write_reg(uint8_t reg, uint16_t val)
{
uint8_t buff[4]; // reg + 2 bytes of data + PEC
buff[0] = reg;
buff[2] = uint8_t(val << 8) & 0xff;
buff[1] = (uint8_t)val;
buff[3] = get_PEC(reg, false, &buff[1], 2); // Append PEC
// write bytes to register
int ret = transfer(buff, 3, nullptr, 0);
if (ret != OK) {
debug("Register write error");
}
// return success or failure
return ret;
}
uint8_t
BATT_SMBUS::read_block(uint8_t reg, uint8_t *data, uint8_t max_len, bool append_zero)
{
@@ -520,6 +820,31 @@ BATT_SMBUS::read_block(uint8_t reg, uint8_t *data, uint8_t max_len, bool append_
return bufflen;
}
uint8_t
BATT_SMBUS::write_block(uint8_t reg, uint8_t *data, uint8_t len)
{
uint8_t buff[len + 3]; // buffer to hold results
usleep(1);
buff[0] = reg;
buff[1] = len;
memcpy(&buff[2], data, len);
buff[len + 2] = get_PEC(reg, false, &buff[1], len + 1); // Append PEC
// send bytes
int ret = transfer(buff, len + 3, nullptr, 0);
// return zero on failure
if (ret != OK) {
debug("Block write error\n");
return 0;
}
// return success
return len;
}
uint8_t
BATT_SMBUS::get_PEC(uint8_t cmd, bool reading, const uint8_t buff[], uint8_t len) const
{
@@ -528,12 +853,32 @@ BATT_SMBUS::get_PEC(uint8_t cmd, bool reading, const uint8_t buff[], uint8_t len
return 0;
}
// prepare temp buffer for calcing crc
uint8_t tmp_buff[len + 3];
/**
* Note: The PEC is calculated on all the message bytes. See http://cache.freescale.com/files/32bit/doc/app_note/AN4471.pdf
* and http://www.ti.com/lit/an/sloa132/sloa132.pdf for more details
*/
// prepare temp buffer for calculating crc
uint8_t tmp_buff_len;
if (reading) {
tmp_buff_len = len + 3;
} else {
tmp_buff_len = len + 2;
}
uint8_t tmp_buff[tmp_buff_len];
tmp_buff[0] = (uint8_t)get_address() << 1;
tmp_buff[1] = cmd;
tmp_buff[2] = tmp_buff[0] | (uint8_t)reading;
memcpy(&tmp_buff[3], buff, len);
if (reading) {
tmp_buff[2] = tmp_buff[0] | (uint8_t)reading;
memcpy(&tmp_buff[3], buff, len);
} else {
memcpy(&tmp_buff[2], buff, len);
}
// initialise crc to zero
uint8_t crc = 0;
@@ -561,17 +906,133 @@ BATT_SMBUS::get_PEC(uint8_t cmd, bool reading, const uint8_t buff[], uint8_t len
return crc;
}
uint8_t
BATT_SMBUS::ManufacturerAccess(uint16_t cmd)
{
// write bytes to Manufacturer Access
int ret = write_reg(BATT_SMBUS_MANUFACTURER_ACCESS, cmd);
if (ret != OK) {
debug("Manufacturer Access error");
}
return ret;
}
void
BATT_SMBUS::check_if_solo_battery()
{
// Check if the SBS information corresponds to that of a 3DR Solo Battery. If, yes, set the solo_battery flag to true;
if (!strcmp(_manufacturer_name, solo_battery.ManufacturerName) && !strcmp(_device_name, solo_battery.DeviceName)
&& !strcmp(_device_chemistry, solo_battery.DeviceChemistry)) {
_is_solo_battery = true;
}
}
///////////////////////// shell functions ///////////////////////
void
batt_smbus_usage()
{
warnx("missing command: try 'start', 'test', 'stop', 'search'");
warnx("missing command: try 'start', 'test', 'stop', 'search', 'man_name', 'man_date', 'dev_name', 'serial_num', 'dev_chem', 'sbs_info'");
warnx("options:");
warnx(" -b i2cbus (%d)", BATT_SMBUS_I2C_BUS);
warnx(" -a addr (0x%x)", BATT_SMBUS_ADDR);
}
int
manufacturer_name()
{
uint8_t man_name[21];
uint8_t len = g_batt_smbus->manufacturer_name(man_name, sizeof(man_name));
if (len > 0) {
warnx("The manufacturer name: %s", man_name);
return OK;
} else {
warnx("Unable to read manufacturer name.");
}
return -1;
}
int
manufacture_date()
{
uint16_t man_date = g_batt_smbus->manufacture_date();
if (man_date > 0) {
// Convert the uint16_t into human-readable date format
uint16_t year = ((man_date >> 9) & 0xFF) + 1980;
uint8_t month = (man_date >> 5) & 0xF;
uint8_t day = man_date & 0x1F;
warnx("The manufacturer date is: %d which is %4d-%02d-%02d", man_date, year, month, day);
return OK;
} else {
warnx("Unable to read the manufacturer date.");
}
return -1;
}
int
device_name()
{
uint8_t device_name[21];
uint8_t len = g_batt_smbus->device_name(device_name, sizeof(device_name));
if (len > 0) {
warnx("The device name: %s", device_name);
return OK;
} else {
warnx("Unable to read device name.");
}
return -1;
}
int
serial_number()
{
uint16_t serial_num = g_batt_smbus->serial_number();
warnx("The serial number: 0x%04x (%d in decimal)", serial_num, serial_num);
return OK;
}
int
device_chemistry()
{
uint8_t device_chemistry[5];
uint8_t len = g_batt_smbus->device_chemistry(device_chemistry, sizeof(device_chemistry));
if (len > 0) {
warnx("The device chemistry: %s", device_chemistry);
return OK;
} else {
warnx("Unable to read device chemistry.");
}
return -1;
}
int
solo_battery_check()
{
if (g_batt_smbus->is_solo_battery()) {
warnx("The battery corresponds to a 3DR Solo Battery");
} else {
warnx("The battery does not correspond to a 3DR Solo Battery");
}
return OK;
}
int
batt_smbus_main(int argc, char *argv[])
{
@@ -649,6 +1110,41 @@ batt_smbus_main(int argc, char *argv[])
exit(0);
}
if (!strcmp(verb, "man_name")) {
manufacturer_name();
exit(0);
}
if (!strcmp(verb, "man_date")) {
manufacture_date();
exit(0);
}
if (!strcmp(verb, "dev_name")) {
device_name();
exit(0);
}
if (!strcmp(verb, "serial_num")) {
serial_number();
exit(0);
}
if (!strcmp(verb, "dev_chem")) {
device_chemistry();
exit(0);
}
if (!strcmp(verb, "sbs_info")) {
manufacturer_name();
manufacture_date();
device_name();
serial_number();
device_chemistry();
solo_battery_check();
exit(0);
}
batt_smbus_usage();
exit(0);
}
src/drivers/boards/aerocore/aerocore_pwm_servo.c → src/drivers/boards/aerocore/aerocore_timer_config.c
+47 -23
View File
@@ -32,9 +32,9 @@
****************************************************************************/
/*
* @file aerocore_pwm_servo.c
* @file aerocore_timer_config.c
*
* Configuration data for the stm32 pwm_servo driver.
* Configuration data for the stm32 pwm_servo, input capture and pwm input driver.
*
* Note that these arrays must always be fully-sized.
*/
@@ -42,76 +42,100 @@
#include <stdint.h>
#include <stm32.h>
#include <stm32_gpio.h>
#include <stm32_gpio_out.h>
#include <stm32_tim.h>
#include <drivers/stm32/drv_pwm_servo.h>
#include <drivers/stm32/drv_io_timer.h>
#include <drivers/drv_pwm_output.h>
#include "board_config.h"
__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = {
__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
.clock_freq = STM32_APB2_TIM1_CLKIN,
.first_channel_index = 0,
.last_channel_index = 3,
.handler = io_timer_handler0,
.vectorno = STM32_IRQ_TIM1CC,
},
{
.base = STM32_TIM3_BASE,
.clock_register = STM32_RCC_APB1ENR,
.clock_bit = RCC_APB1ENR_TIM3EN,
.clock_freq = STM32_APB1_TIM3_CLKIN
.first_channel_index = 4,
.last_channel_index = 7,
.handler = io_timer_handler1,
.vectorno = STM32_IRQ_TIM3,
}
};
__EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = {
__EXPORT const timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = {
{
.gpio = GPIO_TIM1_CH1OUT,
.gpio_out = GPIO_TIM1_CH1OUT,
.gpio_in = GPIO_TIM1_CH1IN,
.timer_index = 0,
.timer_channel = 1,
.default_value = 1500,
.ccr_offset = STM32_GTIM_CCR1_OFFSET,
.masks = GTIM_SR_CC1IF | GTIM_SR_CC1OF
},
{
.gpio = GPIO_TIM1_CH2OUT,
.gpio_out = GPIO_TIM1_CH2OUT,
.gpio_in = GPIO_TIM1_CH2IN,
.timer_index = 0,
.timer_channel = 2,
.default_value = 1500,
.ccr_offset = STM32_GTIM_CCR2_OFFSET,
.masks = GTIM_SR_CC2IF | GTIM_SR_CC2OF
},
{
.gpio = GPIO_TIM1_CH3OUT,
.gpio_out = GPIO_TIM1_CH3OUT,
.gpio_in = GPIO_TIM1_CH3IN,
.timer_index = 0,
.timer_channel = 3,
.default_value = 1500,
.ccr_offset = STM32_GTIM_CCR3_OFFSET,
.masks = GTIM_SR_CC3IF | GTIM_SR_CC3OF
},
{
.gpio = GPIO_TIM1_CH4OUT,
.gpio_out = GPIO_TIM1_CH4OUT,
.gpio_in = GPIO_TIM1_CH4IN,
.timer_index = 0,
.timer_channel = 4,
.default_value = 1500,
.ccr_offset = STM32_GTIM_CCR4_OFFSET,
.masks = GTIM_SR_CC4IF | GTIM_SR_CC4OF
},
{
.gpio = GPIO_TIM3_CH1OUT,
.gpio_out = GPIO_TIM3_CH1OUT,
.gpio_out = GPIO_TIM3_CH1IN,
.timer_index = 1,
.timer_channel = 1,
.default_value = 1500,
.ccr_offset = STM32_GTIM_CCR1_OFFSET,
.masks = GTIM_SR_CC1IF | GTIM_SR_CC1OF
},
{
.gpio = GPIO_TIM3_CH2OUT,
.gpio_out = GPIO_TIM3_CH2OUT,
.gpio_out = GPIO_TIM3_CH2IN,
.timer_index = 1,
.timer_channel = 2,
.default_value = 1500,
.ccr_offset = STM32_GTIM_CCR2_OFFSET,
.masks = GTIM_SR_CC2IF | GTIM_SR_CC2OF
},
{
.gpio = GPIO_TIM3_CH3OUT,
.gpio_out = GPIO_TIM3_CH3OUT,
.gpio_out = GPIO_TIM3_CH3IN,
.timer_index = 1,
.timer_channel = 3,
.default_value = 1500,
.ccr_offset = STM32_GTIM_CCR3_OFFSET,
.masks = GTIM_SR_CC3IF | GTIM_SR_CC3OF
},
{
.gpio = GPIO_TIM3_CH4OUT,
.gpio_out = GPIO_TIM3_CH4OUT,
.gpio_out = GPIO_TIM3_CH4IN,
.timer_index = 1,
.timer_channel = 4,
.default_value = 1500,
.ccr_offset = STM32_GTIM_CCR4_OFFSET,
.masks = GTIM_SR_CC4IF | GTIM_SR_CC4OF
}
};
src/drivers/boards/aerocore/board_config.h
+10
View File
@@ -152,6 +152,16 @@ __BEGIN_DECLS
#define GPIO_TIM3_CH4OUT GPIO_TIM3_CH4OUT_2
#define DIRECT_PWM_OUTPUT_CHANNELS 8
#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_TIM3_CH1IN GPIO_TIM3_CH1IN_3
#define GPIO_TIM3_CH2IN GPIO_TIM3_CH2IN_3
#define GPIO_TIM3_CH3IN GPIO_TIM3_CH3IN_2
#define GPIO_TIM3_CH4IN GPIO_TIM3_CH4IN_2
#define DIRECT_INPUT_TIMER_CHANNELS 8
/* High-resolution timer */
#define HRT_TIMER 8 /* use timer 8 for the HRT */
#define HRT_TIMER_CHANNEL 1 /* use capture/compare channel */
src/drivers/boards/px4fmu-v1/CMakeLists.txt
+1 -1
View File
@@ -37,7 +37,7 @@ px4_add_module(
SRCS
px4fmu_can.c
px4fmu_init.c
px4fmu_pwm_servo.c
px4fmu_timer_config.c
px4fmu_spi.c
px4fmu_usb.c
px4fmu_led.c
src/drivers/boards/px4fmu-v1/board_config.h
+6
View File
@@ -184,6 +184,12 @@ __BEGIN_DECLS
#define GPIO_TIM2_CH4OUT GPIO_TIM2_CH4OUT_1
#define DIRECT_PWM_OUTPUT_CHANNELS 4
#define GPIO_TIM2_CH1IN GPIO_TIM2_CH1IN_1
#define GPIO_TIM2_CH2IN GPIO_TIM2_CH2IN_1
#define GPIO_TIM2_CH3IN GPIO_TIM2_CH3IN_1
#define GPIO_TIM2_CH4IN GPIO_TIM2_CH4IN_1
#define DIRECT_INPUT_TIMER_CHANNELS 4
/* USB OTG FS
*
* PA9 OTG_FS_VBUS VBUS sensing (also connected to the green LED)
src/drivers/boards/px4fmu-v1/px4fmu_pwm_servo.c → src/drivers/boards/px4fmu-v1/px4fmu_timer_config.c
+26 -14
View File
@@ -32,9 +32,9 @@
****************************************************************************/
/*
* @file px4fmu_pwm_servo.c
* @file px4fmu_timer_config.c
*
* Configuration data for the stm32 pwm_servo driver.
* Configuration data for the stm32 pwm_servo, input capture and pwm input driver.
*
* Note that these arrays must always be fully-sized.
*/
@@ -45,43 +45,55 @@
#include <stm32_gpio.h>
#include <stm32_tim.h>
#include <drivers/stm32/drv_pwm_servo.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/stm32/drv_io_timer.h>
#include "board_config.h"
__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = {
__EXPORT const io_timers_t io_timers[MAX_IO_TIMERS] = {
{
.base = STM32_TIM2_BASE,
.clock_register = STM32_RCC_APB1ENR,
.clock_bit = RCC_APB1ENR_TIM2EN,
.clock_freq = STM32_APB1_TIM2_CLKIN
.clock_freq = STM32_APB1_TIM2_CLKIN,
.first_channel_index = 0,
.last_channel_index = 3,
.handler = io_timer_handler0,
.vectorno = STM32_IRQ_TIM2,
}
};
__EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = {
__EXPORT const timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = {
{
.gpio = GPIO_TIM2_CH1OUT,
.gpio_out = GPIO_TIM2_CH1OUT,
.gpio_in = GPIO_TIM2_CH1IN,
.timer_index = 0,
.timer_channel = 1,
.default_value = 1000,
.ccr_offset = STM32_GTIM_CCR1_OFFSET,
.masks = GTIM_SR_CC1IF | GTIM_SR_CC1OF
},
{
.gpio = GPIO_TIM2_CH2OUT,
.gpio_out = GPIO_TIM2_CH2OUT,
.gpio_in = GPIO_TIM2_CH2IN,
.timer_index = 0,
.timer_channel = 2,
.default_value = 1000,
.ccr_offset = STM32_GTIM_CCR2_OFFSET,
.masks = GTIM_SR_CC2IF | GTIM_SR_CC2OF
},
{
.gpio = GPIO_TIM2_CH3OUT,
.gpio_out = GPIO_TIM2_CH3OUT,
.gpio_in = GPIO_TIM2_CH3IN,
.timer_index = 0,
.timer_channel = 3,
.default_value = 1000,
.ccr_offset = STM32_GTIM_CCR3_OFFSET,
.masks = GTIM_SR_CC3IF | GTIM_SR_CC3OF
},
{
.gpio = GPIO_TIM2_CH4OUT,
.gpio_out = GPIO_TIM2_CH4OUT,
.gpio_in = GPIO_TIM2_CH4IN,
.timer_index = 0,
.timer_channel = 4,
.default_value = 1000,
.ccr_offset = STM32_GTIM_CCR4_OFFSET,
.masks = GTIM_SR_CC4IF | GTIM_SR_CC4OF
}
};
src/drivers/boards/px4fmu-v2/CMakeLists.txt
+1 -1
View File
@@ -37,7 +37,7 @@ px4_add_module(
SRCS
px4fmu_can.c
px4fmu2_init.c
px4fmu_pwm_servo.c
px4fmu_timer_config.c
px4fmu_spi.c
px4fmu_usb.c
px4fmu2_led.c
src/drivers/boards/px4fmu-v2/board_config.h
+15 -7
View File
@@ -176,7 +176,7 @@ __BEGIN_DECLS
#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_SET|GPIO_PORTD|GPIO_PIN14)
#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_5V_PERIPH_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN8)
@@ -205,14 +205,22 @@ __BEGIN_DECLS
* CH5 : PD13 : TIM4_CH2
* CH6 : PD14 : TIM4_CH3
*/
#define GPIO_TIM1_CH1OUT GPIO_TIM1_CH1OUT_2
#define GPIO_TIM1_CH2OUT GPIO_TIM1_CH2OUT_2
#define GPIO_TIM1_CH3OUT GPIO_TIM1_CH3OUT_2
#define GPIO_TIM1_CH4OUT GPIO_TIM1_CH4OUT_2
#define GPIO_TIM4_CH2OUT GPIO_TIM4_CH2OUT_2
#define GPIO_TIM4_CH3OUT GPIO_TIM4_CH3OUT_2
#define GPIO_TIM1_CH1OUT (GPIO_ALT|GPIO_AF1|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PUSHPULL|GPIO_PORTE|GPIO_PIN9)
#define GPIO_TIM1_CH2OUT (GPIO_ALT|GPIO_AF1|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PUSHPULL|GPIO_PORTE|GPIO_PIN11)
#define GPIO_TIM1_CH3OUT (GPIO_ALT|GPIO_AF1|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PUSHPULL|GPIO_PORTE|GPIO_PIN13)
#define GPIO_TIM1_CH4OUT (GPIO_ALT|GPIO_AF1|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PUSHPULL|GPIO_PORTE|GPIO_PIN14)
#define GPIO_TIM4_CH2OUT (GPIO_ALT|GPIO_AF2|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PUSHPULL|GPIO_PORTD|GPIO_PIN13)
#define GPIO_TIM4_CH3OUT (GPIO_ALT|GPIO_AF2|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PUSHPULL|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 (also connected to the green LED)
src/drivers/boards/px4fmu-v2/px4fmu2_init.c
+7
View File
@@ -233,6 +233,13 @@ __EXPORT int nsh_archinitialize(void)
stm32_configgpio(GPIO_VDD_SERVO_VALID);
stm32_configgpio(GPIO_VDD_5V_HIPOWER_OC);
stm32_configgpio(GPIO_VDD_5V_PERIPH_OC);
/* 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);
/* configure the high-resolution time/callout interface */
src/drivers/boards/px4fmu-v2/px4fmu_pwm_servo.c → src/drivers/boards/px4fmu-v2/px4fmu_timer_config.c
+40 -19
View File
@@ -32,9 +32,9 @@
****************************************************************************/
/*
* @file px4fmu_pwm_servo.c
* @file px4fmu_timer_config.c
*
* Configuration data for the stm32 pwm_servo driver.
* Configuration data for the stm32 pwm_servo, input capture and pwm input driver.
*
* Note that these arrays must always be fully-sized.
*/
@@ -45,61 +45,82 @@
#include <stm32_gpio.h>
#include <stm32_tim.h>
#include <drivers/stm32/drv_pwm_servo.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/stm32/drv_io_timer.h>
#include "board_config.h"
__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = {
__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
.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
.clock_freq = STM32_APB1_TIM4_CLKIN,
.first_channel_index = 4,
.last_channel_index = 5,
.handler = io_timer_handler1,
.vectorno = STM32_IRQ_TIM4
}
};
__EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = {
__EXPORT const timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = {
{
.gpio = GPIO_TIM1_CH4OUT,
.gpio_out = GPIO_TIM1_CH4OUT,
.gpio_in = GPIO_TIM1_CH4IN,
.timer_index = 0,
.timer_channel = 4,
.default_value = 1000,
.ccr_offset = STM32_GTIM_CCR4_OFFSET,
.masks = GTIM_SR_CC4IF | GTIM_SR_CC4OF
},
{
.gpio = GPIO_TIM1_CH3OUT,
.gpio_out = GPIO_TIM1_CH3OUT,
.gpio_in = GPIO_TIM1_CH3IN,
.timer_index = 0,
.timer_channel = 3,
.default_value = 1000,
.ccr_offset = STM32_GTIM_CCR3_OFFSET,
.masks = GTIM_SR_CC3IF | GTIM_SR_CC3OF
},
{
.gpio = GPIO_TIM1_CH2OUT,
.gpio_out = GPIO_TIM1_CH2OUT,
.gpio_in = GPIO_TIM1_CH2IN,
.timer_index = 0,
.timer_channel = 2,
.default_value = 1000,
.ccr_offset = STM32_GTIM_CCR2_OFFSET,
.masks = GTIM_SR_CC2IF | GTIM_SR_CC2OF
},
{
.gpio = GPIO_TIM1_CH1OUT,
.gpio_out = GPIO_TIM1_CH1OUT,
.gpio_in = GPIO_TIM1_CH1IN,
.timer_index = 0,
.timer_channel = 1,
.default_value = 1000,
.ccr_offset = STM32_GTIM_CCR1_OFFSET,
.masks = GTIM_SR_CC1IF | GTIM_SR_CC1OF
},
{
.gpio = GPIO_TIM4_CH2OUT,
.gpio_out = GPIO_TIM4_CH2OUT,
.gpio_in = GPIO_TIM4_CH2IN,
.timer_index = 1,
.timer_channel = 2,
.default_value = 1000,
.ccr_offset = STM32_GTIM_CCR2_OFFSET,
.masks = GTIM_SR_CC2IF | GTIM_SR_CC2OF
},
{
.gpio = GPIO_TIM4_CH3OUT,
.gpio_out = GPIO_TIM4_CH3OUT,
.gpio_in = GPIO_TIM4_CH3IN,
.timer_index = 1,
.timer_channel = 3,
.default_value = 1000,
.ccr_offset = STM32_GTIM_CCR3_OFFSET,
.masks = GTIM_SR_CC3IF | GTIM_SR_CC3OF
}
};
src/drivers/boards/px4fmu-v4/CMakeLists.txt
+1 -1
View File
@@ -37,7 +37,7 @@ px4_add_module(
SRCS
px4fmu_can.c
px4fmu_init.c
px4fmu_pwm_servo.c
px4fmu_timer_config.c
px4fmu_spi.c
px4fmu_usb.c
px4fmu_led.c
src/drivers/boards/px4fmu-v4/board_config.h
+15 -7
View File
@@ -177,7 +177,7 @@ __BEGIN_DECLS
#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_SET|GPIO_PORTD|GPIO_PIN14)
#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)
@@ -202,14 +202,22 @@ __BEGIN_DECLS
* CH5 : PD13 : TIM4_CH2
* CH6 : PD14 : TIM4_CH3
*/
#define GPIO_TIM1_CH1OUT GPIO_TIM1_CH1OUT_2
#define GPIO_TIM1_CH2OUT GPIO_TIM1_CH2OUT_2
#define GPIO_TIM1_CH3OUT GPIO_TIM1_CH3OUT_2
#define GPIO_TIM1_CH4OUT GPIO_TIM1_CH4OUT_2
#define GPIO_TIM4_CH2OUT GPIO_TIM4_CH2OUT_2
#define GPIO_TIM4_CH3OUT GPIO_TIM4_CH3OUT_2
#define GPIO_TIM1_CH1OUT (GPIO_ALT|GPIO_AF1|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PUSHPULL|GPIO_PORTE|GPIO_PIN9)
#define GPIO_TIM1_CH2OUT (GPIO_ALT|GPIO_AF1|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PUSHPULL|GPIO_PORTE|GPIO_PIN11)
#define GPIO_TIM1_CH3OUT (GPIO_ALT|GPIO_AF1|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PUSHPULL|GPIO_PORTE|GPIO_PIN13)
#define GPIO_TIM1_CH4OUT (GPIO_ALT|GPIO_AF1|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PUSHPULL|GPIO_PORTE|GPIO_PIN14)
#define GPIO_TIM4_CH2OUT (GPIO_ALT|GPIO_AF2|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PUSHPULL|GPIO_PORTD|GPIO_PIN13)
#define GPIO_TIM4_CH3OUT (GPIO_ALT|GPIO_AF2|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PUSHPULL|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
src/drivers/boards/px4fmu-v4/px4fmu_init.c
+8 -1
View File
@@ -222,7 +222,6 @@ __EXPORT int nsh_archinitialize(void)
/* configure power supply control/sense pins */
stm32_configgpio(GPIO_PERIPH_3V3_EN);
stm32_configgpio(GPIO_VDD_BRICK_VALID);
stm32_configgpio(GPIO_GPIO5_OUTPUT);
stm32_configgpio(GPIO_SBUS_INV);
stm32_configgpio(GPIO_8266_GPIO0);
@@ -234,6 +233,14 @@ __EXPORT int nsh_archinitialize(void)
stm32_gpiowrite(GPIO_RC_OUT, 1); /* set it high to pull RC input up */
#endif
/* 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);
/* configure the high-resolution time/callout interface */
hrt_init();
src/drivers/boards/px4fmu-v4/px4fmu_pwm_servo.c → src/drivers/boards/px4fmu-v4/px4fmu_timer_config.c
+40 -19
View File
@@ -32,9 +32,9 @@
****************************************************************************/
/*
* @file px4fmu_pwm_servo.c
* @file px4fmu_timer_config.c
*
* Configuration data for the stm32 pwm_servo driver.
* Configuration data for the stm32 pwm_servo, input capture and pwm input driver.
*
* Note that these arrays must always be fully-sized.
*/
@@ -45,61 +45,82 @@
#include <stm32_gpio.h>
#include <stm32_tim.h>
#include <drivers/stm32/drv_pwm_servo.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/stm32/drv_io_timer.h>
#include "board_config.h"
__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = {
__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
.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
.clock_freq = STM32_APB1_TIM4_CLKIN,
.first_channel_index = 4,
.last_channel_index = 5,
.handler = io_timer_handler1,
.vectorno = STM32_IRQ_TIM4,
}
};
__EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = {
__EXPORT const timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = {
{
.gpio = GPIO_TIM1_CH4OUT,
.gpio_out = GPIO_TIM1_CH4OUT,
.gpio_in = GPIO_TIM1_CH4IN,
.timer_index = 0,
.timer_channel = 4,
.default_value = 900,
.ccr_offset = STM32_GTIM_CCR4_OFFSET,
.masks = GTIM_SR_CC4IF | GTIM_SR_CC4OF
},
{
.gpio = GPIO_TIM1_CH3OUT,
.gpio_out = GPIO_TIM1_CH3OUT,
.gpio_in = GPIO_TIM1_CH3IN,
.timer_index = 0,
.timer_channel = 3,
.default_value = 900,
.ccr_offset = STM32_GTIM_CCR3_OFFSET,
.masks = GTIM_SR_CC3IF | GTIM_SR_CC3OF
},
{
.gpio = GPIO_TIM1_CH2OUT,
.gpio_out = GPIO_TIM1_CH2OUT,
.gpio_in = GPIO_TIM1_CH2IN,
.timer_index = 0,
.timer_channel = 2,
.default_value = 900,
.ccr_offset = STM32_GTIM_CCR2_OFFSET,
.masks = GTIM_SR_CC2IF | GTIM_SR_CC2OF
},
{
.gpio = GPIO_TIM1_CH1OUT,
.gpio_out = GPIO_TIM1_CH1OUT,
.gpio_in = GPIO_TIM1_CH1IN,
.timer_index = 0,
.timer_channel = 1,
.default_value = 900,
.ccr_offset = STM32_GTIM_CCR1_OFFSET,
.masks = GTIM_SR_CC1IF | GTIM_SR_CC1OF
},
{
.gpio = GPIO_TIM4_CH2OUT,
.gpio_out = GPIO_TIM4_CH2OUT,
.gpio_in = GPIO_TIM4_CH2IN,
.timer_index = 1,
.timer_channel = 2,
.default_value = 900,
.ccr_offset = STM32_GTIM_CCR2_OFFSET,
.masks = GTIM_SR_CC2IF | GTIM_SR_CC2OF
},
{
.gpio = GPIO_TIM4_CH3OUT,
.gpio_out = GPIO_TIM4_CH3OUT,
.gpio_in = GPIO_TIM4_CH3IN,
.timer_index = 1,
.timer_channel = 3,
.default_value = 900,
.ccr_offset = STM32_GTIM_CCR3_OFFSET,
.masks = GTIM_SR_CC3IF | GTIM_SR_CC3OF
}
};
src/drivers/boards/px4io-v1/CMakeLists.txt
+1 -1
View File
@@ -36,7 +36,7 @@ px4_add_module(
-Os
SRCS
px4io_init.c
px4io_pwm_servo.c
px4io_timer_config.c
DEPENDS
platforms__common
)
src/drivers/boards/px4io-v1/px4io_pwm_servo.c → src/drivers/boards/px4io-v1/px4io_timer_config.c
+52 -24
View File
@@ -32,7 +32,7 @@
****************************************************************************/
/*
* @file px4fmu_pwm_servo.c
* @file px4fmu_timer_config.c
*
* Configuration data for the stm32 pwm_servo driver.
*
@@ -41,83 +41,111 @@
#include <stdint.h>
#include <drivers/stm32/drv_pwm_servo.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/stm32/drv_io_timer.h>
#include <arch/board/board.h>
#include <drivers/drv_pwm_output.h>
#include <stm32.h>
#include <stm32_gpio.h>
#include <stm32_tim.h>
__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = {
__EXPORT const io_timers_t io_timers[MAX_IO_TIMERS] = {
{
.base = STM32_TIM2_BASE,
.clock_register = STM32_RCC_APB1ENR,
.clock_bit = RCC_APB1ENR_TIM2EN,
.clock_freq = STM32_APB1_TIM2_CLKIN
.clock_freq = STM32_APB1_TIM2_CLKIN,
.first_channel_index = 0,
.last_channel_index = 1,
.handler = io_timer_handler0,
.vectorno = STM32_IRQ_TIM2,
},
{
.base = STM32_TIM3_BASE,
.clock_register = STM32_RCC_APB1ENR,
.clock_bit = RCC_APB1ENR_TIM3EN,
.clock_freq = STM32_APB1_TIM3_CLKIN
.clock_freq = STM32_APB1_TIM3_CLKIN,
.first_channel_index = 4,
.last_channel_index = 7,
.handler = io_timer_handler1,
.vectorno = STM32_IRQ_TIM3,
},
{
.base = STM32_TIM4_BASE,
.clock_register = STM32_RCC_APB1ENR,
.clock_bit = RCC_APB1ENR_TIM4EN,
.clock_freq = STM32_APB1_TIM4_CLKIN
.clock_freq = STM32_APB1_TIM4_CLKIN,
.first_channel_index = 2,
.last_channel_index = 3,
.handler = io_timer_handler2,
.vectorno = STM32_IRQ_TIM4,
}
};
__EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = {
__EXPORT const timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = {
{
.gpio = GPIO_TIM2_CH1OUT,
.gpio_out = GPIO_TIM2_CH1OUT,
.gpio_in = GPIO_TIM2_CH1IN,
.timer_index = 0,
.timer_channel = 1,
.default_value = 1000,
.ccr_offset = STM32_GTIM_CCR1_OFFSET,
.masks = GTIM_SR_CC1IF | GTIM_SR_CC1OF
},
{
.gpio = GPIO_TIM2_CH2OUT,
.gpio_out = GPIO_TIM2_CH2OUT,
.gpio_in = GPIO_TIM2_CH2IN,
.timer_index = 0,
.timer_channel = 2,
.default_value = 1000,
.ccr_offset = STM32_GTIM_CCR2_OFFSET,
.masks = GTIM_SR_CC2IF | GTIM_SR_CC2OF
},
{
.gpio = GPIO_TIM4_CH3OUT,
.gpio_out = GPIO_TIM4_CH3OUT,
.gpio_in = GPIO_TIM4_CH3IN,
.timer_index = 2,
.timer_channel = 3,
.default_value = 1000,
.ccr_offset = STM32_GTIM_CCR3_OFFSET,
.masks = GTIM_SR_CC3IF | GTIM_SR_CC3OF
},
{
.gpio = GPIO_TIM4_CH4OUT,
.gpio_out = GPIO_TIM4_CH4OUT,
.gpio_in = GPIO_TIM4_CH4IN,
.timer_index = 2,
.timer_channel = 4,
.default_value = 1000,
.ccr_offset = STM32_GTIM_CCR4_OFFSET,
.masks = GTIM_SR_CC4IF | GTIM_SR_CC4OF
},
{
.gpio = GPIO_TIM3_CH1OUT,
.gpio_out = GPIO_TIM3_CH1OUT,
.gpio_in = GPIO_TIM3_CH1IN,
.timer_index = 1,
.timer_channel = 1,
.default_value = 1000,
.ccr_offset = STM32_GTIM_CCR1_OFFSET,
.masks = GTIM_SR_CC1IF | GTIM_SR_CC1OF
},
{
.gpio = GPIO_TIM3_CH2OUT,
.gpio_out = GPIO_TIM3_CH2OUT,
.gpio_in = GPIO_TIM3_CH2IN,
.timer_index = 1,
.timer_channel = 2,
.default_value = 1000,
.ccr_offset = STM32_GTIM_CCR2_OFFSET,
.masks = GTIM_SR_CC2IF | GTIM_SR_CC2OF
},
{
.gpio = GPIO_TIM3_CH3OUT,
.gpio_out = GPIO_TIM3_CH3OUT,
.gpio_in = GPIO_TIM3_CH3IN,
.timer_index = 1,
.timer_channel = 3,
.default_value = 1000,
.ccr_offset = STM32_GTIM_CCR3_OFFSET,
.masks = GTIM_SR_CC3IF | GTIM_SR_CC3OF
},
{
.gpio = GPIO_TIM3_CH4OUT,
.gpio_out = GPIO_TIM3_CH4OUT,
.gpio_in = GPIO_TIM3_CH4IN,
.timer_index = 1,
.timer_channel = 4,
.default_value = 1000,
.ccr_offset = STM32_GTIM_CCR4_OFFSET,
.masks = GTIM_SR_CC4IF | GTIM_SR_CC4OF
}
};
src/drivers/boards/px4io-v2/CMakeLists.txt
+1 -1
View File
@@ -36,7 +36,7 @@ px4_add_module(
-Os
SRCS
px4iov2_init.c
px4iov2_pwm_servo.c
px4iov2_timer_config.c
DEPENDS
platforms__common
)
src/drivers/boards/px4io-v2/px4io_init.c
+8 -8
View File
@@ -133,27 +133,27 @@ __EXPORT void stm32_boardinitialize(void)
stm32_configgpio(GPIO_PPM); /* xxx alternate function */
stm32_gpiowrite(GPIO_PWM1, false);
stm32_gpiowrite(GPIO_PWM1, true);
stm32_configgpio(GPIO_PWM1);
stm32_gpiowrite(GPIO_PWM2, false);
stm32_gpiowrite(GPIO_PWM2, true);
stm32_configgpio(GPIO_PWM2);
stm32_gpiowrite(GPIO_PWM3, false);
stm32_gpiowrite(GPIO_PWM3, true);
stm32_configgpio(GPIO_PWM3);
stm32_gpiowrite(GPIO_PWM4, false);
stm32_gpiowrite(GPIO_PWM4, true);
stm32_configgpio(GPIO_PWM4);
stm32_gpiowrite(GPIO_PWM5, false);
stm32_gpiowrite(GPIO_PWM5, true);
stm32_configgpio(GPIO_PWM5);
stm32_gpiowrite(GPIO_PWM6, false);
stm32_gpiowrite(GPIO_PWM6, true);
stm32_configgpio(GPIO_PWM6);
stm32_gpiowrite(GPIO_PWM7, false);
stm32_gpiowrite(GPIO_PWM7, true);
stm32_configgpio(GPIO_PWM7);
stm32_gpiowrite(GPIO_PWM8, false);
stm32_gpiowrite(GPIO_PWM8, true);
stm32_configgpio(GPIO_PWM8);
}
src/drivers/boards/px4io-v2/px4io_pwm_servo.c → src/drivers/boards/px4io-v2/px4io_timer_config.c
+52 -24
View File
@@ -32,7 +32,7 @@
****************************************************************************/
/*
* @file px4iov2_pwm_servo.c
* @file px4iov2_timer_config.c
*
* Configuration data for the stm32 pwm_servo driver.
*
@@ -41,83 +41,111 @@
#include <stdint.h>
#include <drivers/stm32/drv_pwm_servo.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/stm32/drv_io_timer.h>
#include <arch/board/board.h>
#include <drivers/drv_pwm_output.h>
#include <stm32.h>
#include <stm32_gpio.h>
#include <stm32_tim.h>
__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = {
__EXPORT const io_timers_t io_timers[MAX_IO_TIMERS] = {
{
.base = STM32_TIM2_BASE,
.clock_register = STM32_RCC_APB1ENR,
.clock_bit = RCC_APB1ENR_TIM2EN,
.clock_freq = STM32_APB1_TIM2_CLKIN
.clock_freq = STM32_APB1_TIM2_CLKIN,
.first_channel_index = 0,
.last_channel_index = 1,
.handler = io_timer_handler0,
.vectorno = STM32_IRQ_TIM2,
},
{
.base = STM32_TIM3_BASE,
.clock_register = STM32_RCC_APB1ENR,
.clock_bit = RCC_APB1ENR_TIM3EN,
.clock_freq = STM32_APB1_TIM3_CLKIN
.clock_freq = STM32_APB1_TIM3_CLKIN,
.first_channel_index = 4,
.last_channel_index = 7,
.handler = io_timer_handler1,
.vectorno = STM32_IRQ_TIM3,
},
{
.base = STM32_TIM4_BASE,
.clock_register = STM32_RCC_APB1ENR,
.clock_bit = RCC_APB1ENR_TIM4EN,
.clock_freq = STM32_APB1_TIM4_CLKIN
.clock_freq = STM32_APB1_TIM4_CLKIN,
.first_channel_index = 2,
.last_channel_index = 3,
.handler = io_timer_handler2,
.vectorno = STM32_IRQ_TIM4,
}
};
__EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = {
__EXPORT const timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = {
{
.gpio = GPIO_TIM2_CH1OUT,
.gpio_out = GPIO_TIM2_CH1OUT,
.gpio_in = GPIO_TIM2_CH1IN,
.timer_index = 0,
.timer_channel = 1,
.default_value = 1000,
.ccr_offset = STM32_GTIM_CCR1_OFFSET,
.masks = GTIM_SR_CC1IF | GTIM_SR_CC1OF
},
{
.gpio = GPIO_TIM2_CH2OUT,
.gpio_out = GPIO_TIM2_CH2OUT,
.gpio_in = GPIO_TIM2_CH2IN,
.timer_index = 0,
.timer_channel = 2,
.default_value = 1000,
.ccr_offset = STM32_GTIM_CCR2_OFFSET,
.masks = GTIM_SR_CC2IF | GTIM_SR_CC2OF
},
{
.gpio = GPIO_TIM4_CH3OUT,
.gpio_out = GPIO_TIM4_CH3OUT,
.gpio_in = GPIO_TIM4_CH3IN,
.timer_index = 2,
.timer_channel = 3,
.default_value = 1000,
.ccr_offset = STM32_GTIM_CCR3_OFFSET,
.masks = GTIM_SR_CC3IF | GTIM_SR_CC3OF
},
{
.gpio = GPIO_TIM4_CH4OUT,
.gpio_out = GPIO_TIM4_CH4OUT,
.gpio_in = GPIO_TIM4_CH4IN,
.timer_index = 2,
.timer_channel = 4,
.default_value = 1000,
.ccr_offset = STM32_GTIM_CCR4_OFFSET,
.masks = GTIM_SR_CC4IF | GTIM_SR_CC4OF
},
{
.gpio = GPIO_TIM3_CH1OUT,
.gpio_out = GPIO_TIM3_CH1OUT,
.gpio_in = GPIO_TIM3_CH1IN,
.timer_index = 1,
.timer_channel = 1,
.default_value = 1000,
.ccr_offset = STM32_GTIM_CCR1_OFFSET,
.masks = GTIM_SR_CC1IF | GTIM_SR_CC1OF
},
{
.gpio = GPIO_TIM3_CH2OUT,
.gpio_out = GPIO_TIM3_CH2OUT,
.gpio_in = GPIO_TIM3_CH2IN,
.timer_index = 1,
.timer_channel = 2,
.default_value = 1000,
.ccr_offset = STM32_GTIM_CCR2_OFFSET,
.masks = GTIM_SR_CC2IF | GTIM_SR_CC2OF
},
{
.gpio = GPIO_TIM3_CH3OUT,
.gpio_out = GPIO_TIM3_CH3OUT,
.gpio_in = GPIO_TIM3_CH3IN,
.timer_index = 1,
.timer_channel = 3,
.default_value = 1000,
.ccr_offset = STM32_GTIM_CCR3_OFFSET,
.masks = GTIM_SR_CC3IF | GTIM_SR_CC3OF
},
{
.gpio = GPIO_TIM3_CH4OUT,
.gpio_out = GPIO_TIM3_CH4OUT,
.gpio_in = GPIO_TIM3_CH4IN,
.timer_index = 1,
.timer_channel = 4,
.default_value = 1000,
.ccr_offset = STM32_GTIM_CCR4_OFFSET,
.masks = GTIM_SR_CC4IF | GTIM_SR_CC4OF
}
};
src/drivers/camera_trigger/camera_trigger.cpp
+24 -14
View File
@@ -146,6 +146,8 @@ private:
*/
static void disengage(void *arg);
static void trigger(CameraTrigger *trig, bool trigger);
};
struct work_s CameraTrigger::_work;
@@ -226,11 +228,11 @@ CameraTrigger::control(bool on)
if (on) {
// schedule trigger on and off calls
hrt_call_every(&_engagecall, 500, (_interval * 1000),
hrt_call_every(&_engagecall, 0, (_interval * 1000),
(hrt_callout)&CameraTrigger::engage, this);
// schedule trigger on and off calls
hrt_call_every(&_disengagecall, 500 + (_activation_time * 1000), (_interval * 1000),
hrt_call_every(&_disengagecall, 0 + (_activation_time * 1000), (_interval * 1000),
(hrt_callout)&CameraTrigger::disengage, this);
} else {
@@ -238,7 +240,7 @@ CameraTrigger::control(bool on)
hrt_cancel(&_engagecall);
hrt_cancel(&_disengagecall);
// ensure that the pin is off
hrt_call_after(&_disengagecall, 500,
hrt_call_after(&_disengagecall, 0,
(hrt_callout)&CameraTrigger::disengage, this);
}
@@ -314,6 +316,13 @@ CameraTrigger::cycle_trampoline(void *arg)
// need to poll at a very high rate
poll_interval_usec = 100000;
}
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL) {
if (cmd.param5 > 0) {
// One-shot trigger, default 1 ms interval
trig->_interval = 1000;
trig->control(true);
}
}
}
@@ -327,21 +336,16 @@ CameraTrigger::engage(void *arg)
CameraTrigger *trig = reinterpret_cast<CameraTrigger *>(arg);
struct camera_trigger_s trigger;
struct camera_trigger_s report = {};
/* set timestamp the instant before the trigger goes off */
trigger.timestamp = hrt_absolute_time();
report.timestamp = hrt_absolute_time();
for (unsigned i = 0; i < sizeof(trig->_pins) / sizeof(trig->_pins[0]); i++) {
if (trig->_pins[i] >= 0) {
// ACTIVE_LOW == 0
stm32_gpiowrite(trig->_gpios[trig->_pins[i]], trig->_polarity);
}
}
CameraTrigger::trigger(trig, trig->_polarity);
trigger.seq = trig->_trigger_seq++;
report.seq = trig->_trigger_seq++;
orb_publish(ORB_ID(camera_trigger), trig->_trigger_pub, &trigger);
orb_publish(ORB_ID(camera_trigger), trig->_trigger_pub, &report);
}
void
@@ -350,10 +354,16 @@ CameraTrigger::disengage(void *arg)
CameraTrigger *trig = reinterpret_cast<CameraTrigger *>(arg);
CameraTrigger::trigger(trig, !(trig->_polarity));
}
void
CameraTrigger::trigger(CameraTrigger *trig, bool trigger)
{
for (unsigned i = 0; i < sizeof(trig->_pins) / sizeof(trig->_pins[0]); i++) {
if (trig->_pins[i] >= 0) {
// ACTIVE_LOW == 1
stm32_gpiowrite(trig->_gpios[trig->_pins[i]], !(trig->_polarity));
stm32_gpiowrite(trig->_gpios[trig->_pins[i]], trigger);
}
}
}
src/drivers/device/vdev_posix.cpp
+29
View File
@@ -53,6 +53,9 @@
using namespace device;
pthread_mutex_t filemutex = PTHREAD_MUTEX_INITIALIZER;
px4_sem_t lockstep_sem;
bool sim_lockstep = false;
bool sim_delay = false;
extern "C" {
@@ -270,6 +273,10 @@ extern "C" {
#endif
while (sim_delay) {
usleep(100);
}
PX4_DEBUG("Called px4_poll timeout = %d", timeout);
px4_sem_init(&sem, 0, 0);
@@ -398,6 +405,28 @@ extern "C" {
VDev::showFiles();
}
void px4_enable_sim_lockstep()
{
px4_sem_init(&lockstep_sem, 0, 0);
sim_lockstep = true;
sim_delay = false;
}
void px4_sim_start_delay()
{
sim_delay = true;
}
void px4_sim_stop_delay()
{
sim_delay = false;
}
bool px4_sim_delay_enabled()
{
return sim_delay;
}
const char *px4_get_device_names(unsigned int *handle)
{
return VDev::devList(handle);
src/drivers/drv_hrt.h
+33 -16
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2016 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -49,7 +49,7 @@
__BEGIN_DECLS
/*
/**
* Absolute time, in microsecond units.
*
* Absolute time is measured from some arbitrary epoch shortly after
@@ -57,7 +57,7 @@ __BEGIN_DECLS
*/
typedef uint64_t hrt_abstime;
/*
/**
* Callout function type.
*
* Note that callouts run in the timer interrupt context, so
@@ -66,7 +66,7 @@ typedef uint64_t hrt_abstime;
*/
typedef void (* hrt_callout)(void *arg);
/*
/**
* Callout record.
*/
typedef struct hrt_call {
@@ -78,22 +78,22 @@ typedef struct hrt_call {
void *arg;
} *hrt_call_t;
/*
/**
* Get absolute time.
*/
__EXPORT extern hrt_abstime hrt_absolute_time(void);
/*
/**
* Convert a timespec to absolute time.
*/
__EXPORT extern hrt_abstime ts_to_abstime(struct timespec *ts);
/*
/**
* Convert absolute time to a timespec.
*/
__EXPORT extern void abstime_to_ts(struct timespec *ts, hrt_abstime abstime);
/*
/**
* Compute the delta between a timestamp taken in the past
* and now.
*
@@ -102,14 +102,14 @@ __EXPORT extern void abstime_to_ts(struct timespec *ts, hrt_abstime abstime);
*/
__EXPORT extern hrt_abstime hrt_elapsed_time(const volatile hrt_abstime *then);
/*
/**
* Store the absolute time in an interrupt-safe fashion.
*
* This function ensures that the timestamp cannot be seen half-written by an interrupt handler.
*/
__EXPORT extern hrt_abstime hrt_store_absolute_time(volatile hrt_abstime *now);
/*
/**
* Call callout(arg) after delay has elapsed.
*
* If callout is NULL, this can be used to implement a timeout by testing the call
@@ -117,12 +117,12 @@ __EXPORT extern hrt_abstime hrt_store_absolute_time(volatile hrt_abstime *now);
*/
__EXPORT extern void hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg);
/*
/**
* Call callout(arg) at absolute time calltime.
*/
__EXPORT extern void hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, void *arg);
/*
/**
* Call callout(arg) after delay, and then after every interval.
*
* Note thet the interval is timed between scheduled, not actual, call times, so the call rate may
@@ -131,7 +131,7 @@ __EXPORT extern void hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, h
__EXPORT extern void hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval,
hrt_callout callout, void *arg);
/*
/**
* If this returns true, the entry has been invoked and removed from the callout list,
* or it has never been entered.
*
@@ -139,13 +139,13 @@ __EXPORT extern void hrt_call_every(struct hrt_call *entry, hrt_abstime delay, h
*/
__EXPORT extern bool hrt_called(struct hrt_call *entry);
/*
/**
* Remove the entry from the callout list.
*/
__EXPORT extern void hrt_cancel(struct hrt_call *entry);
/*
* initialise a hrt_call structure
/**
* Initialise a hrt_call structure
*/
__EXPORT extern void hrt_call_init(struct hrt_call *entry);
@@ -163,4 +163,21 @@ __EXPORT extern void hrt_call_delay(struct hrt_call *entry, hrt_abstime delay);
*/
__EXPORT extern void hrt_init(void);
#ifdef __PX4_POSIX
/**
* Start to delay the HRT return value.
*
* Until hrt_stop_delay() is called the HRT calls will return the timestamp
* at the instance then hrt_start_delay() was called.
*/
__EXPORT extern void hrt_start_delay(void);
/**
* Stop to delay the HRT.
*/
__EXPORT extern void hrt_stop_delay(void);
#endif
__END_DECLS
src/drivers/drv_input_capture.h
+189
View File
@@ -0,0 +1,189 @@
/****************************************************************************
*
* Copyright (c) 2012-2016 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file Input capture interface.
*
* Input capture interface utilizes the FMU_AUX_PINS to time stamp
* an edge.
*/
#pragma once
#include <px4_defines.h>
#include <stdint.h>
#include <sys/ioctl.h>
#include "drv_hrt.h"
__BEGIN_DECLS
/**
* Path for the default capture input device.
*
*
*/
#define INPUT_CAPTURE_BASE_DEVICE_PATH "/dev/capture"
#define INPUT_CAPTURE0_DEVICE_PATH "/dev/capture0"
typedef void (*capture_callback_t)(void *context, uint32_t chan_index,
hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow);
/**
* Maximum number of PWM output channels supported by the device.
*/
#ifndef INPUT_CAPTURE_MAX_CHANNELS
#define INPUT_CAPTURE_MAX_CHANNELS 6
#endif
typedef uint16_t capture_filter_t;
typedef uint16_t capture_t;
typedef enum input_capture_edge {
Disabled = 0,
Rising = 1,
Falling = 2,
Both = 3
} input_capture_edge;
typedef struct input_capture_element_t {
hrt_abstime time_stamp;
input_capture_edge edge;
bool overrun;
} input_capture_element_t;
typedef struct input_capture_stats_t {
uint32_t chan_in_edges_out;
uint32_t overflows;
uint32_t last_edge;
hrt_abstime last_time;
uint16_t latnecy;
} input_capture_stats_t;
/**
* input capture values for a channel
*
* This allows for Capture input driver values to be set without a
* param_get() dependency
*/
typedef struct input_capture_config_t {
uint8_t channel;
capture_filter_t filter;
input_capture_edge edge;
capture_callback_t callback;
void *context;
} input_capture_config_t;
/*
* ioctl() definitions
*
* Note that ioctls and ORB updates should not be mixed, as the
* behaviour of the system in this case is not defined.
*/
#define _INPUT_CAP_BASE 0x2d00
/** Set Enable a channel arg is pointer to input_capture_config
* with all parameters set.
* edge controls the mode: Disable will free the capture channel.
* (When edge is Disabled call back and context are ignored)
* context may be null. If callback and context are null the
* callback will be disabled.
* */
#define INPUT_CAP_SET _PX4_IOC(_INPUT_CAP_BASE, 0)
/** Set the call back on a capture channel - arg is pointer to
* input_capture_config with channel call back and context set
* context may be null. If both ate null the call back will be
* disabled */
#define INPUT_CAP_SET_CALLBACK _PX4_IOC(_INPUT_CAP_BASE, 1)
/** Get the call back on a capture channel - arg is pointer to
* input_capture_config with channel set.
*/
#define INPUT_CAP_GET_CALLBACK _PX4_IOC(_INPUT_CAP_BASE, 2)
/** Set Edge a channel arg is pointer to input_capture_config
* with channel and edge set */
#define INPUT_CAP_SET_EDGE _PX4_IOC(_INPUT_CAP_BASE, 3)
/** Get Edge for a channel arg is pointer to input_capture_config
* with channel set */
#define INPUT_CAP_GET_EDGE _PX4_IOC(_INPUT_CAP_BASE, 4)
/** Set Filter input filter channel arg is pointer to input_capture_config
* with channel and filter set */
#define INPUT_CAP_SET_FILTER _PX4_IOC(_INPUT_CAP_BASE, 5)
/** Set Filter input filter channel arg is pointer to input_capture_config
* with channel set */
#define INPUT_CAP_GET_FILTER _PX4_IOC(_INPUT_CAP_BASE, 6)
/** Get the number of capture in *(unsigned *)arg */
#define INPUT_CAP_GET_COUNT _PX4_IOC(_INPUT_CAP_BASE, 7)
/** Set the number of capture in (unsigned)arg - allows change of
* split between servos and capture */
#define INPUT_CAP_SET_COUNT _PX4_IOC(_INPUT_CAP_BASE, 8)
/** Get channel stats - arg is pointer to input_capture_config
* with channel set.
*/
#define INPUT_CAP_GET_STATS _PX4_IOC(_INPUT_CAP_BASE, 9)
/** Get channel stats - arg is pointer to input_capture_config
* with channel set.
*/
#define INPUT_CAP_GET_CLR_STATS _PX4_IOC(_INPUT_CAP_BASE, 10)
/*
*
*
* WARNING WARNING WARNING! DO NOT EXCEED 31 IN IOC INDICES HERE!
*
*
*/
__EXPORT int up_input_capture_set(unsigned channel, input_capture_edge edge, capture_filter_t filter,
capture_callback_t callback, void *context);
__EXPORT int up_input_capture_get_filter(unsigned channel, capture_filter_t *filter);
__EXPORT int up_input_capture_set_filter(unsigned channel, capture_filter_t filter);
__EXPORT int up_input_capture_get_trigger(unsigned channel, input_capture_edge *edge);
__EXPORT int up_input_capture_set_trigger(unsigned channel, input_capture_edge edge);
__EXPORT int up_input_capture_get_callback(unsigned channel, capture_callback_t *callback, void **context);
__EXPORT int up_input_capture_set_callback(unsigned channel, capture_callback_t callback, void *context);
__EXPORT int up_input_capture_get_stats(unsigned channel, input_capture_stats_t *stats, bool clear);
__END_DECLS
src/drivers/frsky_telemetry/frsky_data.c
+72
View File
@@ -45,8 +45,10 @@
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <stdbool.h>
#include <arch/math.h>
#include <geo/geo.h>
#include <stdbool.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/sensor_combined.h>
@@ -285,3 +287,73 @@ void frsky_send_frame3(int uart)
frsky_send_startstop(uart);
}
/* parse 11 byte frames */
bool frsky_parse_host(uint8_t *sbuf, int nbytes, struct adc_linkquality *v)
{
bool data_ready = false;
static int dcount = 0;
static uint8_t type = 0;
static uint8_t data[11];
static enum {
HEADER = 0,
TYPE,
DATA,
TRAILER
} state = HEADER;
for (int i = 0; i < nbytes; i++) {
switch (state) {
case HEADER:
if (sbuf[i] == 0x7E) {
state = TYPE;
}
break;
case TYPE:
if (sbuf[i] != 0x7E) {
state = DATA;
type = sbuf[i];
dcount = 0;
}
break;
case DATA:
/* read 8 data bytes */
if (dcount < 7) {
data[dcount++] = sbuf[i];
} else {
/* received all data bytes */
state = TRAILER;
}
break;
case TRAILER:
state = HEADER;
if (sbuf[i] != 0x7E) {
warnx("host packet error: %x", sbuf[i]);
} else {
data_ready = true;
if (type == 0xFE) {
/* this is an adc_linkquality packet */
v->ad1 = data[0];
v->ad2 = data[1];
v->linkq = data[2];
}
}
break;
}
}
return data_ready;
}
src/drivers/frsky_telemetry/frsky_data.h
+9
View File
@@ -42,10 +42,19 @@
#ifndef _FRSKY_DATA_H
#define _FRSKY_DATA_H
#include <stdbool.h>
// Public functions
void frsky_init(void);
void frsky_send_frame1(int uart);
void frsky_send_frame2(int uart);
void frsky_send_frame3(int uart);
struct adc_linkquality {
uint8_t ad1;
uint8_t ad2;
uint8_t linkq;
};
bool frsky_parse_host(uint8_t *sbuf, int nbytes, struct adc_linkquality *v);
#endif /* _FRSKY_TELEMETRY_H */
src/drivers/frsky_telemetry/frsky_telemetry.c
+77 -14
View File
@@ -57,6 +57,7 @@
#include <systemlib/systemlib.h>
#include <termios.h>
#include <drivers/drv_hrt.h>
#include <uORB/topics/sensor_combined.h>
#include "sPort_data.h"
#include "frsky_data.h"
@@ -172,7 +173,7 @@ static int frsky_telemetry_thread_main(int argc, char *argv[])
}
}
/* Open UART */
/* Open UART assuming SmartPort telemetry */
warnx("opening uart");
struct termios uart_config_original;
struct termios uart_config;
@@ -193,16 +194,28 @@ static int frsky_telemetry_thread_main(int argc, char *argv[])
/* Main thread loop */
char sbuf[20];
/* look for polling bytes indicating SmartPort telemetry
* if not found, send D type telemetry frames instead
/* 2 byte polling frames indicate SmartPort telemetry
* 11 byte packets, indicate D type telemetry
*/
int status = poll(fds, sizeof(fds) / sizeof(fds[0]), 3000);
// warnx("poll status: %u", status);
if (status > 0) {
/* traffic on the port, assume it's a SmartPort master */
/* received some data; check size of packet */
usleep(5000);
status = read(uart, &sbuf[0], sizeof(sbuf));
// warnx("received %u bytes", status);
}
if (status > 0 && status < 3) {
/* traffic on the port, D type is 11 bytes per frame, SmartPort is only 2 */
/* Subscribe to topics */
sPort_init();
warnx("sending FrSky SmartPort telemetry");
int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
/* send S.port telemetry */
while (!thread_should_exit) {
@@ -229,6 +242,23 @@ static int frsky_telemetry_thread_main(int argc, char *argv[])
newBytes = read(uart, &sbuf[1], 1);
/* get a local copy of the current sensor values
* in order to apply a lowpass filter to baro pressure.
*/
static float last_baro_alt = 0;
struct sensor_combined_s raw;
memset(&raw, 0, sizeof(raw));
orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
static float filtered_alt = NAN;
if (isnan(filtered_alt)) {
filtered_alt = raw.baro_alt_meter[0];
} else {
filtered_alt = .05f * raw.baro_alt_meter[0] + (1.0f - .05f) * filtered_alt;
}
// allow a minimum of 500usec before reply
usleep(500);
@@ -237,6 +267,7 @@ static int frsky_telemetry_thread_main(int argc, char *argv[])
static hrt_abstime lastALT = 0;
static hrt_abstime lastSPD = 0;
static hrt_abstime lastFUEL = 0;
static hrt_abstime lastVSPD = 0;
switch (sbuf[1]) {
@@ -296,15 +327,31 @@ static int frsky_telemetry_thread_main(int argc, char *argv[])
sPort_send_FUEL(uart);
}
break;
case SMARTPORT_POLL_6:
/* report vertical speed at 10Hz */
if (now - lastVSPD > 100 * 1000) {
/* estimate vertical speed using first difference and delta t */
uint64_t dt = now - lastVSPD;
float speed = (filtered_alt - last_baro_alt) / (1e-6f * (float)dt);
/* save current alt and timestamp */
last_baro_alt = filtered_alt;
lastVSPD = now;
sPort_send_VSPD(uart, speed);
}
break;
}
}
} else {
/* timed out: reconfigure UART and send D type telemetry */
warnx("SmartPort receiver not detected, sending FrSky D type telemetry");
} else if (status >= 0) {
/* either no traffic on the port (0=>timeout), or D type packet */
/* detected D type telemetry: reconfigure UART */
status = set_uart_speed(uart, &uart_config, B9600);
if (status < 0) {
@@ -322,22 +369,38 @@ static int frsky_telemetry_thread_main(int argc, char *argv[])
/* Subscribe to topics */
frsky_init();
warnx("sending FrSky D type telemetry");
struct adc_linkquality host_frame;
uint8_t dbuf[45];
/* send D8 mode telemetry */
while (!thread_should_exit) {
/* Sleep 200 ms */
usleep(200000);
/* Sleep 100 ms */
usleep(100000);
/* parse incoming data */
int nbytes = read(uart, &dbuf[0], sizeof(dbuf));
bool new_input = frsky_parse_host(&dbuf[0], nbytes, &host_frame);
/* the RSSI value could be useful */
if (false && new_input) {
warnx("host frame: ad1:%u, ad2: %u, rssi: %u",
host_frame.ad1, host_frame.ad2, host_frame.linkq);
}
/* Send frame 1 (every 200ms): acceleration values, altitude (vario), temperatures, current & voltages, RPM */
frsky_send_frame1(uart);
if (iteration % 2 == 0) {
frsky_send_frame1(uart);
}
/* Send frame 2 (every 1000ms): course, latitude, longitude, speed, altitude (GPS), fuel level */
if (iteration % 5 == 0) {
if (iteration % 10 == 0) {
frsky_send_frame2(uart);
}
/* Send frame 3 (every 5000ms): date, time */
if (iteration % 25 == 0) {
if (iteration % 50 == 0) {
frsky_send_frame3(uart);
iteration = 0;
@@ -380,7 +443,7 @@ int frsky_telemetry_main(int argc, char *argv[])
frsky_task = px4_task_spawn_cmd("frsky_telemetry",
SCHED_DEFAULT,
200,
2000,
2200,
frsky_telemetry_thread_main,
(char *const *)argv);
src/drivers/frsky_telemetry/sPort_data.c
+8
View File
@@ -203,6 +203,14 @@ void sPort_send_SPD(int uart)
sPort_send_data(uart, SMARTPORT_ID_GPS_SPD, ispeed);
}
// TODO: verify scaling
void sPort_send_VSPD(int uart, float speed)
{
/* send data for VARIO vertical speed: int16 cm/sec */
int32_t ispeed = (int)(100 * speed);
sPort_send_data(uart, SMARTPORT_ID_VARIO, ispeed);
}
// verified scaling
void sPort_send_FUEL(int uart)
{
src/drivers/frsky_telemetry/sPort_data.h
+3
View File
@@ -50,6 +50,7 @@
#define SMARTPORT_POLL_3 0x95
#define SMARTPORT_POLL_4 0x16
#define SMARTPORT_POLL_5 0xB7
#define SMARTPORT_POLL_6 0x00
/* FrSky SmartPort sensor IDs */
#define SMARTPORT_ID_RSSI 0xf101
@@ -82,6 +83,8 @@ void sPort_send_BATV(int uart);
void sPort_send_CUR(int uart);
void sPort_send_ALT(int uart);
void sPort_send_SPD(int uart);
void sPort_send_VSPD(int uart, float speed);
void sPort_send_FUEL(int uart);
#endif /* _SPORT_TELEMETRY_H */
src/drivers/ll40ls/LidarLite.h
+2 -2
View File
@@ -44,8 +44,8 @@
#include <drivers/drv_range_finder.h>
/* Device limits */
#define LL40LS_MIN_DISTANCE (0.00f)
#define LL40LS_MAX_DISTANCE (60.00f)
#define LL40LS_MIN_DISTANCE (0.05f)
#define LL40LS_MAX_DISTANCE (25.00f)
// normal conversion wait time
#define LL40LS_CONVERSION_INTERVAL 50*1000UL /* 50ms */
src/drivers/mpu9250/mpu9250.cpp
+6 -6
View File
@@ -913,27 +913,27 @@ MPU9250::accel_self_test()
/* inspect accel offsets */
if (fabsf(_accel_scale.x_offset) < 0.000001f) {
return 1;
return 2;
}
if (fabsf(_accel_scale.x_scale - 1.0f) > 0.4f || fabsf(_accel_scale.x_scale - 1.0f) < 0.000001f) {
return 1;
return 3;
}
if (fabsf(_accel_scale.y_offset) < 0.000001f) {
return 1;
return 4;
}
if (fabsf(_accel_scale.y_scale - 1.0f) > 0.4f || fabsf(_accel_scale.y_scale - 1.0f) < 0.000001f) {
return 1;
return 5;
}
if (fabsf(_accel_scale.z_offset) < 0.000001f) {
return 1;
return 6;
}
if (fabsf(_accel_scale.z_scale - 1.0f) > 0.4f || fabsf(_accel_scale.z_scale - 1.0f) < 0.000001f) {
return 1;
return 7;
}
return 0;
src/drivers/pwm_input/pwm_input.cpp
+7
View File
@@ -223,6 +223,7 @@
#error PWMIN_TIMER_CHANNEL must be either 1 and 2.
#endif
// XXX refactor this out of this driver
#define TIMEOUT_POLL 300000 /* reset after no response over this time in microseconds [0.3s] */
#define TIMEOUT_READ 200000 /* don't reset if the last read is back more than this time in microseconds [0.2s] */
@@ -324,6 +325,8 @@ void PWMIN::_timer_init(void)
/* configure input pin */
stm32_configgpio(GPIO_PWM_IN);
// XXX refactor this out of this driver
/* configure reset pin */
stm32_configgpio(GPIO_VDD_RANGEFINDER_EN);
@@ -375,6 +378,7 @@ void PWMIN::_timer_init(void)
_timer_started = true;
}
// XXX refactor this out of this driver
void
PWMIN::_freeze_test()
{
@@ -384,18 +388,21 @@ PWMIN::_freeze_test()
}
}
// XXX refactor this out of this driver
void
PWMIN::_turn_on()
{
stm32_gpiowrite(GPIO_VDD_RANGEFINDER_EN, 1);
}
// XXX refactor this out of this driver
void
PWMIN::_turn_off()
{
stm32_gpiowrite(GPIO_VDD_RANGEFINDER_EN, 0);
}
// XXX refactor this out of this driver
void
PWMIN::hard_reset()
{
src/drivers/px4fmu/fmu.cpp
+417 -45
View File
@@ -57,6 +57,7 @@
#include <drivers/device/device.h>
#include <drivers/device/i2c.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_input_capture.h>
#include <drivers/drv_gpio.h>
#include <drivers/drv_hrt.h>
@@ -70,6 +71,7 @@
#include <systemlib/param/param.h>
#include <drivers/drv_mixer.h>
#include <drivers/drv_rc_input.h>
#include <drivers/drv_input_capture.h>
#include <lib/rc/sbus.h>
#include <lib/rc/dsm.h>
@@ -99,9 +101,15 @@ public:
enum Mode {
MODE_NONE,
MODE_2PWM,
MODE_2PWM2CAP,
MODE_3PWM,
MODE_3PWM1CAP,
MODE_4PWM,
MODE_6PWM,
MODE_8PWM,
MODE_4CAP,
MODE_5CAP,
MODE_6CAP,
};
PX4FMU();
virtual ~PX4FMU();
@@ -112,12 +120,17 @@ public:
virtual int init();
int set_mode(Mode mode);
Mode get_mode() { return _mode; }
int set_pwm_alt_rate(unsigned rate);
int set_pwm_alt_channels(uint32_t channels);
int set_i2c_bus_clock(unsigned bus, unsigned clock_hz);
static void capture_trampoline(void *context, uint32_t chan_index,
hrt_abstime edge_time, uint32_t edge_state,
uint32_t overflow);
private:
enum RC_SCAN {
RC_SCAN_PPM = 0,
@@ -161,8 +174,8 @@ private:
volatile bool _initialized;
bool _throttle_armed;
bool _pwm_on;
bool _pwm_init;
uint8_t _pwm_channel_mask;
uint32_t _pwm_mask;
bool _pwm_initialized;
MixerGroup *_mixers;
@@ -187,7 +200,6 @@ private:
static bool arm_nothrottle() { return (_armed.prearmed && !_armed.armed); }
static void cycle_trampoline(void *arg);
void cycle();
void work_start();
void work_stop();
@@ -196,11 +208,15 @@ private:
uint8_t control_group,
uint8_t control_index,
float &input);
void capture_callback(uint32_t chan_index,
hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow);
void subscribe();
int set_pwm_rate(unsigned rate_map, unsigned default_rate, unsigned alt_rate);
int pwm_ioctl(file *filp, int cmd, unsigned long arg);
void update_pwm_rev_mask();
void publish_pwm_outputs(uint16_t *values, size_t numvalues);
void update_pwm_out_state(bool on);
void pwm_output_set(unsigned i, unsigned value);
struct GPIOConfig {
uint32_t input;
@@ -219,6 +235,8 @@ private:
uint32_t gpio_read(void);
int gpio_ioctl(file *filp, int cmd, unsigned long arg);
int capture_ioctl(file *filp, int cmd, unsigned long arg);
/* do not allow to copy due to ptr data members */
PX4FMU(const PX4FMU &);
PX4FMU operator=(const PX4FMU &);
@@ -315,8 +333,8 @@ PX4FMU::PX4FMU() :
_initialized(false),
_throttle_armed(false),
_pwm_on(false),
_pwm_init(false),
_pwm_channel_mask(0),
_pwm_mask(0),
_pwm_initialized(false),
_mixers(nullptr),
_groups_required(0),
_groups_subscribed(0),
@@ -407,6 +425,8 @@ PX4FMU::init()
int
PX4FMU::set_mode(Mode mode)
{
unsigned old_mask = _pwm_mask;
/*
* Configure for PWM output.
*
@@ -415,6 +435,12 @@ PX4FMU::set_mode(Mode mode)
* are presented on the output pins.
*/
switch (mode) {
case MODE_2PWM2CAP: // v1 multi-port with flow control lines as PWM
up_input_capture_set(2, Rising, 0, NULL, NULL);
up_input_capture_set(3, Rising, 0, NULL, NULL);
DEVICE_DEBUG("MODE_2PWM2CAP");
// no break
case MODE_2PWM: // v1 multi-port with flow control lines as PWM
DEVICE_DEBUG("MODE_2PWM");
@@ -422,10 +448,27 @@ PX4FMU::set_mode(Mode mode)
_pwm_default_rate = 50;
_pwm_alt_rate = 50;
_pwm_alt_rate_channels = 0;
_pwm_channel_mask = 0x3;
_pwm_mask = 0x3;
_pwm_initialized = false;
break;
case MODE_3PWM1CAP: // v1 multi-port with flow control lines as PWM
DEVICE_DEBUG("MODE_3PWM1CAP");
up_input_capture_set(3, Rising, 0, NULL, NULL);
// no break
case MODE_3PWM: // v1 multi-port with flow control lines as PWM
DEVICE_DEBUG("MODE_3PWM");
/* default output rates */
_pwm_default_rate = 50;
_pwm_alt_rate = 50;
_pwm_alt_rate_channels = 0;
_pwm_mask = 0x7;
_pwm_initialized = false;
break;
case MODE_4PWM: // v1 or v2 multi-port as 4 PWM outs
DEVICE_DEBUG("MODE_4PWM");
@@ -433,7 +476,8 @@ PX4FMU::set_mode(Mode mode)
_pwm_default_rate = 50;
_pwm_alt_rate = 50;
_pwm_alt_rate_channels = 0;
_pwm_channel_mask = 0xf;
_pwm_mask = 0xf;
_pwm_initialized = false;
break;
@@ -444,7 +488,8 @@ PX4FMU::set_mode(Mode mode)
_pwm_default_rate = 50;
_pwm_alt_rate = 50;
_pwm_alt_rate_channels = 0;
_pwm_channel_mask = 0x3f;
_pwm_mask = 0x3f;
_pwm_initialized = false;
break;
@@ -456,7 +501,8 @@ PX4FMU::set_mode(Mode mode)
_pwm_default_rate = 50;
_pwm_alt_rate = 50;
_pwm_alt_rate_channels = 0;
_pwm_channel_mask = 0xff;
_pwm_mask = 0xff;
_pwm_initialized = false;
break;
#endif
@@ -466,10 +512,13 @@ PX4FMU::set_mode(Mode mode)
_pwm_default_rate = 10; /* artificially reduced output rate */
_pwm_alt_rate = 10;
_pwm_alt_rate_channels = 0;
_pwm_mask = 0x0;
_pwm_initialized = false;
/* disable servo outputs - no need to set rates */
up_pwm_servo_deinit();
_pwm_init = false;
if (old_mask != _pwm_mask) {
/* disable servo outputs - no need to set rates */
up_pwm_servo_deinit();
}
break;
@@ -634,10 +683,26 @@ PX4FMU::cycle_trampoline(void *arg)
dev->cycle();
}
void PX4FMU::fill_rc_in(uint16_t raw_rc_count,
uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS],
hrt_abstime now, bool frame_drop, bool failsafe,
unsigned frame_drops, int rssi = -1)
void
PX4FMU::capture_trampoline(void *context, uint32_t chan_index,
hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow)
{
PX4FMU *dev = reinterpret_cast<PX4FMU *>(context);
dev->capture_callback(chan_index, edge_time, edge_state, overflow);
}
void
PX4FMU::capture_callback(uint32_t chan_index,
hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow)
{
fprintf(stdout, "FMU: Capture chan:%d time:%lld state:%d overflow:%d\n", chan_index, edge_time, edge_state, overflow);
}
void
PX4FMU::fill_rc_in(uint16_t raw_rc_count,
uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS],
hrt_abstime now, bool frame_drop, bool failsafe,
unsigned frame_drops, int rssi = -1)
{
// fill rc_in struct for publishing
_rc_in.channel_count = raw_rc_count;
@@ -654,9 +719,13 @@ void PX4FMU::fill_rc_in(uint16_t raw_rc_count,
_rc_in.timestamp_last_signal = _rc_in.timestamp_publication;
_rc_in.rc_ppm_frame_length = 0;
/* fake rssi if no value was provided */
if (rssi == -1) {
_rc_in.rssi =
(!frame_drop) ? RC_INPUT_RSSI_MAX : (RC_INPUT_RSSI_MAX / 2);
} else {
_rc_in.rssi = rssi;
}
_rc_in.rc_failsafe = failsafe;
@@ -684,13 +753,33 @@ void PX4FMU::rc_io_invert(bool invert)
}
#endif
void
PX4FMU::pwm_output_set(unsigned i, unsigned value)
{
if (_pwm_initialized) {
up_pwm_servo_set(i, value);
}
}
void
PX4FMU::update_pwm_out_state(bool on)
{
if (on && !_pwm_initialized && _pwm_mask != 0) {
up_pwm_servo_init(_pwm_mask);
set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate);
_pwm_initialized = true;
} else {
_pwm_initialized = false;
}
up_pwm_servo_arm(on);
}
void
PX4FMU::cycle()
{
if (!_initialized) {
/* reset GPIOs */
gpio_reset();
/* force a reset of the update rate */
_current_update_rate = 0;
@@ -805,9 +894,15 @@ PX4FMU::cycle()
switch (_mode) {
case MODE_2PWM:
case MODE_2PWM2CAP:
num_outputs = 2;
break;
case MODE_3PWM:
case MODE_3PWM1CAP:
num_outputs = 3;
break;
case MODE_4PWM:
num_outputs = 4;
break;
@@ -844,7 +939,7 @@ PX4FMU::cycle()
/* output to the servos */
for (size_t i = 0; i < num_outputs; i++) {
up_pwm_servo_set(i, pwm_limited[i]);
pwm_output_set(i, pwm_limited[i]);
}
publish_pwm_outputs(pwm_limited, num_outputs);
@@ -867,13 +962,7 @@ PX4FMU::cycle()
if (_pwm_on != pwm_on) {
_pwm_on = pwm_on;
if (!_pwm_init && _pwm_on) {
up_pwm_servo_init(_pwm_channel_mask);
set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate);
_pwm_init = true;
}
up_pwm_servo_arm(pwm_on);
update_pwm_out_state(pwm_on);
}
}
@@ -956,7 +1045,7 @@ PX4FMU::cycle()
case RC_SCAN_DSM:
if (_rc_scan_begin == 0) {
_rc_scan_begin = now;
// // Configure serial port for DSM
// Configure serial port for DSM
dsm_config(_rcs_fd);
rc_io_invert(false);
@@ -987,7 +1076,7 @@ PX4FMU::cycle()
case RC_SCAN_ST24:
if (_rc_scan_begin == 0) {
_rc_scan_begin = now;
// // Configure serial port for DSM
// Configure serial port for DSM
dsm_config(_rcs_fd);
rc_io_invert(false);
@@ -1026,7 +1115,7 @@ PX4FMU::cycle()
case RC_SCAN_SUMD:
if (_rc_scan_begin == 0) {
_rc_scan_begin = now;
// // Configure serial port for DSM
// Configure serial port for DSM
dsm_config(_rcs_fd);
rc_io_invert(false);
@@ -1208,10 +1297,20 @@ PX4FMU::ioctl(file *filp, int cmd, unsigned long arg)
return ret;
}
/* try it as a Capture ioctl next */
ret = capture_ioctl(filp, cmd, arg);
if (ret != -ENOTTY) {
return ret;
}
/* if we are in valid PWM mode, try it as a PWM ioctl as well */
switch (_mode) {
case MODE_2PWM:
case MODE_3PWM:
case MODE_4PWM:
case MODE_2PWM2CAP:
case MODE_3PWM1CAP:
case MODE_6PWM:
#ifdef CONFIG_ARCH_BOARD_AEROCORE
case MODE_8PWM:
@@ -1241,7 +1340,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
switch (cmd) {
case PWM_SERVO_ARM:
up_pwm_servo_arm(true);
update_pwm_out_state(true);
break;
case PWM_SERVO_SET_ARM_OK:
@@ -1252,7 +1351,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
break;
case PWM_SERVO_DISARM:
up_pwm_servo_arm(false);
update_pwm_out_state(false);
break;
case PWM_SERVO_GET_DEFAULT_UPDATE_RATE:
@@ -1469,12 +1568,18 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
/* FALLTHROUGH */
case PWM_SERVO_SET(3):
case PWM_SERVO_SET(2):
if (_mode < MODE_4PWM) {
ret = -EINVAL;
break;
}
/* FALLTHROUGH */
case PWM_SERVO_SET(2):
if (_mode < MODE_3PWM) {
ret = -EINVAL;
break;
}
/* FALLTHROUGH */
case PWM_SERVO_SET(1):
case PWM_SERVO_SET(0):
@@ -1507,12 +1612,18 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
/* FALLTHROUGH */
case PWM_SERVO_GET(3):
case PWM_SERVO_GET(2):
if (_mode < MODE_4PWM) {
ret = -EINVAL;
break;
}
/* FALLTHROUGH */
case PWM_SERVO_GET(2):
if (_mode < MODE_3PWM) {
ret = -EINVAL;
break;
}
/* FALLTHROUGH */
case PWM_SERVO_GET(1):
case PWM_SERVO_GET(0):
@@ -1550,7 +1661,13 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
*(unsigned *)arg = 4;
break;
case MODE_3PWM:
case MODE_3PWM1CAP:
*(unsigned *)arg = 3;
break;
case MODE_2PWM:
case MODE_2PWM2CAP:
*(unsigned *)arg = 2;
break;
@@ -1578,6 +1695,10 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
set_mode(MODE_2PWM);
break;
case 3:
set_mode(MODE_3PWM);
break;
case 4:
set_mode(MODE_4PWM);
break;
@@ -2052,6 +2173,135 @@ PX4FMU::gpio_read(void)
return bits;
}
int
PX4FMU::capture_ioctl(struct file *filp, int cmd, unsigned long arg)
{
int ret = -EINVAL;
lock();
input_capture_config_t *pconfig = 0;
input_capture_stats_t *stats = (input_capture_stats_t *)arg;
if (_mode == MODE_3PWM1CAP || _mode == MODE_2PWM2CAP) {
pconfig = (input_capture_config_t *)arg;
}
switch (cmd) {
case INPUT_CAP_SET:
if (pconfig) {
ret = up_input_capture_set(pconfig->channel, pconfig->edge, pconfig->filter,
pconfig->callback, pconfig->context);
}
break;
case INPUT_CAP_SET_CALLBACK:
if (pconfig) {
ret = up_input_capture_set_callback(pconfig->channel, pconfig->callback, pconfig->context);
}
break;
case INPUT_CAP_GET_CALLBACK:
if (pconfig) {
ret = up_input_capture_get_callback(pconfig->channel, &pconfig->callback, &pconfig->context);
}
break;
case INPUT_CAP_GET_STATS:
if (arg) {
ret = up_input_capture_get_stats(stats->chan_in_edges_out, stats, false);
}
break;
case INPUT_CAP_GET_CLR_STATS:
if (arg) {
ret = up_input_capture_get_stats(stats->chan_in_edges_out, stats, true);
}
break;
case INPUT_CAP_SET_EDGE:
if (pconfig) {
ret = up_input_capture_set_trigger(pconfig->channel, pconfig->edge);
}
break;
case INPUT_CAP_GET_EDGE:
if (pconfig) {
ret = up_input_capture_get_trigger(pconfig->channel, &pconfig->edge);
}
break;
case INPUT_CAP_SET_FILTER:
if (pconfig) {
ret = up_input_capture_set_filter(pconfig->channel, pconfig->filter);
}
break;
case INPUT_CAP_GET_FILTER:
if (pconfig) {
ret = up_input_capture_get_filter(pconfig->channel, &pconfig->filter);
}
break;
case INPUT_CAP_GET_COUNT:
ret = OK;
switch (_mode) {
case MODE_3PWM1CAP:
*(unsigned *)arg = 1;
break;
case MODE_2PWM2CAP:
*(unsigned *)arg = 2;
break;
default:
ret = -EINVAL;
break;
}
break;
case INPUT_CAP_SET_COUNT:
ret = OK;
switch (_mode) {
case MODE_3PWM1CAP:
set_mode(MODE_3PWM1CAP);
break;
case MODE_2PWM2CAP:
set_mode(MODE_2PWM2CAP);
break;
default:
ret = -EINVAL;
break;
}
break;
default:
ret = -ENOTTY;
break;
}
unlock();
return ret;
}
int
PX4FMU::gpio_ioctl(struct file *filp, int cmd, unsigned long arg)
{
@@ -2135,6 +2385,11 @@ enum PortMode {
PORT_PWM_AND_SERIAL,
PORT_PWM_AND_GPIO,
PORT_PWM4,
PORT_PWM3,
PORT_PWM2,
PORT_PWM3CAP1,
PORT_PWM2CAP2,
PORT_CAPTURE,
};
PortMode g_port_mode;
@@ -2144,9 +2399,7 @@ fmu_new_mode(PortMode new_mode)
{
uint32_t gpio_bits;
PX4FMU::Mode servo_mode;
/* reset to all-inputs */
g_fmu->ioctl(0, GPIO_RESET, 0);
bool mode_with_input = false;
gpio_bits = 0;
servo_mode = PX4FMU::MODE_NONE;
@@ -2154,7 +2407,6 @@ fmu_new_mode(PortMode new_mode)
switch (new_mode) {
case PORT_FULL_GPIO:
case PORT_MODE_UNSET:
/* nothing more to do here */
break;
case PORT_FULL_PWM:
@@ -2176,6 +2428,28 @@ fmu_new_mode(PortMode new_mode)
/* select 4-pin PWM mode */
servo_mode = PX4FMU::MODE_4PWM;
break;
case PORT_PWM3:
/* select 4-pin PWM mode */
servo_mode = PX4FMU::MODE_3PWM;
break;
case PORT_PWM3CAP1:
/* select 3-pin PWM mode 1 capture */
servo_mode = PX4FMU::MODE_3PWM1CAP;
mode_with_input = true;
break;
case PORT_PWM2:
/* select 2-pin PWM mode */
servo_mode = PX4FMU::MODE_2PWM;
break;
case PORT_PWM2CAP2:
/* select 2-pin PWM mode 2 capture */
servo_mode = PX4FMU::MODE_2PWM2CAP;
mode_with_input = true;
break;
#endif
/* mixed modes supported on v1 board only */
@@ -2184,11 +2458,13 @@ fmu_new_mode(PortMode new_mode)
case PORT_FULL_SERIAL:
/* set all multi-GPIOs to serial mode */
gpio_bits = GPIO_MULTI_1 | GPIO_MULTI_2 | GPIO_MULTI_3 | GPIO_MULTI_4;
mode_with_input = true;
break;
case PORT_GPIO_AND_SERIAL:
/* set RX/TX multi-GPIOs to serial mode */
gpio_bits = GPIO_MULTI_3 | GPIO_MULTI_4;
mode_with_input = true;
break;
case PORT_PWM_AND_SERIAL:
@@ -2196,11 +2472,13 @@ fmu_new_mode(PortMode new_mode)
servo_mode = PX4FMU::MODE_2PWM;
/* set RX/TX multi-GPIOs to serial mode */
gpio_bits = GPIO_MULTI_3 | GPIO_MULTI_4;
mode_with_input = true;
break;
case PORT_PWM_AND_GPIO:
/* select 2-pin PWM mode */
servo_mode = PX4FMU::MODE_2PWM;
mode_with_input = true;
break;
#endif
@@ -2208,13 +2486,21 @@ fmu_new_mode(PortMode new_mode)
return -1;
}
/* adjust GPIO config for serial mode(s) */
if (gpio_bits != 0) {
g_fmu->ioctl(0, GPIO_SET_ALT_1, gpio_bits);
}
if (servo_mode != g_fmu->get_mode()) {
/* (re)set the PWM output mode */
g_fmu->set_mode(servo_mode);
/* reset to all-inputs */
if (mode_with_input) {
g_fmu->ioctl(0, GPIO_RESET, 0);
/* adjust GPIO config for serial mode(s) */
if (gpio_bits != 0) {
g_fmu->ioctl(0, GPIO_SET_ALT_1, gpio_bits);
}
}
/* (re)set the PWM output mode */
g_fmu->set_mode(servo_mode);
}
return OK;
}
@@ -2304,9 +2590,15 @@ test(void)
{
int fd;
unsigned servo_count = 0;
unsigned capture_count = 0;
unsigned pwm_value = 1000;
int direction = 1;
int ret;
uint32_t rate_limit = 0;
struct input_capture_t {
bool valid;
input_capture_config_t chan;
} capture_conf[INPUT_CAPTURE_MAX_CHANNELS];
fd = open(PX4FMU_DEVICE_PATH, O_RDWR);
@@ -2320,10 +2612,42 @@ test(void)
err(1, "Unable to get servo count\n");
}
warnx("Testing %u servos", (unsigned)servo_count);
if (ioctl(fd, INPUT_CAP_GET_COUNT, (unsigned long)&capture_count) != 0) {
fprintf(stdout, "Not in a capture mode\n");
}
warnx("Testing %u servos and %u input captures", (unsigned)servo_count, capture_count);
memset(capture_conf, 0, sizeof(capture_conf));
if (capture_count != 0) {
for (unsigned i = 0; i < capture_count; i++) {
// Map to channel number
capture_conf[i].chan.channel = i + servo_count;
/* Save handler */
if (ioctl(fd, INPUT_CAP_GET_CALLBACK, (unsigned long)&capture_conf[i].chan.channel) != 0) {
err(1, "Unable to get capture callback for chan %u\n", capture_conf[i].chan.channel);
} else {
input_capture_config_t conf = capture_conf[i].chan;
conf.callback = &PX4FMU::capture_trampoline;
conf.context = g_fmu;
if (ioctl(fd, INPUT_CAP_SET_CALLBACK, (unsigned long)&conf) == 0) {
capture_conf[i].valid = true;
} else {
err(1, "Unable to set capture callback for chan %u\n", capture_conf[i].chan.channel);
}
}
}
}
struct pollfd fds;
fds.fd = 0; /* stdin */
fds.events = POLLIN;
warnx("Press CTRL-C or 'c' to abort.");
@@ -2383,6 +2707,29 @@ test(void)
}
}
if (capture_count != 0 && (++rate_limit % 500 == 0)) {
for (unsigned i = 0; i < capture_count; i++) {
if (capture_conf[i].valid) {
input_capture_stats_t stats;
stats.chan_in_edges_out = capture_conf[i].chan.channel;
if (ioctl(fd, INPUT_CAP_GET_STATS, (unsigned long)&stats) != 0) {
err(1, "Unable to get stats for chan %u\n", capture_conf[i].chan.channel);
} else {
fprintf(stdout, "FMU: Status chan:%u edges: %d last time:%lld last state:%d overflows:%d lantency:%u\n",
capture_conf[i].chan.channel,
stats.chan_in_edges_out,
stats.last_time,
stats.last_edge,
stats.overflows,
stats.latnecy);
}
}
}
}
/* Check if user wants to quit */
char c;
ret = poll(&fds, 1, 0);
@@ -2398,8 +2745,21 @@ test(void)
}
}
if (capture_count != 0) {
for (unsigned i = 0; i < capture_count; i++) {
// Map to channel number
if (capture_conf[i].valid) {
/* Save handler */
if (ioctl(fd, INPUT_CAP_SET_CALLBACK, (unsigned long)&capture_conf[i].chan) != 0) {
err(1, "Unable to set capture callback for chan %u\n", capture_conf[i].chan.channel);
}
}
}
}
close(fd);
exit(0);
}
@@ -2481,6 +2841,18 @@ fmu_main(int argc, char *argv[])
} else if (!strcmp(verb, "mode_pwm4")) {
new_mode = PORT_PWM4;
} else if (!strcmp(verb, "mode_pwm2")) {
new_mode = PORT_PWM2;
} else if (!strcmp(verb, "mode_pwm3")) {
new_mode = PORT_PWM3;
} else if (!strcmp(verb, "mode_pwm3cap1")) {
new_mode = PORT_PWM3CAP1;
} else if (!strcmp(verb, "mode_pwm2cap2")) {
new_mode = PORT_PWM2CAP2;
#endif
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
src/drivers/px4io/px4io.cpp
+18 -4
View File
@@ -626,7 +626,7 @@ PX4IO::init()
{
int ret;
param_t sys_restart_param;
int sys_restart_val = DM_INIT_REASON_VOLATILE;
int32_t sys_restart_val = DM_INIT_REASON_VOLATILE;
ASSERT(_task == -1);
@@ -634,7 +634,12 @@ PX4IO::init()
if (sys_restart_param != PARAM_INVALID) {
/* Indicate restart type is unknown */
param_set(sys_restart_param, &sys_restart_val);
int32_t prev_val;
param_get(sys_restart_param, &prev_val);
if (prev_val != DM_INIT_REASON_POWER_ON) {
param_set_no_notification(sys_restart_param, &sys_restart_val);
}
}
/* do regular cdev init */
@@ -815,8 +820,12 @@ PX4IO::init()
/* Indicate restart type is in-flight */
sys_restart_val = DM_INIT_REASON_IN_FLIGHT;
param_set(sys_restart_param, &sys_restart_val);
int32_t prev_val;
param_get(sys_restart_param, &prev_val);
if (prev_val != sys_restart_val) {
param_set(sys_restart_param, &sys_restart_val);
}
/* regular boot, no in-air restart, init IO */
@@ -849,7 +858,12 @@ PX4IO::init()
/* Indicate restart type is power on */
sys_restart_val = DM_INIT_REASON_POWER_ON;
param_set(sys_restart_param, &sys_restart_val);
int32_t prev_val;
param_get(sys_restart_param, &prev_val);
if (prev_val != sys_restart_val) {
param_set(sys_restart_param, &sys_restart_val);
}
}
src/drivers/sf10a/CMakeLists.txt
+44
View File
@@ -0,0 +1,44 @@
############################################################################
#
# Copyright (c) 2015 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
px4_add_module(
MODULE drivers__sf10a
MAIN sf10a
COMPILE_FLAGS
-Os
SRCS
sf10a.cpp
DEPENDS
platforms__common
)
# vim: set noet ft=cmake fenc=utf-8 ff=unix :
src/drivers/sf10a/sf10a.cpp
+881
View File
@@ -0,0 +1,881 @@
/****************************************************************************
*
* Copyright (c) 2013-2015 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 sf10a.cpp
*
* @author ecmnet <ecm@gmx.de>
*
* Driver for the Lightware SF10x lidar range finder series.
* Default I2C address 0x55 is used.
*/
#include <px4_config.h>
#include <drivers/device/i2c.h>
#include <sys/types.h>
#include <stdint.h>
#include <stdlib.h>
#include <stdbool.h>
#include <semaphore.h>
#include <string.h>
#include <fcntl.h>
#include <poll.h>
#include <errno.h>
#include <stdio.h>
#include <math.h>
#include <unistd.h>
#include <vector>
#include <nuttx/arch.h>
#include <nuttx/wqueue.h>
#include <nuttx/clock.h>
#include <systemlib/perf_counter.h>
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_range_finder.h>
#include <drivers/device/ringbuffer.h>
#include <uORB/uORB.h>
#include <uORB/topics/subsystem_info.h>
#include <uORB/topics/distance_sensor.h>
#include <board_config.h>
/* Configuration Constants */
#define SF10A_BUS PX4_I2C_BUS_EXPANSION
#define SF10A_BASEADDR 0x55
#define SF10A_DEVICE_PATH "/dev/sf10a"
/* Device limits */
#define SF10A_MIN_DISTANCE (0.0f)
#define SF10A_MAX_DISTANCE (25.0f)
/* conversion rates */
//#define SF10A_CONVERSION_INTERVAL 25000 // Overclocking SF10a to 40 Hz
#define SF10A_CONVERSION_INTERVAL 31250 // Maximum rate according to datasheet is 32Hz
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
#ifndef CONFIG_SCHED_WORKQUEUE
# error This requires CONFIG_SCHED_WORKQUEUE.
#endif
class SF10A : public device::I2C
{
public:
SF10A(int bus = SF10A_BUS, int address = SF10A_BASEADDR);
virtual ~SF10A();
virtual int init();
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
/**
* Diagnostics - print some basic information about the driver.
*/
void print_info();
protected:
virtual int probe();
private:
float _min_distance;
float _max_distance;
work_s _work;
ringbuffer::RingBuffer *_reports;
bool _sensor_ok;
int _measure_ticks;
int _class_instance;
int _orb_class_instance;
orb_advert_t _distance_sensor_topic;
perf_counter_t _sample_perf;
perf_counter_t _comms_errors;
perf_counter_t _buffer_overflows;
/**
* Test whether the device supported by the driver is present at a
* specific address.
*
* @param address The I2C bus address to probe.
* @return True if the device is present.
*/
int probe_address(uint8_t address);
/**
* Initialise the automatic measurement state machine and start it.
*
* @note This function is called at open and error time. It might make sense
* to make it more aggressive about resetting the bus in case of errors.
*/
void start();
/**
* Stop the automatic measurement state machine.
*/
void stop();
/**
* Set the min and max distance thresholds if you want the end points of the sensors
* range to be brought in at all, otherwise it will use the defaults SF10A_MIN_DISTANCE
* and SF10A_MAX_DISTANCE
*/
void set_minimum_distance(float min);
void set_maximum_distance(float max);
float get_minimum_distance();
float get_maximum_distance();
/**
* Perform a poll cycle; collect from the previous measurement
* and start a new one.
*/
void cycle();
int measure();
int collect();
/**
* Static trampoline from the workq context; because we don't have a
* generic workq wrapper yet.
*
* @param arg Instance pointer for the driver that is polling.
*/
static void cycle_trampoline(void *arg);
};
/*
* Driver 'main' command.
*/
extern "C" __EXPORT int sf10a_main(int argc, char *argv[]);
SF10A::SF10A(int bus, int address) :
I2C("SF10A", SF10A_DEVICE_PATH, bus, address, 100000),
_min_distance(SF10A_MIN_DISTANCE),
_max_distance(SF10A_MAX_DISTANCE),
_reports(nullptr),
_sensor_ok(false),
_measure_ticks(0),
_class_instance(-1),
_orb_class_instance(-1),
_distance_sensor_topic(nullptr),
_sample_perf(perf_alloc(PC_ELAPSED, "sf10a_read")),
_comms_errors(perf_alloc(PC_COUNT, "sf10a_comms_errors")),
_buffer_overflows(perf_alloc(PC_COUNT, "sf10a_buffer_overflows"))
{
/* enable debug() calls */
_debug_enabled = false;
/* work_cancel in the dtor will explode if we don't do this... */
memset(&_work, 0, sizeof(_work));
}
SF10A::~SF10A()
{
/* make sure we are truly inactive */
stop();
/* free any existing reports */
if (_reports != nullptr) {
delete _reports;
}
if (_class_instance != -1) {
unregister_class_devname(RANGE_FINDER_BASE_DEVICE_PATH, _class_instance);
}
/* free perf counters */
perf_free(_sample_perf);
perf_free(_comms_errors);
perf_free(_buffer_overflows);
}
int
SF10A::init()
{
int ret = ERROR;
/* do I2C init (and probe) first */
if (I2C::init() != OK) {
return ret;
}
/* allocate basic report buffers */
_reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s));
set_address(SF10A_BASEADDR);
if (_reports == nullptr) {
return ret;
}
_class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH);
/* get a publish handle on the range finder topic */
struct distance_sensor_s ds_report = {};
_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
&_orb_class_instance, ORB_PRIO_HIGH);
if (_distance_sensor_topic == nullptr) {
DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?");
}
int ret2 = measure();
if (ret2 == 0) {
ret = OK;
_sensor_ok = true;
DEVICE_LOG("SF10x with address %d found", SF10A_BASEADDR);
}
return ret;
}
int
SF10A::probe()
{
return measure();
}
void
SF10A::set_minimum_distance(float min)
{
_min_distance = min;
}
void
SF10A::set_maximum_distance(float max)
{
_max_distance = max;
}
float
SF10A::get_minimum_distance()
{
return _min_distance;
}
float
SF10A::get_maximum_distance()
{
return _max_distance;
}
int
SF10A::ioctl(struct file *filp, int cmd, unsigned long arg)
{
switch (cmd) {
case SENSORIOCSPOLLRATE: {
switch (arg) {
/* switching to manual polling */
case SENSOR_POLLRATE_MANUAL:
stop();
_measure_ticks = 0;
return OK;
/* external signalling (DRDY) not supported */
case SENSOR_POLLRATE_EXTERNAL:
/* zero would be bad */
case 0:
return -EINVAL;
/* set default/max polling rate */
case SENSOR_POLLRATE_MAX:
case SENSOR_POLLRATE_DEFAULT: {
/* do we need to start internal polling? */
bool want_start = (_measure_ticks == 0);
/* set interval for next measurement to minimum legal value */
_measure_ticks = USEC2TICK(SF10A_CONVERSION_INTERVAL);
/* if we need to start the poll state machine, do it */
if (want_start) {
start();
}
return OK;
}
/* adjust to a legal polling interval in Hz */
default: {
/* do we need to start internal polling? */
bool want_start = (_measure_ticks == 0);
/* convert hz to tick interval via microseconds */
int ticks = USEC2TICK(1000000 / arg);
/* check against maximum rate */
if (ticks < USEC2TICK(SF10A_CONVERSION_INTERVAL)) {
return -EINVAL;
}
/* update interval for next measurement */
_measure_ticks = ticks;
/* if we need to start the poll state machine, do it */
if (want_start) {
start();
}
return OK;
}
}
}
case SENSORIOCGPOLLRATE:
if (_measure_ticks == 0) {
return SENSOR_POLLRATE_MANUAL;
}
return (1000 / _measure_ticks);
case SENSORIOCSQUEUEDEPTH: {
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 1) || (arg > 100)) {
return -EINVAL;
}
irqstate_t flags = irqsave();
if (!_reports->resize(arg)) {
irqrestore(flags);
return -ENOMEM;
}
irqrestore(flags);
return OK;
}
case SENSORIOCGQUEUEDEPTH:
return _reports->size();
case SENSORIOCRESET:
/* XXX implement this */
return -EINVAL;
case RANGEFINDERIOCSETMINIUMDISTANCE: {
set_minimum_distance(*(float *)arg);
return 0;
}
break;
case RANGEFINDERIOCSETMAXIUMDISTANCE: {
set_maximum_distance(*(float *)arg);
return 0;
}
break;
default:
/* give it to the superclass */
return I2C::ioctl(filp, cmd, arg);
}
}
ssize_t
SF10A::read(struct file *filp, char *buffer, size_t buflen)
{
unsigned count = buflen / sizeof(struct distance_sensor_s);
struct distance_sensor_s *rbuf = reinterpret_cast<struct distance_sensor_s *>(buffer);
int ret = 0;
/* buffer must be large enough */
if (count < 1) {
return -ENOSPC;
}
/* if automatic measurement is enabled */
if (_measure_ticks > 0) {
/*
* While there is space in the caller's buffer, and reports, copy them.
* Note that we may be pre-empted by the workq thread while we are doing this;
* we are careful to avoid racing with them.
*/
while (count--) {
if (_reports->get(rbuf)) {
ret += sizeof(*rbuf);
rbuf++;
}
}
/* if there was no data, warn the caller */
return ret ? ret : -EAGAIN;
}
/* manual measurement - run one conversion */
do {
_reports->flush();
/* trigger a measurement */
if (OK != measure()) {
ret = -EIO;
break;
}
/* wait for it to complete */
usleep(SF10A_CONVERSION_INTERVAL);
/* run the collection phase */
if (OK != collect()) {
ret = -EIO;
break;
}
/* state machine will have generated a report, copy it out */
if (_reports->get(rbuf)) {
ret = sizeof(*rbuf);
}
} while (0);
return ret;
}
int
SF10A::measure()
{
int ret;
/*
* Send the command to begin a measurement.
*/
uint8_t cmd[1];
cmd[0] = SF10A_BASEADDR;
ret = transfer(cmd, 1, nullptr, 0);
if (OK != ret) {
perf_count(_comms_errors);
DEVICE_DEBUG("i2c::transfer returned %d", ret);
return ret;
}
ret = OK;
return ret;
}
int
SF10A::collect()
{
int ret = -EIO;
/* read from the sensor */
uint8_t val[2] = {0, 0};
uint8_t cmd = SF10A_BASEADDR;
perf_begin(_sample_perf);
ret = transfer(&cmd, 1, nullptr, 0);
ret = transfer(nullptr, 0, &val[0], 2);
if (ret < 0) {
DEVICE_DEBUG("error reading from sensor: %d", ret);
perf_count(_comms_errors);
perf_end(_sample_perf);
return ret;
}
uint16_t distance_cm = val[0] << 8 | val[1];
float distance_m = float(distance_cm) * 1e-2f;
struct distance_sensor_s report;
report.timestamp = hrt_absolute_time();
report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
report.orientation = 8;
report.current_distance = distance_m;
report.min_distance = get_minimum_distance();
report.max_distance = get_maximum_distance();
report.covariance = 0.0f;
/* TODO: set proper ID */
report.id = 0;
/* publish it, if we are the primary */
if (_distance_sensor_topic != nullptr) {
orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report);
}
if (_reports->force(&report)) {
perf_count(_buffer_overflows);
}
/* notify anyone waiting for data */
poll_notify(POLLIN);
ret = OK;
perf_end(_sample_perf);
return ret;
}
void
SF10A::start()
{
/* reset the report ring and state machine */
_reports->flush();
/* schedule a cycle to start things */
work_queue(HPWORK, &_work, (worker_t)&SF10A::cycle_trampoline, this, 5);
/* notify about state change */
struct subsystem_info_s info = {
true,
true,
true,
subsystem_info_s::SUBSYSTEM_TYPE_RANGEFINDER
};
static orb_advert_t pub = nullptr;
if (pub != nullptr) {
orb_publish(ORB_ID(subsystem_info), pub, &info);
} else {
pub = orb_advertise(ORB_ID(subsystem_info), &info);
}
}
void
SF10A::stop()
{
work_cancel(HPWORK, &_work);
}
void
SF10A::cycle_trampoline(void *arg)
{
SF10A *dev = (SF10A *)arg;
dev->cycle();
}
void
SF10A::cycle()
{
set_address(SF10A_BASEADDR);
/* Collect results */
if (OK != collect()) {
DEVICE_DEBUG("collection error");
/* if error restart the measurement state machine */
start();
return;
}
/* Trigger measurement */
if (OK != measure()) {
DEVICE_DEBUG("measure error lidar");
}
/* schedule a fresh cycle call when the measurement is done */
work_queue(HPWORK,
&_work,
(worker_t)&SF10A::cycle_trampoline,
this,
USEC2TICK(SF10A_CONVERSION_INTERVAL));
}
void
SF10A::print_info()
{
perf_print_counter(_sample_perf);
perf_print_counter(_comms_errors);
perf_print_counter(_buffer_overflows);
printf("poll interval: %u ticks\n", _measure_ticks);
_reports->print_info("report queue");
}
/**
* Local functions in support of the shell command.
*/
namespace sf10a
{
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
const int ERROR = -1;
SF10A *g_dev;
void start();
void stop();
void test();
void reset();
void info();
/**
* Start the driver.
*/
void
start()
{
int fd;
if (g_dev != nullptr) {
errx(1, "already started");
}
/* create the driver */
g_dev = new SF10A(SF10A_BUS);
if (g_dev == nullptr) {
goto fail;
}
if (OK != g_dev->init()) {
goto fail;
}
/* set the poll rate to default, starts automatic data collection */
fd = open(SF10A_DEVICE_PATH, O_RDONLY);
if (fd < 0) {
goto fail;
}
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
goto fail;
}
exit(0);
fail:
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
}
errx(1, "driver start failed");
}
/**
* Stop the driver
*/
void stop()
{
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
} else {
errx(1, "driver not running");
}
exit(0);
}
/**
* Perform some basic functional tests on the driver;
* make sure we can collect data from the sensor in polled
* and automatic modes.
*/
void
test()
{
struct distance_sensor_s report;
ssize_t sz;
int ret;
int fd = open(SF10A_DEVICE_PATH, O_RDONLY);
if (fd < 0) {
err(1, "%s open failed (try 'sf10a start' if the driver is not running", SF10A_DEVICE_PATH);
}
/* do a simple demand read */
sz = read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
err(1, "immediate read failed");
}
warnx("single read");
warnx("measurement: %0.2f m", (double)report.current_distance);
warnx("time: %llu", report.timestamp);
/* start the sensor polling at 2Hz */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
errx(1, "failed to set 2Hz poll rate");
}
/* read the sensor 5x and report each value */
for (unsigned i = 0; i < 5; i++) {
struct pollfd fds;
/* wait for data to be ready */
fds.fd = fd;
fds.events = POLLIN;
ret = poll(&fds, 1, 2000);
if (ret != 1) {
errx(1, "timed out waiting for sensor data");
}
/* now go get it */
sz = read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
err(1, "periodic read failed");
}
warnx("periodic read %u", i);
warnx("valid %u", (float)report.current_distance > report.min_distance
&& (float)report.current_distance < report.max_distance ? 1 : 0);
warnx("measurement: %0.3f", (double)report.current_distance);
warnx("time: %llu", report.timestamp);
}
/* reset the sensor polling to default rate */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) {
errx(1, "failed to set default poll rate");
}
errx(0, "PASS");
}
/**
* Reset the driver.
*/
void
reset()
{
int fd = open(SF10A_DEVICE_PATH, O_RDONLY);
if (fd < 0) {
err(1, "failed ");
}
if (ioctl(fd, SENSORIOCRESET, 0) < 0) {
err(1, "driver reset failed");
}
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
err(1, "driver poll restart failed");
}
exit(0);
}
/**
* Print a little info about the driver.
*/
void
info()
{
if (g_dev == nullptr) {
errx(1, "driver not running");
}
printf("state @ %p\n", g_dev);
g_dev->print_info();
exit(0);
}
} /* namespace */
int
sf10a_main(int argc, char *argv[])
{
/*
* Start/load the driver.
*/
if (!strcmp(argv[1], "start")) {
sf10a::start();
}
/*
* Stop the driver
*/
if (!strcmp(argv[1], "stop")) {
sf10a::stop();
}
/*
* Test the driver/device.
*/
if (!strcmp(argv[1], "test")) {
sf10a::test();
}
/*
* Reset the driver.
*/
if (!strcmp(argv[1], "reset")) {
sf10a::reset();
}
/*
* Print driver information.
*/
if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) {
sf10a::info();
}
errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
}
src/drivers/stm32/CMakeLists.txt
+2
View File
@@ -36,7 +36,9 @@ px4_add_module(
-Os
SRCS
drv_hrt.c
drv_io_timer.c
drv_pwm_servo.c
drv_input_capture.c
DEPENDS
platforms__common
)
src/drivers/stm32/drv_input_capture.c
+506
View File
@@ -0,0 +1,506 @@
/****************************************************************************
*
* Copyright (C) 2012-2016 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/*
* @file drv_input_capture.c
*
* Servo driver supporting input capture connected to STM32 timer blocks.
*
* Works with any of the 'generic' or 'advanced' STM32 timers that
* have input pins.
*
* Require an interrupt.
*
* The use of thie interface is mutually exclusive with the pwm
* because the same timers are used and there is a resource contention
* with the ARR as it sets the pwm rate and in this driver needs to match
* that of the hrt to back calculate the actual point in time the edge
* was detected.
*
* This is accomplished by taking the difference between the current
* count rCNT snapped at the time interrupt and the rCCRx captured on the
* edge transition. This delta is applied to hrt time and the resulting
* value is the absolute system time the edge occured.
*
*
*/
#include <px4_config.h>
#include <nuttx/arch.h>
#include <nuttx/irq.h>
#include <sys/types.h>
#include <stdbool.h>
#include <assert.h>
#include <debug.h>
#include <time.h>
#include <queue.h>
#include <errno.h>
#include <string.h>
#include <stdio.h>
#include <arch/board/board.h>
#include <drivers/drv_input_capture.h>
#include "drv_io_timer.h"
#include "drv_input_capture.h"
#include <chip.h>
#include <up_internal.h>
#include <up_arch.h>
#include <stm32.h>
#include <stm32_gpio.h>
#include <stm32_tim.h>
#define _REG32(_base, _reg) (*(volatile uint32_t *)(_base + _reg))
#define REG(_tmr, _reg) _REG32(io_timers[_tmr].base, _reg)
#define rCCMR1(_tmr) REG(_tmr, STM32_GTIM_CCMR1_OFFSET)
#define rCCMR2(_tmr) REG(_tmr, STM32_GTIM_CCMR2_OFFSET)
#define rCCER(_tmr) REG(_tmr, STM32_GTIM_CCER_OFFSET)
#define GTIM_SR_CCOF (GTIM_SR_CC4OF|GTIM_SR_CC3OF|GTIM_SR_CC2OF|GTIM_SR_CC1OF)
static input_capture_stats_t channel_stats[MAX_TIMER_IO_CHANNELS];
static struct channel_handler_entry {
capture_callback_t callback;
void *context;
} channel_handlers[MAX_TIMER_IO_CHANNELS];
static void input_capture_chan_handler(void *context, const io_timers_t *timer, uint32_t chan_index,
const timer_io_channels_t *chan,
hrt_abstime isrs_time , uint16_t isrs_rcnt)
{
uint16_t capture = _REG32(timer, chan->ccr_offset);
channel_stats[chan_index].last_edge = stm32_gpioread(chan->gpio_in);
if ((isrs_rcnt - capture) > channel_stats[chan_index].latnecy) {
channel_stats[chan_index].latnecy = (isrs_rcnt - capture);
}
channel_stats[chan_index].chan_in_edges_out++;
channel_stats[chan_index].last_time = isrs_time - (isrs_rcnt - capture);
uint32_t overflow = _REG32(timer, STM32_GTIM_SR_OFFSET) & chan->masks & GTIM_SR_CCOF;
if (overflow) {
/* Error we has a second edge before we cleared CCxR */
channel_stats[chan_index].overflows++;
}
if (channel_handlers[chan_index].callback) {
channel_handlers[chan_index].callback(channel_handlers[chan_index].context, chan_index,
channel_stats[chan_index].last_time,
channel_stats[chan_index].last_edge, overflow);
}
}
static void input_capture_bind(unsigned channel, capture_callback_t callback, void *context)
{
irqstate_t flags = irqsave();
channel_handlers[channel].callback = callback;
channel_handlers[channel].context = context;
irqrestore(flags);
}
static void input_capture_unbind(unsigned channel)
{
input_capture_bind(channel, NULL, NULL);
}
int up_input_capture_set(unsigned channel, input_capture_edge edge, capture_filter_t filter,
capture_callback_t callback, void *context)
{
if (filter > GTIM_CCMR1_IC1F_MASK) {
return -EINVAL;
}
if (edge > Both) {
return -EINVAL;
}
int rv = io_timer_validate_channel_index(channel);
if (rv == 0) {
if (edge == Disabled) {
io_timer_set_enable(false, IOTimerChanMode_Capture, 1 << channel);
input_capture_unbind(channel);
} else {
if (-EBUSY == io_timer_is_channel_free(channel)) {
io_timer_free_channel(channel);
}
input_capture_bind(channel, callback, context);
int rv = io_timer_channel_init(channel, IOTimerChanMode_Capture, input_capture_chan_handler, context);
if (rv != 0) {
return rv;
}
rv = up_input_capture_set_filter(channel, filter);
if (rv == 0) {
rv = up_input_capture_set_trigger(channel, edge);
if (rv == 0) {
rv = io_timer_set_enable(true, IOTimerChanMode_Capture, 1 << channel);
}
}
}
}
return rv;
}
int up_input_capture_get_filter(unsigned channel, capture_filter_t *filter)
{
int rv = io_timer_validate_channel_index(channel);
if (rv == 0) {
rv = -ENXIO;
/* Any pins in capture mode */
if (io_timer_get_channel_mode(channel) == IOTimerChanMode_Capture) {
uint32_t timer = timer_io_channels[channel].timer_index;
uint16_t rvalue;
rv = OK;
switch (timer_io_channels[channel].timer_channel) {
case 1:
rvalue = rCCMR1(timer) & GTIM_CCMR1_IC1F_MASK;
*filter = (rvalue << GTIM_CCMR1_IC1F_SHIFT);
break;
case 2:
rvalue = rCCMR1(timer) & GTIM_CCMR1_IC2F_MASK;
*filter = (rvalue << GTIM_CCMR1_IC2F_SHIFT);
break;
case 3:
rvalue = rCCMR2(timer) & GTIM_CCMR2_IC3F_MASK;
*filter = (rvalue << GTIM_CCMR2_IC3F_SHIFT);
break;
case 4:
rvalue = rCCMR2(timer) & GTIM_CCMR2_IC4F_MASK;
*filter = (rvalue << GTIM_CCMR2_IC4F_SHIFT);
break;
default:
rv = -EIO;
}
}
}
return rv;
}
int up_input_capture_set_filter(unsigned channel, capture_filter_t filter)
{
if (filter > GTIM_CCMR1_IC1F_MASK) {
return -EINVAL;
}
int rv = io_timer_validate_channel_index(channel);
if (rv == 0) {
rv = -ENXIO;
/* Any pins in capture mode */
if (io_timer_get_channel_mode(channel) == IOTimerChanMode_Capture) {
rv = OK;
uint32_t timer = timer_io_channels[channel].timer_index;
uint16_t rvalue;
irqstate_t flags = irqsave();
switch (timer_io_channels[channel].timer_channel) {
case 1:
rvalue = rCCMR1(timer) & ~GTIM_CCMR1_IC1F_MASK;
rvalue |= (filter << GTIM_CCMR1_IC1F_SHIFT);
rCCMR1(timer) = rvalue;
break;
case 2:
rvalue = rCCMR1(timer) & ~GTIM_CCMR1_IC2F_MASK;
rvalue |= (filter << GTIM_CCMR1_IC2F_SHIFT);
rCCMR1(timer) = rvalue;
break;
case 3:
rvalue = rCCMR2(timer) & ~GTIM_CCMR2_IC3F_MASK;
rvalue |= (filter << GTIM_CCMR2_IC3F_SHIFT);
rCCMR2(timer) = rvalue;
break;
case 4:
rvalue = rCCMR2(timer) & ~GTIM_CCMR2_IC4F_MASK;
rvalue |= (filter << GTIM_CCMR2_IC4F_SHIFT);
rCCMR1(timer) = rvalue;
break;
default:
rv = -EIO;
}
irqrestore(flags);
}
}
return rv;
}
int up_input_capture_get_trigger(unsigned channel, input_capture_edge *edge)
{
int rv = io_timer_validate_channel_index(channel);
if (rv == 0) {
rv = -ENXIO;
/* Any pins in capture mode */
if (io_timer_get_channel_mode(channel) == IOTimerChanMode_Capture) {
rv = OK;
uint32_t timer = timer_io_channels[channel].timer_index;
uint16_t rvalue;
switch (timer_io_channels[channel].timer_channel) {
case 1:
rvalue = rCCER(timer) & (GTIM_CCER_CC1P | GTIM_CCER_CC1NP);
break;
case 2:
rvalue = rCCER(timer) & (GTIM_CCER_CC2P | GTIM_CCER_CC2NP);
rvalue >>= 4;
break;
case 3:
rvalue = rCCER(timer) & (GTIM_CCER_CC3P | GTIM_CCER_CC3NP);
rvalue >>= 8;
break;
case 4:
rvalue = rCCER(timer) & (GTIM_CCER_CC4P | GTIM_CCER_CC4NP);
rvalue >>= 12;
break;
default:
rv = -EIO;
}
if (rv == 0) {
switch (rvalue) {
case 0:
*edge = Rising;
break;
case (GTIM_CCER_CC1P | GTIM_CCER_CC1NP):
*edge = Both;
break;
case (GTIM_CCER_CC1P):
*edge = Falling;
break;
default:
rv = -EIO;
}
}
}
}
return rv;
}
int up_input_capture_set_trigger(unsigned channel, input_capture_edge edge)
{
int rv = io_timer_validate_channel_index(channel);
if (rv == 0) {
rv = -ENXIO;
/* Any pins in capture mode */
if (io_timer_get_channel_mode(channel) == IOTimerChanMode_Capture) {
uint16_t edge_bits = 0xffff;
switch (edge) {
case Disabled:
break;
case Rising:
edge_bits = 0;
break;
case Falling:
edge_bits = GTIM_CCER_CC1P;
break;
case Both:
edge_bits = GTIM_CCER_CC1P | GTIM_CCER_CC1NP;
break;
default:
return -EINVAL;;
}
uint32_t timer = timer_io_channels[channel].timer_index;
uint16_t rvalue;
rv = OK;
irqstate_t flags = irqsave();
switch (timer_io_channels[channel].timer_channel) {
case 1:
rvalue = rCCER(timer);
rvalue &= ~(GTIM_CCER_CC1P | GTIM_CCER_CC1NP);
rvalue |= edge_bits;
rCCER(timer) = rvalue;
break;
case 2:
rvalue = rCCER(timer);
rvalue &= ~(GTIM_CCER_CC2P | GTIM_CCER_CC2NP);
rvalue |= (edge_bits << 4);
rCCER(timer) = rvalue;
break;
case 3:
rvalue = rCCER(timer);
rvalue &= ~(GTIM_CCER_CC3P | GTIM_CCER_CC3NP);
rvalue |= edge_bits << 8;
rCCER(timer) = rvalue;
break;
case 4:
rvalue = rCCER(timer);
rvalue &= ~(GTIM_CCER_CC4P | GTIM_CCER_CC4NP);
rvalue |= edge_bits << 12;
rCCER(timer) = rvalue;
break;
default:
rv = -EIO;
}
irqrestore(flags);
}
}
return rv;
}
int up_input_capture_get_callback(unsigned channel, capture_callback_t *callback, void **context)
{
int rv = io_timer_validate_channel_index(channel);
if (rv == 0) {
rv = -ENXIO;
/* Any pins in capture mode */
if (io_timer_get_channel_mode(channel) == IOTimerChanMode_Capture) {
irqstate_t flags = irqsave();
*callback = channel_handlers[channel].callback;
*context = channel_handlers[channel].context;
irqrestore(flags);
rv = OK;
}
}
return rv;
}
int up_input_capture_set_callback(unsigned channel, capture_callback_t callback, void *context)
{
int rv = io_timer_validate_channel_index(channel);
if (rv == 0) {
rv = -ENXIO;
/* Any pins in capture mode */
if (io_timer_get_channel_mode(channel) == IOTimerChanMode_Capture) {
input_capture_bind(channel, callback, context);
rv = 0;
}
}
return rv;
}
int up_input_capture_get_stats(unsigned channel, input_capture_stats_t *stats, bool clear)
{
int rv = io_timer_validate_channel_index(channel);
if (rv == 0) {
irqstate_t flags = irqsave();
*stats = channel_stats[channel];
if (clear) {
memset(&channel_stats[channel], 0, sizeof(*stats));
}
irqrestore(flags);
}
return rv;
}
src/drivers/stm32/drv_input_capture.h
+42
View File
@@ -0,0 +1,42 @@
/****************************************************************************
*
* Copyright (C) 2012-2016 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file drv_input_capture.h
*
* stm32-specific input capture data.
*/
#pragma once
#include <drivers/drv_input_capture.h>
src/drivers/stm32/drv_io_timer.c
+729
View File
@@ -0,0 +1,729 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/*
* @file drv_pwm_servo.c
*
* Servo driver supporting PWM servos connected to STM32 timer blocks.
*
* Works with any of the 'generic' or 'advanced' STM32 timers that
* have output pins, does not require an interrupt.
*/
#include <px4_config.h>
#include <nuttx/arch.h>
#include <nuttx/irq.h>
#include <sys/types.h>
#include <stdbool.h>
#include <assert.h>
#include <debug.h>
#include <time.h>
#include <queue.h>
#include <errno.h>
#include <string.h>
#include <stdio.h>
#include <arch/board/board.h>
#include <drivers/drv_pwm_output.h>
#include "drv_io_timer.h"
#include <chip.h>
#include <up_internal.h>
#include <up_arch.h>
#include <stm32.h>
#include <stm32_gpio.h>
#include <stm32_tim.h>
#define arraySize(a) (sizeof((a))/sizeof(((a)[0])))
#define MAX_CHANNELS_PER_TIMER 4
#define _REG32(_base, _reg) (*(volatile uint32_t *)(_base + _reg))
#define REG(_tmr, _reg) _REG32(io_timers[_tmr].base, _reg)
#define rCR1(_tmr) REG(_tmr, STM32_GTIM_CR1_OFFSET)
#define rCR2(_tmr) REG(_tmr, STM32_GTIM_CR2_OFFSET)
#define rSMCR(_tmr) REG(_tmr, STM32_GTIM_SMCR_OFFSET)
#define rDIER(_tmr) REG(_tmr, STM32_GTIM_DIER_OFFSET)
#define rSR(_tmr) REG(_tmr, STM32_GTIM_SR_OFFSET)
#define rEGR(_tmr) REG(_tmr, STM32_GTIM_EGR_OFFSET)
#define rCCMR1(_tmr) REG(_tmr, STM32_GTIM_CCMR1_OFFSET)
#define rCCMR2(_tmr) REG(_tmr, STM32_GTIM_CCMR2_OFFSET)
#define rCCER(_tmr) REG(_tmr, STM32_GTIM_CCER_OFFSET)
#define rCNT(_tmr) REG(_tmr, STM32_GTIM_CNT_OFFSET)
#define rPSC(_tmr) REG(_tmr, STM32_GTIM_PSC_OFFSET)
#define rARR(_tmr) REG(_tmr, STM32_GTIM_ARR_OFFSET)
#define rCCR1(_tmr) REG(_tmr, STM32_GTIM_CCR1_OFFSET)
#define rCCR2(_tmr) REG(_tmr, STM32_GTIM_CCR2_OFFSET)
#define rCCR3(_tmr) REG(_tmr, STM32_GTIM_CCR3_OFFSET)
#define rCCR4(_tmr) REG(_tmr, STM32_GTIM_CCR4_OFFSET)
#define rDCR(_tmr) REG(_tmr, STM32_GTIM_DCR_OFFSET)
#define rDMAR(_tmr) REG(_tmr, STM32_GTIM_DMAR_OFFSET)
#define rBDTR(_tmr) REG(_tmr, STM32_ATIM_BDTR_OFFSET)
#define GTIM_SR_CCIF (GTIM_SR_CC4IF|GTIM_SR_CC3IF|GTIM_SR_CC2IF|GTIM_SR_CC1IF)
#define GTIM_SR_CCOF (GTIM_SR_CC4OF|GTIM_SR_CC3OF|GTIM_SR_CC2OF|GTIM_SR_CC1OF)
#define CCMR_C1_RESET 0x00ff
#define CCMR_C1_NUM_BITS 8
#define CCER_C1_NUM_BITS 4
#define CCMR_C1_CAPTURE_INIT (GTIM_CCMR_CCS_CCIN1 << GTIM_CCMR1_CC1S_SHIFT) | \
(GTIM_CCMR_ICPSC_NOPSC << GTIM_CCMR1_IC1PSC_SHIFT) | \
(GTIM_CCMR_ICF_NOFILT << GTIM_CCMR1_IC1F_SHIFT)
#define CCMR_C1_PWMOUT_INIT (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR1_OC1M_SHIFT) | GTIM_CCMR1_OC1PE
#define CCMR_C1_PWMIN_INIT 0 // TBD
// NotUsed PWMOut PWMIn Capture
io_timer_channel_allocation_t channel_allocations[IOTimerChanModeSize] = { UINT8_MAX, 0 , 0 , 0 };
typedef uint8_t io_timer_allocation_t; /* big enough to hold MAX_IO_TIMERS */
static io_timer_allocation_t once = 0;
typedef struct channel_stat_t {
uint32_t isr_cout;
uint32_t overflows;
} channel_stat_t;
static channel_stat_t io_timer_channel_stats[MAX_TIMER_IO_CHANNELS];
static struct channel_handler_entry {
channel_handler_t callback;
void *context;
} channel_handlers[MAX_TIMER_IO_CHANNELS];
static int io_timer_handler(uint16_t timer_index)
{
/* Read the count at the time of the interrupt */
uint16_t count = rCNT(timer_index);
/* Read the HRT at the time of the interrupt */
hrt_abstime now = hrt_absolute_time();
const io_timers_t *tmr = &io_timers[timer_index];
/* What is pending and enabled? */
uint16_t statusr = rSR(timer_index);
uint16_t enabled = rDIER(timer_index) & GTIM_SR_CCIF;
/* Iterate over the timer_io_channels table */
for (int chan_index = tmr->first_channel_index; chan_index <= tmr->last_channel_index; chan_index++) {
uint16_t masks = timer_io_channels[chan_index].masks;
/* Do we have an enabled channel */
if (enabled & masks) {
if (statusr & masks & GTIM_SR_CCIF) {
io_timer_channel_stats[chan_index].isr_cout++;
/* Call the client to read the CCxR etc and clear the CCxIF */
if (channel_handlers[chan_index].callback) {
channel_handlers[chan_index].callback(channel_handlers[chan_index].context, tmr,
chan_index, &timer_io_channels[chan_index],
now , count);
}
}
if (statusr & masks & GTIM_SR_CCOF) {
/* Error we has a second edge before we cleared CCxR */
io_timer_channel_stats[chan_index].overflows++;
}
}
}
/* Clear all the SR bits for interrupt enabled channels only */
rSR(timer_index) = ~(statusr & (enabled | enabled << 8));
return 0;
}
int io_timer_handler0(int irq, void *context)
{
return io_timer_handler(0);
}
int io_timer_handler1(int irq, void *context)
{
return io_timer_handler(1);
}
int io_timer_handler2(int irq, void *context)
{
return io_timer_handler(2);
}
int io_timer_handler3(int irq, void *context)
{
return io_timer_handler(3);
}
static inline int validate_timer_index(unsigned timer)
{
return (timer < MAX_IO_TIMERS && io_timers[timer].base != 0) ? 0 : -EINVAL;
}
static inline int is_timer_uninitalized(unsigned timer)
{
int rv = 0;
if (once & 1 << timer) {
rv = -EBUSY;
}
return rv;
}
static inline void set_timer_initalized(unsigned timer)
{
once |= 1 << timer;
}
static inline void set_timer_deinitalized(unsigned timer)
{
once &= ~(1 << timer);
}
static inline int channels_timer(unsigned channel)
{
return timer_io_channels[channel].timer_index;
}
static uint32_t get_timer_channels(unsigned timer)
{
uint32_t channels = 0;
if (validate_timer_index(timer) == 0) {
const io_timers_t *tmr = &io_timers[timer];
/* Gather the channel bit that belong to the timer */
for (int chan_index = tmr->first_channel_index; chan_index <= tmr->last_channel_index; chan_index++) {
channels |= 1 << chan_index;
}
}
return channels;
}
static inline int is_channels_timer_uninitalized(unsigned channel)
{
return is_timer_uninitalized(channels_timer(channel));
}
int io_timer_is_channel_free(unsigned channel)
{
int rv = io_timer_validate_channel_index(channel);
if (rv == 0) {
if (0 == (channel_allocations[IOTimerChanMode_NotUsed] & (1 << channel))) {
rv = -EBUSY;
}
}
return rv;
}
int io_timer_validate_channel_index(unsigned channel)
{
int rv = -EINVAL;
if (channel < MAX_TIMER_IO_CHANNELS && timer_io_channels[channel].timer_channel != 0) {
unsigned timer = timer_io_channels[channel].timer_index;
/* test timer for validity */
if ((io_timers[timer].base != 0) &&
(timer_io_channels[channel].gpio_out != 0) &&
(timer_io_channels[channel].gpio_in != 0)) {
rv = 0;
}
}
return rv;
}
int io_timer_get_mode_channels(io_timer_channel_mode_t mode)
{
if (mode < IOTimerChanModeSize) {
return channel_allocations[mode];
}
return 0;
}
int io_timer_get_channel_mode(unsigned channel)
{
io_timer_channel_allocation_t bit = 1 << channel;
for (int mode = IOTimerChanMode_NotUsed; mode < IOTimerChanModeSize; mode++) {
if (bit & channel_allocations[mode]) {
return mode;
}
}
return -1;
}
static inline int allocate_channel_resource(unsigned channel, io_timer_channel_mode_t mode)
{
int rv = io_timer_is_channel_free(channel);
if (rv == 0) {
io_timer_channel_allocation_t bit = 1 << channel;
channel_allocations[IOTimerChanMode_NotUsed] &= ~bit;
channel_allocations[mode] |= bit;
}
return rv;
}
static inline int free_channel_resource(unsigned channel)
{
int mode = io_timer_get_channel_mode(channel);
if (mode > IOTimerChanMode_NotUsed) {
io_timer_channel_allocation_t bit = 1 << channel;
channel_allocations[mode] &= ~bit;
channel_allocations[IOTimerChanMode_NotUsed] |= bit;
}
return mode;
}
int io_timer_free_channel(unsigned channel)
{
if (io_timer_validate_channel_index(channel) != 0) {
return -EINVAL;
}
int mode = io_timer_get_channel_mode(channel);
if (mode > IOTimerChanMode_NotUsed) {
io_timer_set_enable(false, mode, 1 << channel);
free_channel_resource(channel);
}
return 0;
}
static int allocate_channel(unsigned channel, io_timer_channel_mode_t mode)
{
int rv = -EINVAL;
if (mode != IOTimerChanMode_NotUsed) {
rv = io_timer_validate_channel_index(channel);
if (rv == 0) {
rv = allocate_channel_resource(channel, mode);
}
}
return rv;
}
static int timer_set_rate(unsigned timer, unsigned rate)
{
/* configure the timer to update at the desired rate */
rARR(timer) = 1000000 / rate;
/* generate an update event; reloads the counter and all registers */
rEGR(timer) = GTIM_EGR_UG;
return 0;
}
static int io_timer_init_timer(unsigned timer)
{
/* Do this only once per timer */
int rv = is_timer_uninitalized(timer);
if (rv == 0) {
irqstate_t flags = irqsave();
set_timer_initalized(timer);
/* enable the timer clock before we try to talk to it */
modifyreg32(io_timers[timer].clock_register, 0, io_timers[timer].clock_bit);
/* disable and configure the timer */
rCR1(timer) = 0;
rCR2(timer) = 0;
rSMCR(timer) = 0;
rDIER(timer) = 0;
rCCER(timer) = 0;
rCCMR1(timer) = 0;
rCCMR2(timer) = 0;
rCCR1(timer) = 0;
rCCR2(timer) = 0;
rCCR3(timer) = 0;
rCCR4(timer) = 0;
rCCER(timer) = 0;
rDCR(timer) = 0;
if ((io_timers[timer].base == STM32_TIM1_BASE) || (io_timers[timer].base == STM32_TIM8_BASE)) {
/* master output enable = on */
rBDTR(timer) = ATIM_BDTR_MOE;
}
/* configure the timer to free-run at 1MHz */
rPSC(timer) = (io_timers[timer].clock_freq / 1000000) - 1;
/*
* Note we do the Standard PWM Out init here
* default to updating at 50Hz
*/
timer_set_rate(timer, 50);
/*
* Note that the timer is left disabled with IRQ subs installed
* and active but DEIR bits are not set.
*/
irq_attach(io_timers[timer].vectorno, io_timers[timer].handler);
up_enable_irq(io_timers[timer].vectorno);
irqrestore(flags);
}
return rv;
}
int io_timer_set_rate(unsigned timer, unsigned rate)
{
/* Gather the channel bit that belong to the timer */
uint32_t channels = get_timer_channels(timer);
/* Check ownership of PWM out */
if ((channels & channel_allocations[IOTimerChanMode_PWMOut]) != 0) {
/* Change only a timer that is owned by pwm */
timer_set_rate(timer, rate);
}
return 0;
}
int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode,
channel_handler_t channel_handler, void *context)
{
uint32_t gpio = 0;
uint32_t clearbits = CCMR_C1_RESET;
uint32_t setbits = CCMR_C1_CAPTURE_INIT;
uint32_t ccer_setbits = GTIM_CCER_CC1E;
uint32_t dier_setbits = GTIM_DIER_CC1IE;
/* figure out the GPIO config first */
switch (mode) {
case IOTimerChanMode_PWMOut:
ccer_setbits = 0;
dier_setbits = 0;
setbits = CCMR_C1_PWMOUT_INIT;
break;
case IOTimerChanMode_PWMIn:
setbits = CCMR_C1_PWMIN_INIT;
gpio = timer_io_channels[channel].gpio_in;
break;
case IOTimerChanMode_Capture:
setbits = CCMR_C1_CAPTURE_INIT;
gpio = timer_io_channels[channel].gpio_in;
break;
case IOTimerChanMode_NotUsed:
setbits = 0;
break;
default:
return -EINVAL;
}
int rv = allocate_channel(channel, mode);
/* Valid channel should now be reserved in new mode */
if (rv >= 0) {
/* Blindly try to initialize the time - it will only do it once */
io_timer_init_timer(channels_timer(channel));
irqstate_t flags = irqsave();
/* Set up IO */
if (gpio) {
stm32_configgpio(gpio);
}
unsigned timer = channels_timer(channel);
/* configure the channel */
uint32_t shifts = timer_io_channels[channel].timer_channel - 1;
/* Map shifts timer channel 1-4 to 0-3 */
uint32_t ccmr_offset = STM32_GTIM_CCMR1_OFFSET + ((shifts >> 1) * sizeof(uint32_t));
uint32_t ccr_offset = STM32_GTIM_CCR1_OFFSET + (shifts * sizeof(uint32_t));
clearbits <<= (shifts & 1) * CCMR_C1_NUM_BITS;
setbits <<= (shifts & 1) * CCMR_C1_NUM_BITS;
uint16_t rvalue = REG(timer, ccmr_offset);
rvalue &= ~clearbits;
rvalue |= setbits;
REG(timer, ccmr_offset) = rvalue;
/*
* The beauty here is that per DocID018909 Rev 8 18.3.5 Input capture mode
* As soon as CCxS (in SSMRx becomes different from 00, the channel is configured
* in input and the TIMx_CCR1 register becomes read-only.
* so the next line does nothing in capture mode and initializes an PWM out to
* 0
*/
REG(timer, ccr_offset) = 0;
/* on PWM Out ccer_setbits is 0 */
clearbits = (GTIM_CCER_CC1E | GTIM_CCER_CC1P | GTIM_CCER_CC1NP) << (shifts * CCER_C1_NUM_BITS);
setbits = ccer_setbits << (shifts * CCER_C1_NUM_BITS);
rvalue = rCCER(timer);
rvalue &= ~clearbits;
rvalue |= setbits;
rCCER(timer) = rvalue;
channel_handlers[channel].callback = channel_handler;
channel_handlers[channel].context = context;
rDIER(timer) |= dier_setbits << shifts;
irqrestore(flags);
}
return rv;
}
int io_timer_set_enable(bool state, io_timer_channel_mode_t mode, io_timer_channel_allocation_t masks)
{
struct action_cache_t {
uint32_t ccer_clearbits;
uint32_t ccer_setbits;
uint32_t dier_setbits;
uint32_t dier_clearbits;
uint32_t base;
uint32_t gpio[MAX_CHANNELS_PER_TIMER];
} action_cache[MAX_IO_TIMERS];
memset(action_cache, 0, sizeof(action_cache));
uint32_t dier_bit = state ? GTIM_DIER_CC1IE : 0;
uint32_t ccer_bit = state ? GTIM_CCER_CC1E : 0;
switch (mode) {
case IOTimerChanMode_NotUsed:
case IOTimerChanMode_PWMOut:
dier_bit = 0;
break;
case IOTimerChanMode_PWMIn:
case IOTimerChanMode_Capture:
break;
default:
return -EINVAL;
}
/* Was the request for all channels in this mode ?*/
if (masks == IO_TIMER_ALL_MODES_CHANNELS) {
/* Yes - we provide them */
masks = channel_allocations[mode];
} else {
/* No - caller provided mask */
/* Only allow the channels in that mode to be affected */
masks &= channel_allocations[mode];
}
/* Pre calculate all the changes */
for (int chan_index = 0; masks != 0 && chan_index < MAX_TIMER_IO_CHANNELS; chan_index++) {
if (masks & (1 << chan_index)) {
masks &= ~(1 << chan_index);
uint32_t shifts = timer_io_channels[chan_index].timer_channel - 1;
uint32_t timer = channels_timer(chan_index);
action_cache[timer].base = io_timers[timer].base;
action_cache[timer].ccer_clearbits |= GTIM_CCER_CC1E << (shifts * CCER_C1_NUM_BITS);
action_cache[timer].ccer_setbits |= ccer_bit << (shifts * CCER_C1_NUM_BITS);
action_cache[timer].dier_clearbits |= GTIM_DIER_CC1IE << shifts;
action_cache[timer].dier_setbits |= dier_bit << shifts;
if (state && mode == IOTimerChanMode_PWMOut) {
action_cache[timer].gpio[shifts] = timer_io_channels[chan_index].gpio_out;
}
}
}
irqstate_t flags = irqsave();
for (int actions = 0; actions < arraySize(action_cache) && action_cache[actions].base != 0 ; actions++) {
uint32_t rvalue = _REG32(action_cache[actions].base, STM32_GTIM_CCER_OFFSET);
rvalue &= ~action_cache[actions].ccer_clearbits;
rvalue |= action_cache[actions].ccer_setbits;
_REG32(action_cache[actions].base, STM32_GTIM_CCER_OFFSET) = rvalue;
uint32_t after = rvalue & (GTIM_CCER_CC1E | GTIM_CCER_CC2E | GTIM_CCER_CC3E | GTIM_CCER_CC4E);
rvalue = _REG32(action_cache[actions].base, STM32_GTIM_DIER_OFFSET);
rvalue &= ~action_cache[actions].dier_clearbits;
rvalue |= action_cache[actions].dier_setbits;
_REG32(action_cache[actions].base, STM32_GTIM_DIER_OFFSET) = rvalue;
/* Any On ?*/
if (after != 0) {
/* force an update to preload all registers */
rEGR(actions) = GTIM_EGR_UG;
for (int chan = 0; chan < arraySize(action_cache[actions].gpio); chan++) {
if (action_cache[actions].gpio[chan]) {
stm32_configgpio(action_cache[actions].gpio[chan]);
action_cache[actions].gpio[chan] = 0;
}
}
/* arm requires the timer be enabled */
rCR1(actions) |= GTIM_CR1_CEN | GTIM_CR1_ARPE;
} else {
rCR1(actions) = 0;
}
}
irqrestore(flags);
return 0;
}
int io_timer_set_ccr(unsigned channel, uint16_t value)
{
int rv = io_timer_validate_channel_index(channel);
if (rv == 0) {
if (io_timer_get_channel_mode(channel) != IOTimerChanMode_PWMOut) {
rv = -EIO;
} else {
/* configure the channel */
if (value > 0) {
value--;
}
REG(channels_timer(channel), timer_io_channels[channel].ccr_offset) = value;
}
}
return rv;
}
uint16_t io_channel_get_ccr(unsigned channel)
{
uint16_t value = 0;
if (io_timer_validate_channel_index(channel) == 0 &&
io_timer_get_channel_mode(channel) == IOTimerChanMode_PWMOut) {
value = REG(channels_timer(channel), timer_io_channels[channel].ccr_offset) + 1;
}
return value;
}
uint32_t io_timer_get_group(unsigned timer)
{
return get_timer_channels(timer);
}
src/drivers/stm32/drv_io_timer.h
+113
View File
@@ -0,0 +1,113 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file drv_io_timer.h
*
* stm32-specific PWM output data.
*/
#include <px4_config.h>
#include <nuttx/arch.h>
#include <nuttx/irq.h>
#include <drivers/drv_hrt.h>
#pragma once
__BEGIN_DECLS
/* configuration limits */
#define MAX_IO_TIMERS 4
#define MAX_TIMER_IO_CHANNELS 8
#define IO_TIMER_ALL_MODES_CHANNELS 0
typedef enum io_timer_channel_mode_t {
IOTimerChanMode_NotUsed = 0,
IOTimerChanMode_PWMOut = 1,
IOTimerChanMode_PWMIn = 2,
IOTimerChanMode_Capture = 3,
IOTimerChanModeSize
} io_timer_channel_mode_t;
typedef uint8_t io_timer_channel_allocation_t; /* big enough to hold MAX_TIMER_IO_CHANNELS */
/* array of timers dedicated to PWM in and out and capture use */
typedef struct io_timers_t {
uint32_t base;
uint32_t clock_register;
uint32_t clock_bit;
uint32_t clock_freq;
uint32_t vectorno;
uint32_t first_channel_index;
uint32_t last_channel_index;
xcpt_t handler;
} io_timers_t;
/* array of channels in logical order */
typedef struct timer_io_channels_t {
uint32_t gpio_out;
uint32_t gpio_in;
uint8_t timer_index;
uint8_t timer_channel;
uint16_t masks;
uint8_t ccr_offset;
} timer_io_channels_t;
typedef void (*channel_handler_t)(void *context, const io_timers_t *timer, uint32_t chan_index,
const timer_io_channels_t *chan,
hrt_abstime isrs_time , uint16_t isrs_rcnt);
/* supplied by board-specific code */
__EXPORT extern const io_timers_t io_timers[MAX_IO_TIMERS];
__EXPORT extern const timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS];
__EXPORT extern io_timer_channel_allocation_t allocations[IOTimerChanModeSize];
__EXPORT int io_timer_handler0(int irq, void *context);
__EXPORT int io_timer_handler1(int irq, void *context);
__EXPORT int io_timer_handler2(int irq, void *context);
__EXPORT int io_timer_handler3(int irq, void *context);
__EXPORT int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode,
channel_handler_t channel_handler, void *context);
__EXPORT int io_timer_set_rate(unsigned timer, unsigned rate);
__EXPORT int io_timer_set_enable(bool state, io_timer_channel_mode_t mode,
io_timer_channel_allocation_t masks);
__EXPORT int io_timer_set_rate(unsigned timer, unsigned rate);
__EXPORT uint16_t io_channel_get_ccr(unsigned channel);
__EXPORT int io_timer_set_ccr(unsigned channel, uint16_t value);
__EXPORT uint32_t io_timer_get_group(unsigned timer);
__EXPORT int io_timer_validate_channel_index(unsigned channel);
__EXPORT int io_timer_is_channel_free(unsigned channel);
__EXPORT int io_timer_free_channel(unsigned channel);
__EXPORT int io_timer_get_channel_mode(unsigned channel);
__EXPORT int io_timer_get_mode_channels(io_timer_channel_mode_t mode);
__END_DECLS
src/drivers/stm32/drv_pwm_servo.c
+35 -229
View File
@@ -58,6 +58,7 @@
#include <arch/board/board.h>
#include <drivers/drv_pwm_output.h>
#include "drv_io_timer.h"
#include "drv_pwm_servo.h"
#include <chip.h>
@@ -65,224 +66,58 @@
#include <up_arch.h>
#include <stm32.h>
#include <stm32_gpio.h>
#include <stm32_tim.h>
#define REG(_tmr, _reg) (*(volatile uint32_t *)(pwm_timers[_tmr].base + _reg))
#define rCR1(_tmr) REG(_tmr, STM32_GTIM_CR1_OFFSET)
#define rCR2(_tmr) REG(_tmr, STM32_GTIM_CR2_OFFSET)
#define rSMCR(_tmr) REG(_tmr, STM32_GTIM_SMCR_OFFSET)
#define rDIER(_tmr) REG(_tmr, STM32_GTIM_DIER_OFFSET)
#define rSR(_tmr) REG(_tmr, STM32_GTIM_SR_OFFSET)
#define rEGR(_tmr) REG(_tmr, STM32_GTIM_EGR_OFFSET)
#define rCCMR1(_tmr) REG(_tmr, STM32_GTIM_CCMR1_OFFSET)
#define rCCMR2(_tmr) REG(_tmr, STM32_GTIM_CCMR2_OFFSET)
#define rCCER(_tmr) REG(_tmr, STM32_GTIM_CCER_OFFSET)
#define rCNT(_tmr) REG(_tmr, STM32_GTIM_CNT_OFFSET)
#define rPSC(_tmr) REG(_tmr, STM32_GTIM_PSC_OFFSET)
#define rARR(_tmr) REG(_tmr, STM32_GTIM_ARR_OFFSET)
#define rCCR1(_tmr) REG(_tmr, STM32_GTIM_CCR1_OFFSET)
#define rCCR2(_tmr) REG(_tmr, STM32_GTIM_CCR2_OFFSET)
#define rCCR3(_tmr) REG(_tmr, STM32_GTIM_CCR3_OFFSET)
#define rCCR4(_tmr) REG(_tmr, STM32_GTIM_CCR4_OFFSET)
#define rDCR(_tmr) REG(_tmr, STM32_GTIM_DCR_OFFSET)
#define rDMAR(_tmr) REG(_tmr, STM32_GTIM_DMAR_OFFSET)
#define rBDTR(_tmr) REG(_tmr, STM32_ATIM_BDTR_OFFSET)
static void pwm_timer_init(unsigned timer);
static void pwm_timer_set_rate(unsigned timer, unsigned rate);
static void pwm_channel_init(unsigned channel);
static void
pwm_timer_init(unsigned timer)
int up_pwm_servo_set(unsigned channel, servo_position_t value)
{
/* enable the timer clock before we try to talk to it */
modifyreg32(pwm_timers[timer].clock_register, 0, pwm_timers[timer].clock_bit);
/* disable and configure the timer */
rCR1(timer) = 0;
rCR2(timer) = 0;
rSMCR(timer) = 0;
rDIER(timer) = 0;
rCCER(timer) = 0;
rCCMR1(timer) = 0;
rCCMR2(timer) = 0;
rCCER(timer) = 0;
rDCR(timer) = 0;
if ((pwm_timers[timer].base == STM32_TIM1_BASE) || (pwm_timers[timer].base == STM32_TIM8_BASE)) {
/* master output enable = on */
rBDTR(timer) = ATIM_BDTR_MOE;
}
/* configure the timer to free-run at 1MHz */
rPSC(timer) = (pwm_timers[timer].clock_freq / 1000000) - 1;
/* default to updating at 50Hz */
pwm_timer_set_rate(timer, 50);
/* note that the timer is left disabled - arming is performed separately */
return io_timer_set_ccr(channel, value);
}
static void
pwm_timer_set_rate(unsigned timer, unsigned rate)
servo_position_t up_pwm_servo_get(unsigned channel)
{
/* configure the timer to update at the desired rate */
rARR(timer) = 1000000 / rate;
/* generate an update event; reloads the counter and all registers */
rEGR(timer) = GTIM_EGR_UG;
return io_channel_get_ccr(channel);
}
static void
pwm_channel_init(unsigned channel)
int up_pwm_servo_init(uint32_t channel_mask)
{
unsigned timer = pwm_channels[channel].timer_index;
/* Init channels */
uint32_t current = io_timer_get_mode_channels(IOTimerChanMode_PWMOut);
/* configure the GPIO first */
stm32_configgpio(pwm_channels[channel].gpio);
// First free the current set of PWMs
/* configure the channel */
switch (pwm_channels[channel].timer_channel) {
case 1:
rCCMR1(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR1_OC1M_SHIFT) | GTIM_CCMR1_OC1PE;
rCCR1(timer) = pwm_channels[channel].default_value;
rCCER(timer) |= GTIM_CCER_CC1E;
break;
case 2:
rCCMR1(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR1_OC2M_SHIFT) | GTIM_CCMR1_OC2PE;
rCCR2(timer) = pwm_channels[channel].default_value;
rCCER(timer) |= GTIM_CCER_CC2E;
break;
case 3:
rCCMR2(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR2_OC3M_SHIFT) | GTIM_CCMR2_OC3PE;
rCCR3(timer) = pwm_channels[channel].default_value;
rCCER(timer) |= GTIM_CCER_CC3E;
break;
case 4:
rCCMR2(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR2_OC4M_SHIFT) | GTIM_CCMR2_OC4PE;
rCCR4(timer) = pwm_channels[channel].default_value;
rCCER(timer) |= GTIM_CCER_CC4E;
break;
}
}
int
up_pwm_servo_set(unsigned channel, servo_position_t value)
{
if (channel >= PWM_SERVO_MAX_CHANNELS) {
return -1;
}
unsigned timer = pwm_channels[channel].timer_index;
/* test timer for validity */
if ((pwm_timers[timer].base == 0) ||
(pwm_channels[channel].gpio == 0)) {
return -1;
}
/* configure the channel */
if (value > 0) {
value--;
}
switch (pwm_channels[channel].timer_channel) {
case 1:
rCCR1(timer) = value;
break;
case 2:
rCCR2(timer) = value;
break;
case 3:
rCCR3(timer) = value;
break;
case 4:
rCCR4(timer) = value;
break;
default:
return -1;
}
return 0;
}
servo_position_t
up_pwm_servo_get(unsigned channel)
{
if (channel >= PWM_SERVO_MAX_CHANNELS) {
return 0;
}
unsigned timer = pwm_channels[channel].timer_index;
servo_position_t value = 0;
/* test timer for validity */
if ((pwm_timers[timer].base == 0) ||
(pwm_channels[channel].timer_channel == 0)) {
return 0;
}
/* configure the channel */
switch (pwm_channels[channel].timer_channel) {
case 1:
value = rCCR1(timer);
break;
case 2:
value = rCCR2(timer);
break;
case 3:
value = rCCR3(timer);
break;
case 4:
value = rCCR4(timer);
break;
}
return value + 1;
}
int
up_pwm_servo_init(uint32_t channel_mask)
{
/* do basic timer initialisation first */
for (unsigned i = 0; i < PWM_SERVO_MAX_TIMERS; i++) {
if (pwm_timers[i].base != 0) {
pwm_timer_init(i);
for (unsigned channel = 0; current != 0 && channel < MAX_TIMER_IO_CHANNELS; channel++) {
if (current & (1 << channel)) {
io_timer_free_channel(channel);
current &= ~(1 << channel);
}
}
/* now init channels */
for (unsigned i = 0; i < PWM_SERVO_MAX_CHANNELS; i++) {
/* don't do init for disabled channels; this leaves the pin configs alone */
if (((1 << i) & channel_mask) && (pwm_channels[i].timer_channel != 0)) {
pwm_channel_init(i);
// Now allocate the new set
for (unsigned channel = 0; channel_mask != 0 && channel < MAX_TIMER_IO_CHANNELS; channel++) {
if (channel_mask & (1 << channel)) {
// First free any that were not PWM mode before
if (-EBUSY == io_timer_is_channel_free(channel)) {
io_timer_free_channel(channel);
}
io_timer_channel_init(channel, IOTimerChanMode_PWMOut, NULL, NULL);
channel_mask &= ~(1 << channel);
}
}
return OK;
}
void
up_pwm_servo_deinit(void)
void up_pwm_servo_deinit(void)
{
/* disable the timers */
up_pwm_servo_arm(false);
}
int
up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate)
int up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate)
{
/* limit update rate to 1..10000Hz; somewhat arbitrary but safe */
if (rate < 1) {
@@ -293,60 +128,31 @@ up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate)
return -ERANGE;
}
if ((group >= PWM_SERVO_MAX_TIMERS) || (pwm_timers[group].base == 0)) {
if ((group >= MAX_IO_TIMERS) || (io_timers[group].base == 0)) {
return ERROR;
}
pwm_timer_set_rate(group, rate);
io_timer_set_rate(group, rate);
return OK;
}
int
up_pwm_servo_set_rate(unsigned rate)
int up_pwm_servo_set_rate(unsigned rate)
{
for (unsigned i = 0; i < PWM_SERVO_MAX_TIMERS; i++) {
for (unsigned i = 0; i < MAX_IO_TIMERS; i++) {
up_pwm_servo_set_rate_group_update(i, rate);
}
return 0;
}
uint32_t
up_pwm_servo_get_rate_group(unsigned group)
uint32_t up_pwm_servo_get_rate_group(unsigned group)
{
unsigned channels = 0;
for (unsigned i = 0; i < PWM_SERVO_MAX_CHANNELS; i++) {
if ((pwm_channels[i].gpio != 0) && (pwm_channels[i].timer_index == group)) {
channels |= (1 << i);
}
}
return channels;
return io_timer_get_group(group);
}
void
up_pwm_servo_arm(bool armed)
{
/* iterate timers and arm/disarm appropriately */
for (unsigned i = 0; i < PWM_SERVO_MAX_TIMERS; i++) {
if (pwm_timers[i].base != 0) {
if (armed) {
/* force an update to preload all registers */
rEGR(i) = GTIM_EGR_UG;
/* arm requires the timer be enabled */
rCR1(i) |= GTIM_CR1_CEN | GTIM_CR1_ARPE;
} else {
// XXX This leads to FMU PWM being still active
// but uncontrollable. Just disable the timer
// and risk a runt.
///* on disarm, just stop auto-reload so we don't generate runts */
//rCR1(i) &= ~GTIM_CR1_ARPE;
rCR1(i) = 0;
}
}
}
io_timer_set_enable(armed, IOTimerChanMode_PWMOut, IO_TIMER_ALL_MODES_CHANNELS);
}
src/drivers/stm32/drv_pwm_servo.h
-23
View File
@@ -41,26 +41,3 @@
#include <drivers/drv_pwm_output.h>
/* configuration limits */
#define PWM_SERVO_MAX_TIMERS 4
#define PWM_SERVO_MAX_CHANNELS 8
/* array of timers dedicated to PWM servo use */
struct pwm_servo_timer {
uint32_t base;
uint32_t clock_register;
uint32_t clock_bit;
uint32_t clock_freq;
};
/* array of channels in logical order */
struct pwm_servo_channel {
uint32_t gpio;
uint8_t timer_index;
uint8_t timer_channel;
servo_position_t default_value;
};
/* supplied by board-specific code */
__EXPORT extern const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS];
__EXPORT extern const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS];
src/firmware/posix/CMakeLists.txt
+1 -1
View File
@@ -36,7 +36,7 @@ add_dependencies(run_config mainapp)
foreach(viewer none jmavsim gazebo)
foreach(debugger none gdb lldb ddd valgrind)
foreach(model none iris tailsitter standard_vtol)
foreach(model none iris iris_opt_flow tailsitter standard_vtol)
if (debugger STREQUAL "none")
if (model STREQUAL "none")
set(_targ_name "${viewer}")
src/lib/DriverFramework
+1 -1
Submodule src/lib/DriverFramework updated: 98c4dd676f...add11d4da6
src/lib/ecl
+1 -1
Submodule src/lib/ecl updated: 8d0022ab1e...2b2c490382
src/lib/external_lgpl/tecs/tecs.cpp
+2 -1
View File
@@ -512,6 +512,7 @@ void TECS::_initialise_states(float pitch, float throttle_cruise, float baro_alt
_hgt_dem_in_old = _hgt_dem_adj_last;
_TAS_dem_last = _TAS_dem;
_TAS_dem_adj = _TAS_dem;
_pitch_dem_unc = pitch;
_underspeed = false;
_badDescent = false;
@@ -621,7 +622,7 @@ void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, f
_tecs_state.total_energy_error = _STE_error;
_tecs_state.energy_distribution_error = _SEB_error;
_tecs_state.total_energy_rate_error = _STEdot_error;
_tecs_state.energy_distribution_error = _SEBdot_error;
_tecs_state.energy_distribution_rate_error = _SEBdot_error;
_tecs_state.energy_error_integ = _integ6_state;
_tecs_state.energy_distribution_error_integ = _integ7_state;
src/lib/external_lgpl/tecs/tecs.h
+18 -1
View File
@@ -58,20 +58,33 @@ public:
_integ5_state(0.0f),
_integ6_state(0.0f),
_integ7_state(0.0f),
_last_throttle_dem(0.0f),
_last_pitch_dem(0.0f),
_vel_dot(0.0f),
_EAS(0.0f),
_TASmax(30.0f),
_TASmin(3.0f),
_TAS_dem(0.0f),
_TAS_dem_last(0.0f),
_hgt_dem_in_old(0.0f),
_hgt_dem_adj(0.0f),
_hgt_dem_adj_last(0.0f),
_hgt_rate_dem(0.0f),
_hgt_dem_prev(0.0f),
_TAS_dem_adj(0.0f),
_TAS_rate_dem(0.0f),
_STEdotErrLast(0.0f),
_underspeed(false),
_detect_underspeed_enabled(true),
_badDescent(false),
_climbOutDem(false),
_pitch_dem_unc(0.0f),
_STEdot_max(0.0f),
_STEdot_min(0.0f),
_THRmaxf(0.0f),
_THRminf(0.0f),
_PITCHmaxf(0.5f),
_PITCHminf(-0.5f),
_SPE_dem(0.0f),
_SKE_dem(0.0f),
_SPEdot_dem(0.0f),
@@ -84,10 +97,14 @@ public:
_STEdot_error(0.0f),
_SEB_error(0.0f),
_SEBdot_error(0.0f),
_DT(0.02f),
_airspeed_enabled(false),
_states_initalized(false),
_in_air(false),
_throttle_slewrate(0.0f)
_throttle_slewrate(0.0f),
_indicated_airspeed_min(3.0f),
_indicated_airspeed_max(30.0f)
{
}
src/lib/launchdetection/LaunchDetector.cpp
+1 -1
View File
@@ -48,7 +48,7 @@ LaunchDetector::LaunchDetector() :
SuperBlock(NULL, "LAUN"),
activeLaunchDetectionMethodIndex(-1),
launchdetection_on(this, "ALL_ON"),
throttlePreTakeoff(this, "THR_PRE")
throttlePreTakeoff(nullptr, "FW_THR_IDLE")
{
/* init all detectors */
launchMethods[0] = new CatapultLaunchMethod(this);

Some files were not shown because too many files have changed in this diff Show More