Skip to content
Snippets Groups Projects
Commit 2ece14ba authored by Bob-F's avatar Bob-F Committed by Beat Küng
Browse files

Port PX4 to BeagleBone Blue Board using library librobotcontrol instead of a submodule

parent e989c802
No related branches found
No related tags found
No related merge requests found
Showing
with 1123 additions and 6 deletions
# This file is shared between posix_bbblue_native.cmake
# and posix_bbblue_cross.cmake.
# This definition allows to differentiate if this just the usual POSIX build
# or if it is for the bbblue.
add_definitions(
-D__PX4_POSIX_BBBLUE
-D__PX4_POSIX
-D__DF_LINUX # For DriverFramework
-D__DF_BBBLUE # For DriverFramework
-DRC_AUTOPILOT_EXT # Enable extensions in Robotics Cape Library
# -DDEBUG_BUILD
)
#optional __DF_BBBLUE_USE_RC_BMP280_IMP
add_definitions(
-D__DF_BBBLUE_USE_RC_BMP280_IMP
)
#optional __PX4_BBBLUE_DEFAULT_MAVLINK_WIFI, default is "SoftAp"
#add_definitions(
# -D__PX4_BBBLUE_DEFAULT_MAVLINK_WIFI="wlan"
#)
set(config_module_list
#
# Board support modules
#
#drivers/barometer
drivers/batt_smbus
drivers/differential_pressure
drivers/distance_sensor
#drivers/telemetry
#drivers/boards
modules/sensors
platforms/posix/drivers/df_mpu9250_wrapper
platforms/posix/drivers/df_bmp280_wrapper
#
# System commands
#
systemcmds/param
systemcmds/led_control
systemcmds/mixer
systemcmds/ver
systemcmds/esc_calib
systemcmds/reboot
systemcmds/topic_listener
systemcmds/tune_control
systemcmds/perf
#
# Estimation modules
#
modules/attitude_estimator_q
modules/position_estimator_inav
modules/local_position_estimator
modules/landing_target_estimator
modules/ekf2
#
# Vehicle Control
#
modules/fw_att_control
modules/fw_pos_control_l1
modules/gnd_att_control
modules/gnd_pos_control
modules/mc_att_control
modules/mc_pos_control
modules/vtol_att_control
#
# Library modules
#
modules/logger
modules/commander
modules/dataman
modules/land_detector
modules/navigator
modules/mavlink
#
# PX4 drivers
#
drivers/linux_sbus
drivers/gps
drivers/bbblue_adc
drivers/linux_gpio
drivers/linux_pwm_out
drivers/pwm_out_sim
)
#
# DriverFramework driver
#
set(config_df_driver_list
mpu9250
bmp280
)
include(configs/posix_bbblue_common)
SET(CMAKE_TOOLCHAIN_FILE ${PX4_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-linux-gnueabihf.cmake)
include(configs/posix_bbblue_common)
add_definitions(
-D __DF_BBBLUE
)
set(CMAKE_TOOLCHAIN_FILE ${PX4_SOURCE_DIR}/cmake/toolchains/Toolchain-native.cmake)
......@@ -69,6 +69,24 @@ if ("${BOARD}" STREQUAL "rpi")
USES_TERMINAL
)
elseif ("${BOARD}" STREQUAL "bbblue")
target_link_libraries(px4 PRIVATE robotcontrol)
add_custom_target(upload
COMMAND scp -r ${PX4_SOURCE_DIR}/posix-configs/bbblue/*.config debian@BBBluePX4:/home/debian/px4/posix-configs
COMMAND scp -r ${PX4_SOURCE_DIR}/ROMFS $<TARGET_FILE:px4> debian@BBBluePX4:/home/debian/px4
DEPENDS px4
COMMENT "uploading px4 and data files"
USES_TERMINAL
)
add_custom_target(upload_px4
COMMAND scp -r $<TARGET_FILE:px4> debian@BBBluePX4:/home/debian/px4
DEPENDS px4
COMMENT "uploading px4"
USES_TERMINAL
)
elseif ("${BOARD}" STREQUAL "bebop")
add_custom_target(upload
......
......@@ -283,6 +283,20 @@ function(px4_os_add_flags)
elseif ("${BOARD}" STREQUAL "bebop")
# TODO: Wmissing-field-initializers ignored on older toolchain, can be removed eventually
list(APPEND added_cxx_flags -Wno-missing-field-initializers)
elseif ("${BOARD}" STREQUAL "bbblue")
set(BBBLUE_COMPILE_FLAGS -mcpu=cortex-a8 -mfpu=neon -mfloat-abi=hard -mtune=cortex-a8)
list(APPEND added_c_flags ${BBBLUE_COMPILE_FLAGS})
list(APPEND added_cxx_flags ${BBBLUE_COMPILE_FLAGS})
# TODO: Wmissing-field-initializers ignored on older toolchain, can be removed eventually
# On cross compile host system and native build system:
# a) install robotcontrol.h and rc/* into /usr/local/include
# b) install pre-built native (ARM) version of librobotcontrol.* into /usr/local/lib
list(APPEND added_cxx_flags -I/usr/local/include -Wno-missing-field-initializers)
list(APPEND added_c_flags -I/usr/local/include)
list(APPEND added_exe_linker_flags -L/usr/local/lib)
endif()
# output
......
......@@ -335,8 +335,10 @@ int main(int argc, char **argv)
string data_path;
string node_name;
bool skippingOutputRedirect = false;
// parse arguments
while (index < argc) {
while (index < argc && !skippingOutputRedirect) {
//cout << "arg: " << index << " : " << argv[index] << endl;
if (argv[index][0] == '-') {
......@@ -366,6 +368,9 @@ int main(int argc, char **argv)
cout << "node name: " << node_name << endl;
}
} else if (strchr(argv[index], '>')) {
skippingOutputRedirect = true;
} else {
//cout << "positional argument" << endl;
......
# config for a quad
# modified from ../rpi/px4.config
uorb start
param load
# Auto-start script index. Defines the auto-start script used to bootstrap the system.
# It seems that SYS_AUTOSTART does not work as intended on posix platform.
# For now, find the corresponding settings, and manually set them in ground control.
#
# 4001: Generic Quadrotor X; 4011: DJI Flame Wheel F450
param set SYS_AUTOSTART 4011
# DJI ESCs do not support calibration and need higher PWM_MIN
# http://www.dji.com/e2000/info indicates E2000 Operating Pulse Width: 1120 to 1920 μs
# It seems that all latest DJI ESC have the same range.
# Note that the setting here applies to all PWM channels.
# param set PWM_MIN 1120
# param set PWM_MAX 1920
# Not using DJI 430 LITE ESC anymore due to its hiccups:
# each random motor stop would cause a scary flip in the fly
# Replacing with 4 BLHeli32 (Wraith32 V2) ESCs solved the main problem in BBBlue porting
# Broadcast heartbeats on local network. This allows a ground control station
# to automatically find the drone on the local network.
param set MAV_BROADCAST 1
# MAV_TYPE: 1 Fixed wing aircraft, 2 Quadrotor
param set MAV_TYPE 2
# Set multicopter estimator group, 1 local_position_estimator, attitude_estimator_q, 2 ekf2
param set SYS_MC_EST_GROUP 2
# Three possible main power battery sources for BBBlue:
# 1. onboard 2S LiPo battery connector, which is connect to ADC channel 6
# 2. onboard 9-18V DC Jack, which is connect to ADC channel 5. This is the board default.
# 3. other power source (e.g., LiPo battery more than 4S/18V).
# Scale the voltage to below 1.8V and connect it to ADC channel # 0, 1, 2 or 3.
param set BAT_ADC_CHANNEL 5
# 12-bit 1.8V ADC, 1.8V / 2^12 = 0.000439453125 V/CNT
param set BAT_CNT_V_VOLT 0.0004394531
# Battery voltage scale factor, from BBBlue schematics: (4.7K + 47K) / 4.7K = 11
param set BAT_V_DIV 11.0
#param set BAT_CNT_V_CURR 0.001
#param set BAT_A_PER_V 15.391030303
dataman start
df_bmp280_wrapper start -D /dev/i2c-2
df_mpu9250_wrapper start
# options: -R rotation
gps start -d /dev/ttyS2 -s -p ubx
#rgbled start -b 1
bbblue_adc start
bbblue_adc test
sensors start
commander start
navigator start
ekf2 start
#land_detector start multicopter
mc_pos_control start
mc_att_control start
#fw_att_control start
#fw_pos_control_l1 start
mavlink start -x -u 14556 -r 1000000
# -n name of wifi interface: SoftAp, wlan or your custom interface,
# e.g., -n wlan . The default on BBBlue is SoftAp
sleep 1
mavlink stream -u 14556 -s HIGHRES_IMU -r 20
mavlink stream -u 14556 -s ATTITUDE -r 20
mavlink stream -u 14556 -s MANUAL_CONTROL -r 10
linux_sbus start -d /dev/ttyS4 -c 16
# DSM2 port is mapped to /dev/ttyS4 (default for linux_sbus)
# default: ROMFS/px4fmu_common/mixers/quad_x.main.mix, 8 output channels
linux_pwm_out start -p bbblue_rc -m ROMFS/px4fmu_common/mixers/quad_x.main.mix
#linux_pwm_out start -p bbblue_rc -m ROMFS/px4fmu_common/mixers/AETRFG.main.mix
logger start -t -b 200
mavlink boot_complete
# config for fixed wing (FW)
# modified from ./px4.config, switch att/pos_control & mixer
uorb start
param load
# Auto-start script index. Defines the auto-start script used to bootstrap the system.
# It seems that SYS_AUTOSTART does not work as intended on posix platform.
# For now, find the corresponding settings, and manually set them in ground control.
#
# 4001: Generic Quadrotor X; 4011: DJI Flame Wheel F450
param set SYS_AUTOSTART 4011
# DJI ESCs do not support calibration and need higher PWM_MIN
# http://www.dji.com/e2000/info indicates E2000 Operating Pulse Width: 1120 to 1920 μs
# It seems that all latest DJI ESC have the same range.
# Note that the setting here applies to all PWM channels.
# param set PWM_MIN 1120
# param set PWM_MAX 1920
# Not using DJI 430 LITE ESC anymore due to its hiccups:
# each random motor stop would cause a scary flip in the fly
# Replacing with 4 BLHeli32 (Wraith32 V2) ESCs solved the main problem in BBBlue porting
# Broadcast heartbeats on local network. This allows a ground control station
# to automatically find the drone on the local network.
param set MAV_BROADCAST 1
# MAV_TYPE: 1 Fixed wing aircraft, 2 Quadrotor
param set MAV_TYPE 2
# Set multicopter estimator group, 1 local_position_estimator, attitude_estimator_q, 2 ekf2
param set SYS_MC_EST_GROUP 2
# Three possible main power battery sources for BBBlue:
# 1. onboard 2S LiPo battery connector, which is connect to ADC channel 6
# 2. onboard 9-18V DC Jack, which is connect to ADC channel 5. This is the board default.
# 3. other power source (e.g., LiPo battery more than 4S/18V).
# Scale the voltage to below 1.8V and connect it to ADC channel # 0, 1, 2 or 3.
param set BAT_ADC_CHANNEL 5
# 12-bit 1.8V ADC, 1.8V / 2^12 = 0.000439453125 V/CNT
param set BAT_CNT_V_VOLT 0.0004394531
# Battery voltage scale factor, from BBBlue schematics: (4.7K + 47K) / 4.7K = 11
param set BAT_V_DIV 11.0
#param set BAT_CNT_V_CURR 0.001
#param set BAT_A_PER_V 15.391030303
dataman start
df_bmp280_wrapper start -D /dev/i2c-2
df_mpu9250_wrapper start
# options: -R rotation
gps start -d /dev/ttyS2 -s -p ubx
#rgbled start -b 1
bbblue_adc start
bbblue_adc test
sensors start
commander start
navigator start
ekf2 start
#land_detector start multicopter
#mc_pos_control start
#mc_att_control start
fw_att_control start
fw_pos_control_l1 start
mavlink start -x -u 14556 -r 1000000
# -n name of wifi interface: SoftAp, wlan or your custom interface,
# e.g., -n wlan . The default on BBBlue is SoftAp
sleep 1
mavlink stream -u 14556 -s HIGHRES_IMU -r 20
mavlink stream -u 14556 -s ATTITUDE -r 20
mavlink stream -u 14556 -s MANUAL_CONTROL -r 10
linux_sbus start -d /dev/ttyS4 -c 16
# DSM2 port is mapped to /dev/ttyS4 (default for linux_sbus)
# default: ROMFS/px4fmu_common/mixers/quad_x.main.mix, 8 output channels
#linux_pwm_out start -p bbblue_rc -m ROMFS/px4fmu_common/mixers/quad_x.main.mix
linux_pwm_out start -p bbblue_rc -m ROMFS/px4fmu_common/mixers/AETRFG.main.mix
logger start -t -b 200
mavlink boot_complete
############################################################################
#
# Copyright (c) 2015-2017 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 ("${BOARD}" STREQUAL "bbblue")
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wno-error")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-error")
px4_add_module(
MODULE drivers__bbblue_adc
MAIN bbblue_adc
COMPILE_FLAGS
SRCS
bbblue_adc.cpp
# Previous board specific code is now precluded from build after
# the following was added to Frimware/CMakeLists.txt:
#
# add_subdirectory(src/drivers/boards EXCLUDE_FROM_ALL)
#
# so include bbblue board specific code here:
../boards/bbblue/init.c
DEPENDS
platforms__common
)
endif()
/****************************************************************************
*
* Copyright (c) 2012-2018 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 bbblue_adc.cpp
*
* BBBlue ADC Driver
*
*/
#include <px4_config.h>
#include <px4_tasks.h>
#include <px4_posix.h>
#include <drivers/drv_adc.h>
#include <VirtDevObj.hpp>
#include <unistd.h>
#include <stdio.h>
#include <poll.h>
#include <string.h>
#ifdef __DF_BBBLUE
#include <robotcontrol.h>
#include <board_config.h>
#endif
#define ADC_BASE_DEV_PATH "/dev/null"
#define BBBLUE_MAX_ADC_CHANNELS (6)
#define BBBLUE_MAX_ADC_USER_CHANNELS (4)
#define BBBLUE_ADC_DC_JACK_VOLTAGE_CHANNEL (5)
#define BBBLUE_ADC_LIPO_BATTERY_VOLTAGE_CHANNEL (6)
__BEGIN_DECLS
__EXPORT int bbblue_adc_main(int argc, char *argv[]);
__END_DECLS
class BBBlueADC: public DriverFramework::VirtDevObj
{
public:
BBBlueADC();
virtual ~BBBlueADC();
virtual int init();
virtual ssize_t devRead(void *buf, size_t count) override;
virtual int devIOCTL(unsigned long request, unsigned long arg) override;
protected:
virtual void _measure() override;
private:
pthread_mutex_t _samples_lock;
px4_adc_msg_t _samples[BBBLUE_MAX_ADC_CHANNELS];
};
BBBlueADC::BBBlueADC()
: DriverFramework::VirtDevObj("bbblue_adc", ADC0_DEVICE_PATH, ADC_BASE_DEV_PATH, 1e6 / 100)
{
pthread_mutex_init(&_samples_lock, NULL);
}
BBBlueADC::~BBBlueADC()
{
pthread_mutex_destroy(&_samples_lock);
rc_cleaning();
}
void BBBlueADC::_measure()
{
#ifdef __DF_BBBLUE
px4_adc_msg_t tmp_samples[BBBLUE_MAX_ADC_CHANNELS];
for (int i = 0; i < BBBLUE_MAX_ADC_USER_CHANNELS; ++i) {
tmp_samples[i].am_channel = i;
tmp_samples[i].am_data = rc_adc_read_raw(i);
}
tmp_samples[BBBLUE_MAX_ADC_USER_CHANNELS].am_channel = BBBLUE_ADC_LIPO_BATTERY_VOLTAGE_CHANNEL;
tmp_samples[BBBLUE_MAX_ADC_USER_CHANNELS].am_data = rc_adc_read_raw(BBBLUE_ADC_LIPO_BATTERY_VOLTAGE_CHANNEL);
tmp_samples[BBBLUE_ADC_DC_JACK_VOLTAGE_CHANNEL].am_channel = BBBLUE_ADC_DC_JACK_VOLTAGE_CHANNEL;
tmp_samples[BBBLUE_ADC_DC_JACK_VOLTAGE_CHANNEL].am_data = rc_adc_read_raw(BBBLUE_ADC_DC_JACK_VOLTAGE_CHANNEL);
pthread_mutex_lock(&_samples_lock);
memcpy(&_samples, &tmp_samples, sizeof(tmp_samples));
pthread_mutex_unlock(&_samples_lock);
#endif
}
int BBBlueADC::init()
{
rc_init();
int ret = DriverFramework::VirtDevObj::init();
if (ret != PX4_OK) {
PX4_ERR("init failed");
return ret;
}
_measure(); // start the initial conversion so that the test command right
// after the start command can return values
return PX4_OK;
}
int BBBlueADC::devIOCTL(unsigned long request, unsigned long arg)
{
return -ENOTTY;
}
ssize_t BBBlueADC::devRead(void *buf, size_t count)
{
const size_t maxsize = sizeof(_samples);
int ret;
if (count > maxsize) {
count = maxsize;
}
ret = pthread_mutex_trylock(&_samples_lock);
if (ret != 0) {
return 0;
}
memcpy(buf, &_samples, count);
pthread_mutex_unlock(&_samples_lock);
return count;
}
static BBBlueADC *instance = nullptr;
int bbblue_adc_main(int argc, char *argv[])
{
int ret;
if (argc < 2) {
PX4_WARN("usage: {start|stop|test}");
return PX4_ERROR;
}
if (!strcmp(argv[1], "start")) {
if (instance) {
PX4_WARN("already started");
return PX4_OK;
}
instance = new BBBlueADC;
if (!instance) {
PX4_WARN("not enough memory");
return PX4_ERROR;
}
if (instance->init() != PX4_OK) {
delete instance;
instance = nullptr;
PX4_WARN("init failed");
return PX4_ERROR;
}
PX4_INFO("BBBlueADC started");
return PX4_OK;
} else if (!strcmp(argv[1], "stop")) {
if (!instance) {
PX4_WARN("already stopped");
return PX4_OK;
}
delete instance;
instance = nullptr;
return PX4_OK;
} else if (!strcmp(argv[1], "test")) {
if (!instance) {
PX4_ERR("start first");
return PX4_ERROR;
}
px4_adc_msg_t adc_msgs[BBBLUE_MAX_ADC_CHANNELS];
ret = instance->devRead((char *)&adc_msgs, sizeof(adc_msgs));
if (ret < 0) {
PX4_ERR("ret: %s (%d)\n", strerror(ret), ret);
return ret;
} else if (ret != sizeof(adc_msgs)) {
PX4_ERR("incomplete read: %d expected %d", ret, sizeof(adc_msgs));
return ret;
}
for (int i = 0; i < BBBLUE_MAX_ADC_USER_CHANNELS; ++i) {
PX4_INFO("ADC channel: %d; value: %d", (int)adc_msgs[i].am_channel,
adc_msgs[i].am_data);
}
for (int i = BBBLUE_MAX_ADC_USER_CHANNELS; i < BBBLUE_MAX_ADC_CHANNELS; ++i) {
PX4_INFO("ADC channel: %d; value: %d, voltage: %6.2f V", (int)adc_msgs[i].am_channel,
adc_msgs[i].am_data, adc_msgs[i].am_data* 1.8/4095.0 * 11.0);
}
return PX4_OK;
} else {
PX4_WARN("action (%s) not supported", argv[1]);
return PX4_ERROR;
}
return PX4_OK;
}
############################################################################
#
# Copyright (c) 2018 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 ("${BOARD}" STREQUAL "bbblue")
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wno-error")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-error")
px4_add_library(drivers_board
init.c
)
endif()
\ No newline at end of file
/****************************************************************************
*
* Copyright (c) 2018 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
*
* BBBLUE internal definitions
*/
#pragma once
#ifndef BOARD_CONFIG_H
#define BOARD_CONFIG_H
#define BOARD_OVERRIDE_UUID "BBBLUEID " // must be of length 12 (PX4_CPU_UUID_BYTE_LENGTH)
#define BOARD_OVERRIDE_MFGUID BOARD_OVERRIDE_UUID
#define BOARD_NAME "BBBLUE"
#define BOARD_BATTERY1_V_DIV (11.0f)
//#define BOARD_BATTERY1_A_PER_V (15.391030303f)
// Battery ADC channels
#define ADC_BATTERY_VOLTAGE_CHANNEL 5
#define ADC_BATTERY_CURRENT_CHANNEL ((uint8_t)(-1))
#define ADC_AIRSPEED_VOLTAGE_CHANNEL ((uint8_t)(-1))
#define BOARD_HAS_NO_BOOTLOADER
#define BOARD_MAX_LEDS 4 // Number external of LED's this board has
/*
* I2C busses
*/
#define PX4_I2C_BUS_EXPANSION 1
#define PX4_NUMBER_I2C_BUSES 1
#include <system_config.h>
#include "../common/board_common.h"
#ifdef __cplusplus
extern "C" {
#endif
int rc_init(void);
void rc_cleaning(void);
#ifdef __cplusplus
}
#endif
#ifdef __RC_V0_3
#define rc_i2c_lock_bus rc_i2c_claim_bus
#define rc_i2c_unlock_bus rc_i2c_release_bus
#define rc_i2c_get_lock rc_i2c_get_in_use_state
#define rc_bmp_init rc_initialize_barometer
#define rc_adc_read_raw rc_adc_raw
#define rc_servo_send_pulse_us rc_send_servo_pulse_us
#define rc_filter_empty rc_empty_filter
#define rc_filter_march rc_march_filter
#define rc_filter_prefill_inputs rc_prefill_filter_inputs
#define rc_filter_prefill_outputs rc_prefill_filter_outputs
#define rc_filter_butterworth_lowpass rc_butterworth_lowpass
/**
* struct to hold the data retreived during one read of the barometer.
*/
typedef struct rc_bmp_data_t{
float temp_c; ///< temperature in degrees celcius
float alt_m; ///< altitude in meters
float pressure_pa; ///< current pressure in pascals
} rc_bmp_data_t;
#ifdef __cplusplus
extern "C" {
#endif
int rc_bmp_read(rc_bmp_data_t* data);
#ifdef __cplusplus
}
#endif
#endif
#endif // BOARD_CONFIG_H
/****************************************************************************
*
* Copyright (c) 2018 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 init.c
*
* BBBLUE specific initialization
*/
#include <stddef.h>
#include <px4_log.h>
#include <robotcontrol.h>
#include "board_config.h"
// initialize roboticscape library similar to the deprecated rc_initialize()
int rc_init(void)
{
#ifdef __RC_V0_3
return rc_initialize();
#else
#ifdef __DF_BBBLUE
if (rc_get_state() == RUNNING) { return 0; }
PX4_INFO("Initializing Robotics Cape library ...");
// make sure another instance isn't running
rc_kill_existing_process(2.0f);
// make PID file to indicate your project is running
rc_make_pid_file();
// start state as Uninitialized
rc_set_state(UNINITIALIZED);
// initialize pinmux
/*
if (rc_pinmux_set_default()) {
PX4_ERR("rc_init failed to run rc_pinmux_set_default()");
return -1;
}
// rc_pinmux_set_default() includes: rc_pinmux_set(DSM_HEADER_PIN, PINMUX_UART);
*/
/*
// Due to device tree issue, rc_pinmux_set_default() currently does not work correctly
// with kernel 4.14, use a simplified version for now
//
// shared pins
int ret = 0;
ret |= rc_pinmux_set(DSM_HEADER_PIN, PINMUX_UART);
ret |= rc_pinmux_set(GPS_HEADER_PIN_3, PINMUX_UART);
ret |= rc_pinmux_set(GPS_HEADER_PIN_4, PINMUX_UART);
ret |= rc_pinmux_set(UART1_HEADER_PIN_3, PINMUX_UART);
ret |= rc_pinmux_set(UART1_HEADER_PIN_4, PINMUX_UART);
if (ret != 0) {
PX4_ERR("rc_init failed to set default pinmux");
return -1;
}
*/
// no direct equivalent of configure_gpio_pins()
if (rc_adc_init()) {
PX4_ERR("rc_init failed to run rc_adc_init()");
return -1;
}
if (rc_servo_init()) { // Configures the PRU to send servo pulses
PX4_ERR("rc_init failed to run rc_servo_init()");
return -1;
}
if(rc_servo_power_rail_en(1)) { // Turning On 6V Servo Power Rail
PX4_ERR("rc_init failed to run rc_servo_power_rail_en(1)");
return -1;
}
//i2c, barometer and mpu will be initialized later
rc_set_state(RUNNING);
// wait to let threads start up
rc_usleep(10000);
#endif
return 0;
#endif
}
void rc_cleaning(void)
{
#ifdef __RC_V0_3
rc_cleanup(); return ;
#else
#ifdef __DF_BBBLUE
if (rc_get_state() == EXITING) { return; }
rc_set_state(EXITING);
rc_adc_cleanup();
rc_servo_power_rail_en(0);
rc_servo_cleanup();
rc_remove_pid_file();
#endif
#endif
}
#ifdef __RC_V0_3
int rc_bmp_read(rc_bmp_data_t* data) {
int rtn = rc_read_barometer();
data->temp_c = rc_bmp_get_temperature();
data->alt_m = rc_bmp_get_altitude_m();
data->pressure_pa = rc_bmp_get_pressure_pa();
return rtn;
}
#endif
......@@ -30,6 +30,7 @@
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
px4_add_module(
MODULE drivers__linux_pwm_out
MAIN linux_pwm_out
......@@ -39,6 +40,7 @@ px4_add_module(
navio_sysfs.cpp
linux_pwm_out.cpp
ocpoc_mmap.cpp
bbblue_pwm_rc.cpp
DEPENDS
pwm_limit
)
......
/****************************************************************************
*
* Copyright (c) 2018 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.
*
****************************************************************************/
#ifdef __DF_BBBLUE
#include <fcntl.h>
#include <errno.h>
#include <px4_log.h>
#include <robotcontrol.h>
#include <board_config.h>
#include "bbblue_pwm_rc.h"
using namespace linux_pwm_out;
BBBlueRcPWMOut::BBBlueRcPWMOut(int max_num_outputs) : _num_outputs(max_num_outputs)
{
if (_num_outputs > MAX_NUM_PWM) {
PX4_WARN("number of outputs too large. Setting to %i", MAX_NUM_PWM);
_num_outputs = MAX_NUM_PWM;
}
}
BBBlueRcPWMOut::~BBBlueRcPWMOut()
{
rc_cleaning();
}
int BBBlueRcPWMOut::init()
{
rc_init();
return 0;
}
int BBBlueRcPWMOut::send_output_pwm(const uint16_t *pwm, int num_outputs)
{
if (num_outputs > _num_outputs) {
num_outputs = _num_outputs;
}
int ret = 0;
// pwm[ch] is duty_cycle in us
for (int ch = 0; ch < num_outputs; ++ch) {
ret += rc_servo_send_pulse_us(ch+1, pwm[ch]); // converts to 1-based channel #
}
return ret;
}
#endif // __DF_BBBLUE
/****************************************************************************
*
* Copyright (c) 2018 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.
*
****************************************************************************/
#pragma once
#include "common.h"
namespace linux_pwm_out
{
/**
** class BBBlueRcPWMOut
* PWM output class for BeagleBone Blue with Robotics Cape Library
*
* Ref: https://github.com/StrawsonDesign/Robotics_Cape_Installer
* http://www.strawsondesign.com/#!manual-servos
*/
class BBBlueRcPWMOut : public PWMOutBase
{
public:
BBBlueRcPWMOut(int max_num_outputs);
virtual ~BBBlueRcPWMOut();
int init() override;
int send_output_pwm(const uint16_t *pwm, int num_outputs) override;
private:
static const int MAX_NUM_PWM = 8;
static const int MIN_FREQUENCY_PWM = 40;
int _num_outputs;
};
}
......@@ -58,6 +58,7 @@
#include "navio_sysfs.h"
#include "PCA9685.h"
#include "ocpoc_mmap.h"
#include "bbblue_pwm_rc.h"
namespace linux_pwm_out
{
......@@ -233,6 +234,12 @@ void task_main(int argc, char *argv[])
PX4_INFO("Starting PWM output in ocpoc_mmap mode");
pwm_out = new OcpocMmapPWMOut(_max_num_outputs);
#ifdef __DF_BBBLUE
} else if (strcmp(_protocol, "bbblue_rc") == 0) {
PX4_INFO("Starting PWM output in bbblue_rc mode");
pwm_out = new BBBlueRcPWMOut(_max_num_outputs);
#endif
} else { /* navio */
PX4_INFO("Starting PWM output in Navio mode");
pwm_out = new NavioSysfsPWMOut(_device, _max_num_outputs);
......@@ -493,7 +500,7 @@ void usage()
PX4_INFO(" (default /sys/class/pwm/pwmchip0)");
PX4_INFO(" -m mixerfile : path to mixerfile");
PX4_INFO(" (default ROMFS/px4fmu_common/mixers/quad_x.main.mix)");
PX4_INFO(" -p protocol : driver output protocol (navio|pca9685|ocpoc_mmap)");
PX4_INFO(" -p protocol : driver output protocol (navio|pca9685|ocpoc_mmap|bbblue_rc)");
PX4_INFO(" (default is navio)");
PX4_INFO(" -n num_outputs : maximum number of outputs the driver should use");
PX4_INFO(" (default is 8)");
......
......@@ -42,7 +42,8 @@ int RcInput::init()
/**
* initialize the data of each channel
*/
for (i = 0; i < input_rc_s::RC_INPUT_MAX_CHANNELS; ++i) {
for (i = 0; i < input_rc_s::RC_INPUT_MAX_CHANNELS && i<16; ++i) {
//_data.values defined as int[16] and RC_INPUT_MAX_CHANNELS (=18) > 16
_data.values[i] = UINT16_MAX;
}
......@@ -104,7 +105,7 @@ int RcInput::start(char *device, int channels)
{
int result = 0;
strcpy(_device, device);
PX4_WARN("Device %s , channels: %d \n", device, channels);
PX4_INFO("Device %s , channels: %d \n", device, channels);
_channels = channels;
result = init();
......@@ -231,6 +232,7 @@ void RcInput::_measure(void)
_data.rc_failsafe = (_sbusData[23] & (1 << 3)) ? true : false;
_data.rc_lost = (_sbusData[23] & (1 << 2)) ? true : false;
_data.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_SBUS;
orb_publish(ORB_ID(input_rc), _rcinput_pub, &_data);
}
//---------------------------------------------------------------------------------------------------------//
......
......@@ -685,6 +685,9 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status,
#elif defined(__PX4_POSIX_RPI)
PX4_WARN("Preflight checks for mag, acc, gyro always pass on RPI");
checkSensors = false;
#elif defined(__PX4_POSIX_BBBLUE)
PX4_WARN("Preflight checks for mag, acc, gyro always pass on BBBLUE");
checkSensors = false;
#elif defined(__PX4_POSIX_BEBOP)
PX4_WARN("Preflight checks always pass on Bebop.");
checkSensors = false;
......
......@@ -279,7 +279,7 @@ int led_init()
led_control.timestamp = hrt_absolute_time();
led_control_pub = orb_advertise_queue(ORB_ID(led_control), &led_control, LED_UORB_QUEUE_LENGTH);
#ifndef CONFIG_ARCH_BOARD_RPI
#if !defined(CONFIG_ARCH_BOARD_RPI) && !defined(CONFIG_ARCH_BOARD_BBBLUE)
/* first open normal LEDs */
DevMgr::getHandle(LED0_DEVICE_PATH, h_leds);
......@@ -313,7 +313,7 @@ int led_init()
void led_deinit()
{
orb_unadvertise(led_control_pub);
#ifndef CONFIG_ARCH_BOARD_RPI
#if !defined(CONFIG_ARCH_BOARD_RPI) && !defined(CONFIG_ARCH_BOARD_BBBLUE)
DevMgr::releaseHandle(h_leds);
#endif
}
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment