diff --git a/.ci/Jenkinsfile-compile b/.ci/Jenkinsfile-compile index 1836b21c88..fea59e2ea0 100644 --- a/.ci/Jenkinsfile-compile +++ b/.ci/Jenkinsfile-compile @@ -16,7 +16,7 @@ pipeline { ] def armhf_builds = [ - target: ["aerotenna_ocpoc_default", "beaglebone_blue_default", "emlid_navio2_default", "px4_raspberrypi_default"], + target: ["aerotenna_ocpoc_default", "beaglebone_blue_default", "emlid_navio2_default", "px4_raspberrypi_default", "scumaker_airpi2_default"], image: docker_images.armhf, archive: false ] diff --git a/Tools/docker_run.sh b/Tools/docker_run.sh index 65dbe22e18..9f3a2ea4aa 100755 --- a/Tools/docker_run.sh +++ b/Tools/docker_run.sh @@ -5,8 +5,8 @@ if [ -z ${PX4_DOCKER_REPO+x} ]; then if [[ $@ =~ .*px4_fmu.* ]]; then # nuttx-px4fmu-v{1,2,3,4,5} PX4_DOCKER_REPO="px4io/px4-dev-nuttx-focal:2020-09-14" - elif [[ $@ =~ .*ocpoc.* ]] || [[ $@ =~ .*navio2.* ]] || [[ $@ =~ .*raspberry.* ]] || [[ $@ =~ .*beaglebone.* ]]; then - # aerotenna_ocpoc_default, beaglebone_blue_default, emlid_navio2_default, px4_raspberrypi_default + elif [[ $@ =~ .*ocpoc.* ]] || [[ $@ =~ .*navio2.* ]] || [[ $@ =~ .*raspberry.* ]] || [[ $@ =~ .*beaglebone.* ]] || [[ $@ =~ .*airpi2.* ]]; then + # aerotenna_ocpoc_default, beaglebone_blue_default, emlid_navio2_default, px4_raspberrypi_default, scumaker_airpi2_default PX4_DOCKER_REPO="px4io/px4-dev-armhf:2020-04-01" elif [[ $@ =~ .*eagle.* ]] || [[ $@ =~ .*excelsior.* ]]; then # eagle, excelsior diff --git a/boards/px4/fmu-v3/default.cmake b/boards/px4/fmu-v3/default.cmake index 78a4cb667f..b3ff1af803 100644 --- a/boards/px4/fmu-v3/default.cmake +++ b/boards/px4/fmu-v3/default.cmake @@ -49,6 +49,7 @@ px4_add_board( optical_flow/px4flow #osd pca9685 + pca9685_pwm_out #power_monitor/ina226 #protocol_splitter pwm_input diff --git a/boards/px4/fmu-v4/default.cmake b/boards/px4/fmu-v4/default.cmake index 743bae159d..df06a17cdc 100644 --- a/boards/px4/fmu-v4/default.cmake +++ b/boards/px4/fmu-v4/default.cmake @@ -43,6 +43,7 @@ px4_add_board( optical_flow # all available optical flow drivers #osd pca9685 + pca9685_pwm_out #protocol_splitter pwm_input pwm_out_sim diff --git a/boards/px4/fmu-v4pro/default.cmake b/boards/px4/fmu-v4pro/default.cmake index e1c3002be2..b80e684953 100644 --- a/boards/px4/fmu-v4pro/default.cmake +++ b/boards/px4/fmu-v4pro/default.cmake @@ -42,6 +42,7 @@ px4_add_board( optical_flow/px4flow #osd pca9685 + pca9685_pwm_out power_monitor/ina226 #protocol_splitter pwm_input diff --git a/boards/px4/fmu-v5/default.cmake b/boards/px4/fmu-v5/default.cmake index 19d3d9ff39..8019bee896 100644 --- a/boards/px4/fmu-v5/default.cmake +++ b/boards/px4/fmu-v5/default.cmake @@ -45,6 +45,7 @@ px4_add_board( optical_flow # all available optical flow drivers #osd pca9685 + pca9685_pwm_out power_monitor/ina226 #protocol_splitter pwm_input diff --git a/boards/px4/fmu-v5x/default.cmake b/boards/px4/fmu-v5x/default.cmake index 84ee93a475..b37add5bf4 100644 --- a/boards/px4/fmu-v5x/default.cmake +++ b/boards/px4/fmu-v5x/default.cmake @@ -44,6 +44,7 @@ px4_add_board( optical_flow # all available optical flow drivers #osd pca9685 + pca9685_pwm_out power_monitor/ina226 #protocol_splitter pwm_input diff --git a/boards/scumaker/airpi2/cmake/upload.cmake b/boards/scumaker/airpi2/cmake/upload.cmake new file mode 100644 index 0000000000..a6b66aaf50 --- /dev/null +++ b/boards/scumaker/airpi2/cmake/upload.cmake @@ -0,0 +1,47 @@ +############################################################################ +# +# Copyright (c) 2020 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +if(DEFINED ENV{AUTOPILOT_HOST}) + set(AUTOPILOT_HOST $ENV{AUTOPILOT_HOST}) +else() + set(AUTOPILOT_HOST "raspberrypi") +endif() + +add_custom_target(upload + COMMAND rsync -arh --progress + ${CMAKE_RUNTIME_OUTPUT_DIRECTORY} ${PX4_SOURCE_DIR}/posix-configs/rpi/airpi2*.config ${PX4_BINARY_DIR}/etc # source + pi@\$\{AUTOPILOT_HOST\}:/home/pi/px4 # destination + DEPENDS px4 + COMMENT "uploading px4" + USES_TERMINAL +) diff --git a/boards/scumaker/airpi2/default.cmake b/boards/scumaker/airpi2/default.cmake new file mode 100644 index 0000000000..37eaa39f01 --- /dev/null +++ b/boards/scumaker/airpi2/default.cmake @@ -0,0 +1,93 @@ +add_definitions( + -D__PX4_LINUX +) + +px4_add_board( + VENDOR scumaker + MODEL airpi2 + LABEL default + PLATFORM posix + ARCHITECTURE cortex-a53 + ROMFSROOT px4fmu_common + TOOLCHAIN arm-linux-gnueabihf + TESTING + DRIVERS + adc/ads1115 + #barometer # all available barometer drivers + barometer/ms5611 + batt_smbus + camera_capture + camera_trigger + differential_pressure # all available differential pressure drivers + distance_sensor # all available distance sensor drivers + gps + #imu # all available imu drivers + imu/invensense/icm42688p + imu/invensense/icm42605 + #magnetometer # all available magnetometer drivers + magnetometer/hmc5883 + magnetometer/isentek/ist8310 + pca9685_pwm_out + pwm_out_sim + rc_input + #telemetry # all available telemetry drivers + MODULES + airspeed_selector + attitude_estimator_q + battery_status + camera_feedback + commander + dataman + ekf2 + events + fw_att_control + fw_pos_control_l1 + land_detector + landing_target_estimator + load_mon + local_position_estimator + logger + mavlink + mc_att_control + mc_hover_thrust_estimator + mc_pos_control + mc_rate_control + #micrortps_bridge + navigator + rc_update + rover_pos_control + sensors + temperature_compensation + sih + #simulator + vmount + vtol_att_control + SYSTEMCMDS + dyn + esc_calib + led_control + mixer + motor_ramp + param + perf + pwm + sd_bench + shutdown + tests # tests and test runner + #top + topic_listener + tune_control + ver + work_queue + EXAMPLES + dyn_hello # dynamically loading modules example + fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control + hello + #hwtest # Hardware test + #matlab_csv_serial + px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html + px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html + rover_steering_control # Rover example app + uuv_example_app + work_item + ) diff --git a/boards/scumaker/airpi2/src/CMakeLists.txt b/boards/scumaker/airpi2/src/CMakeLists.txt new file mode 100644 index 0000000000..fbe674842d --- /dev/null +++ b/boards/scumaker/airpi2/src/CMakeLists.txt @@ -0,0 +1,37 @@ +############################################################################ +# +# Copyright (c) 2020 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +add_library(drivers_board + i2c.cpp + spi.cpp +) diff --git a/boards/scumaker/airpi2/src/board_config.h b/boards/scumaker/airpi2/src/board_config.h new file mode 100644 index 0000000000..4ce9b73e38 --- /dev/null +++ b/boards/scumaker/airpi2/src/board_config.h @@ -0,0 +1,60 @@ +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file board_config.h + * + * RPI internal definitions + */ + +#pragma once + +#define BOARD_OVERRIDE_UUID "RPIID00000000000" // must be of length 16 +#define PX4_SOC_ARCH_ID PX4_SOC_ARCH_ID_RPI + +/* + * I2C busses + */ +#define PX4_NUMBER_I2C_BUSES 2 + +#define ADC_BATTERY_VOLTAGE_CHANNEL 0 +#define ADC_BATTERY_CURRENT_CHANNEL -1 +#define ADC_AIRSPEED_VOLTAGE_CHANNEL 2 + +#define BOARD_BATTERY1_V_DIV 5.7f // 1K + 4.7K +#define ADC_DP_V_DIV 1.0f + +#define BOARD_ADC_OPEN_CIRCUIT_V 5.3f // Powered from USB + +#include +#include diff --git a/boards/scumaker/airpi2/src/i2c.cpp b/boards/scumaker/airpi2/src/i2c.cpp new file mode 100644 index 0000000000..2546cd0d8e --- /dev/null +++ b/boards/scumaker/airpi2/src/i2c.cpp @@ -0,0 +1,39 @@ +/**************************************************************************** + * + * Copyright (C) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include + +constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { + initI2CBusExternal(0), + initI2CBusInternal(1), +}; diff --git a/boards/scumaker/airpi2/src/spi.cpp b/boards/scumaker/airpi2/src/spi.cpp new file mode 100644 index 0000000000..55ff4a90c9 --- /dev/null +++ b/boards/scumaker/airpi2/src/spi.cpp @@ -0,0 +1,42 @@ +/**************************************************************************** + * + * Copyright (C) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include + +constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { + initSPIBus(0, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, 0), + initSPIDevice(DRV_IMU_DEVTYPE_ICM42605, 0), + }), +}; diff --git a/posix-configs/rpi/airpi2_mc.config b/posix-configs/rpi/airpi2_mc.config new file mode 100644 index 0000000000..c82b87f5bf --- /dev/null +++ b/posix-configs/rpi/airpi2_mc.config @@ -0,0 +1,64 @@ +#!/bin/sh +# PX4 commands need the 'px4-' prefix in bash. +# (px4-alias.sh is expected to be in the PATH) +. px4-alias.sh + +uorb start + +if [ -f eeprom/parameters ] +then + param load +fi +param set CBRK_SUPPLY_CHK 894281 +#param set SYS_AUTOSTART 4001 +param set MAV_BROADCAST 1 +param set MAV_TYPE 2 + +dataman start + +load_mon start + +battery_status start + +# internal IMU +if ! icm42688p start -s -R 4 +then + # some boards has ICM42605 inside + icm42605 start -s -R 4 +fi +ist8310 start -X -a 15 -R 4 +ms5611 start -I + +# ADC +ads1115 start -I + +# PWM +pca9685_pwm_out start +mixer load /dev/pwm_output0 etc/mixers/quad_x.main.mix + +# external GPS & compass +gps start -d /dev/ttySC0 -i uart -p ubx -s +#hmc5883 start -X +#ist8310 start -X + +rc_input start -d /dev/ttyAMA0 + +rc_update start +sensors start +commander start +navigator start +ekf2 start +land_detector start multicopter +mc_hover_thrust_estimator start +mc_pos_control start +mc_att_control start +mc_rate_control start + +mavlink start -x -u 14556 -r 1000000 + +# Telem +mavlink start -x -Z -d /dev/ttySC1 + +logger start -t -b 200 + +mavlink boot_complete diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 333bdab2d3..6ea1b295e2 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -163,7 +163,7 @@ #define DRV_MAG_DEVTYPE_UAVCAN 0x88 #define DRV_DIST_DEVTYPE_UAVCAN 0x89 -#define DRV_ADC_DEVTYPE_ADS1115 0x90 +#define DRV_ADC_DEVTYPE_ADS1115 0x90 #define DRV_DEVTYPE_UNUSED 0xff diff --git a/src/drivers/pca9685_pwm_out/CMakeLists.txt b/src/drivers/pca9685_pwm_out/CMakeLists.txt index d584d0daf3..dfb2a46375 100644 --- a/src/drivers/pca9685_pwm_out/CMakeLists.txt +++ b/src/drivers/pca9685_pwm_out/CMakeLists.txt @@ -31,7 +31,7 @@ # ############################################################################ px4_add_module( - MODULE drivers__pca9685 + MODULE drivers__pca9685_pwm_out MAIN pca9685_pwm_out COMPILE_FLAGS SRCS diff --git a/src/drivers/pca9685_pwm_out/PCA9685.cpp b/src/drivers/pca9685_pwm_out/PCA9685.cpp index 25a1aa5e4e..a0eb18459c 100644 --- a/src/drivers/pca9685_pwm_out/PCA9685.cpp +++ b/src/drivers/pca9685_pwm_out/PCA9685.cpp @@ -74,16 +74,16 @@ int PCA9685::updatePWM(const uint16_t *outputs, unsigned num_outputs) return 0; } -int PCA9685::setFreq(int freq) +int PCA9685::setFreq(float freq) { - uint16_t realResolution = floorl((double)PCA9685_CLOCK_FREQ / freq); + uint16_t realResolution = floorl((float)PCA9685_CLOCK_FREQ / freq); if (realResolution < PCA9685_PWM_RES) { // unable to provide enough resolution PX4_DEBUG("frequency too high"); return -EINVAL; } - uint16_t divider = (uint16_t)round((double)PCA9685_CLOCK_FREQ / freq / PCA9685_PWM_RES) - 1; + uint16_t divider = (uint16_t)round((float)PCA9685_CLOCK_FREQ / freq / PCA9685_PWM_RES) - 1; if (divider > 0x00FF) { // out of divider PX4_DEBUG("frequency too low"); diff --git a/src/drivers/pca9685_pwm_out/PCA9685.h b/src/drivers/pca9685_pwm_out/PCA9685.h index 655026f0b7..b0f630c7ac 100644 --- a/src/drivers/pca9685_pwm_out/PCA9685.h +++ b/src/drivers/pca9685_pwm_out/PCA9685.h @@ -84,13 +84,17 @@ namespace drv_pca9685_pwm #define PCA9685_PWM_CHANNEL_COUNT 16 #define PCA9685_PWM_RES 4096 //Resolution 4096=12bit -#define PCA9685_CLOCK_INT 25000000.0 //25MHz internal clock +/* This should be 25000000 ideally, + * but it seems most chips have its oscillator working at a higher frequency + * Reference: https://github.com/adafruit/Adafruit-PWM-Servo-Driver-Library/blob/6664ce936210eea53259b814062009d9569a4213/Adafruit_PWMServoDriver.h#L66 */ +#define PCA9685_CLOCK_INT 26075000.0 //25MHz internal clock #ifndef PCA9685_CLOCL_EXT #define PCA9685_CLOCK_FREQ PCA9685_CLOCK_INT // use int clk #else #define PCA9685_CLOCK_FREQ PCA9685_CLOCK_EXT // use ext clk #endif +#define PCA9685_DEVICE_BASE_PATH "/dev/pca9685" #define PWM_DEFAULT_FREQUENCY 50 // default pwm frequency //! Main class that exports features for PCA9685 chip @@ -108,7 +112,7 @@ public: */ int updatePWM(const uint16_t *outputs, unsigned num_outputs); - int setFreq(int freq); + int setFreq(float freq); ~PCA9685() override = default; diff --git a/src/drivers/pca9685_pwm_out/main.cpp b/src/drivers/pca9685_pwm_out/main.cpp index 35f5621bfd..bd49b00266 100644 --- a/src/drivers/pca9685_pwm_out/main.cpp +++ b/src/drivers/pca9685_pwm_out/main.cpp @@ -49,9 +49,6 @@ #include "PCA9685.h" -#define PCA9685_MAX_OUTPUT_CHANNELS 16 -#define PCA9685_DEVICE_BASE_PATH "/dev/pca9685" - #define PCA9685_DEFAULT_IICBUS 1 #define PCA9685_DEFAULT_ADDRESS (0x40) @@ -88,6 +85,8 @@ public: private: perf_counter_t _cycle_perf; + int _class_instance{-1}; + void Run() override; protected: @@ -101,14 +100,11 @@ protected: uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; // param handle - MixingOutput _mixing_output{PCA9685_MAX_OUTPUT_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, true}; - -private: - + MixingOutput _mixing_output{PCA9685_PWM_CHANNEL_COUNT, *this, MixingOutput::SchedulingPolicy::Disabled, true}; }; PWMDriverWrapper::PWMDriverWrapper() : - CDev(PCA9685_DEVICE_BASE_PATH), + CDev(nullptr), OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default), _cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")) { @@ -142,11 +138,11 @@ int PWMDriverWrapper::init() return ret; } - updatePWMParams(); + this->ChangeWorkQeue(px4::device_bus_to_wq(pca9685->get_device_id())); - if (!_mixing_output.updateSubscriptions(true)) { // change to proper wq - ScheduleNow(); - } + updatePWMParams(); // Schedule is done inside + + _class_instance = register_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH); return PX4_OK; } @@ -160,23 +156,21 @@ void PWMDriverWrapper::updateParams() void PWMDriverWrapper::updatePWMParams() { // update pwm params - const char *pname_format_main_max = "PWM_MAIN_MAX%d"; - const char *pname_format_main_min = "PWM_MAIN_MIN%d"; - const char *pname_format_main_fail = "PWM_MAIN_FAIL%d"; - const char *pname_format_main_dis = "PWM_MAIN_DIS%d"; - const char *pname_format_main_rev = "PWM_MAIN_REV%d"; - const char *pname_format_aux_max = "PWM_AUX_MAX%d"; - const char *pname_format_aux_min = "PWM_AUX_MIN%d"; - const char *pname_format_aux_fail = "PWM_AUX_FAIL%d"; - const char *pname_format_aux_dis = "PWM_AUX_DIS%d"; - const char *pname_format_aux_rev = "PWM_AUX_REV%d"; + const char *pname_format_pwm_ch_max[2] = {"PWM_MAIN_MAX%d", "PWM_AUX_MAX%d"}; + const char *pname_format_pwm_ch_min[2] = {"PWM_MAIN_MIN%d", "PWM_AUX_MIN%d"}; + const char *pname_format_pwm_ch_fail[2] = {"PWM_MAIN_FAIL%d", "PWM_AUX_FAIL%d"}; + const char *pname_format_pwm_ch_dis[2] = {"PWM_MAIN_DIS%d", "PWM_AUX_DIS%d"}; + const char *pname_format_pwm_ch_rev[2] = {"PWM_MAIN_REV%d", "PWM_AUX_REV%d"}; + + int32_t default_pwm_max = PWM_DEFAULT_MAX, + default_pwm_min = PWM_DEFAULT_MIN, + default_pwm_fail = PWM_DEFAULT_MIN, + default_pwm_dis = PWM_MOTOR_OFF; param_t param_h = param_find("PWM_MAX"); if (param_h != PARAM_INVALID) { - int32_t pval = 0; - param_get(param_h, &pval); - _mixing_output.setAllMaxValues(pval); + param_get(param_h, &default_pwm_max); } else { PX4_DEBUG("PARAM_INVALID: %s", "PWM_MAX"); @@ -185,9 +179,7 @@ void PWMDriverWrapper::updatePWMParams() param_h = param_find("PWM_MIN"); if (param_h != PARAM_INVALID) { - int32_t pval = 0; - param_get(param_h, &pval); - _mixing_output.setAllMinValues(pval); + param_get(param_h, &default_pwm_min); } else { PX4_DEBUG("PARAM_INVALID: %s", "PWM_MIN"); @@ -199,18 +191,35 @@ void PWMDriverWrapper::updatePWMParams() int32_t pval = 0; param_get(param_h, &pval); - if (pca9685->setFreq(pval) != PX4_OK) { - PX4_DEBUG("failed to set pwm frequency"); + if (pca9685->setFreq((float)pval) != PX4_OK) { + PX4_ERR("failed to set pwm frequency, fall back to 50Hz"); + pca9685->setFreq((float)50); // this should not fail + ScheduleClear(); + ScheduleOnInterval(1000000 / pca9685->getFrequency(), 1000000 / pca9685->getFrequency()); + + } else { + ScheduleClear(); + ScheduleOnInterval(1000000 / pval, 1000000 / pval); } } else { PX4_DEBUG("PARAM_INVALID: %s", "PWM_RATE"); } - for (int i = 0; i < 8; i++) { + for (int i = 0; i < PCA9685_PWM_CHANNEL_COUNT; i++) { char pname[16]; + uint8_t param_group, param_index; - sprintf(pname, pname_format_main_max, i + 1); + if (i <= 7) { // Main channel + param_group = 0; + param_index = i + 1; + + } else { // AUX + param_group = 1; + param_index = i - 8 + 1; + } + + sprintf(pname, pname_format_pwm_ch_max[param_group], param_index); param_h = param_find(pname); if (param_h != PARAM_INVALID) { @@ -219,13 +228,16 @@ void PWMDriverWrapper::updatePWMParams() if (pval != -1) { _mixing_output.maxValue(i) = pval; + + } else { + _mixing_output.maxValue(i) = default_pwm_max; } } else { PX4_DEBUG("PARAM_INVALID: %s", pname); } - sprintf(pname, pname_format_main_min, i + 1); + sprintf(pname, pname_format_pwm_ch_min[param_group], param_index); param_h = param_find(pname); if (param_h != PARAM_INVALID) { @@ -234,13 +246,16 @@ void PWMDriverWrapper::updatePWMParams() if (pval != -1) { _mixing_output.minValue(i) = pval; + + } else { + _mixing_output.minValue(i) = default_pwm_min; } } else { PX4_DEBUG("PARAM_INVALID: %s", pname); } - sprintf(pname, pname_format_main_fail, i + 1); + sprintf(pname, pname_format_pwm_ch_fail[param_group], param_index); param_h = param_find(pname); if (param_h != PARAM_INVALID) { @@ -249,13 +264,16 @@ void PWMDriverWrapper::updatePWMParams() if (pval != -1) { _mixing_output.failsafeValue(i) = pval; + + } else { + _mixing_output.failsafeValue(i) = default_pwm_fail; } } else { PX4_DEBUG("PARAM_INVALID: %s", pname); } - sprintf(pname, pname_format_main_dis, i + 1); + sprintf(pname, pname_format_pwm_ch_dis[param_group], param_index); param_h = param_find(pname); if (param_h != PARAM_INVALID) { @@ -264,13 +282,16 @@ void PWMDriverWrapper::updatePWMParams() if (pval != -1) { _mixing_output.disarmedValue(i) = pval; + + } else { + _mixing_output.disarmedValue(i) = default_pwm_dis; } } else { PX4_DEBUG("PARAM_INVALID: %s", pname); } - sprintf(pname, pname_format_main_rev, i + 1); + sprintf(pname, pname_format_pwm_ch_rev[param_group], param_index); param_h = param_find(pname); if (param_h != PARAM_INVALID) { @@ -283,80 +304,6 @@ void PWMDriverWrapper::updatePWMParams() } else { PX4_DEBUG("PARAM_INVALID: %s", pname); } - - sprintf(pname, pname_format_aux_max, i + 1); - param_h = param_find(pname); - - if (param_h != PARAM_INVALID) { - int32_t pval = 0; - param_get(param_h, &pval); - - if (pval != -1) { - _mixing_output.maxValue(i + 8) = pval; - } - - } else { - PX4_DEBUG("PARAM_INVALID: %s", pname); - } - - sprintf(pname, pname_format_aux_min, i + 1); - param_h = param_find(pname); - - if (param_h != PARAM_INVALID) { - int32_t pval = 0; - param_get(param_h, &pval); - - if (pval != -1) { - _mixing_output.minValue(i + 8) = pval; - } - - } else { - PX4_DEBUG("PARAM_INVALID: %s", pname); - } - - sprintf(pname, pname_format_aux_fail, i + 1); - param_h = param_find(pname); - - if (param_h != PARAM_INVALID) { - int32_t pval = 0; - param_get(param_h, &pval); - - if (pval != -1) { - _mixing_output.failsafeValue(i + 8) = pval; - } - - } else { - PX4_DEBUG("PARAM_INVALID: %s", pname); - } - - sprintf(pname, pname_format_aux_dis, i + 1); - param_h = param_find(pname); - - if (param_h != PARAM_INVALID) { - int32_t pval = 0; - param_get(param_h, &pval); - - if (pval != -1) { - _mixing_output.disarmedValue(i + 8) = pval; - } - - } else { - PX4_DEBUG("PARAM_INVALID: %s", pname); - } - - sprintf(pname, pname_format_aux_rev, i + 1); - param_h = param_find(pname); - - if (param_h != PARAM_INVALID) { - uint16_t &reverse_pwm_mask = _mixing_output.reverseOutputMask(); - int32_t pval = 0; - param_get(param_h, &pval); - reverse_pwm_mask &= (0xfffe << (i + 8)); // clear this bit - reverse_pwm_mask |= (((uint16_t)(pval != 0)) << (i + 8)); // set to new val - - } else { - PX4_DEBUG("PARAM_INVALID: %s", pname); - } } if (_mixing_output.mixers()) { // only update trims if mixer loaded @@ -366,62 +313,54 @@ void PWMDriverWrapper::updatePWMParams() void PWMDriverWrapper::updatePWMParamTrim() { - const char *pname_format_main_trim = "PWM_MAIN_TRIM%d"; - const char *pname_format_aux_trim = "PWM_AUX_TRIM%d"; + const char *pname_format_pwm_ch_trim[2] = {"PWM_MAIN_TRIM%d", "PWM_AUX_TRIM%d"}; - int16_t trim_values[PCA9685_MAX_OUTPUT_CHANNELS] = {}; + int16_t trim_values[PCA9685_PWM_CHANNEL_COUNT] = {}; - for (int i = 0; i < 8; i++) { + for (int i = 0; i < PCA9685_PWM_CHANNEL_COUNT; i++) { char pname[16]; - sprintf(pname, pname_format_main_trim, i + 1); - param_t param_h = param_find(pname); + uint8_t param_group, param_index; - if (param_h != PARAM_INVALID) { - float pval = 0.0f; - param_get(param_h, &pval); - trim_values[i] = (int16_t)(10000 * pval); + if (i <= 7) { // Main channel + param_group = 0; + param_index = i + 1; - } else { - PX4_DEBUG("PARAM_INVALID: %s", pname); + } else { // AUX + param_group = 1; + param_index = i - 8 + 1; } - sprintf(pname, pname_format_aux_trim, i + 1); - param_h = param_find(pname); + sprintf(pname, pname_format_pwm_ch_trim[param_group], param_index); + param_t param_h = param_find(pname); + int32_t val; if (param_h != PARAM_INVALID) { - float pval = 0.0f; - param_get(param_h, &pval); - trim_values[i + 8] = (int16_t)(10000 * pval); + param_get(param_h, &val); + trim_values[i] = (int16_t)val; } else { PX4_DEBUG("PARAM_INVALID: %s", pname); } } - unsigned n_out = _mixing_output.mixers()->set_trims(trim_values, PCA9685_MAX_OUTPUT_CHANNELS); + unsigned n_out = _mixing_output.mixers()->set_trims(trim_values, PCA9685_PWM_CHANNEL_COUNT); PX4_DEBUG("set %d trims", n_out); } bool PWMDriverWrapper::updateOutputs(bool stop_motors, uint16_t *outputs, unsigned num_outputs, unsigned num_control_groups_updated) { - /*char buf[1024]="PWM:"; - for(uint i=0;iupdatePWM(outputs, num_outputs);; + return pca9685->updatePWM(outputs, num_outputs) == 0 ? true : false; } void PWMDriverWrapper::Run() { - hrt_abstime timestamp = hrt_absolute_time(); - if (should_exit()) { PX4_INFO("PCA9685 stopping."); ScheduleClear(); _mixing_output.unregister(); + unregister_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH, _class_instance); pca9685->Stop(); delete pca9685; @@ -445,17 +384,9 @@ void PWMDriverWrapper::Run() updateParams(); } + _mixing_output.updateSubscriptions(false); + perf_end(_cycle_perf); - - int nextDelay = 1000000 / pca9685->getFrequency() - (hrt_absolute_time() - timestamp); - - if (nextDelay < 0) { - PX4_DEBUG("PCA9685: can not follow up. %d us"); - ScheduleNow(); - - } else { - ScheduleDelayed((uint32_t)nextDelay); - } } // TODO @@ -480,6 +411,19 @@ int PWMDriverWrapper::ioctl(cdev::file_t *filep, int cmd, unsigned long arg) break; } + case PWM_SERVO_GET_COUNT: + *(unsigned *)arg = PCA9685_PWM_CHANNEL_COUNT; + + break; + + case PWM_SERVO_SET_ARM_OK: + case PWM_SERVO_SET_FORCE_SAFETY_OFF: + case PWM_SERVO_CLEAR_ARM_OK: + case PWM_SERVO_SET_FORCE_SAFETY_ON: + case PWM_SERVO_ARM: + case PWM_SERVO_DISARM: + break; + default: ret = -ENOTTY; break; @@ -516,7 +460,9 @@ It is typically started with: $ pca9685_pwm_out start -a 64 -b 1 Use the `mixer` command to load mixer files. -`mixer load /dev/pca9685 etc/mixers/quad_x.main.mix` +`mixer load /dev/pwm_outputX etc/mixers/quad_x.main.mix` +The number X can be acquired by executing +`pca9685_pwm_out status` when this driver is running. )DESCR_STR"); PRINT_MODULE_USAGE_NAME("pca9685_pwm_out", "driver"); @@ -534,6 +480,7 @@ int PWMDriverWrapper::print_status() { pca9685->get_device_bus(), pca9685->get_device_address(), (double)(pca9685->getFrequency())); + PX4_INFO("CDev path: %s%d", PWM_OUTPUT_BASE_DEVICE_PATH, this->_class_instance); return ret; } @@ -605,13 +552,11 @@ void PWMDriverWrapper::mixerChanged() { if (_mixing_output.mixers()) { // only update trims if mixer loaded updatePWMParamTrim(); } - if(!_mixing_output.updateSubscriptions(true)) { // change to proper wq - ScheduleNow(); - } + _mixing_output.updateSubscriptions(false); } extern "C" __EXPORT int pca9685_pwm_out_main(int argc, char *argv[]); int pca9685_pwm_out_main(int argc, char *argv[]){ return PWMDriverWrapper::main(argc, argv); -} \ No newline at end of file +}