dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

New board Scumaker AirPi HAT for Raspberry Pi B series

This commit is contained in:
SalimTerryLi 2020-10-01 23:22:47 +08:00 committed by GitHub
parent 967f741a0e
commit 25eca31e3a
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
19 changed files with 497 additions and 161 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
.ci/Jenkinsfile-compile
View File

@ -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
]

4
Tools/docker_run.sh
View File

@ -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

1
boards/px4/fmu-v3/default.cmake
View File

@ -49,6 +49,7 @@ px4_add_board(
optical_flow/px4flow
#osd
pca9685
pca9685_pwm_out
#power_monitor/ina226
#protocol_splitter
pwm_input

1
boards/px4/fmu-v4/default.cmake
View File

@ -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

1
boards/px4/fmu-v4pro/default.cmake
View File

@ -42,6 +42,7 @@ px4_add_board(
optical_flow/px4flow
#osd
pca9685
pca9685_pwm_out
power_monitor/ina226
#protocol_splitter
pwm_input

1
boards/px4/fmu-v5/default.cmake
View File

@ -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

1
boards/px4/fmu-v5x/default.cmake
View File

@ -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

47
boards/scumaker/airpi2/cmake/upload.cmake Normal file
View File

@ -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
)

93
boards/scumaker/airpi2/default.cmake Normal file
View File

@ -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
)

37
boards/scumaker/airpi2/src/CMakeLists.txt Normal file
View File

@ -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
)

60
boards/scumaker/airpi2/src/board_config.h Normal file
View File

@ -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>

39
boards/scumaker/airpi2/src/i2c.cpp Normal file
View File

@ -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),
};

42
boards/scumaker/airpi2/src/spi.cpp Normal file
View File

@ -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),
}),
};

64
posix-configs/rpi/airpi2_mc.config Normal file
View File

@ -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

2
src/drivers/pca9685_pwm_out/CMakeLists.txt
View File

@ -31,7 +31,7 @@
#
############################################################################
px4_add_module(
MODULE drivers__pca9685
MODULE drivers__pca9685_pwm_out
MAIN pca9685_pwm_out
COMPILE_FLAGS
SRCS

6
src/drivers/pca9685_pwm_out/PCA9685.cpp
View File

@ -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");

8
src/drivers/pca9685_pwm_out/PCA9685.h
View File

@ -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;

251
src/drivers/pca9685_pwm_out/main.cpp
View File

@ -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);
uint8_t param_group, param_index;
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_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] = (int16_t)(10000 * pval);
} else {
PX4_DEBUG("PARAM_INVALID: %s", pname);
}
sprintf(pname, pname_format_aux_trim, i + 1);
param_h = param_find(pname);
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,9 +552,7 @@ 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[]);