forked from Archive/PX4-Autopilot
New board Scumaker AirPi HAT for Raspberry Pi B series
This commit is contained in:
parent
967f741a0e
commit
25eca31e3a
|
@ -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
|
||||
]
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -49,6 +49,7 @@ px4_add_board(
|
|||
optical_flow/px4flow
|
||||
#osd
|
||||
pca9685
|
||||
pca9685_pwm_out
|
||||
#power_monitor/ina226
|
||||
#protocol_splitter
|
||||
pwm_input
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -42,6 +42,7 @@ px4_add_board(
|
|||
optical_flow/px4flow
|
||||
#osd
|
||||
pca9685
|
||||
pca9685_pwm_out
|
||||
power_monitor/ina226
|
||||
#protocol_splitter
|
||||
pwm_input
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
)
|
|
@ -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
|
||||
)
|
|
@ -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
|
||||
)
|
|
@ -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 <system_config.h>
|
||||
#include <px4_platform_common/board_common.h>
|
|
@ -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 <px4_arch/i2c_hw_description.h>
|
||||
|
||||
constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = {
|
||||
initI2CBusExternal(0),
|
||||
initI2CBusInternal(1),
|
||||
};
|
|
@ -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 <px4_arch/spi_hw_description.h>
|
||||
#include <drivers/drv_sensor.h>
|
||||
|
||||
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),
|
||||
}),
|
||||
};
|
|
@ -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
|
|
@ -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
|
||||
|
||||
|
|
|
@ -31,7 +31,7 @@
|
|||
#
|
||||
############################################################################
|
||||
px4_add_module(
|
||||
MODULE drivers__pca9685
|
||||
MODULE drivers__pca9685_pwm_out
|
||||
MAIN pca9685_pwm_out
|
||||
COMPILE_FLAGS
|
||||
SRCS
|
||||
|
|
|
@ -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");
|
||||
|
|
|
@ -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;
|
||||
|
||||
|
|
|
@ -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;i<num_outputs;++i){
|
||||
sprintf(buf,"%s %.4d",buf,outputs[i]);
|
||||
}
|
||||
PX4_INFO("%s",buf);*/
|
||||
return pca9685->updatePWM(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);
|
||||
}
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue