├── .project ├── README.md ├── cmake ├── configs │ ├── posix_rpi_cross_no_shield.cmake │ ├── posix_rpi_cross_single.cmake │ ├── posix_rpi_native_single.cmake │ └── toolchains │ │ ├── Toolchain-arm-bcm2835-linux-gnueabihf-raspbian.cmake │ │ └── Toolchain-cross-generic.cmake └── toolchains │ └── Toolchain-arm-bcm2835-linx-gnueabihf-raspbian.cmake ├── posix-configs ├── arm │ └── px4_linux.config └── rpi │ ├── px4.config │ ├── px4_fw.config │ ├── px4_hil.config │ ├── px4_no_shield.config │ ├── px4_noshield.config │ └── px4_rpi.config └── src └── drivers ├── linux_pca9685 ├── CMakeLists.txt ├── linux_pca9685.cpp └── linux_pca9685.hpp ├── linux_sbus ├── CMakeLists.txt ├── linux_sbus.cpp └── linux_sbus.h ├── rpi_pca9685_pwm_out ├── CMakeLists.txt ├── PCA9685.cpp ├── PCA9685.h ├── rpi_pca9685_pwm_out.cpp └── rpi_pca9685_pwm_out.h └── rpi_rc_in ├── CMakeLists.txt ├── rpi_rc_in.cpp └── rpi_rc_in.h /.project: -------------------------------------------------------------------------------- 1 | 2 | 3 | raspberry-pi-px4firmware 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # raspberry-pi-px4firmware 2 | 树莓派飞控模块（遥控输入，PWM输出，编译配置文件，启动配置文件） 3 | 4 | # 使用方法 5 | 6 | git clone https://github.com/crossa/raspberry-pi-px4firmware 7 | 8 | git clone https://github.com/PX4/Firmware.git 9 | 10 | cd Firmware/ 11 | 12 | git checkout <分支版本号> #此处必须要做，否则编译会出错，因为主程序里面会读取版本号，切换过分枝才会有版本号 13 | 14 | cp -rf raspberry-pi-px4firmware/* Firmware/ 15 | 16 | # 追加环境变量 17 | export AUTOPILOT_HOST=XXX.XXX.XXX #此处填写ip地址 18 | 19 | export RPI_TOOLCHAIN_DIR= #工具链目录，bin目录上一层 20 | 21 | export CROSS_COMPILE_PREFIX= # 工具链前缀 22 | 23 | export PATH=$RPI_TOOLCHAIN_DIR/bin:$PATH 24 | 25 | 26 | #编译飞控固件，遇到子模块版本相关提示，默认选Y 27 | 28 | make posix_rpi_cross_single 29 | 30 | # 上传固件 31 | 32 | make posix_rpi_cross_signle upload 33 | 34 | # 首次启动飞控 35 | 36 | 登录到飞控主机后 37 | 38 | ./px4 px4_rpi.config 39 | 40 | 41 | # 注意事项 42 | 43 | 此飞控模块需配合PPM解码模块使用 44 | 45 | git clone https://github.com/crossa/raspberry-pi-ppm-rc-in 46 | 47 | ./configuration 48 | 49 | make 50 | 51 | make install 52 | 53 | sudo nohup ppmdecode -P 54 | -------------------------------------------------------------------------------- /cmake/configs/posix_rpi_cross_no_shield.cmake: -------------------------------------------------------------------------------- 1 | # 使用这个文件可在上位机交叉编译px4firmware 2 | # use this file to cross compile px4 for raspberry pi without any shield 3 | include(configs/posix_rpi_common) 4 | 5 | if("$ENV{RPI_USE_CLANG}" STREQUAL "1") 6 | set(CMAKE_TOOLCHAIN_FILE ${PX4_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-linux-gnueabihf-raspbian-clang.cmake) 7 | else() 8 | set(CMAKE_TOOLCHAIN_FILE ${PX4_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-linux-gnueabihf-raspbian.cmake) 9 | endif() 10 | 11 | set(CMAKE_PROGRAM_PATH 12 | "${RPI_TOOLCHAIN_DIR}/gcc-linaro-arm-linux-gnueabihf-raspbian/bin" 13 | ${CMAKE_PROGRAM_PATH} 14 | ) 15 | -------------------------------------------------------------------------------- /cmake/configs/posix_rpi_cross_single.cmake: -------------------------------------------------------------------------------- 1 | # 使用这个文件可在上位机交叉编译px4firmware 2 | include(configs/posix_rpi_common) 3 | 4 | # 设定交叉工具链 5 | set(CMAKE_TOOLCHAIN_FILE ${PX4_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-bcm2835-linux-gnueabihf-raspbian.cmake) 6 | 7 | -------------------------------------------------------------------------------- /cmake/configs/posix_rpi_native_single.cmake: -------------------------------------------------------------------------------- 1 | # This file is shared between posix_rpi_native.cmake 2 | # and posix_rpi_cross.cmake. 3 | 4 | include(posix/px4_impl_posix) 5 | 6 | # This definition allows to differentiate if this just the usual POSIX build 7 | # or if it is for the RPi. 8 | add_definitions( 9 | -D__PX4_POSIX_RPI 10 | -D__DF_LINUX # For DriverFramework 11 | -D__DF_RPI # For raspberry pi 12 | ) 13 | 14 | 15 | set(config_module_list 16 | # 17 | # Board support modules 18 | # 19 | drivers/device 20 | modules/sensors 21 | platforms/posix/drivers/df_mpu9250_wrapper 22 | #platforms/posix/drivers/df_lsm9ds1_wrapper 23 | platforms/posix/drivers/df_ms5611_wrapper 24 | #platforms/posix/drivers/df_hmc5883_wrapper 25 | platforms/posix/drivers/df_trone_wrapper 26 | #platforms/posix/drivers/df_isl29501_wrapper 27 | 28 | # 29 | # System commands 30 | # 31 | systemcmds/param 32 | systemcmds/mixer 33 | systemcmds/ver 34 | systemcmds/esc_calib 35 | systemcmds/reboot 36 | systemcmds/topic_listener 37 | systemcmds/perf 38 | 39 | # 40 | # Estimation modules 41 | # 42 | modules/attitude_estimator_q 43 | modules/position_estimator_inav 44 | modules/local_position_estimator 45 | modules/ekf2 46 | 47 | # 48 | # Vehicle Control 49 | # 50 | modules/mc_att_control 51 | modules/mc_pos_control 52 | modules/fw_att_control 53 | modules/fw_pos_control_l1 54 | modules/vtol_att_control 55 | 56 | # 57 | # Library modules 58 | # 59 | modules/sdlog2 60 | modules/logger 61 | modules/commander 62 | modules/param 63 | modules/systemlib 64 | modules/systemlib/mixer 65 | modules/uORB 66 | modules/dataman 67 | modules/land_detector 68 | modules/navigator 69 | modules/mavlink 70 | 71 | # 72 | # PX4 drivers 73 | # 74 | drivers/gps 75 | drivers/rpi_rc_in 76 | drivers/rpi_pca9685_pwm_out 77 | #drivers/navio_sysfs_rc_in 78 | #drivers/navio_sysfs_pwm_out 79 | #drivers/navio_gpio 80 | #drivers/navio_rgbled 81 | 82 | # 83 | # Libraries 84 | # 85 | lib/controllib 86 | lib/mathlib 87 | lib/mathlib/math/filter 88 | lib/geo 89 | lib/ecl 90 | lib/geo_lookup 91 | lib/launchdetection 92 | lib/external_lgpl 93 | lib/conversion 94 | lib/terrain_estimation 95 | lib/runway_takeoff 96 | lib/tailsitter_recovery 97 | lib/DriverFramework/framework 98 | 99 | # 100 | # POSIX 101 | # 102 | platforms/common 103 | platforms/posix/px4_layer 104 | platforms/posix/work_queue 105 | ) 106 | 107 | # 108 | # DriverFramework driver 109 | # 110 | set(config_df_driver_list 111 | mpu9250 112 | #lsm9ds1 113 | ms5611 114 | #hmc5883 115 | trone 116 | #isl29501 117 | ) 118 | 119 | set(CMAKE_TOOLCHAIN_FILE ${PX4_SOURCE_DIR}/cmake/toolchains/Toolchain-native.cmake) 120 | -------------------------------------------------------------------------------- /cmake/configs/toolchains/Toolchain-arm-bcm2835-linux-gnueabihf-raspbian.cmake: -------------------------------------------------------------------------------- 1 | # defines: 2 | # 3 | # NM 4 | # OBJCOPY 5 | # LD 6 | # CXX_COMPILER 7 | # C_COMPILER 8 | # CMAKE_SYSTEM_NAME 9 | # CMAKE_SYSTEM_VERSION 10 | # LINKER_FLAGS 11 | # CMAKE_EXE_LINKER_FLAGS 12 | # CMAKE_FIND_ROOT_PATH 13 | # CMAKE_FIND_ROOT_PATH_MODE_PROGRAM 14 | # CMAKE_FIND_ROOT_PATH_MODE_LIBRARY 15 | # CMAKE_FIND_ROOT_PATH_MODE_INCLUDE 16 | 17 | include(CMakeForceCompiler) 18 | 19 | if ("$ENV{RPI_TOOLCHAIN_DIR}" STREQUAL "") 20 | message(FATAL_ERROR "RPI_TOOLCHAIN_DIR not set") 21 | else() 22 | set(RPI_TOOLCHAIN_DIR $ENV{RPI_TOOLCHAIN_DIR}) 23 | endif() 24 | 25 | if ("$ENV{CROSS_COMPILE_PREFIX}" STREQUAL "") 26 | message(FATAL_ERROR "CROSS_COMPILE_PREFIX not set") 27 | else() 28 | set(CROSS_COMPILE_PREFIX $ENV{CROSS_COMPILE_PREFIX}) 29 | endif() 30 | 31 | 32 | 33 | 34 | 35 | # this one is important 36 | set(CMAKE_SYSTEM_NAME Generic) 37 | 38 | #this one not so much 39 | set(CMAKE_SYSTEM_VERSION 1) 40 | 41 | # specify the cross compiler 42 | find_program(C_COMPILER ${CROSS_COMPILE_PREFIX}-gcc 43 | PATHS ${RPI_TOOLCHAIN_DIR}/bin 44 | NO_DEFAULT_PATH 45 | ) 46 | 47 | if(NOT C_COMPILER) 48 | message(FATAL_ERROR "could not find ${CROSS_COMPILE_PREFIX}-gcc compiler") 49 | endif() 50 | cmake_force_c_compiler(${C_COMPILER} GNU) 51 | 52 | find_program(CXX_COMPILER ${CROSS_COMPILE_PREFIX}-g++ 53 | PATHS ${RPI_TOOLCHAIN_DIR}/bin 54 | NO_DEFAULT_PATH 55 | ) 56 | 57 | if(NOT CXX_COMPILER) 58 | message(FATAL_ERROR "could not find ${CROSS_COMPILE_PREFIX}-g++ compiler") 59 | endif() 60 | cmake_force_cxx_compiler(${CXX_COMPILER} GNU) 61 | 62 | # compiler tools 63 | foreach(tool objcopy nm ld) 64 | string(TOUPPER ${tool} TOOL) 65 | find_program(${TOOL} ${CROSS_COMPILE_PREFIX}-${tool} 66 | PATHS ${RPI_TOOLCHAIN_DIR}/bin 67 | NO_DEFAULT_PATH 68 | ) 69 | if(NOT ${TOOL}) 70 | message(FATAL_ERROR "could not find ${CROSS_COMPILE_PREFIX}-${tool}") 71 | endif() 72 | endforeach() 73 | 74 | # os tools 75 | foreach(tool echo grep rm mkdir nm cp touch make unzip) 76 | string(TOUPPER ${tool} TOOL) 77 | find_program(${TOOL} ${tool}) 78 | if(NOT ${TOOL}) 79 | message(FATAL_ERROR "could not find ${TOOL}") 80 | endif() 81 | endforeach() 82 | 83 | add_definitions( 84 | -D __DF_RPI 85 | ) 86 | 87 | set(LINKER_FLAGS "-Wl,-gc-sections") 88 | set(CMAKE_EXE_LINKER_FLAGS ${LINKER_FLAGS}) 89 | set(CMAKE_C_FLAGS ${C_FLAGS}) 90 | set(CMAKE_CXX_LINKER_FLAGS ${C_FLAGS}) 91 | 92 | # where is the target environment 93 | set(CMAKE_FIND_ROOT_PATH get_file_component(${C_COMPILER} PATH)) 94 | 95 | # search for programs in the build host directories 96 | set(CMAKE_FIND_ROOT_PATH_MODE_PROGRAM NEVER) 97 | # for libraries and headers in the target directories 98 | set(CMAKE_FIND_ROOT_PATH_MODE_LIBRARY ONLY) 99 | set(CMAKE_FIND_ROOT_PATH_MODE_INCLUDE ONLY) -------------------------------------------------------------------------------- /cmake/configs/toolchains/Toolchain-cross-generic.cmake: -------------------------------------------------------------------------------- 1 | # defines: 2 | # 3 | # NM 4 | # OBJCOPY 5 | # LD 6 | # CXX_COMPILER 7 | # C_COMPILER 8 | # CMAKE_SYSTEM_NAME 9 | # CMAKE_SYSTEM_VERSION 10 | # LINKER_FLAGS 11 | # CMAKE_EXE_LINKER_FLAGS 12 | # CMAKE_FIND_ROOT_PATH 13 | # CMAKE_FIND_ROOT_PATH_MODE_PROGRAM 14 | # CMAKE_FIND_ROOT_PATH_MODE_LIBRARY 15 | # CMAKE_FIND_ROOT_PATH_MODE_INCLUDE 16 | 17 | include(CMakeForceCompiler) 18 | 19 | if ("$ENV{TOOLCHAIN_BIN_DIR}" STREQUAL "") 20 | message(FATAL_ERROR "TOOLCHAIN_BIN_DIR not set") 21 | else() 22 | set(TOOLCHAIN_BIN_DIR $ENV{TOOLCHAIN_BIN_DIR}) 23 | endif() 24 | 25 | if ("$ENV{CROSS_COMPILE}" STREQUAL "") 26 | message(FATAL_ERROR "CROSS_COMPILE not set") 27 | else() 28 | set(CROSS_COMPILE $ENV{CROSS_COMPILE}) 29 | endif() 30 | 31 | # 此处定义CROSS_COMPILE 32 | set(CROSS_COMPILE $ENV{CROSS_COMPILE}) 33 | 34 | # this one is important 35 | set(CMAKE_SYSTEM_NAME Generic) 36 | 37 | #this one not so much 38 | set(CMAKE_SYSTEM_VERSION 1) 39 | 40 | # specify the cross compiler 41 | find_program(C_COMPILER ${CROSS_COMPILE}gcc 42 | PATHS ${TOOLCHAIN_BIN_DIR} 43 | NO_DEFAULT_PATH 44 | ) 45 | 46 | if(NOT C_COMPILER) 47 | message(FATAL_ERROR "could not find ${CROSS_COMPILE}gcc compiler") 48 | endif() 49 | cmake_force_c_compiler(${C_COMPILER} GNU) 50 | 51 | find_program(CXX_COMPILER ${CROSS_COMPILE}g++ 52 | PATHS ${TOOLCHAIN_BIN_DIR} 53 | NO_DEFAULT_PATH 54 | ) 55 | if(NOT CXX_COMPILER) 56 | message(FATAL_ERROR "could not find ${CROSS_COMPILE}g++ compiler") 57 | endif() 58 | cmake_force_cxx_compiler(${CXX_COMPILER} GNU) 59 | 60 | # compiler tools 61 | foreach(tool objcopy nm ld) 62 | string(TOUPPER ${tool} TOOL) 63 | find_program(${TOOL} ${CROSS_COMPILE}${tool} 64 | PATHS ${TOOLCHAIN_BIN_DIR} 65 | NO_DEFAULT_PATH 66 | ) 67 | if(NOT ${TOOL}) 68 | message(FATAL_ERROR "could not find ${CROSS_COMPILE}${tool}") 69 | endif() 70 | endforeach() 71 | 72 | # os tools 73 | foreach(tool echo grep rm mkdir nm cp touch make unzip) 74 | string(TOUPPER ${tool} TOOL) 75 | find_program(${TOOL} ${tool}) 76 | if(NOT ${TOOL}) 77 | message(FATAL_ERROR "could not find ${TOOL}") 78 | endif() 79 | endforeach() 80 | 81 | #add_definitions( 82 | # -D __DF_RPI 83 | #) 84 | 85 | set(LINKER_FLAGS "-Wl,-gc-sections") 86 | set(CMAKE_EXE_LINKER_FLAGS ${LINKER_FLAGS}) 87 | set(CMAKE_C_FLAGS ${C_FLAGS}) 88 | set(CMAKE_CXX_LINKER_FLAGS ${C_FLAGS}) 89 | 90 | # where is the target environment 91 | set(CMAKE_FIND_ROOT_PATH get_file_component(${C_COMPILER} PATH)) 92 | 93 | # search for programs in the build host directories 94 | set(CMAKE_FIND_ROOT_PATH_MODE_PROGRAM NEVER) 95 | # for libraries and headers in the target directories 96 | set(CMAKE_FIND_ROOT_PATH_MODE_LIBRARY ONLY) 97 | set(CMAKE_FIND_ROOT_PATH_MODE_INCLUDE ONLY) 98 | -------------------------------------------------------------------------------- /cmake/toolchains/Toolchain-arm-bcm2835-linx-gnueabihf-raspbian.cmake: -------------------------------------------------------------------------------- 1 | # defines: 2 | # 3 | # NM 4 | # OBJCOPY 5 | # LD 6 | # CXX_COMPILER 7 | # C_COMPILER 8 | # CMAKE_SYSTEM_NAME 9 | # CMAKE_SYSTEM_VERSION 10 | # LINKER_FLAGS 11 | # CMAKE_EXE_LINKER_FLAGS 12 | # CMAKE_FIND_ROOT_PATH 13 | # CMAKE_FIND_ROOT_PATH_MODE_PROGRAM 14 | # CMAKE_FIND_ROOT_PATH_MODE_LIBRARY 15 | # CMAKE_FIND_ROOT_PATH_MODE_INCLUDE 16 | 17 | include(CMakeForceCompiler) 18 | 19 | if ("$ENV{RPI_TOOLCHAIN_DIR}" STREQUAL "") 20 | message(FATAL_ERROR "RPI_TOOLCHAIN_DIR not set") 21 | else() 22 | set(RPI_TOOLCHAIN_DIR $ENV{RPI_TOOLCHAIN_DIR}) 23 | endif() 24 | 25 | if ("$ENV{CROSS_COMPILE_PREFIX}" STREQUAL "") 26 | message(FATAL_ERROR "CROSS_COMPILE_PREFIX not set") 27 | else() 28 | set(CROSS_COMPILE_PREFIX $ENV{CROSS_COMPILE_PREFIX}) 29 | endif() 30 | 31 | 32 | 33 | 34 | 35 | # this one is important 36 | set(CMAKE_SYSTEM_NAME Generic) 37 | 38 | #this one not so much 39 | set(CMAKE_SYSTEM_VERSION 1) 40 | 41 | # specify the cross compiler 42 | find_program(C_COMPILER ${CROSS_COMPILE_PREFIX}-gcc 43 | PATHS ${RPI_TOOLCHAIN_DIR}/bin 44 | NO_DEFAULT_PATH 45 | ) 46 | 47 | if(NOT C_COMPILER) 48 | message(FATAL_ERROR "could not find ${CROSS_COMPILE_PREFIX}-gcc compiler") 49 | endif() 50 | cmake_force_c_compiler(${C_COMPILER} GNU) 51 | 52 | find_program(CXX_COMPILER ${CROSS_COMPILE_PREFIX}-g++ 53 | PATHS ${RPI_TOOLCHAIN_DIR}/bin 54 | NO_DEFAULT_PATH 55 | ) 56 | 57 | if(NOT CXX_COMPILER) 58 | message(FATAL_ERROR "could not find ${CROSS_COMPILE_PREFIX}-g++ compiler") 59 | endif() 60 | cmake_force_cxx_compiler(${CXX_COMPILER} GNU) 61 | 62 | # compiler tools 63 | foreach(tool objcopy nm ld) 64 | string(TOUPPER ${tool} TOOL) 65 | find_program(${TOOL} ${CROSS_COMPILE_PREFIX}-${tool} 66 | PATHS ${RPI_TOOLCHAIN_DIR}/bin 67 | NO_DEFAULT_PATH 68 | ) 69 | if(NOT ${TOOL}) 70 | message(FATAL_ERROR "could not find ${CROSS_COMPILE_PREFIX}-${tool}") 71 | endif() 72 | endforeach() 73 | 74 | # os tools 75 | foreach(tool echo grep rm mkdir nm cp touch make unzip) 76 | string(TOUPPER ${tool} TOOL) 77 | find_program(${TOOL} ${tool}) 78 | if(NOT ${TOOL}) 79 | message(FATAL_ERROR "could not find ${TOOL}") 80 | endif() 81 | endforeach() 82 | 83 | add_definitions( 84 | -D __DF_RPI 85 | ) 86 | 87 | set(LINKER_FLAGS "-Wl,-gc-sections") 88 | set(CMAKE_EXE_LINKER_FLAGS ${LINKER_FLAGS}) 89 | set(CMAKE_C_FLAGS ${C_FLAGS}) 90 | set(CMAKE_CXX_LINKER_FLAGS ${C_FLAGS}) 91 | 92 | # where is the target environment 93 | set(CMAKE_FIND_ROOT_PATH get_file_component(${C_COMPILER} PATH)) 94 | 95 | # search for programs in the build host directories 96 | set(CMAKE_FIND_ROOT_PATH_MODE_PROGRAM NEVER) 97 | # for libraries and headers in the target directories 98 | set(CMAKE_FIND_ROOT_PATH_MODE_LIBRARY ONLY) 99 | set(CMAKE_FIND_ROOT_PATH_MODE_INCLUDE ONLY) 100 | -------------------------------------------------------------------------------- /posix-configs/arm/px4_linux.config: -------------------------------------------------------------------------------- 1 | uorb start 2 | param load 3 | param set SYS_AUTOSTART 4001 4 | param set MAV_BROADCAST 1 5 | param set MAV_TYPE 2 6 | param set SYS_MC_EST_GROUP 2 7 | param set BAT_CNT_V_VOLT 0.001 8 | param set BAT_V_DIV 10.9176300578 9 | param set BAT_CNT_V_CURR 0.001 10 | param set BAT_A_PER_V 15.391030303 11 | dataman start 12 | df_mpu9250_wrapper start -R 10 13 | #df_ms5611_wrapper start 14 | #gps start -d /dev/spidev0.0 -i spi -p ubx 15 | sensors start 16 | commander start 17 | navigator start 18 | ekf2 start 19 | land_detector start multicopter 20 | mc_pos_control start 21 | mc_att_control start 22 | mavlink start -u 14556 -r 1000000 23 | mavlink stream -u 14556 -s HIGHRES_IMU -r 50 24 | mavlink stream -u 14556 -s ATTITUDE -r 50 25 | #mavlink start -d /dev/ttyUSB0 26 | #mavlink stream -d /dev/ttyUSB0 -s HIGHRES_IMU -r 50 27 | #mavlink stream -d /dev/ttyUSB0 -s ATTITUDE -r 50 28 | linux_sbus start -d /dev/serial2 -c 8 29 | logger start -t -b 200 30 | mavlink boot_complete 31 | -------------------------------------------------------------------------------- /posix-configs/rpi/px4.config: -------------------------------------------------------------------------------- 1 | uorb start 2 | param load 3 | param set SYS_AUTOSTART 4001 4 | param set MAV_BROADCAST 1 5 | param set MAV_TYPE 2 6 | param set SYS_MC_EST_GROUP 2 7 | param set BAT_CNT_V_VOLT 0.001 8 | param set BAT_V_DIV 10.9176300578 9 | param set BAT_CNT_V_CURR 0.001 10 | param set BAT_A_PER_V 15.391030303 11 | dataman start 12 | df_lsm9ds1_wrapper start -R 4 13 | #df_mpu9250_wrapper start -R 10 14 | #df_hmc5883_wrapper start 15 | df_ms5611_wrapper start 16 | navio_rgbled start 17 | navio_adc start 18 | gps start -d /dev/spidev0.0 -i spi -p ubx 19 | sensors start 20 | commander start 21 | navigator start 22 | ekf2 start 23 | land_detector start multicopter 24 | mc_pos_control start 25 | mc_att_control start 26 | mavlink start -u 14556 -r 1000000 27 | mavlink stream -u 14556 -s HIGHRES_IMU -r 50 28 | mavlink stream -u 14556 -s ATTITUDE -r 50 29 | mavlink start -d /dev/ttyUSB0 30 | mavlink stream -d /dev/ttyUSB0 -s HIGHRES_IMU -r 50 31 | mavlink stream -d /dev/ttyUSB0 -s ATTITUDE -r 50 32 | navio_sysfs_rc_in start 33 | navio_sysfs_pwm_out start 34 | logger start -t -b 200 35 | mavlink boot_complete 36 | -------------------------------------------------------------------------------- /posix-configs/rpi/px4_fw.config: -------------------------------------------------------------------------------- 1 | uorb start 2 | param load 3 | param set MAV_BROADCAST 1 4 | #param set SYS_AUTOSTART 2104 5 | param set MAV_TYPE 1 6 | param set SYS_MC_EST_GROUP 2 7 | param set BAT_CNT_V_VOLT 0.001 8 | param set BAT_V_DIV 10.9176300578 9 | param set BAT_CNT_V_CURR 0.001 10 | param set BAT_A_PER_V 15.391030303 11 | dataman start 12 | df_lsm9ds1_wrapper start -R 4 13 | df_ms5611_wrapper start 14 | navio_rgbled start 15 | navio_adc start 16 | gps start -d /dev/spidev0.0 -i spi -p ubx 17 | sensors start 18 | commander start 19 | navigator start 20 | ekf2 start 21 | fw_att_control start 22 | fw_pos_control_l1 start 23 | mavlink start -u 14556 -r 1000000 24 | sleep 1 25 | mavlink stream -u 14556 -s HIGHRES_IMU -r 20 26 | mavlink stream -u 14556 -s ATTITUDE -r 20 27 | mavlink stream -u 14556 -s MANUAL_CONTROL -r 10 28 | 29 | 30 | navio_sysfs_rc_in start 31 | navio_sysfs_pwm_out start -m ROMFS/px4fmu_common/mixers/AERT.main.mix 32 | logger start -t -b 200 33 | mavlink boot_complete 34 | -------------------------------------------------------------------------------- /posix-configs/rpi/px4_hil.config: -------------------------------------------------------------------------------- 1 | # HITL configuration 2 | # connect to it with jMAVSim: 3 | # ./Tools/jmavsim_run.sh -q -i -p 14577 -r 250 4 | 5 | uorb start 6 | param load 7 | param set SYS_AUTOSTART 1001 8 | param set MAV_BROADCAST 1 9 | param set MAV_TYPE 2 10 | param set SYS_MC_EST_GROUP 0 11 | dataman start 12 | sensors start -hil 13 | commander start -hil 14 | navigator start 15 | ekf2 start 16 | land_detector start multicopter 17 | mc_pos_control start 18 | mc_att_control start 19 | mavlink start -u 14577 -r 1000000 20 | navio_sysfs_rc_in start 21 | pwm_out_sim mode_pwm16 22 | mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/quad_x.main.mix 23 | logger start -t -b 200 24 | mavlink boot_complete 25 | 26 | -------------------------------------------------------------------------------- /posix-configs/rpi/px4_no_shield.config: -------------------------------------------------------------------------------- 1 | uorb start 2 | param load 3 | param set SYS_AUTOSTART 4001 4 | param set MAV_BROADCAST 1 5 | sleep 1 6 | param set MAV_TYPE 2 7 | df_mpu9250_wrapper start -R 10 8 | df_ms5611_wrapper start 9 | gps start -d /dev/ttyACM0 -i uart -p ubx 10 | sensors start 11 | commander start 12 | attitude_estimator_q start 13 | position_estimator_inav start 14 | land_detector start multicopter 15 | mc_pos_control start 16 | mc_att_control start 17 | mavlink start -u 14556 -r 1000000 18 | sleep 1 19 | mavlink stream -u 14556 -s HIGHRES_IMU -r 50 20 | mavlink stream -u 14556 -s ATTITUDE -r 50 21 | #mavlink start -d /dev/ttyUSB0 22 | #mavlink stream -d /dev/ttyUSB0 -s HIGHRES_IMU -r 50 23 | #mavlink stream -d /dev/ttyUSB0 -s ATTITUDE -r 50 24 | rpi_rc_in start 25 | rpi_pca9685_pwm_out start 26 | logger start -t 27 | mavlink boot_complete 28 | -------------------------------------------------------------------------------- /posix-configs/rpi/px4_noshield.config: -------------------------------------------------------------------------------- 1 | uorb start 2 | param load 3 | param set SYS_AUTOSTART 4001 4 | param set MAV_BROADCAST 1 5 | sleep 1 6 | param set MAV_TYPE 2 7 | df_mpu9250_wrapper start 8 | df_ms5611_wrapper start 9 | gps start -d /dev/ttyACM0 -i uart -p ubx 10 | sensors start 11 | commander start 12 | attitude_estimator_q start 13 | position_estimator_inav start 14 | land_detector start multicopter 15 | mc_pos_control start 16 | mc_att_control start 17 | mavlink start -u 14556 -r 1000000 18 | sleep 1 19 | mavlink stream -u 14556 -s HIGHRES_IMU -r 50 20 | mavlink stream -u 14556 -s ATTITUDE -r 50 21 | #mavlink start -d /dev/ttyUSB0 22 | #mavlink stream -d /dev/ttyUSB0 -s HIGHRES_IMU -r 50 23 | #mavlink stream -d /dev/ttyUSB0 -s ATTITUDE -r 50 24 | rpi_rc_in start 25 | rpi_pca9685_pwm_out start 26 | #navio_sysfs_pwm_out start 27 | logger start -t 28 | mavlink boot_complete 29 | -------------------------------------------------------------------------------- /posix-configs/rpi/px4_rpi.config: -------------------------------------------------------------------------------- 1 | uorb start 2 | param load 3 | param set SYS_AUTOSTART 4001 4 | param set MAV_BROADCAST 1 5 | sleep 1 6 | param set MAV_TYPE 2 7 | df_mpu9250_wrapper start -R 10 8 | df_ms5611_wrapper start 9 | #gps start -d /dev/spidev0.0 -i spi -p ubx 10 | sensors start 11 | commander start 12 | attitude_estimator_q start 13 | position_estimator_inav start 14 | land_detector start multicopter 15 | mc_pos_control start 16 | mc_att_control start 17 | mavlink start -u 14556 -r 1000000 18 | sleep 1 19 | mavlink stream -u 14556 -s HIGHRES_IMU -r 50 20 | mavlink stream -u 14556 -s ATTITUDE -r 50 21 | #mavlink start -d /dev/ttyUSB0 22 | #mavlink stream -d /dev/ttyUSB0 -s HIGHRES_IMU -r 50 23 | #mavlink stream -d /dev/ttyUSB0 -s ATTITUDE -r 50 24 | rpi_rc_in start 25 | #navio_sysfs_pwm_out start 26 | logger start -t 27 | mavlink boot_complete 28 | -------------------------------------------------------------------------------- /src/drivers/linux_pca9685/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ############################################################################ 2 | # 3 | # Copyright (c) 2015-2016 PX4 Development Team. All rights reserved. 4 | # 5 | # Redistribution and use in source and binary forms, with or without 6 | # modification, are permitted provided that the following conditions 7 | # are met: 8 | # 9 | # 1. Redistributions of source code must retain the above copyright 10 | # notice, this list of conditions and the following disclaimer. 11 | # 2. Redistributions in binary form must reproduce the above copyright 12 | # notice, this list of conditions and the following disclaimer in 13 | # the documentation and/or other materials provided with the 14 | # distribution. 15 | # 3. Neither the name PX4 nor the names of its contributors may be 16 | # used to endorse or promote products derived from this software 17 | # without specific prior written permission. 18 | # 19 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | # OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | # AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | # POSSIBILITY OF SUCH DAMAGE. 31 | # 32 | ############################################################################ 33 | px4_add_module( 34 | MODULE drivers__linux_pca9685 35 | MAIN linux_pca9685 36 | COMPILE_FLAGS 37 | SRCS 38 | linux_pca9685.cpp 39 | DEPENDS 40 | platforms__common 41 | ) 42 | # vim: set noet ft=cmake fenc=utf-8 ff=unix : 43 | -------------------------------------------------------------------------------- /src/drivers/linux_pca9685/linux_pca9685.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2015-2016 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | #include "linux_pca9685.hpp" 34 | 35 | using namespace linux_pca9685; 36 | //--------------------------------------------------------------------------// 37 | int linux_pca9685::mixer_control_callback(uintptr_t handle, 38 | uint8_t control_group, uint8_t control_index, float &input) { 39 | const actuator_controls_s *controls = (actuator_controls_s *) handle; 40 | input = controls[control_group].control[control_index]; 41 | 42 | return 0; 43 | } 44 | 45 | //----------------------------------------------------------------------------// 46 | int linux_pca9685::initialize_mixer(const char *mixer_filename) { 47 | char buf[4096]; 48 | unsigned buflen = sizeof(buf); 49 | memset(buf, '\0', buflen); 50 | 51 | _mixer_group = new MixerGroup(mixer_control_callback, 52 | (uintptr_t) &_controls); 53 | 54 | // PX4_INFO("Trying to initialize mixer from config file %s", mixer_filename); 55 | 56 | if (load_mixer_file(mixer_filename, buf, buflen) == 0) { 57 | if (_mixer_group->load_from_buf(buf, buflen) == 0) { 58 | PX4_INFO("Loaded mixer from file %s", mixer_filename); 59 | return 0; 60 | 61 | } else { 62 | PX4_ERR("Unable to parse from mixer config file %s", 63 | mixer_filename); 64 | } 65 | 66 | } else { 67 | PX4_ERR("Unable to load config file %s", mixer_filename); 68 | } 69 | 70 | if (_mixer_group->count() <= 0) { 71 | PX4_ERR("Mixer initialization failed"); 72 | return -1; 73 | } 74 | 75 | return 0; 76 | } 77 | //-----------------------------------------------------------------------------// 78 | int linux_pca9685::open_device() { 79 | /*configure pca9685*/ 80 | if (0 > (device_fd = open(device_path, O_RDWR))) { 81 | PX4_ERR("Couldn't open pca9685 on %s,errno: %d .", device_path, errno); 82 | close(device_fd); 83 | return -1; 84 | } 85 | if (0 > ioctl(device_fd, I2C_SLAVE, PCA_9685_ADDR)) { 86 | PX4_ERR("Couldnn't set pca9685 as slave,errno: %d .", errno); 87 | close(device_fd); 88 | return -1; 89 | } 90 | return 0; 91 | } 92 | 93 | //----------------------------------------------------------------------------// 94 | int linux_pca9685::pwm_initialize() { 95 | /**************初始化PCA9685开始*************/ 96 | /**************PCA965 initializing********/ 97 | /*configure pca9685*/ 98 | if(0>linux_pca9685::open_device()) 99 | return -1; 100 | //Normal mode 101 | if (-1 == write_byte(device_fd, MODE1, 0x00)) { 102 | close(device_fd); 103 | return -1; 104 | } 105 | if (-1 == write_byte(device_fd, MODE2, 0x04)) { 106 | close(device_fd); 107 | return -1; 108 | } 109 | 110 | //set frequency,200HZ 111 | uint8_t prescale = (CLOCK_FREQ / MAX_PWM_RES / 200) - 1; 112 | uint8_t oldmode = 0; 113 | uint8_t newmode = (0 & 0x7F) | 0x10; //sleep 114 | write_byte(device_fd, MODE1, newmode); // go to sleep 115 | write_byte(device_fd, PRE_SCALE, prescale); 116 | write_byte(device_fd, MODE1, oldmode); 117 | usleep(10 * 1000); 118 | write_byte(device_fd, MODE1, oldmode | 0x80); 119 | /**************初始化PCA9685结束************/ 120 | /**************PCA965 initialized********/ 121 | close(device_fd); 122 | return 0; 123 | } 124 | //----------------------------------------------------------------------------// 125 | void linux_pca9685::pwm_deinitialize() { 126 | //Set output pwm as minmux pwm and reset 127 | if(0>linux_pca9685::open_device()) 128 | return; 129 | 130 | uint16_t pwm[NUM_PWM]; 131 | for (int start = 0; start < NUM_PWM; ++start) { 132 | pwm[start] = (uint16_t) _pwm_disarmed; 133 | } 134 | linux_pca9685::send_outputs_pwm(pwm); 135 | write_byte(device_fd, MODE1, 0x00); 136 | write_byte(device_fd, MODE2, 0x04); 137 | close(device_fd); 138 | } 139 | //----------------------------------------------------------------------------// 140 | void linux_pca9685::send_outputs_pwm(const uint16_t *pwm) { 141 | /*************向PCA9685发送数据*************/ 142 | /*************send pwm signal to pca9685*************/ 143 | if(0>linux_pca9685::open_device()) 144 | return; 145 | int i; 146 | 147 | for (i = 0; i < NUM_PWM; ++i) { 148 | write_byte(device_fd, LED0_ON_L + LED_MULTIPLYER * i, 0 & 0xFF); 149 | write_byte(device_fd, LED0_ON_H + LED_MULTIPLYER * i, 0 >> 8); 150 | write_byte(device_fd, LED0_OFF_L + LED_MULTIPLYER * i, 151 | *(pwm + i) & 0xFF); 152 | write_byte(device_fd, LED0_OFF_H + LED_MULTIPLYER * i, *(pwm + i) >> 8); 153 | } 154 | close(device_fd); 155 | } 156 | //----------------------------------------------------------------------------// 157 | void linux_pca9685::subscribe() { 158 | memset(_controls, 0, sizeof(_controls)); 159 | memset(_poll_fds, 0, sizeof(_poll_fds)); 160 | 161 | /* set up ORB topic names */ 162 | _controls_topics[0] = ORB_ID(actuator_controls_0); 163 | _controls_topics[1] = ORB_ID(actuator_controls_1); 164 | _controls_topics[2] = ORB_ID(actuator_controls_2); 165 | _controls_topics[3] = ORB_ID(actuator_controls_3); 166 | 167 | // Subscribe for orb topics 168 | for (uint8_t i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; 169 | i++) { 170 | if (_groups_required & (1 << i)) { 171 | PX4_DEBUG("subscribe to actuator_controls_%d", i); 172 | _controls_subs[i] = orb_subscribe(_controls_topics[i]); 173 | 174 | } else { 175 | _controls_subs[i] = -1; 176 | } 177 | 178 | if (_controls_subs[i] >= 0) { 179 | _poll_fds[_poll_fds_num].fd = _controls_subs[i]; 180 | _poll_fds[_poll_fds_num].events = POLLIN; 181 | _poll_fds_num++; 182 | } 183 | 184 | _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); 185 | 186 | } 187 | } 188 | //----------------------------------------------------------------------------// 189 | void linux_pca9685::task_main(int argc, char *argv[]) { 190 | _is_running = true; 191 | 192 | /***************初始化PCA9685************/ 193 | /***************Inialize pca9685*************/ 194 | linux_pca9685::pwm_initialize(); 195 | 196 | // Set up mixer 197 | if (initialize_mixer(_mixer_filename) < 0) { 198 | PX4_ERR("无法初始化通道混合配置文件 Can't loading mixer file"); 199 | return; 200 | } 201 | 202 | _mixer_group->groups_required(_groups_required); 203 | // subscribe and set up polling 204 | subscribe(); 205 | 206 | // Start disarmed 207 | _armed.armed = false; 208 | _armed.prearmed = false; 209 | 210 | pwm_limit_init(&_pwm_limit); 211 | 212 | // Main loop 213 | while (!_task_should_exit) { 214 | int pret = px4_poll(_poll_fds, _poll_fds_num, 10); 215 | 216 | /* Timed out, do a periodic check for _task_should_exit. */ 217 | if (pret == 0) { 218 | continue; 219 | } 220 | 221 | /* This is undesirable but not much we can do. */ 222 | if (pret < 0) { 223 | PX4_WARN("poll error %d, %d", pret, errno); 224 | /* sleep a bit before next try */ 225 | usleep(10000); 226 | continue; 227 | } 228 | 229 | /* get controls for required topics */ 230 | unsigned poll_id = 0; 231 | 232 | for (uint8_t i = 0; 233 | i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { 234 | if (_controls_subs[i] >= 0) { 235 | if (_poll_fds[poll_id].revents & POLLIN) { 236 | orb_copy(_controls_topics[i], _controls_subs[i], 237 | &_controls[i]); 238 | } 239 | 240 | poll_id++; 241 | } 242 | } 243 | 244 | if (nullptr != _mixer_group) { 245 | _outputs.timestamp = hrt_absolute_time(); 246 | /* do mixing */ 247 | _outputs.noutputs = _mixer_group->mix(_outputs.output, 248 | _outputs.NUM_ACTUATOR_OUTPUTS, 249 | NULL); 250 | 251 | /* disable unused ports by setting their output to NaN */ 252 | for (size_t i = _outputs.noutputs; 253 | i < _outputs.NUM_ACTUATOR_OUTPUTS; i++) { 254 | _outputs.output[i] = NAN; 255 | } 256 | 257 | const uint16_t reverse_mask = 0; 258 | uint16_t disarmed_pwm[NUM_PWM]; 259 | uint16_t min_pwm[NUM_PWM]; 260 | uint16_t max_pwm[NUM_PWM]; 261 | 262 | for (unsigned int i = 0; i < NUM_PWM; i++) { 263 | disarmed_pwm[i] = _pwm_disarmed; 264 | min_pwm[i] = _pwm_min; 265 | max_pwm[i] = _pwm_max; 266 | } 267 | 268 | uint16_t pwm[4]; 269 | 270 | // TODO FIXME: pre-armed seems broken 271 | pwm_limit_calc(_armed.armed, 272 | false /*_armed.prearmed*/, _outputs.noutputs, reverse_mask, 273 | disarmed_pwm, min_pwm, max_pwm, _outputs.output, pwm, 274 | &_pwm_limit); 275 | 276 | if (_armed.lockdown) { 277 | send_outputs_pwm(disarmed_pwm); 278 | } else { 279 | send_outputs_pwm(pwm); 280 | } 281 | 282 | if (_outputs_pub != nullptr) { 283 | orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &_outputs); 284 | 285 | } else { 286 | _outputs_pub = orb_advertise(ORB_ID(actuator_outputs), 287 | &_outputs); 288 | } 289 | 290 | } else { 291 | PX4_ERR("Could not mix output! Exiting..."); 292 | _task_should_exit = true; 293 | } 294 | 295 | bool updated; 296 | orb_check(_armed_sub, &updated); 297 | 298 | if (updated) { 299 | orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); 300 | } 301 | } 302 | pwm_deinitialize(); 303 | 304 | for (uint8_t i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; 305 | i++) { 306 | if (_controls_subs[i] >= 0) { 307 | orb_unsubscribe(_controls_subs[i]); 308 | } 309 | } 310 | 311 | orb_unsubscribe(_armed_sub); 312 | 313 | _is_running = false; 314 | 315 | } 316 | //----------------------------------------------------------------------------// 317 | void linux_pca9685::task_main_trampoline(int argc, char *argv[]) { 318 | task_main(argc, argv); 319 | } 320 | //----------------------------------------------------------------------------// 321 | void linux_pca9685::start() { 322 | ASSERT(_task_handle == -1); 323 | _task_should_exit = false; 324 | /* init pca9685 device*/ 325 | if (-1 == linux_pca9685::pwm_initialize()) { 326 | return; 327 | } 328 | /* start the task */ 329 | _task_handle = px4_task_spawn_cmd("pwm_out_main", SCHED_DEFAULT, 330 | SCHED_PRIORITY_MAX, 1500, (px4_main_t) &task_main_trampoline, nullptr); 331 | 332 | if (_task_handle < 0) { 333 | PX4_WARN("task start failed"); 334 | return; 335 | } 336 | 337 | } 338 | //---------------------------------------------------------------------------------------------------------------------// 339 | int linux_pca9685::write_byte(int fd, uint8_t address, uint8_t data) { 340 | uint8_t buff[2]; 341 | buff[0] = address; 342 | buff[1] = data; 343 | if (2 != write(fd, buff, sizeof(buff))) { 344 | PX4_ERR("Faild to write data into PCA9685, errno: %d", errno); 345 | usleep(5000); 346 | return -1; 347 | } 348 | return 0; 349 | } 350 | //----------------------------------------------------------------------------// 351 | void linux_pca9685::stop() { 352 | _task_should_exit = true; 353 | 354 | while (_is_running) { 355 | usleep(200000); 356 | PX4_INFO("."); 357 | } 358 | _task_handle = -1; 359 | } 360 | //----------------------------------------------------------------------------// 361 | void linux_pca9685::usage() { 362 | PX4_INFO( 363 | "usage: linux_pca9685 start [-d pca9685 device] -m mixerfile --motors 4"); 364 | PX4_INFO(" -d pwmdevice : pca9685 device for pwm generation"); 365 | PX4_INFO(" (default /sys/class/pwm/pwmchip0)"); 366 | PX4_INFO(" -m mixerfile : path to mixerfile"); 367 | PX4_INFO(" --motors quantity of motors : 4 is default"); 368 | PX4_INFO( 369 | " (default ROMFS/px4fmu_common/mixers/quad_x.main.mix)"); 370 | PX4_INFO(" linux_pca9685 stop"); 371 | PX4_INFO(" linux_pca9685 status"); 372 | } 373 | //----------------------------------------------------------------------------// 374 | int linux_pca9685_main(int argc, char **argv) { 375 | if (5 > argc) { 376 | linux_pca9685::usage(); 377 | return 1; 378 | } 379 | 380 | int command = -1; //指令 0:start,1:stop 2:status 381 | 382 | //读取控制台参数 383 | int start; 384 | for (start = 0; start < argc; ++start) { 385 | if (0 == strcmp("start", *(argv + start))) { 386 | command = 0; 387 | continue; 388 | } 389 | 390 | if (0 == strcmp("stop", *(argv + start))) { 391 | command = 1; 392 | continue; 393 | } 394 | 395 | if (0 == strcmp("status", *(argv + start))) { 396 | command = 2; 397 | continue; 398 | } 399 | 400 | if (0 == strcmp("-d", *(argv + start))) { 401 | strncpy(device_path, *(argv + start + 1), sizeof(device_path)); 402 | continue; 403 | } 404 | 405 | if (0 == strcmp("-m", *(argv + start + 1))) { 406 | strncpy(_mixer_filename, *(argv + start + 1), 407 | sizeof(_mixer_filename)); 408 | continue; 409 | } 410 | 411 | if (0 == strcmp("--motors", *(argv + start))) { 412 | NUM_PWM = atoi(*(argv + start) + 1); 413 | if (NUM_PWM > 8 || NUM_PWM < 4) 414 | NUM_PWM = 4; 415 | continue; 416 | } 417 | } 418 | 419 | //如果没有接收到指令，则退出 420 | if (-1 == command) { 421 | linux_pca9685::usage(); 422 | return 1; 423 | } 424 | 425 | // gets the parameters for the esc's pwm 426 | param_get(param_find("PWM_DISARMED"), &linux_pca9685::_pwm_disarmed); 427 | param_get(param_find("PWM_MIN"), &linux_pca9685::_pwm_min); 428 | param_get(param_find("PWM_MAX"), &linux_pca9685::_pwm_max); 429 | 430 | // 处理命令 431 | switch (command) { 432 | case 0: //start 433 | if (linux_pca9685::_is_running) { 434 | PX4_WARN("linux_pca9685 already running"); 435 | return 1; 436 | } 437 | linux_pca9685::start(); 438 | break; 439 | case 1: //stop; 440 | if (!linux_pca9685::_is_running) { 441 | PX4_WARN("linux_pca9685 is not running"); 442 | return 1; 443 | } 444 | linux_pca9685::stop(); 445 | break; 446 | case 2: //status 447 | PX4_WARN("linux_pca9685 is %s", 448 | linux_pca9685::_is_running ? "running" : "not running"); 449 | break; 450 | 451 | } 452 | return 0; 453 | } 454 | -------------------------------------------------------------------------------- /src/drivers/linux_pca9685/linux_pca9685.hpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2015-2016 zhangfan. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | /** 35 | * @Author Zhangfna 421395590@qq.com 36 | * 37 | */ 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include /* Standard I/O functions */ 44 | #include 45 | #include /* Syslog functionallity */ 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | #include 53 | #include 54 | #include 55 | #include // NAN 56 | 57 | #include 58 | #include 59 | #include 60 | #include 61 | #include 62 | #include 63 | #include 64 | #include 65 | #include 66 | #include 67 | 68 | 69 | // Register Definitions 70 | // 寄存器定义 71 | #define PCA_9685_ADDR 0x40 //Mode register 1 72 | #define MODE1 0x00 //Mode register 1 73 | #define MODE2 0x01 //Mode register 2 74 | #define SUBADR1 0x02 //I2C-bus subaddress 1 75 | #define SUBADR2 0x03 //I2C-bus subaddress 2 76 | #define SUBADR3 0x04 //I2C-bus subaddress 3 77 | #define ALLCALLADR 0x05 //LED All Call I2C-bus address 78 | #define LED0 0x6 //LED0 start register 79 | #define LED0_ON_L 0x6 //LED0 output and brightness control byte 0 80 | #define LED0_ON_H 0x7 //LED0 output and brightness control byte 1 81 | #define LED0_OFF_L 0x8 //LED0 output and brightness control byte 2 82 | #define LED0_OFF_H 0x9 //LED0 output and brightness control byte 3 83 | #define LED_MULTIPLYER 4 // For the other 15 channels 84 | #define ALLLED_ON_L 0xFA //load all the LEDn_ON registers, byte 0 (turn 0-7 channels on) 85 | #define ALLLED_ON_H 0xFB //load all the LEDn_ON registers, byte 1 (turn 8-15 channels on) 86 | #define ALLLED_OFF_L 0xFC //load all the LEDn_OFF registers, byte 0 (turn 0-7 channels off) 87 | #define ALLLED_OFF_H 0xFD //load all the LEDn_OFF registers, byte 1 (turn 8-15 channels off) 88 | #define PRE_SCALE 0xFE //prescaler for output frequency 89 | #define MAX_PWM_RES 4096 //Resolution 4096=12bit 分辨率，按2的阶乘计算，12bit为4096 90 | #define CLOCK_FREQ 25000000.0 //25MHz default osc clock 91 | #define BUFFER_SIZE 0x08 //1 byte buffer 92 | #define PCA9685_DEFAULT_I2C_ADDR 0x40 // default i2c address for pca9685 默认i2c地址为0x40 93 | #define PCA9685_DEFAULT_I2C_BUS 1 // default i2c bus for pca9685 默认为1 94 | 95 | namespace linux_pca9685 96 | { 97 | static px4_task_t _task_handle = -1; 98 | volatile bool _task_should_exit = false; 99 | static bool _is_running = false; 100 | 101 | static int NUM_PWM = 4; 102 | 103 | 104 | static const int FREQUENCY_PWM = 400; 105 | static char _mixer_filename[255] = "ROMFS/px4fmu_common/mixers/quad_x.main.mix"; 106 | 107 | // subscriptions 108 | int _controls_subs[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; 109 | int _armed_sub; 110 | 111 | // publications 112 | orb_advert_t _outputs_pub = nullptr; 113 | orb_advert_t _rc_pub = nullptr; 114 | 115 | // topic structures 116 | actuator_controls_s _controls[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; 117 | orb_id_t _controls_topics[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; 118 | actuator_outputs_s _outputs; 119 | actuator_armed_s _armed; 120 | 121 | // polling 122 | uint8_t _poll_fds_num = 0; 123 | px4_pollfd_struct_t _poll_fds[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; 124 | 125 | // control groups related 126 | uint32_t _groups_required = 0; 127 | uint32_t _groups_subscribed = 0; 128 | 129 | pwm_limit_t _pwm_limit; 130 | 131 | // esc parameters 132 | int32_t _pwm_disarmed; 133 | int32_t _pwm_min; 134 | int32_t _pwm_max; 135 | MixerGroup *_mixer_group = nullptr; 136 | 137 | // device param 138 | int device_fd=-1; 139 | char device_path[30]="/dev/i2c-0"; 140 | 141 | void usage(); 142 | 143 | void start(); 144 | 145 | void stop(); 146 | 147 | int pwm_initialize(); 148 | 149 | void pwm_deinitialize(); 150 | 151 | int open_device(); 152 | 153 | void send_outputs_pwm(const uint16_t *pwm); 154 | 155 | void task_main_trampoline(int argc, char *argv[]); 156 | 157 | void subscribe(); 158 | 159 | void task_main(int argc, char *argv[]); 160 | 161 | int write_byte(int fd, uint8_t address, uint8_t data); 162 | /* mixer initialization */ 163 | int initialize_mixer(const char *mixer_filename); 164 | int mixer_control_callback(uintptr_t handle, uint8_t control_group, 165 | uint8_t control_index, float &input); 166 | } 167 | extern "C" __EXPORT int linux_pca9685_main(int, char **); 168 | -------------------------------------------------------------------------------- /src/drivers/linux_sbus/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ############################################################################ 2 | # 3 | # Copyright (c) 2016 PX4 Development Team. All rights reserved. 4 | # 5 | # Redistribution and use in source and binary forms, with or without 6 | # modification, are permitted provided that the following conditions 7 | # are met: 8 | # 9 | # 1. Redistributions of source code must retain the above copyright 10 | # notice, this list of conditions and the following disclaimer. 11 | # 2. Redistributions in binary form must reproduce the above copyright 12 | # notice, this list of conditions and the following disclaimer in 13 | # the documentation and/or other materials provided with the 14 | # distribution. 15 | # 3. Neither the name PX4 nor the names of its contributors may be 16 | # used to endorse or promote products derived from this software 17 | # without specific prior written permission. 18 | # 19 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | # OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | # AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | # POSSIBILITY OF SUCH DAMAGE. 31 | # 32 | ############################################################################ 33 | px4_add_module( 34 | MODULE drivers__linux_sbus 35 | MAIN linux_sbus 36 | STACK_MAIN 1200 37 | COMPILE_FLAGS 38 | SRCS 39 | linux_sbus.cpp 40 | DEPENDS 41 | platforms__common 42 | ) 43 | # vim: set noet ft=cmake fenc=utf-8 ff=unix : 44 | -------------------------------------------------------------------------------- /src/drivers/linux_sbus/linux_sbus.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2015 Mark Charl. All rights reserved. 4 | * Copyright (C) 2017 Fan.zhang. All rights reserved. 421395590@qq.com 5 | * Copyright (C) 2016 PX4 Development Team. All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * 1. Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * 2. Redistributions in binary form must reproduce the above copyright 14 | * notice, this list of conditions and the following disclaimer in 15 | * the documentation and/or other materials provided with the 16 | * distribution. 17 | * 3. Neither the name PX4 nor the names of its contributors may be 18 | * used to endorse or promote products derived from this software 19 | * without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 28 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 29 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | * 34 | ****************************************************************************/ 35 | #include "linux_sbus.h" 36 | 37 | using namespace linux_sbus; 38 | //---------------------------------------------------------------------------------------------------------// 39 | int RcInput::init() { 40 | int i; 41 | //--------------发布所有通道的数据------------------------// 42 | for (i = 0; i < input_rc_s::RC_INPUT_MAX_CHANNELS; ++i) { 43 | _data.values[i] = UINT16_MAX; 44 | } 45 | 46 | _rcinput_pub = orb_advertise(ORB_ID(input_rc), &_data); 47 | 48 | if (nullptr == _rcinput_pub) { 49 | PX4_WARN("error: advertise failed"); 50 | return -1; 51 | } 52 | 53 | //初始化串口，供sbus读写 54 | _device_fd = open(_device, O_RDWR | O_NONBLOCK | O_CLOEXEC); 55 | if (-1 == _device_fd) { 56 | PX4_ERR("Open SBUS input %s faild,status %d \n", _device, 57 | (int ) _device_fd); 58 | fflush (stdout); 59 | return -1; 60 | } 61 | struct termios2 tio { }; 62 | 63 | if (0 != ioctl(_device_fd, TCGETS2, &tio)) { 64 | close(_device_fd); 65 | _device_fd = -1; 66 | return -1; 67 | } 68 | // Setting serial port,8E2, non-blocking.100Kbps 69 | tio.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL 70 | | IXON); 71 | tio.c_iflag |= (INPCK | IGNPAR); 72 | tio.c_oflag &= ~OPOST; 73 | tio.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN); 74 | tio.c_cflag &= ~(CSIZE | CRTSCTS | PARODD | CBAUD); 75 | // use BOTHER to specify speed directly in c_[io]speed member 76 | tio.c_cflag |= (CS8 | CSTOPB | CLOCAL | PARENB | BOTHER | CREAD); 77 | tio.c_ispeed = 100000; 78 | tio.c_ospeed = 100000; 79 | // see select() comment below 80 | tio.c_cc[VMIN] = 25; 81 | tio.c_cc[VTIME] = 0; 82 | if (0 != ioctl(_device_fd, TCSETS2, &tio)) { 83 | close(_device_fd); 84 | _device_fd = -1; 85 | return -1; 86 | } 87 | return 0; 88 | } 89 | //---------------------------------------------------------------------------------------------------------// 90 | int RcInput::start(char *device, int channels) { 91 | int result = 0; 92 | strcpy(_device, device); 93 | _channels = channels; 94 | result = init(); 95 | 96 | if (0 != result) { 97 | PX4_WARN("error: RC initialization failed"); 98 | return -1; 99 | } 100 | 101 | _isRunning = true; 102 | result = work_queue(HPWORK, &_work, (worker_t) & RcInput::cycle_trampoline, 103 | this, 0); 104 | 105 | if (result == -1) { 106 | _isRunning = false; 107 | } 108 | 109 | return result; 110 | } 111 | //---------------------------------------------------------------------------------------------------------// 112 | void RcInput::stop() { 113 | close(_device_fd); 114 | _shouldExit = true; 115 | } 116 | //---------------------------------------------------------------------------------------------------------// 117 | void RcInput::cycle_trampoline(void *arg) { 118 | RcInput *dev = reinterpret_cast(arg); 119 | dev->_cycle(); 120 | } 121 | //---------------------------------------------------------------------------------------------------------// 122 | void RcInput::_cycle() { 123 | _measure(); 124 | 125 | if (!_shouldExit) { 126 | work_queue(HPWORK, &_work, (worker_t) & RcInput::cycle_trampoline, this, 127 | USEC2TICK(RCINPUT_MEASURE_INTERVAL_US)); 128 | } 129 | } 130 | //---------------------------------------------------------------------------------------------------------// 131 | void RcInput::_measure(void) { 132 | uint64_t ts; 133 | 134 | //开始解析SBUS数据 135 | int nread; 136 | fd_set fds; 137 | struct timeval tv; 138 | FD_ZERO(&fds); 139 | FD_SET(_device_fd, &fds); 140 | select(_device_fd + 1, &fds, nullptr, nullptr, &tv); 141 | int count = 0; //error counter; 142 | while (1) { 143 | nread = read(_device_fd, &_sbusData, sizeof(_sbusData)); 144 | if (25 == nread) { 145 | if (0x0f == _sbusData[0] && 0x00 == _sbusData[24]) { 146 | break; 147 | } 148 | } 149 | ++count; 150 | usleep(RCINPUT_MEASURE_INTERVAL_US); 151 | } 152 | 153 | // pars sbus data to pwm 154 | _channels_data[0] = (uint16_t) (((_sbusData[1] | _sbusData[2] << 8) & 0x07FF) 155 | * SBUS_SCALE_FACTOR + .5f) + SBUS_SCALE_OFFSET; 156 | _channels_data[1] = 157 | (uint16_t) (((_sbusData[2] >> 3 | _sbusData[3] << 5) & 0x07FF) 158 | * SBUS_SCALE_FACTOR + .5f) + SBUS_SCALE_OFFSET; 159 | _channels_data[2] = (uint16_t) (((_sbusData[3] >> 6 | _sbusData[4] << 2 160 | | _sbusData[5] << 10) & 0x07FF) * SBUS_SCALE_FACTOR + .5f) 161 | + SBUS_SCALE_OFFSET; 162 | _channels_data[3] = 163 | (uint16_t) (((_sbusData[5] >> 1 | _sbusData[6] << 7) & 0x07FF) 164 | * SBUS_SCALE_FACTOR + .5f) + SBUS_SCALE_OFFSET; 165 | _channels_data[4] = 166 | (uint16_t) (((_sbusData[6] >> 4 | _sbusData[7] << 4) & 0x07FF) 167 | * SBUS_SCALE_FACTOR + .5f) + SBUS_SCALE_OFFSET; 168 | _channels_data[5] = (uint16_t) (((_sbusData[7] >> 7 | _sbusData[8] << 1 169 | | _sbusData[9] << 9) & 0x07FF) * SBUS_SCALE_FACTOR + .5f) 170 | + SBUS_SCALE_OFFSET; 171 | _channels_data[6] = (uint16_t) (((_sbusData[9] >> 2 | _sbusData[10] << 6) 172 | & 0x07FF) * SBUS_SCALE_FACTOR + .5f) + SBUS_SCALE_OFFSET; 173 | _channels_data[7] = (uint16_t) (((_sbusData[10] >> 5 | _sbusData[11] << 3) 174 | & 0x07FF) * SBUS_SCALE_FACTOR + .5f) + SBUS_SCALE_OFFSET; // & the other 8 + 2 channels if you need them 175 | _channels_data[8] = (uint16_t) (((_sbusData[12] | _sbusData[13] << 8) & 0x07FF) 176 | * SBUS_SCALE_FACTOR + .5f) + SBUS_SCALE_OFFSET; 177 | ; 178 | _channels_data[9] = (uint16_t) (((_sbusData[13] >> 3 | _sbusData[14] << 5) 179 | & 0x07FF) * SBUS_SCALE_FACTOR + .5f) + SBUS_SCALE_OFFSET; 180 | ; 181 | _channels_data[10] = (uint16_t) (((_sbusData[14] >> 6 | _sbusData[15] << 2 182 | | _sbusData[16] << 10) & 0x07FF) * SBUS_SCALE_FACTOR + .5f) 183 | + SBUS_SCALE_OFFSET; 184 | _channels_data[11] = (uint16_t) (((_sbusData[16] >> 1 | _sbusData[17] << 7) 185 | & 0x07FF) * SBUS_SCALE_FACTOR + .5f) + SBUS_SCALE_OFFSET; 186 | _channels_data[12] = (uint16_t) (((_sbusData[17] >> 4 | _sbusData[18] << 4) 187 | & 0x07FF) * SBUS_SCALE_FACTOR + .5f) + SBUS_SCALE_OFFSET; 188 | _channels_data[13] = (uint16_t) (((_sbusData[18] >> 7 | _sbusData[19] << 1 189 | | _sbusData[20] << 9) & 0x07FF) * SBUS_SCALE_FACTOR + .5f) 190 | + SBUS_SCALE_OFFSET; 191 | _channels_data[14] = (uint16_t) (((_sbusData[20] >> 2 | _sbusData[21] << 6) 192 | & 0x07FF) * SBUS_SCALE_FACTOR + .5f) + SBUS_SCALE_OFFSET; 193 | _channels_data[15] = (uint16_t) (((_sbusData[21] >> 5 | _sbusData[22] << 3) 194 | & 0x07FF) * SBUS_SCALE_FACTOR + .5f) + SBUS_SCALE_OFFSET; 195 | 196 | // PWM数据发布 197 | int i = 0; 198 | for (i = 0; i < _channels; ++i) { 199 | _data.values[i] = _channels_data[i]; 200 | } 201 | ts = hrt_absolute_time(); 202 | _data.timestamp = ts; 203 | _data.timestamp_last_signal = ts; 204 | _data.channel_count = _channels; 205 | _data.rssi = 100; 206 | _data.rc_lost_frame_count = 0; 207 | _data.rc_total_frame_count = 1; 208 | _data.rc_ppm_frame_length = 0; 209 | _data.rc_failsafe = (_sbusData[23] & (1 << 3)) ? true : false; 210 | _data.rc_lost = (_sbusData[23] & (1 << 2)) ? true : false; 211 | _data.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_SBUS; 212 | orb_publish(ORB_ID(input_rc), _rcinput_pub, &_data); 213 | } 214 | //---------------------------------------------------------------------------------------------------------// 215 | /** 216 | * Print the correct usage. 217 | */ 218 | 219 | static void linux_sbus::usage(const char *reason) { 220 | if (reason) { 221 | PX4_ERR("%s", reason); 222 | } 223 | PX4_INFO("用法: linux_sbus {start|stop|status} -d -c "); 224 | } 225 | //---------------------------------------------------------------------------------------------------------// 226 | int linux_sbus_main(int argc, char **argv) { 227 | if (argc < 7) { 228 | usage("linux_sbus {start|stop|status} -d -c "); 229 | return 1; 230 | } 231 | int start; 232 | char device[30]; 233 | int max_channel = 8; //8 channel for default setting 234 | 235 | // Get device and channels count from console 236 | for (start = 0; start < argc; ++start) { 237 | if (0 == strcmp(argv[start], "-d")) { 238 | strcpy(device, argv[start] + 1); 239 | continue; 240 | } 241 | 242 | if (0 == strcmp(argv[start], "-c")) { 243 | max_channel = atoi(argv[start]+1); 244 | continue; 245 | } 246 | } 247 | //Channels count should less than 16; 248 | max_channel = (max_channel > 16) ? 16 : max_channel; 249 | 250 | if (!strcmp(argv[1], "start")) { 251 | 252 | if (nullptr != rc_input && rc_input->isRunning()) { 253 | PX4_WARN("运行中。running"); 254 | /* this is not an error */ 255 | return 0; 256 | } 257 | 258 | rc_input = new RcInput(); 259 | 260 | // Check if alloc worked. 261 | if (nullptr == rc_input) { 262 | PX4_ERR("遥控输入模块初始化错误。Rc input moduel initialization faild"); 263 | return -1; 264 | } 265 | 266 | int ret = rc_input->start(device, max_channel); 267 | 268 | if (ret != 0) { 269 | PX4_ERR("遥控输入模块未能启动。 Rc input module failure"); 270 | } 271 | 272 | return 0; 273 | } 274 | 275 | if (!strcmp(argv[1], "stop")) { 276 | 277 | if (rc_input == nullptr || !rc_input->isRunning()) { 278 | PX4_WARN("模块未运行。 Not runing"); 279 | /* this is not an error */ 280 | return 0; 281 | } 282 | 283 | rc_input->stop(); 284 | 285 | // Wait for task to die 286 | int i = 0; 287 | 288 | do { 289 | /* wait up to 3s */ 290 | usleep(100000); 291 | 292 | } while (rc_input->isRunning() && ++i < 30); 293 | 294 | delete rc_input; 295 | rc_input = nullptr; 296 | 297 | return 0; 298 | } 299 | 300 | if (!strcmp(argv[1], "status")) { 301 | if (rc_input != nullptr && rc_input->isRunning()) { 302 | PX4_INFO("运行中。 running"); 303 | 304 | } else { 305 | PX4_INFO("未运行。 Not runing\n"); 306 | } 307 | 308 | return 0; 309 | } 310 | 311 | usage("不知道你要做什么。 linux_sbus start|stop|status -d -c "); 312 | return 1; 313 | 314 | } 315 | //---------------------------------------------------------------------------------------------------------// 316 | -------------------------------------------------------------------------------- /src/drivers/linux_sbus/linux_sbus.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2015 Mark Charl. All rights reserved. 4 | * Copyright (C) 2017 Fan.zhang. All rights reserved. 421395590@qq.com 5 | * Copyright (C) 2016 PX4 Development Team. All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * 1. Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * 2. Redistributions in binary form must reproduce the above copyright 14 | * notice, this list of conditions and the following disclaimer in 15 | * the documentation and/or other materials provided with the 16 | * distribution. 17 | * 3. Neither the name PX4 nor the names of its contributors may be 18 | * used to endorse or promote products derived from this software 19 | * without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 28 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 29 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | * 34 | ****************************************************************************/ 35 | 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include /*Unix 标准函数定义*/ 43 | #include /*文件控制定义*/ 44 | #include 45 | #include 46 | #include /*错误号定义*/ 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | #include 53 | #define RCINPUT_MEASURE_INTERVAL_US 4700 54 | /* define range mapping here, -+100% -> 1000..2000 */ 55 | #define SBUS_RANGE_MIN 200.0f 56 | #define SBUS_RANGE_MAX 1800.0f 57 | 58 | #define SBUS_TARGET_MIN 1000.0f 59 | #define SBUS_TARGET_MAX 2000.0f 60 | 61 | /* pre-calculate the floating point stuff as far as possible at compile time */ 62 | #define SBUS_SCALE_FACTOR ((SBUS_TARGET_MAX - SBUS_TARGET_MIN) / (SBUS_RANGE_MAX - SBUS_RANGE_MIN)) 63 | #define SBUS_SCALE_OFFSET (int)(SBUS_TARGET_MIN - (SBUS_SCALE_FACTOR * SBUS_RANGE_MIN + 0.5f)) 64 | 65 | namespace linux_sbus { 66 | class RcInput { 67 | public: 68 | RcInput() : 69 | _shouldExit(false), _isRunning(false), _work { }, _rcinput_pub( 70 | nullptr), //D8R-II plus 71 | _data { }, _sbusData { 0x0f, 0x01, 0x04, 0x20, 0x00, 0xff, 0x07, 72 | 0x40, 0x00, 0x02, 0x10, 0x80, 0x2c, 0x64, 0x21, 0x0b, 0x59, 73 | 0x08, 0x40, 0x00, 0x02, 0x10, 0x80, 0x00, 0x00 } { 74 | //memset(_ch_fd, 0, sizeof(_ch_fd)); 75 | } 76 | ~RcInput() { 77 | work_cancel(HPWORK, &_work); 78 | _isRunning = false; 79 | close(_device_fd); 80 | } 81 | 82 | /* @return 0 on success, -errno on failure */ 83 | int start(char *device, int channels); 84 | 85 | /* @return 0 on success, -errno on failure */ 86 | void stop(); 87 | 88 | /* Trampoline for the work queue. */ 89 | static void cycle_trampoline(void *arg); 90 | 91 | bool isRunning() { 92 | return _isRunning; 93 | } 94 | 95 | private: 96 | void _cycle(); 97 | void _measure(); 98 | bool _shouldExit; 99 | bool _isRunning; 100 | struct work_s _work; 101 | orb_advert_t _rcinput_pub; 102 | struct input_rc_s _data; 103 | uint8_t _sbusData[25]; 104 | int _channels; 105 | int _device_fd; // serial port device to read SBUS; 106 | int _channels_data[16]; // 16 channels support; 107 | uint8_t _buffer[25]; 108 | char _device[30]; 109 | bool _failsafe; 110 | bool _rc_loss; 111 | int init(); 112 | }; 113 | 114 | static void usage(const char *reason); 115 | static RcInput *rc_input = nullptr; 116 | } 117 | extern "C" __EXPORT int linux_sbus_main(int argc, char **argv); 118 | -------------------------------------------------------------------------------- /src/drivers/rpi_pca9685_pwm_out/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ############################################################################ 2 | # 3 | # Copyright (c) 2015-2016 PX4 Development Team. All rights reserved. 4 | # 5 | # Redistribution and use in source and binary forms, with or without 6 | # modification, are permitted provided that the following conditions 7 | # are met: 8 | # 9 | # 1. Redistributions of source code must retain the above copyright 10 | # notice, this list of conditions and the following disclaimer. 11 | # 2. Redistributions in binary form must reproduce the above copyright 12 | # notice, this list of conditions and the following disclaimer in 13 | # the documentation and/or other materials provided with the 14 | # distribution. 15 | # 3. Neither the name PX4 nor the names of its contributors may be 16 | # used to endorse or promote products derived from this software 17 | # without specific prior written permission. 18 | # 19 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | # OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | # AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | # POSSIBILITY OF SUCH DAMAGE. 31 | # 32 | ############################################################################ 33 | px4_add_module( 34 | MODULE drivers__rpi_pca9685_pwm_out 35 | MAIN rpi_pca9685_pwm_out 36 | COMPILE_FLAGS 37 | SRCS 38 | PCA9685.cpp 39 | rpi_pca9685_pwm_out.cpp 40 | DEPENDS 41 | platforms__common 42 | ) 43 | # vim: set noet ft=cmake fenc=utf-8 ff=unix : 44 | -------------------------------------------------------------------------------- /src/drivers/rpi_pca9685_pwm_out/PCA9685.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2015-2017 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | * 33 | * Original Author : Georgi Todorov 34 | * Edited by : Tord Wessman 35 | * Created on : Nov 22, 2013 36 | * Rewrite : Fan.zhang 421395590@qq.com 37 | * Updated on : Mar 2, 2017 38 | ****************************************************************************/ 39 | 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include /* Standard I/O functions */ 45 | #include 46 | #include /* Syslog functionallity */ 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | // 53 | 54 | #include "PCA9685.h" 55 | 56 | //! Constructor takes bus and address arguments 57 | /*! 58 | \param bus the bus to use in /dev/i2c-%d. 59 | \param address the device address on bus 60 | */ 61 | 62 | void PCA9685::init(int bus, int address) { 63 | _i2cbus = bus; 64 | _i2caddr = address; 65 | snprintf(busfile, sizeof(busfile), "/dev/i2c-%d", bus); 66 | reset(); 67 | //usleep(10*1000); 68 | } 69 | 70 | 71 | PCA9685::PCA9685() : 72 | _i2caddr(PCA9685_DEFAULT_I2C_ADDR), 73 | _i2cbus(PCA9685_DEFAULT_I2C_BUS), 74 | dataBuffer {} 75 | { 76 | 77 | } 78 | 79 | PCA9685::PCA9685(int bus,int address) : 80 | busfile {}, 81 | dataBuffer {} 82 | { 83 | _i2cbus = bus; 84 | _i2caddr = address; 85 | snprintf(busfile, sizeof(busfile), "/dev/i2c-%d", bus); 86 | reset(); 87 | } 88 | 89 | PCA9685::~PCA9685() { 90 | reset(); 91 | } 92 | //! Sets PCA9685 mode to 00 93 | void PCA9685::reset() { 94 | int fd = openfd(); 95 | if (fd != -1) { 96 | write_byte(fd, MODE1, 0x00); //Normal mode 97 | write_byte(fd, MODE2, 0x04); //Normal mode 98 | close(fd); 99 | } 100 | } 101 | //! Set the frequency of PWM 102 | /*! 103 | \param freq desired frequency. 40Hz to 1000Hz using internal 25MHz oscillator. 104 | */ 105 | void PCA9685::setPWMFreq(int freq) { 106 | int fd = openfd(); 107 | if (fd != -1) { 108 | uint8_t prescale = (CLOCK_FREQ / MAX_PWM_RES / freq) - 1; 109 | //printf ("Setting prescale value to: %d\n", prescale); 110 | //printf ("Using Frequency: %d\n", freq); 111 | 112 | uint8_t oldmode = read_byte(fd, MODE1); 113 | uint8_t newmode = (oldmode & 0x7F) | 0x10; //sleep 114 | write_byte(fd, MODE1, newmode); // go to sleep 115 | write_byte(fd, PRE_SCALE, prescale); 116 | write_byte(fd, MODE1, oldmode); 117 | usleep(10*1000); 118 | write_byte(fd, MODE1, oldmode | 0x80); 119 | 120 | close(fd); 121 | } 122 | } 123 | 124 | //! PWM a single channel 125 | /*! 126 | \param led channel to set PWM value for 127 | \param value 0-4095 value for PWM 128 | */ 129 | void PCA9685::setPWM(uint8_t led, int value) { 130 | setPWM(led, 0, value); 131 | } 132 | //! PWM a single channel with custom on time 133 | /*! 134 | \param led channel to set PWM value for 135 | \param on_value 0-4095 value to turn on the pulse 136 | \param off_value 0-4095 value to turn off the pulse 137 | */ 138 | void PCA9685::setPWM(uint8_t led, int on_value, int off_value) { 139 | int fd = openfd(); 140 | if (fd != -1) { 141 | 142 | write_byte(fd, LED0_ON_L + LED_MULTIPLYER * led, on_value & 0xFF); 143 | 144 | write_byte(fd, LED0_ON_H + LED_MULTIPLYER * led, on_value >> 8); 145 | 146 | write_byte(fd, LED0_OFF_L + LED_MULTIPLYER * led, off_value & 0xFF); 147 | 148 | write_byte(fd, LED0_OFF_H + LED_MULTIPLYER * led, off_value >> 8); 149 | 150 | close(fd); 151 | } 152 | 153 | } 154 | 155 | //! Read a single byte from PCA9685 156 | /*! 157 | \param fd file descriptor for I/O 158 | \param address register address to read from 159 | */ 160 | uint8_t PCA9685::read_byte(int fd, uint8_t address) { 161 | 162 | return 0; 163 | 164 | uint8_t buff[BUFFER_SIZE]; 165 | buff[0] = address; 166 | if (write(fd, buff, BUFFER_SIZE) != BUFFER_SIZE) { 167 | printf("I2C slave 0x%x failed to go to register 0x%x [read_byte():write %d]", _i2caddr, address, errno); 168 | return (-1); 169 | } else { 170 | if (read(fd, dataBuffer, BUFFER_SIZE) != BUFFER_SIZE) { 171 | printf ("Could not read from I2C slave 0x%x, register 0x%x [read_byte():read %d]", _i2caddr, address, errno); 172 | return (-1); 173 | } 174 | } 175 | 176 | 177 | } 178 | //! Write a single byte from PCA9685 179 | /*! 180 | \param fd file descriptor for I/O 181 | \param address register address to write to 182 | \param data 8 bit data to write 183 | */ 184 | void PCA9685::write_byte(int fd, uint8_t address, uint8_t data) { 185 | uint8_t buff[2]; 186 | buff[0] = address; 187 | buff[1] = data; 188 | if (write(fd, buff, sizeof(buff)) != 2) { 189 | printf("Failed to write to I2C Slave 0x%x @ register 0x%x [write_byte():write %d]", _i2caddr, address, errno); 190 | usleep(5000); 191 | }else{ 192 | //printf("Wrote to I2C Slave 0x%x @ register 0x%x [0x%x]\n", _i2caddr, address, data); 193 | } 194 | } 195 | //! Open device file for PCA9685 I2C bus 196 | /*! 197 | \return fd returns the file descriptor number or -1 on error 198 | */ 199 | int PCA9685::openfd() { 200 | int fd; 201 | if ((fd = open(busfile, O_RDWR)) < 0) { 202 | printf ("Couldn't open I2C Bus %d [openfd():open %d]", _i2cbus, errno); 203 | return -1; 204 | } 205 | if (ioctl(fd, I2C_SLAVE, _i2caddr) < 0) { 206 | printf ("I2C slave %d failed [openfd():ioctl %d]", _i2caddr, errno); 207 | return -1; 208 | } 209 | 210 | return fd; 211 | } 212 | -------------------------------------------------------------------------------- /src/drivers/rpi_pca9685_pwm_out/PCA9685.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2015-2017 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | * 33 | * Original Author : Georgi Todorov 34 | * Edited by : Tord Wessman 35 | * Created on : Nov 22, 2013 36 | * Rewrite : Fan.zhang 421395590@qq.com 37 | * Updated on : Mar 2, 2017 38 | ****************************************************************************/ 39 | 40 | #ifndef _PCA9685_H 41 | #define _PCA9685_H 42 | #include 43 | 44 | // Register Definitions 45 | // 寄存器定义 46 | #define MODE1 0x00 //Mode register 1 47 | #define MODE2 0x01 //Mode register 2 48 | #define SUBADR1 0x02 //I2C-bus subaddress 1 49 | #define SUBADR2 0x03 //I2C-bus subaddress 2 50 | #define SUBADR3 0x04 //I2C-bus subaddress 3 51 | #define ALLCALLADR 0x05 //LED All Call I2C-bus address 52 | #define LED0 0x6 //LED0 start register 53 | #define LED0_ON_L 0x6 //LED0 output and brightness control byte 0 54 | #define LED0_ON_H 0x7 //LED0 output and brightness control byte 1 55 | #define LED0_OFF_L 0x8 //LED0 output and brightness control byte 2 56 | #define LED0_OFF_H 0x9 //LED0 output and brightness control byte 3 57 | #define LED_MULTIPLYER 4 // For the other 15 channels 58 | #define ALLLED_ON_L 0xFA //load all the LEDn_ON registers, byte 0 (turn 0-7 channels on) 59 | #define ALLLED_ON_H 0xFB //load all the LEDn_ON registers, byte 1 (turn 8-15 channels on) 60 | #define ALLLED_OFF_L 0xFC //load all the LEDn_OFF registers, byte 0 (turn 0-7 channels off) 61 | #define ALLLED_OFF_H 0xFD //load all the LEDn_OFF registers, byte 1 (turn 8-15 channels off) 62 | #define PRE_SCALE 0xFE //prescaler for output frequency 63 | #define MAX_PWM_RES 4096 //Resolution 4096=12bit 分辨率，按2的阶乘计算，12bit为4096 64 | #define CLOCK_FREQ 25000000.0 //25MHz default osc clock 65 | #define BUFFER_SIZE 0x08 //1 byte buffer 66 | #define PCA9685_DEFAULT_I2C_ADDR 0x40 // default i2c address for pca9685 默认i2c地址为0x40 67 | #define PCA9685_DEFAULT_I2C_BUS 1 // default i2c bus for pca9685 默认为1 68 | //! Main class that exports features for PCA9685 chip 69 | class PCA9685 { 70 | public: 71 | 72 | PCA9685(); 73 | PCA9685(int,int); 74 | void init(int,int); 75 | virtual ~PCA9685(); 76 | void reset(void); 77 | void setPWMFreq(int); 78 | void setPWM(uint8_t, int, int); 79 | void setPWM(uint8_t, int); 80 | private: 81 | int _i2caddr; 82 | int _i2cbus; 83 | char busfile[64]; 84 | uint8_t dataBuffer[BUFFER_SIZE]; 85 | uint8_t read_byte(int, uint8_t); 86 | void write_byte(int, uint8_t, uint8_t); 87 | int openfd(); 88 | }; 89 | #endif 90 | -------------------------------------------------------------------------------- /src/drivers/rpi_pca9685_pwm_out/rpi_pca9685_pwm_out.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2015-2016 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | #include "rpi_pca9685_pwm_out.h" 34 | 35 | using namespace rpi_pca9685_pwm_out; 36 | //--------------------------------------------------------------------------// 37 | int rpi_pca9685_pwm_out::mixer_control_callback(uintptr_t handle, 38 | uint8_t control_group, 39 | uint8_t control_index, 40 | float &input) 41 | { 42 | const actuator_controls_s *controls = (actuator_controls_s *)handle; 43 | input = controls[control_group].control[control_index]; 44 | 45 | return 0; 46 | } 47 | 48 | //----------------------------------------------------------------------------// 49 | int rpi_pca9685_pwm_out::initialize_mixer(const char *mixer_filename) 50 | { 51 | char buf[4096]; 52 | unsigned buflen = sizeof(buf); 53 | memset(buf, '\0', buflen); 54 | 55 | _mixer_group = new MixerGroup(mixer_control_callback, (uintptr_t) &_controls); 56 | 57 | // PX4_INFO("Trying to initialize mixer from config file %s", mixer_filename); 58 | 59 | if (load_mixer_file(mixer_filename, buf, buflen) == 0) { 60 | if (_mixer_group->load_from_buf(buf, buflen) == 0) { 61 | PX4_INFO("Loaded mixer from file %s", mixer_filename); 62 | return 0; 63 | 64 | } else { 65 | PX4_ERR("Unable to parse from mixer config file %s", mixer_filename); 66 | } 67 | 68 | } else { 69 | PX4_ERR("Unable to load config file %s", mixer_filename); 70 | } 71 | 72 | if (_mixer_group->count() <= 0) { 73 | PX4_ERR("Mixer initialization failed"); 74 | return -1; 75 | } 76 | 77 | return 0; 78 | } 79 | //----------------------------------------------------------------------------// 80 | int rpi_pca9685_pwm_out::pwm_initialize() 81 | { 82 | /**************初始化PCA9685开始*************/ 83 | /**************PCA965 initializing********/ 84 | pwm.init(_pca9685_bus, 0x40); 85 | usleep(1000 * 100); 86 | /****12BIT 精度输出下，好赢电调可以到200HZ刷新***/ 87 | /****200HZ for 12bit Resolution, support most of the esc***/ 88 | pwm.setPWMFreq(200); 89 | usleep(1000 * 1000); 90 | /**************初始化PCA9685结束************/ 91 | /**************PCA965 initialized********/ 92 | return 0; 93 | } 94 | //----------------------------------------------------------------------------// 95 | void rpi_pca9685_pwm_out::pwm_deinitialize() 96 | { 97 | rpi_pca9685_pwm_out::pwm.reset(); 98 | } 99 | //----------------------------------------------------------------------------// 100 | void rpi_pca9685_pwm_out::send_outputs_pwm(const uint16_t *pwm) 101 | { 102 | /*************向PCA9685发送数据*************/ 103 | /*************send pwm signal to pca9685 initializing*************/ 104 | int i; 105 | 106 | for (i = 0; i < NUM_PWM; ++i) { 107 | //PX4_WARN("PWM%d:%d\n",i,*(pwm+i)); 108 | rpi_pca9685_pwm_out::pwm.setPWM(i, *(pwm + i)); 109 | } 110 | } 111 | //----------------------------------------------------------------------------// 112 | void rpi_pca9685_pwm_out::subscribe() 113 | { 114 | memset(_controls, 0, sizeof(_controls)); 115 | memset(_poll_fds, 0, sizeof(_poll_fds)); 116 | 117 | /* set up ORB topic names */ 118 | _controls_topics[0] = ORB_ID(actuator_controls_0); 119 | _controls_topics[1] = ORB_ID(actuator_controls_1); 120 | _controls_topics[2] = ORB_ID(actuator_controls_2); 121 | _controls_topics[3] = ORB_ID(actuator_controls_3); 122 | 123 | // Subscribe for orb topics 124 | for (uint8_t i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { 125 | if (_groups_required & (1 << i)) { 126 | PX4_DEBUG("subscribe to actuator_controls_%d", i); 127 | _controls_subs[i] = orb_subscribe(_controls_topics[i]); 128 | 129 | } else { 130 | _controls_subs[i] = -1; 131 | } 132 | 133 | if (_controls_subs[i] >= 0) { 134 | _poll_fds[_poll_fds_num].fd = _controls_subs[i]; 135 | _poll_fds[_poll_fds_num].events = POLLIN; 136 | _poll_fds_num++; 137 | } 138 | 139 | _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); 140 | 141 | } 142 | } 143 | //----------------------------------------------------------------------------// 144 | void rpi_pca9685_pwm_out::task_main(int argc, char *argv[]) 145 | { 146 | _is_running = true; 147 | 148 | /***************初始化PCA9685************/ 149 | /***************rpc_pca9685_pwm_out*************/ 150 | rpi_pca9685_pwm_out::pwm_initialize(); 151 | 152 | // Set up mixer 153 | if (initialize_mixer(_mixer_filename) < 0) { 154 | PX4_ERR("无法初始化通道混合配置文件 Can't loading mixer file"); 155 | return; 156 | } 157 | 158 | _mixer_group->groups_required(_groups_required); 159 | // subscribe and set up polling 160 | subscribe(); 161 | 162 | // Start disarmed 163 | _armed.armed = false; 164 | _armed.prearmed = false; 165 | 166 | pwm_limit_init(&_pwm_limit); 167 | 168 | // Main loop 169 | while (!_task_should_exit) { 170 | int pret = px4_poll(_poll_fds, _poll_fds_num, 10); 171 | 172 | /* Timed out, do a periodic check for _task_should_exit. */ 173 | if (pret == 0) { 174 | continue; 175 | } 176 | 177 | /* This is undesirable but not much we can do. */ 178 | if (pret < 0) { 179 | PX4_WARN("poll error %d, %d", pret, errno); 180 | /* sleep a bit before next try */ 181 | usleep(10000); 182 | continue; 183 | } 184 | 185 | /* get controls for required topics */ 186 | unsigned poll_id = 0; 187 | 188 | for (uint8_t i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { 189 | if (_controls_subs[i] >= 0) { 190 | if (_poll_fds[poll_id].revents & POLLIN) { 191 | orb_copy(_controls_topics[i], _controls_subs[i], &_controls[i]); 192 | } 193 | 194 | poll_id++; 195 | } 196 | } 197 | 198 | if (_mixer_group != nullptr) { 199 | _outputs.timestamp = hrt_absolute_time(); 200 | /* do mixing */ 201 | _outputs.noutputs = _mixer_group->mix(_outputs.output, 202 | _outputs.NUM_ACTUATOR_OUTPUTS, 203 | NULL); 204 | 205 | /* disable unused ports by setting their output to NaN */ 206 | for (size_t i = _outputs.noutputs; 207 | i < _outputs.NUM_ACTUATOR_OUTPUTS; 208 | i++) { 209 | _outputs.output[i] = NAN; 210 | } 211 | 212 | const uint16_t reverse_mask = 0; 213 | uint16_t disarmed_pwm[4]; 214 | uint16_t min_pwm[4]; 215 | uint16_t max_pwm[4]; 216 | 217 | for (unsigned int i = 0; i < NUM_PWM; i++) { 218 | disarmed_pwm[i] = _pwm_disarmed; 219 | min_pwm[i] = _pwm_min; 220 | max_pwm[i] = _pwm_max; 221 | } 222 | 223 | uint16_t pwm[4]; 224 | 225 | // TODO FIXME: pre-armed seems broken 226 | pwm_limit_calc(_armed.armed, 227 | false/*_armed.prearmed*/, 228 | _outputs.noutputs, 229 | reverse_mask, 230 | disarmed_pwm, 231 | min_pwm, 232 | max_pwm, 233 | _outputs.output, 234 | pwm, 235 | &_pwm_limit); 236 | 237 | if (_armed.lockdown) { 238 | send_outputs_pwm(disarmed_pwm); 239 | 240 | } else { 241 | send_outputs_pwm(pwm); 242 | } 243 | 244 | if (_outputs_pub != nullptr) { 245 | orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &_outputs); 246 | 247 | } else { 248 | _outputs_pub = orb_advertise(ORB_ID(actuator_outputs), &_outputs); 249 | } 250 | 251 | } else { 252 | PX4_ERR("Could not mix output! Exiting..."); 253 | _task_should_exit = true; 254 | } 255 | 256 | bool updated; 257 | orb_check(_armed_sub, &updated); 258 | 259 | if (updated) { 260 | orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); 261 | } 262 | } 263 | 264 | pwm_deinitialize(); 265 | 266 | for (uint8_t i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { 267 | if (_controls_subs[i] >= 0) { 268 | orb_unsubscribe(_controls_subs[i]); 269 | } 270 | } 271 | 272 | orb_unsubscribe(_armed_sub); 273 | 274 | _is_running = false; 275 | 276 | } 277 | //----------------------------------------------------------------------------// 278 | void rpi_pca9685_pwm_out::task_main_trampoline(int argc, char *argv[]) 279 | { 280 | task_main(argc, argv); 281 | } 282 | //----------------------------------------------------------------------------// 283 | void rpi_pca9685_pwm_out::start() 284 | { 285 | ASSERT(_task_handle == -1); 286 | _task_should_exit = false; 287 | 288 | /* start the task */ 289 | _task_handle = px4_task_spawn_cmd("pwm_out_main", 290 | SCHED_DEFAULT, 291 | SCHED_PRIORITY_MAX, 292 | 1500, 293 | (px4_main_t)&task_main_trampoline, 294 | nullptr); 295 | 296 | if (_task_handle < 0) { 297 | warn("task start failed"); 298 | return; 299 | } 300 | 301 | } 302 | //----------------------------------------------------------------------------// 303 | void rpi_pca9685_pwm_out::stop() 304 | { 305 | _task_should_exit = true; 306 | 307 | while (_is_running) { 308 | usleep(200000); 309 | PX4_INFO("."); 310 | } 311 | 312 | _task_handle = -1; 313 | } 314 | //----------------------------------------------------------------------------// 315 | void rpi_pca9685_pwm_out::usage() 316 | { 317 | PX4_INFO("usage: pwm_out start [-d pwmdevice] -m mixerfile"); 318 | PX4_INFO(" -d pwmdevice : sysfs device for pwm generation"); 319 | PX4_INFO(" (default /sys/class/pwm/pwmchip0)"); 320 | PX4_INFO(" -m mixerfile : path to mixerfile"); 321 | PX4_INFO(" (default ROMFS/px4fmu_common/mixers/quad_x.main.mix)"); 322 | PX4_INFO(" pwm_out stop"); 323 | PX4_INFO(" pwm_out status"); 324 | } 325 | //----------------------------------------------------------------------------// 326 | int rpi_pca9685_pwm_out_main(int argc, char **argv) 327 | { 328 | int ch; 329 | int myoptind = 1; 330 | const char *myoptarg = nullptr; 331 | 332 | char *verb = nullptr; 333 | 334 | if (argc >= 2) { 335 | verb = argv[1]; 336 | 337 | } else { 338 | return 1; 339 | } 340 | 341 | while ((ch = px4_getopt(argc, argv, "d:m:", &myoptind, &myoptarg)) != EOF) { 342 | switch (ch) { 343 | case 'd': 344 | strncpy(rpi_pca9685_pwm_out::_device, myoptarg, sizeof(rpi_pca9685_pwm_out::_device)); 345 | break; 346 | 347 | case 'm': 348 | strncpy(rpi_pca9685_pwm_out::_mixer_filename, myoptarg, sizeof(rpi_pca9685_pwm_out::_mixer_filename)); 349 | break; 350 | } 351 | } 352 | 353 | // gets the parameters for the esc's pwm 354 | param_get(param_find("PWM_DISARMED"), &rpi_pca9685_pwm_out::_pwm_disarmed); 355 | param_get(param_find("PWM_MIN"), &rpi_pca9685_pwm_out::_pwm_min); 356 | param_get(param_find("PWM_MAX"), &rpi_pca9685_pwm_out::_pwm_max); 357 | 358 | /* 359 | * Start/load the driver. 360 | */ 361 | if (!strcmp(verb, "start")) { 362 | if (rpi_pca9685_pwm_out::_is_running) { 363 | PX4_WARN("pwm_out already running"); 364 | return 1; 365 | } 366 | 367 | rpi_pca9685_pwm_out::start(); 368 | } 369 | 370 | else if (!strcmp(verb, "stop")) { 371 | if (!rpi_pca9685_pwm_out::_is_running) { 372 | PX4_WARN("pwm_out is not running"); 373 | return 1; 374 | } 375 | 376 | rpi_pca9685_pwm_out::stop(); 377 | } 378 | 379 | else if (!strcmp(verb, "status")) { 380 | PX4_WARN("pwm_out is %s", rpi_pca9685_pwm_out::_is_running ? "running" : "not running"); 381 | return 0; 382 | 383 | } else { 384 | rpi_pca9685_pwm_out::usage(); 385 | return 1; 386 | } 387 | 388 | return 0; 389 | } 390 | -------------------------------------------------------------------------------- /src/drivers/rpi_pca9685_pwm_out/rpi_pca9685_pwm_out.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2015-2016 zhangfan. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | #include 34 | 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include // NAN 40 | 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include "PCA9685.h" 52 | 53 | namespace rpi_pca9685_pwm_out 54 | { 55 | static px4_task_t _task_handle = -1; 56 | volatile bool _task_should_exit = false; 57 | static bool _is_running = false; 58 | 59 | static const int NUM_PWM = 4; 60 | static char _device[64] = "/sys/class/pwm/pwmchip0"; 61 | //static int _pwm_fd[NUM_PWM]; 62 | 63 | static const int FREQUENCY_PWM = 400; 64 | static char _mixer_filename[64] = "ROMFS/px4fmu_common/mixers/quad_x.main.mix"; 65 | 66 | // subscriptions 67 | int _controls_subs[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; 68 | int _armed_sub; 69 | 70 | // publications 71 | orb_advert_t _outputs_pub = nullptr; 72 | orb_advert_t _rc_pub = nullptr; 73 | 74 | // topic structures 75 | actuator_controls_s _controls[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; 76 | orb_id_t _controls_topics[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; 77 | actuator_outputs_s _outputs; 78 | actuator_armed_s _armed; 79 | 80 | // polling 81 | uint8_t _poll_fds_num = 0; 82 | px4_pollfd_struct_t _poll_fds[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; 83 | 84 | // control groups related 85 | uint32_t _groups_required = 0; 86 | uint32_t _groups_subscribed = 0; 87 | 88 | pwm_limit_t _pwm_limit; 89 | 90 | // esc parameters 91 | int32_t _pwm_disarmed; 92 | int32_t _pwm_min; 93 | int32_t _pwm_max; 94 | 95 | //PCA9685 device 96 | ::PCA9685 pwm; 97 | int _pca9685_bus=1; //默认兼容树莓派 98 | 99 | MixerGroup *_mixer_group = nullptr; 100 | 101 | void usage(); 102 | 103 | void start(); 104 | 105 | void stop(); 106 | 107 | int pwm_initialize(); 108 | 109 | void pwm_deinitialize(); 110 | 111 | void send_outputs_pwm(const uint16_t *pwm); 112 | 113 | void task_main_trampoline(int argc, char *argv[]); 114 | 115 | void subscribe(); 116 | 117 | void task_main(int argc, char *argv[]); 118 | 119 | /* mixer initialization */ 120 | int initialize_mixer(const char *mixer_filename); 121 | int mixer_control_callback(uintptr_t handle, uint8_t control_group, 122 | uint8_t control_index, float &input); 123 | } 124 | extern "C" __EXPORT int rpi_pca9685_pwm_out_main(int, char **); 125 | -------------------------------------------------------------------------------- /src/drivers/rpi_rc_in/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ############################################################################ 2 | # 3 | # Copyright (c) 2016 PX4 Development Team. All rights reserved. 4 | # 5 | # Redistribution and use in source and binary forms, with or without 6 | # modification, are permitted provided that the following conditions 7 | # are met: 8 | # 9 | # 1. Redistributions of source code must retain the above copyright 10 | # notice, this list of conditions and the following disclaimer. 11 | # 2. Redistributions in binary form must reproduce the above copyright 12 | # notice, this list of conditions and the following disclaimer in 13 | # the documentation and/or other materials provided with the 14 | # distribution. 15 | # 3. Neither the name PX4 nor the names of its contributors may be 16 | # used to endorse or promote products derived from this software 17 | # without specific prior written permission. 18 | # 19 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | # OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | # AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | # POSSIBILITY OF SUCH DAMAGE. 31 | # 32 | ############################################################################ 33 | px4_add_module( 34 | MODULE drivers__rpi_rc_in 35 | MAIN rpi_rc_in 36 | STACK_MAIN 1200 37 | COMPILE_FLAGS 38 | SRCS 39 | rpi_rc_in.cpp 40 | DEPENDS 41 | platforms__common 42 | ) 43 | # vim: set noet ft=cmake fenc=utf-8 ff=unix : 44 | -------------------------------------------------------------------------------- /src/drivers/rpi_rc_in/rpi_rc_in.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2015 Mark Charl. All rights reserved. 4 | * Copyright (C) 2017 Fan.zhang. All rights reserved. 421395590@qq.com 5 | * Copyright (C) 2016 PX4 Development Team. All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * 1. Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * 2. Redistributions in binary form must reproduce the above copyright 14 | * notice, this list of conditions and the following disclaimer in 15 | * the documentation and/or other materials provided with the 16 | * distribution. 17 | * 3. Neither the name PX4 nor the names of its contributors may be 18 | * used to endorse or promote products derived from this software 19 | * without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 28 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 29 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | * 34 | ****************************************************************************/ 35 | #include "rpi_rc_in.h" 36 | 37 | using namespace rpi_rc_in; 38 | //---------------------------------------------------------------------------------------------------------// 39 | int RcInput::rpi_rc_init() { 40 | int i; 41 | //--------------初始化共享内存映射----------------------------// 42 | if ((this->shmid = shmget(this->key, sizeof(int) * this->_channels, 0666)) 43 | < 0) { 44 | PX4_WARN("无法访问共享内存。Faild to access shared memory"); 45 | return -1; 46 | } 47 | 48 | if ((this->mem = (int*) shmat(this->shmid, NULL, 0)) == (void *) -1) { 49 | PX4_WARN("无法映射共享内存。Faild to mapping shared memory"); 50 | return -1; 51 | } 52 | 53 | //--------------发布所有通道的数据------------------------// 54 | for (i = 0; i < input_rc_s::RC_INPUT_MAX_CHANNELS; ++i) { 55 | _data.values[i] = UINT16_MAX; 56 | } 57 | 58 | _rcinput_pub = orb_advertise(ORB_ID(input_rc), &_data); 59 | 60 | if (_rcinput_pub == nullptr) { 61 | PX4_WARN("error: advertise failed"); 62 | return -1; 63 | } 64 | 65 | return 0; 66 | } 67 | //---------------------------------------------------------------------------------------------------------// 68 | int RcInput::start() { 69 | int result = 0; 70 | 71 | result = rpi_rc_init(); 72 | 73 | if (result != 0) { 74 | PX4_WARN("error: RC initialization failed"); 75 | return -1; 76 | } 77 | 78 | _isRunning = true; 79 | result = work_queue(HPWORK, &_work, (worker_t) & RcInput::cycle_trampoline, 80 | this, 0); 81 | 82 | if (result == -1) { 83 | _isRunning = false; 84 | } 85 | 86 | return result; 87 | } 88 | //---------------------------------------------------------------------------------------------------------// 89 | void RcInput::stop() { 90 | _shouldExit = true; 91 | } 92 | //---------------------------------------------------------------------------------------------------------// 93 | void RcInput::cycle_trampoline(void *arg) { 94 | RcInput *dev = reinterpret_cast(arg); 95 | dev->_cycle(); 96 | } 97 | //---------------------------------------------------------------------------------------------------------// 98 | void RcInput::_cycle() { 99 | _measure(); 100 | 101 | if (!_shouldExit) { 102 | work_queue(HPWORK, &_work, (worker_t) & RcInput::cycle_trampoline, this, 103 | USEC2TICK(RCINPUT_MEASURE_INTERVAL_US)); 104 | } 105 | } 106 | //---------------------------------------------------------------------------------------------------------// 107 | void RcInput::_measure(void) { 108 | uint64_t ts; 109 | // PWM数据发布 110 | // read pwm value from shared memory 111 | int i=0; 112 | for(i=0;i<_channels;++i){ 113 | _data.values[i] = (*(this->mem+i)<=0)?UINT16_MAX:*(this->mem+i); 114 | } 115 | 116 | ts = hrt_absolute_time(); 117 | _data.timestamp = ts; 118 | _data.timestamp_last_signal = ts; 119 | _data.channel_count = _channels; 120 | _data.rssi = 100; 121 | _data.rc_lost_frame_count = 0; 122 | _data.rc_total_frame_count = 1; 123 | _data.rc_ppm_frame_length = 100; 124 | _data.rc_failsafe = false; 125 | _data.rc_lost = false; 126 | _data.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_PPM; 127 | 128 | orb_publish(ORB_ID(input_rc), _rcinput_pub, &_data); 129 | } 130 | //---------------------------------------------------------------------------------------------------------// 131 | /** 132 | * Print the correct usage. 133 | */ 134 | 135 | static void rpi_rc_in::usage(const char *reason) { 136 | if (reason) { 137 | PX4_ERR("%s", reason); 138 | } 139 | PX4_INFO("用法: rpi_rc_in {start|stop|status}"); 140 | } 141 | //---------------------------------------------------------------------------------------------------------// 142 | int rpi_rc_in_main(int argc, char **argv) { 143 | if (argc < 2) { 144 | usage("missing command"); 145 | return 1; 146 | } 147 | 148 | if (!strcmp(argv[1], "start")) { 149 | 150 | if (rc_input != nullptr && rc_input->isRunning()) { 151 | PX4_WARN("运行中。running"); 152 | /* this is not an error */ 153 | return 0; 154 | } 155 | 156 | rc_input = new RcInput(); 157 | 158 | // Check if alloc worked. 159 | if (nullptr == rc_input) { 160 | PX4_ERR("遥控输入模块初始化错误。Rc input moduel initialization faild"); 161 | return -1; 162 | } 163 | 164 | int ret = rc_input->start(); 165 | 166 | if (ret != 0) { 167 | PX4_ERR("遥控输入模块未能启动。 Rc input module failure"); 168 | } 169 | 170 | return 0; 171 | } 172 | 173 | if (!strcmp(argv[1], "stop")) { 174 | 175 | if (rc_input == nullptr || !rc_input->isRunning()) { 176 | PX4_WARN("模块未运行。 Not runing"); 177 | /* this is not an error */ 178 | return 0; 179 | } 180 | 181 | rc_input->stop(); 182 | 183 | // Wait for task to die 184 | int i = 0; 185 | 186 | do { 187 | /* wait up to 3s */ 188 | usleep(100000); 189 | 190 | } while (rc_input->isRunning() && ++i < 30); 191 | 192 | delete rc_input; 193 | rc_input = nullptr; 194 | 195 | return 0; 196 | } 197 | 198 | if (!strcmp(argv[1], "status")) { 199 | if (rc_input != nullptr && rc_input->isRunning()) { 200 | PX4_INFO("运行中。 running"); 201 | 202 | } else { 203 | PX4_INFO("未运行。 Not runing\n"); 204 | } 205 | 206 | return 0; 207 | } 208 | 209 | usage("不知道你要做什么。 rpi_rc_in start|stop|status"); 210 | return 1; 211 | 212 | } 213 | //---------------------------------------------------------------------------------------------------------// 214 | -------------------------------------------------------------------------------- /src/drivers/rpi_rc_in/rpi_rc_in.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2015 Mark Charl. All rights reserved. 4 | * Copyright (C) 2017 Fan.zhang. All rights reserved. 421395590@qq.com 5 | * Copyright (C) 2016 PX4 Development Team. All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * 1. Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * 2. Redistributions in binary form must reproduce the above copyright 14 | * notice, this list of conditions and the following disclaimer in 15 | * the documentation and/or other materials provided with the 16 | * distribution. 17 | * 3. Neither the name PX4 nor the names of its contributors may be 18 | * used to endorse or promote products derived from this software 19 | * without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 28 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 29 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | * 34 | ****************************************************************************/ 35 | 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | 44 | #include 45 | #include 46 | #include 47 | 48 | #include 49 | 50 | #include 51 | #include 52 | #define RCINPUT_MEASURE_INTERVAL_US 20000 53 | namespace rpi_rc_in { 54 | class RcInput { 55 | public: 56 | int *mem; 57 | key_t key; 58 | int shmid; 59 | RcInput() : 60 | mem(nullptr),key(4096),shmid(0),_shouldExit(false), _isRunning(false), _work { }, _rcinput_pub( 61 | nullptr), _channels(8),//D8R-II plus 62 | _data { } { 63 | //memset(_ch_fd, 0, sizeof(_ch_fd)); 64 | } 65 | ~RcInput() { 66 | this->mem = nullptr; 67 | work_cancel(HPWORK, &_work); 68 | _isRunning = false; 69 | } 70 | 71 | /* @return 0 on success, -errno on failure */ 72 | int start(); 73 | 74 | /* @return 0 on success, -errno on failure */ 75 | void stop(); 76 | 77 | /* Trampoline for the work queue. */ 78 | static void cycle_trampoline(void *arg); 79 | 80 | bool isRunning() { 81 | return _isRunning; 82 | } 83 | 84 | private: 85 | void _cycle(); 86 | void _measure(); 87 | 88 | bool _shouldExit;bool _isRunning; 89 | struct work_s _work; 90 | orb_advert_t _rcinput_pub; 91 | int _channels; 92 | //int _ch_fd[input_rc_s::RC_INPUT_MAX_CHANNELS]; 93 | struct input_rc_s _data; 94 | 95 | int rpi_rc_init(); 96 | }; 97 | 98 | static void usage(const char *reason); 99 | static RcInput *rc_input = nullptr; 100 | } 101 | extern "C" __EXPORT int rpi_rc_in_main(int argc, char **argv); 102 | --------------------------------------------------------------------------------