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

delete parrot bebop board support 

This target was never fully supported and is heavily dependent on a number of DriverFramework drivers that have no in tree equivalents (bebop bus, flow, rangefinder, etc). Deleting this will make it easier to fully drop DriverFramework shortly.
This commit is contained in:
Daniel Agar 2020-01-05 19:46:51 -05:00 committed by GitHub
parent ce1e9762b0
commit 44f9de5e37
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
28 changed files with 3 additions and 2483 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", "parrot_bebop_default", "px4_raspberrypi_default"],
target: ["aerotenna_ocpoc_default", "beaglebone_blue_default", "emlid_navio2_default", "px4_raspberrypi_default"],
image: docker_images.armhf,
archive: false
]

1
README.md
View File

@ -73,7 +73,6 @@ This repository contains code supporting these boards:
* [Snapdragon Flight](https://docs.px4.io/master/en/flight_controller/snapdragon_flight.html)
* [Intel Aero](https://docs.px4.io/master/en/flight_controller/intel_aero.html)
* [Raspberry PI with Navio 2](https://docs.px4.io/master/en/flight_controller/raspberry_pi_navio2.html)
* [Parrot Bebop 2](https://dev.px4.io/master/en/advanced/parrot_bebop.html)
* FMUv2
* [Pixhawk](https://docs.px4.io/master/en/flight_controller/pixhawk.html)
* [Pixfalcon](https://docs.px4.io/master/en/flight_controller/pixfalcon.html)

43
ROMFS/px4fmu_common/init.d/airframes/4013_bebop
View File

@ -1,43 +0,0 @@
#!/bin/sh
#
# @name Parrot Bebop Frame
#
# @type Quadrotor x
# @class Copter
#
# @board px4_fmu-v2 exclude
# @board px4_fmu-v3 exclude
# @board px4_fmu-v4 exclude
# @board px4_fmu-v4pro exclude
# @board px4_fmu-v5 exclude
# @board px4_fmu-v5x exclude
# @board intel_aerofc-v1 exclude
#
# @maintainer Michael Schaeuble
#
sh /etc/init.d/rc.mc_defaults
#
# Load default params for this platform
#
if [ $AUTOCNF = yes ]
then
# Set all params here, then disable autoconfig
param set MC_ROLL_P 6.5
param set MC_ROLLRATE_P 0.109999999403953552
param set MC_ROLLRATE_I 0
param set MC_ROLLRATE_D 0.0006
param set MC_PITCH_P 6.5
param set MC_PITCHRATE_P 0.1
param set MC_PITCHRATE_I 0
param set MC_PITCHRATE_D 0.000799999
param set MC_YAW_P 1.049999
param set MC_YAWRATE_P 0.05
param set MC_YAWRATE_I 0.001
param set MC_YAWRATE_D 0
fi
set OUTPUT_MODE bebop
set USE_IO no
set MIXER bebop

1
ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt
View File

@ -67,7 +67,6 @@ px4_add_romfs_files(
4010_dji_f330
4011_dji_f450
4012_quad_x_can
4013_bebop
4014_s500
4015_holybro_s500
4016_holybro_px4vision

1
ROMFS/px4fmu_common/mixers/CMakeLists.txt
View File

@ -36,7 +36,6 @@ px4_add_romfs_files(
AAVVTWFF.main.mix
AERT.main.mix
AETRFG.main.mix
bebop.main.mix
blade130.main.mix
caipi.main.mix
CCPM.main.mix

16
ROMFS/px4fmu_common/mixers/bebop.main.mix
View File

@ -1,16 +0,0 @@
#
# @board px4_fmu-v2 exclude
# @board px4_fmu-v3 exclude
# @board px4_fmu-v4 exclude
# @board px4_fmu-v4pro exclude
# @board px4_fmu-v5 exclude
# @board aerofc-v1 exclude
#
Multirotor mixer for Parrot Bebop
=================================
This file defines a single mixer for a quadrotor in the x configuration. All controls
are mixed 100%.
R: 4x 10000 10000 -10000 0

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:2019-10-24"
elif [[ $@ =~ .*ocpoc.* ]] || [[ $@ =~ .*navio2.* ]] || [[ $@ =~ .*raspberry.* ]] || [[ $@ =~ .*beaglebone.* ]] || [[ $@ =~ .*bebop.* ]]; then
# aerotenna_ocpoc_default, beaglebone_blue_default, emlid_navio2_default, px4_raspberrypi_default, parrot_bebop_default
elif [[ $@ =~ .*ocpoc.* ]] || [[ $@ =~ .*navio2.* ]] || [[ $@ =~ .*raspberry.* ]] || [[ $@ =~ .*beaglebone.* ]]; then
# aerotenna_ocpoc_default, beaglebone_blue_default, emlid_navio2_default, px4_raspberrypi_default
PX4_DOCKER_REPO="px4io/px4-dev-armhf:2019-10-24"
elif [[ $@ =~ .*eagle.* ]] || [[ $@ =~ .*excelsior.* ]]; then
# eagle, excelsior

34
boards/parrot/bebop/CMakeLists.txt
View File

@ -1,34 +0,0 @@
############################################################################
#
# 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.
#
############################################################################
add_subdirectory(flow)

41
boards/parrot/bebop/cmake/upload.cmake
View File

@ -1,41 +0,0 @@
############################################################################
#
# 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_custom_target(upload
COMMAND ${PX4_BOARD_DIR}/scripts/adb_upload_to_bebop.sh
${CMAKE_RUNTIME_OUTPUT_DIRECTORY}
/data/ftp/internal_000/px4
DEPENDS px4 ${PX4_BOARD_DIR}/scripts/adb_upload_to_bebop.sh
COMMENT "uploading px4"
USES_TERMINAL
)

66
boards/parrot/bebop/default.cmake
View File

@ -1,66 +0,0 @@
px4_add_board(
VENDOR parrot
MODEL bebop
PLATFORM posix
ARCHITECTURE cortex-a53
TOOLCHAIN arm-linux-gnueabihf
DRIVERS
barometer/ms5611
gps
linux_sbus
pwm_out_sim
DF_DRIVERS # NOTE: DriverFramework is migrating to intree PX4 drivers
mpu6050
ak8963
bebop_bus
bebop_rangefinder
mt9v117
MODULES
attitude_estimator_q
#camera_feedback
commander
dataman
ekf2
events
#fw_att_control
#fw_pos_control_l1
#rover_pos_control
land_detector
landing_target_estimator
load_mon
local_position_estimator
logger
mavlink
mc_att_control
mc_rate_control
mc_pos_control
navigator
rc_update
sensors
sih
#vtol_att_control
airspeed_selector
SYSTEMCMDS
#config
esc_calib
led_control
mixer
motor_ramp
param
perf
pwm
reboot
sd_bench
#tests # tests and test runner
top
topic_listener
tune_control
ver
work_queue
)

47
boards/parrot/bebop/flow/CMakeLists.txt
View File

@ -1,47 +0,0 @@
############################################################################
#
# Copyright (c) 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.
#
############################################################################
px4_add_module(
MODULE drivers__bebop_flow
MAIN bebop_flow
INCLUDES
${PX4_SOURCE_DIR}/src/lib/DriverFramework/drivers
SRCS
bebop_flow.cpp
video_device.cpp
dump_pgm.cpp
DEPENDS
git_driverframework
df_driver_framework
df_mt9v117
)

323
boards/parrot/bebop/flow/bebop_flow.cpp
View File

@ -1,323 +0,0 @@
/****************************************************************************
*
* Copyright (c) 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.
*
****************************************************************************/
/**
* @file bebop_flow.cpp
*
* This is a wrapper around the Parrot Bebop's downward-facing camera and integrates
* an optical flow computation.
*/
#include <stdlib.h>
#include <stdint.h>
#include <string.h>
#include <px4_platform_common/tasks.h>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/posix.h>
#include "video_device.h"
#include "dump_pgm.h"
#include <mt9v117/MT9V117.hpp>
extern "C" { __EXPORT int bebop_flow_main(int argc, char *argv[]); }
using namespace DriverFramework;
namespace bebop_flow
{
MT9V117 *image_sensor = nullptr; // I2C image sensor
VideoDevice *g_dev = nullptr; // interface to the video device
volatile bool _task_should_exit = false; // flag indicating if bebop flow task should exit
static bool _is_running = false; // flag indicating if bebop flow app is running
static px4_task_t _task_handle = -1; // handle to the task main thread
volatile unsigned int _trigger = 0; // Number of images to write as pgm
const char *dev_name = "/dev/video0"; // V4L video device
int start();
int stop();
int info();
int trigger(int count);
int clear_errors();
void usage();
void task_main(int argc, char *argv[]);
void task_main(int argc, char *argv[])
{
_is_running = true;
int ret = 0;
struct frame_data frame {};
uint32_t timeout_cnt = 0;
// Main loop
while (!_task_should_exit) {
ret = g_dev->get_frame(frame);
if (ret < 0) {
PX4_ERR("Get Frame failed");
continue;
} else if (ret == 1) {
// No image in buffer
usleep(1000);
++timeout_cnt;
if (timeout_cnt > 1000) {
PX4_WARN("No frames received for 1 sec");
timeout_cnt = 0;
}
continue;
}
timeout_cnt = 0;
// Write images into a file
if (_trigger > 0) {
PX4_INFO("Trigger camera");
dump_pgm(frame.data, frame.bytes, frame.seq, frame.timestamp);
--_trigger;
}
/***************************************************************
*
* Optical Flow computation
*
**************************************************************/
ret = g_dev->put_frame(frame);
if (ret < 0) {
PX4_ERR("Put Frame failed");
}
}
_is_running = false;
}
int start()
{
if (_is_running) {
PX4_WARN("bebop_flow already running");
return -1;
}
// Prepare the I2C device
image_sensor = new MT9V117(IMAGE_DEVICE_PATH);
if (image_sensor == nullptr) {
PX4_ERR("failed instantiating image sensor object");
return -1;
}
int ret = image_sensor->start();
if (ret != 0) {
PX4_ERR("Image sensor start failed");
return ret;
}
// Start the video device
g_dev = new VideoDevice(dev_name, 6);
if (g_dev == nullptr) {
PX4_ERR("failed instantiating video device object");
return -1;
}
ret = g_dev->start();
if (ret != 0) {
PX4_ERR("Video device start failed");
return ret;
}
/* start the task */
_task_handle = px4_task_spawn_cmd("bebop_flow",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
2000,
(px4_main_t)&task_main,
nullptr);
if (_task_handle < 0) {
PX4_WARN("task start failed");
return -1;
}
return 0;
}
int stop()
{
// Stop bebop flow task
_task_should_exit = true;
while (_is_running) {
usleep(200000);
PX4_INFO(".");
}
_task_handle = -1;
_task_should_exit = false;
_trigger = 0;
if (g_dev == nullptr) {
PX4_ERR("driver not running");
return -1;
}
int ret = g_dev->stop();
if (ret != 0) {
PX4_ERR("driver could not be stopped");
return ret;
}
if (image_sensor == nullptr) {
PX4_ERR("Image sensor not running");
return -1;
}
ret = image_sensor->stop();
if (ret != 0) {
PX4_ERR("Image sensor driver could not be stopped");
return ret;
}
delete g_dev;
delete image_sensor;
g_dev = nullptr;
image_sensor = nullptr;
return 0;
}
/**
* Print a little info about the driver.
*/
int
info()
{
if (g_dev == nullptr) {
PX4_ERR("driver not running");
return 1;
}
PX4_DEBUG("state @ %p", g_dev);
int ret = g_dev->print_info();
if (ret != 0) {
PX4_ERR("Unable to print info");
return ret;
}
return 0;
}
int trigger(int count)
{
if (_is_running) {
_trigger = count;
} else {
PX4_WARN("bebop_flow is not running");
}
return OK;
}
void
usage()
{
PX4_INFO("Usage: bebop_flow 'start', 'info', 'stop', 'trigger [-n #]'");
}
} /* bebop flow namespace*/
int
bebop_flow_main(int argc, char *argv[])
{
int ch;
int ret = 0;
int myoptind = 1;
const char *myoptarg = NULL;
unsigned int trigger_count = 1;
/* jump over start/off/etc and look at options first */
while ((ch = px4_getopt(argc, argv, "n:", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
case 'n':
trigger_count = atoi(myoptarg);
break;
default:
bebop_flow::usage();
return 0;
}
}
if (argc <= 1) {
bebop_flow::usage();
return 1;
}
const char *verb = argv[myoptind];
if (!strcmp(verb, "start")) {
ret = bebop_flow::start();
}
else if (!strcmp(verb, "stop")) {
ret = bebop_flow::stop();
}
else if (!strcmp(verb, "info")) {
ret = bebop_flow::info();
}
else if (!strcmp(verb, "trigger")) {
ret = bebop_flow::trigger(trigger_count);
}
else {
bebop_flow::usage();
return 1;
}
return ret;
}

87
boards/parrot/bebop/flow/dump_pgm.cpp
View File

@ -1,87 +0,0 @@
/****************************************************************************
*
* Copyright (c) 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.
*
****************************************************************************/
#include "dump_pgm.h"
#include <string.h>
#include <sys/fcntl.h>
#include <sys/stat.h>
#include <px4_platform_common/posix.h>
#define HRES_STR "320"
#define VRES_STR "240"
char pgm_header[] = "P5\n#99999999999999 usec \n" HRES_STR " " VRES_STR "\n255\n";
char pgm_dumpname[] = "image";
char pgm_path[] = "/home/root/images/";
void dump_pgm(const void *data, uint32_t size, uint32_t seq, uint32_t timestamp)
{
// Check if dump directory exists
struct stat sb = {};
if (!(stat(pgm_path, &sb) == 0 && S_ISDIR(sb.st_mode))) {
PX4_ERR("Dump directory does not exist: %s", pgm_path);
PX4_ERR("No images are written!");
return;
}
// Construct the absolute filename
char file_path[100] = {0};
snprintf(file_path, sizeof(file_path), "%s%s%08u.pgm", pgm_path, pgm_dumpname, seq);
PX4_INFO("%s", file_path);
int fd = open(file_path, O_WRONLY | O_NONBLOCK | O_CREAT, 00666);
if (fd < 0) {
PX4_ERR("Dump: Unable to open file");
return;
}
// Write pgm header
snprintf(&pgm_header[4], 15, "%014d", (int)timestamp);
size_t written = write(fd, pgm_header, sizeof(pgm_header));
// Write image data
size_t total = 0;
do {
written = write(fd, data, size);
total += written;
} while (total < size);
PX4_INFO("Wrote %d bytes\n", total);
close(fd);
}

46
boards/parrot/bebop/flow/dump_pgm.h
View File

@ -1,46 +0,0 @@
/****************************************************************************
*
* Copyright (c) 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.
*
****************************************************************************/
/**
* @file dump_pgm.h
*
* This is a simple implementation to write an image into a pgm file.
*
*/
#pragma once
#include <stdint.h>
void dump_pgm(const void *data, uint32_t size, uint32_t seq, uint32_t timestamp);

390
boards/parrot/bebop/flow/video_device.cpp
View File

@ -1,390 +0,0 @@
/****************************************************************************
*
* Copyright (c) 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.
*
****************************************************************************/
/**
* @file video_device.cpp
*
* Wrapper for a V4L2 device using the memory mapped interface.
*
*/
#include "video_device.h"
#include <stdlib.h>
#include <sys/stat.h>
#include <sys/ioctl.h>
#include <sys/mman.h>
#include <errno.h>
#include <string.h>
#include <linux/videodev2.h>
#include <px4_platform_common/posix.h>
int VideoDevice::start()
{
int ret = open_device();
if (ret < 0) {
return ret;
}
ret = init_device();
if (ret < 0) {
return ret;
}
return start_capturing();
}
int VideoDevice::stop()
{
int result = stop_capturing();
if (result < 0) {
PX4_ERR("Error stop stream");
}
// Unmap buffers
for (unsigned int i = 0; i < _n_buffers; ++i) {
result = munmap(_buffers[i].start, _buffers[i].length);
if (result < 0) {
PX4_ERR("Error: Unmapping buffer");
}
}
free(_buffers);
result = close_device();
if (result < 0) {
return result;
}
return OK;
}
int VideoDevice::print_info()
{
PX4_INFO("Device: %s", _dev_name);
return 0;
}
int VideoDevice::open_device()
{
struct stat st;
// Check if device is usable
int ret = stat(_dev_name, &st);
if (ret < 0) {
PX4_ERR("Cannot identify %s: %d", _dev_name, errno);
return -EIO;
}
if (!S_ISCHR(st.st_mode)) {
PX4_ERR("%s is no device", _dev_name);
return -EIO;
}
// Open V4L2 device nonblocking
_fd = open(_dev_name, O_RDWR | O_NONBLOCK);
if (_fd < 0) {
PX4_ERR("Unable to open %s", _dev_name);
return -EIO;
}
return OK;
}
int VideoDevice::close_device()
{
int ret = close(_fd);
if (ret < 0) {
PX4_ERR("Error closing %s", _dev_name);
return -EIO;
}
return OK;
}
int VideoDevice::init_device()
{
struct v4l2_capability cap {};
int ret = ioctl(_fd, VIDIOC_QUERYCAP, &cap);
if (ret < 0) {
if (EINVAL == errno) {
PX4_ERR("No V4L2 device: %s", _dev_name);
return -EINVAL;
} else {
PX4_ERR("VIDIOC_QUERYCAP failed: %s", _dev_name);
return -1;
}
}
if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
PX4_ERR("Device is no video capture device: %s", _dev_name);
return -EIO;
}
ret = init_crop();
if (ret < 0) {
PX4_ERR("Error when setting image crop");
return -1;
}
return init_buffers();
}
int VideoDevice::init_crop()
{
struct v4l2_cropcap cropcap {};
struct v4l2_crop crop {};
cropcap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
int ret = ioctl(_fd, VIDIOC_CROPCAP, &cropcap);
if (ret < 0) {
PX4_WARN("CROPCAP failed");
}
crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
crop.c = cropcap.defrect; // reset to default
ret = ioctl(_fd, VIDIOC_S_CROP, &crop);
if (ret < 0) {
PX4_WARN("S_CROP failed");
}
return init_format();
}
int VideoDevice::init_format()
{
usleep(10000);
struct v4l2_format fmt {};
fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
fmt.fmt.pix.width = VIDEO_DEVICE_IMAGE_WIDTH;
fmt.fmt.pix.height = VIDEO_DEVICE_IMAGE_HEIGHT;
fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_NV12;
fmt.fmt.pix.colorspace = V4L2_COLORSPACE_REC709;
int ret = ioctl(_fd, VIDIOC_S_FMT, &fmt);
if (ret < 0) {
PX4_ERR("Unable to set format");
return -1;
}
const char *format_string;
switch (fmt.fmt.pix.pixelformat) {
case V4L2_PIX_FMT_YUYV:
format_string = "YUYV";
break;
case V4L2_PIX_FMT_YVYU:
format_string = "YVYU";
break;
case V4L2_PIX_FMT_NV12:
format_string = "NV12";
break;
default:
format_string = "None";
}
PX4_INFO("Set image format: %ux%u\n Format: %s\n Size: %u",
fmt.fmt.pix.width,
fmt.fmt.pix.height,
format_string,
fmt.fmt.pix.sizeimage);
return OK;
}
int VideoDevice::init_buffers()
{
struct v4l2_requestbuffers req {};
req.count = _n_buffers;
req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
req.memory = V4L2_MEMORY_MMAP;
int ret = ioctl(_fd, VIDIOC_REQBUFS, &req);
if (ret < 0) {
PX4_ERR("Unable to request buffers: %s", _dev_name);
return -1;
}
_buffers = (struct buffer *) malloc(_n_buffers * sizeof(_buffers[0]));
if (_buffers == nullptr) {
PX4_ERR("Unable to allocate buffers");
return -1;
}
for (unsigned int i = 0; i < _n_buffers; ++i) {
struct v4l2_buffer buf {};
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
buf.memory = V4L2_MEMORY_MMAP;
buf.index = i;
ret = ioctl(_fd, VIDIOC_QUERYBUF, &buf);
if (ret < 0) {
PX4_ERR("Error QUERYBUF");
return -1;
}
_buffers[i].length = buf.length;
_buffers[i].start = mmap(nullptr, buf.length, PROT_READ | PROT_WRITE,
MAP_SHARED, _fd, buf.m.offset);
if (_buffers[i].start == MAP_FAILED) {
PX4_ERR("Out of memory");
return -1;
}
}
return OK;
}
int VideoDevice::start_capturing()
{
for (unsigned int i = 0; i < _n_buffers; ++i) {
struct v4l2_buffer buf {};
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
buf.memory = V4L2_MEMORY_MMAP;
buf.index = i;
int ret = ioctl(_fd, VIDIOC_QBUF, &buf);
if (ret < 0) {
PX4_ERR("Unable to queue buffer: %d", i);
return -1;
}
}
enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
int ret = ioctl(_fd, VIDIOC_STREAMON, &type);
if (ret < 0) {
PX4_ERR("Unable to start streaming");
return -1;
}
PX4_INFO("Streaming started: %s", _dev_name);
return OK;
}
int VideoDevice::stop_capturing()
{
enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
int ret = ioctl(_fd, VIDIOC_STREAMOFF, &type);
if (ret < 0) {
PX4_ERR("Unable to stop streaming");
return -1;
}
PX4_INFO("Streaming stopped: %s", _dev_name);
return OK;
}
int VideoDevice::get_frame(struct frame_data &frame)
{
struct v4l2_buffer buf {};
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
buf.memory = V4L2_MEMORY_MMAP;
int ret = ioctl(_fd, VIDIOC_DQBUF, &buf);
if (ret < 0) {
if (errno == EAGAIN) {
//PX4_INFO("No frame ready");
return 1;
} else if (errno == EIO) {
PX4_INFO("EIO");
return 1;
} else {
PX4_ERR("Buffer deque error");
return -1;
}
}
frame.data = _buffers[buf.index].start;
frame.seq = buf.sequence;
frame.timestamp = buf.timestamp.tv_sec * 1000000 + buf.timestamp.tv_usec;
frame.bytes = buf.bytesused;
frame.index = buf.index;
return 0;
}
int VideoDevice::put_frame(struct frame_data &frame)
{
struct v4l2_buffer buf {};
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
buf.memory = V4L2_MEMORY_MMAP;
buf.index = frame.index;
buf.length = _buffers[frame.index].length;
int ret = ioctl(_fd, VIDIOC_QBUF, &buf);
if (ret < 0) {
PX4_ERR("Buffer deque error");
return -1;
}
return OK;
}

93
boards/parrot/bebop/flow/video_device.h
View File

@ -1,93 +0,0 @@
/****************************************************************************
*
* Copyright (c) 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.
*
****************************************************************************/
#pragma once
#include <stdint.h>
#include <unistd.h>
#define VIDEO_DEVICE_IMAGE_WIDTH 320
#define VIDEO_DEVICE_IMAGE_HEIGHT 240
struct frame_data {
uint32_t timestamp;
uint32_t seq;
uint32_t bytes;
uint32_t index;
void *data;
};
struct buffer {
void *start;
size_t length;
};
class VideoDevice
{
public:
VideoDevice(char const *dev_name, uint32_t n_buffers) :
_fd(-1), _dev_name(dev_name), _n_buffers(n_buffers), _buffers(nullptr) {}
~VideoDevice() = default;
/// Start the device
int start();
/// Stop the device
int stop();
/// Print various infos
int print_info();
/// Non-blocking call to fetch an image. Returns 0 if the images was read, -1 on error
/// and 1 no new image is ready.
int get_frame(struct frame_data &frame);
/// Return a frame when the data is not needed any more.
int put_frame(struct frame_data &frame);
private:
int _fd;
const char *_dev_name;
uint32_t _n_buffers;
struct buffer *_buffers;
int open_device();
int close_device();
int init_device();
int init_buffers();
int init_crop();
int init_format();
int start_capturing();
int stop_capturing();
};

32
boards/parrot/bebop/scripts/adb_upload.sh
View File

@ -1,32 +0,0 @@
#!/bin/bash
if [[ "$#" < 2 ]]; then
echo "usage: adb_upload.sh SRC1 [SRC2 ...] DEST"
exit
fi
# Get last argument
for last; do true; done
echo "Wait for device..."
adb wait-for-device
echo "Creating folder structure..."
adb shell mkdir -p $last
echo "Uploading..."
# Go through source files and push them one by one.
i=0
for arg
do
if [[ $((i+1)) == "$#" ]]; then
break
fi
echo "Pushing $arg to $last"
adb push $arg $last
((i+=1))
done
# Make sure they are synced to the file system
echo "Syncing FS..."
adb shell sync

71
boards/parrot/bebop/scripts/adb_upload_to_bebop.sh
View File

@ -1,71 +0,0 @@
#!/bin/bash
BASEDIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )"
SRC_DIR="$BASEDIR/../../../../"
if [ -z ${BEBOP_IP+x} ]; then
ip=192.168.42.1
echo "\$BEBOP_IP is not set (use default: $ip)"
else
ip=$BEBOP_IP
echo "\$BEBOP_IP is set to $ip"
fi
port=9050
echo "Connecting to bebop: $ip:$port"
# adb returns also 0 as exit status if the connection fails
adb_return=$(adb connect $ip:$port)
adb_status=$(echo $adb_return | cut -f 1 -d " ")
if [[ $adb_status == "unable" ]]; then
echo ""
echo "Connection with Parrot Bebop could not be established:"
echo " Make sure you are connected with the Bebop's WiFi and"
echo " enable access to the board by pressing the power button 4 times."
echo ""
exit 50
fi
echo "Connection successfully established"
sleep 1
adb shell mount -o remount,rw /
adb shell touch /home/root/parameters
adb shell mkdir -p /data/ftp/internal_000/fs/microsd
# kill PX4 if it is already running from autostart
restart_px4=false
adb_return=$(adb shell killall -KILL px4)
if [[ $adb_return == "" ]]; then
echo "Killed running PX4 process"
restart_px4=true
fi
# upload PX4
$BASEDIR/adb_upload.sh $@
# upload mixer and config files
echo "Uploading mixer and config files to /home/root"
adb push $SRC_DIR/ROMFS/px4fmu_common/mixers/bebop.main.mix /home/root
adb push $SRC_DIR/posix-configs/bebop/px4.config /home/root
# restart the process after uploading
if [ "$restart_px4" = true ]; then
echo "Restarting PX4 process"
adb shell /etc/init.d/rcS_mode_default 2>/dev/null 1>/dev/null &
fi
# make sure all buffered blocks are written to disk
echo "Syncing FS..."
adb shell sync
echo "Disconnecting from Bebop"
adb disconnect
echo ""
echo "To start PX4, run the following command on the Bebop:"
echo "/data/ftp/internal_000/px4/px4 -s /home/root/px4.config /data/ftp/internal_000/px4/"

36
boards/parrot/bebop/src/CMakeLists.txt
View File

@ -1,36 +0,0 @@
############################################################################
#
# Copyright (c) 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.
#
############################################################################
add_library(drivers_board
empty.c
)

57
boards/parrot/bebop/src/board_config.h
View File

@ -1,57 +0,0 @@
/****************************************************************************
*
* Copyright (c) 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.
*
****************************************************************************/
/**
* @file board_config.h
*
* BEBOP internal definitions
*/
#pragma once
#define BOARD_OVERRIDE_UUID "BEBOPID000000000" // must be of length 16
#define PX4_SOC_ARCH_ID PX4_SOC_ARCH_ID_BEBOP
#define BOARD_HAS_NO_RESET
#define BOARD_HAS_NO_BOOTLOADER
#define BOARD_NUMBER_BRICKS 0
/*
* I2C busses
*/
#define PX4_I2C_BUS_EXPANSION 1
#define PX4_NUMBER_I2C_BUSES 1
#include <system_config.h>
#include <px4_platform_common/board_common.h>

0
boards/parrot/bebop/src/empty.c
View File

2
platforms/common/include/px4_platform_common/defines.h
View File

@ -123,8 +123,6 @@ __END_DECLS
# if defined(__PX4_POSIX_EAGLE) || defined(__PX4_POSIX_EXCELSIOR)
# define PX4_ROOTFSDIR "/home/linaro"
# elif defined(__PX4_POSIX_BEBOP)
# define PX4_ROOTFSDIR "/data/ftp/internal_000/px4"
# else
# define PX4_ROOTFSDIR "."
# endif

13
platforms/posix/cmake/px4_impl_os.cmake
View File

@ -269,19 +269,6 @@ function(px4_os_add_flags)
-D__DF_RPI
)
elseif ("${PX4_BOARD}" MATCHES "bebop")
#TODO: move to board support
add_definitions(
-D__PX4_LINUX
-D__PX4_POSIX_BEBOP # TODO: remove
# For DriverFramework
-D__DF_LINUX
-D__DF_BEBOP
)
elseif ("${PX4_BOARD}" MATCHES "aerotenna_ocpoc")
#TODO: move to board support

63
posix-configs/bebop/px4.config
View File

@ -1,63 +0,0 @@
#!/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
param select /home/root/parameters
if [ -f /home/root/parameters ]
then
param load
fi
param set SYS_AUTOSTART 4013
param set MAV_BROADCAST 1
param set MAV_TYPE 2
param set MPC_XY_VEL_P 0.12
param set MPC_XY_P 1.3
param set MPC_XY_VEL_D 0.006
param set MPC_XY_VEL_I 0.05
param set MPC_XY_VEL_MAX 12
param set MPC_Z_VEL_P 0.3
param set MPC_Z_VEL_I 0.1
param set MC_YAW_P 3.0
param set MC_YAWRATE_P 0.05
param set MC_YAWRATE_I 3.0
param set MC_ROLLRATE_P 0.05
param set MC_ROLLRATE_I 0.5
param set MC_ROLLRATE_D 0.0
param set MC_RR_INT_LIM 0.3
param set MC_PITCHRATE_P 0.1
param set MC_PITCHRATE_I 0.8
param set MC_PITCHRATE_D 0.0
param set MC_PR_INT_LIM 0.3
ms5611 -T 5607 -I start
df_mpu6050_wrapper start -R 8
df_ak8963_wrapper start -R 32
# Rangefinder disabled for now. It was found to cause delays of more than 10ms
#df_bebop_rangefinder_wrapper start
gps start -d /dev/ttyPA1
bebop_flow start
rc_update start
sensors start
commander start
ekf2 start
dataman start
navigator start
land_detector start multicopter
mc_pos_control start
mc_att_control start
mc_rate_control start
sleep 1
mavlink start -x -u 14556 -r 20000
sleep 1
mavlink stream -u 14556 -s HIGHRES_IMU -r 50
mavlink stream -u 14556 -s ATTITUDE -r 50
df_bebop_bus_wrapper start
mavlink boot_complete
logger start -b 200 -e -t

46
src/drivers/driver_framework_wrapper/df_bebop_bus_wrapper/CMakeLists.txt
View File

@ -1,46 +0,0 @@
############################################################################
#
# Copyright (c) 2016 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_directories(../../../lib/DriverFramework/drivers)
px4_add_module(
MODULE platforms__posix__drivers__df_bebop_bus_wrapper
MAIN df_bebop_bus_wrapper
SRCS
df_bebop_bus_wrapper.cpp
DEPENDS
git_driverframework
df_driver_framework
df_bebop_bus
battery
)

618
src/drivers/driver_framework_wrapper/df_bebop_bus_wrapper/df_bebop_bus_wrapper.cpp
View File

@ -1,618 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2016 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 df_bebop_bus_wrapper.cpp
*
* This is a wrapper around the Parrot Bebop bus driver of the DriverFramework. It sends the
* motor and contol commands to the Bebop and reads its status and informations.
*/
#include <stdlib.h>
#include <stdint.h>
#include <px4_platform_common/tasks.h>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/posix.h>
#include <errno.h>
#include <string.h>
#include <math.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/esc_status.h>
#include <lib/mixer/MixerGroup.hpp>
#include <lib/mixer/mixer_load.h>
#include <battery/battery.h>
#include <bebop_bus/BebopBus.hpp>
#include <DevMgr.hpp>
extern "C" { __EXPORT int df_bebop_bus_wrapper_main(int argc, char *argv[]); }
using namespace DriverFramework;
class DfBebopBusWrapper : public BebopBus
{
public:
DfBebopBusWrapper();
~DfBebopBusWrapper() = default;
/// Start and initialize the driver
int start();
/// Stop the driver
int stop();
/// Print various infos (version, type, flights, errors
int print_info();
/// Start the motors
int start_motors();
/// Stop the motors
int stop_motors();
/// Reset pending errors on the Bebop hardware
int clear_errors();
/// Set the ESC speeds [front left, front right, back right, back left]
int set_esc_speeds(const float speed_scaled[4]);
/// Capture the last throttle value for the battey computation
void set_last_throttle(float throttle) {_last_throttle = throttle;};
private:
orb_advert_t _battery_topic;
orb_advert_t _esc_topic;
Battery _battery{1, nullptr};
bool _armed;
float _last_throttle;
int _battery_orb_class_instance;
// map for bebop motor index to PX4 motor index
const uint8_t _esc_map[4] = {0, 2, 3, 1};
int _publish(struct bebop_state_data &data);
};
DfBebopBusWrapper::DfBebopBusWrapper() :
BebopBus(BEBOP_BUS_DEVICE_PATH), _battery_topic(nullptr), _esc_topic(nullptr), _battery(1, nullptr), _armed(false),
_last_throttle(0.0f),
_battery_orb_class_instance(-1)
{}
int DfBebopBusWrapper::start()
{
/* Init device and start sensor. */
int ret = init();
if (ret != 0) {
PX4_ERR("BebopBus init fail: %d", ret);
return ret;
}
ret = BebopBus::start();
if (ret < 0) {
PX4_ERR("BebopBus start fail: %d", ret);
return ret;
}
// Signal bus start on Bebop
BebopBus::_play_sound(BebopBus::BOOT);
return 0;
}
int DfBebopBusWrapper::stop()
{
int ret = BebopBus::stop();
if (ret < 0) {
PX4_ERR("BebopBus stop fail: %d", ret);
return ret;
}
return 0;
}
int DfBebopBusWrapper::print_info()
{
bebop_bus_info info;
int ret = _get_info(&info);
if (ret < 0) {
return -1;
}
PX4_INFO("Bebop Controller Info");
PX4_INFO(" Software Version: %d.%d", info.version_major, info.version_minor);
PX4_INFO(" Software Type: %d", info.type);
PX4_INFO(" Number of controlled motors: %d", info.n_motors_controlled);
PX4_INFO(" Number of flights: %d", info.n_flights);
PX4_INFO(" Last flight time: %d", info.last_flight_time);
PX4_INFO(" Total flight time: %d", info.total_flight_time);
PX4_INFO(" Last Error: %d\n", info.last_error);
return 0;
}
int DfBebopBusWrapper::start_motors()
{
_armed = true;
return BebopBus::_start_motors();
}
int DfBebopBusWrapper::stop_motors()
{
_armed = false;
return BebopBus::_stop_motors();
}
int DfBebopBusWrapper::clear_errors()
{
return BebopBus::_clear_errors();
}
int DfBebopBusWrapper::set_esc_speeds(const float speed_scaled[4])
{
return BebopBus::_set_esc_speed(speed_scaled);
}
int DfBebopBusWrapper::_publish(struct bebop_state_data &data)
{
const hrt_abstime timestamp = hrt_absolute_time();
// TODO Check if this is the right way for the Bebop
// We don't have current measurements
_battery.updateBatteryStatus(timestamp, data.battery_voltage_v, 0.0, true, true, 0, _last_throttle, false);
esc_status_s esc_status = {};
uint16_t esc_speed_setpoint_rpm[4] = {};
BebopBus::_get_esc_speed_setpoint(esc_speed_setpoint_rpm);
esc_status.timestamp = hrt_absolute_time();
esc_status.esc_count = 4;
for (int i = 0; i < 4; i++) {
esc_status.esc[_esc_map[i]].timestamp = esc_status.timestamp;
esc_status.esc[_esc_map[i]].esc_rpm = data.rpm[i];
}
// TODO: when is this ever blocked?
if (!(m_pub_blocked)) {
_battery.publish();
if (_esc_topic == nullptr) {
_esc_topic = orb_advertise(ORB_ID(esc_status), &esc_status);
} else {
orb_publish(ORB_ID(esc_status), _esc_topic, &esc_status);
}
}
return 0;
}
namespace df_bebop_bus_wrapper
{
DfBebopBusWrapper *g_dev = nullptr; // interface to the Bebop's I2C device
volatile bool _task_should_exit = false; // flag indicating if bebop esc control task should exit
static bool _is_running = false; // flag indicating if bebop esc app is running
static bool _motors_running = false; // flag indicating if the motors are running
static px4_task_t _task_handle = -1; // handle to the task main thread
static const char *MIXER_FILENAME = "/home/root/bebop.main.mix";
// subscriptions
int _controls_sub;
int _armed_sub;
// publications
orb_advert_t _outputs_pub = nullptr;
orb_advert_t _rc_pub = nullptr;
// topic structures
actuator_controls_s _controls[1];
actuator_outputs_s _outputs;
actuator_armed_s _armed;
MixerGroup *_mixers = nullptr;
int start();
int stop();
int info();
int clear_errors();
void usage();
void task_main(int argc, char *argv[]);
/* mixers initialization */
int initialize_mixers(const char *mixers_filename);
int mixers_control_callback(uintptr_t handle, uint8_t control_group,
uint8_t control_index, float &input);
int mixers_control_callback(uintptr_t handle,
uint8_t control_group,
uint8_t control_index,
float &input)
{
const actuator_controls_s *controls = (actuator_controls_s *)handle;
input = controls[control_group].control[control_index];
return 0;
}
int initialize_mixers(const char *mixers_filename)
{
char buf[2048] = {0};
size_t buflen = sizeof(buf);
PX4_INFO("Trying to initialize mixers from config file %s", mixers_filename);
if (load_mixer_file(mixers_filename, &buf[0], sizeof(buf)) < 0) {
PX4_ERR("can't load mixer: %s", mixers_filename);
return -1;
}
if (_mixers == nullptr) {
_mixers = new MixerGroup();
}
if (_mixers == nullptr) {
PX4_ERR("No mixers available");
return -1;
} else {
int ret = _mixers->load_from_buf(mixers_control_callback, (uintptr_t)_controls, buf, buflen);
if (ret != 0) {
PX4_ERR("Unable to parse mixers file");
delete _mixers;
_mixers = nullptr;
ret = -1;
}
return 0;
}
}
void task_main(int argc, char *argv[])
{
_is_running = true;
// Subscribe for orb topics
_controls_sub = orb_subscribe(ORB_ID(actuator_controls_0));
_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
// Start disarmed
_armed.armed = false;
_armed.prearmed = false;
_motors_running = false;
// Set up poll topic
px4_pollfd_struct_t fds[1];
fds[0].fd = _controls_sub;
fds[0].events = POLLIN;
/* Don't limit poll intervall for now, 250 Hz should be fine. */
//orb_set_interval(_controls_sub, 10);
// Set up mixers
if (initialize_mixers(MIXER_FILENAME) < 0) {
PX4_ERR("Mixer initialization failed.");
return;
}
// Main loop
while (!_task_should_exit) {
int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 10);
/* Timed out, do a periodic check for _task_should_exit. */
if (pret == 0) {
continue;
}
/* This is undesirable but not much we can do. */
if (pret < 0) {
PX4_WARN("poll error %d, %d", pret, errno);
/* sleep a bit before next try */
usleep(100000);
continue;
}
if (fds[0].revents & POLLIN) {
orb_copy(ORB_ID(actuator_controls_0), _controls_sub, &_controls[0]);
_outputs.timestamp = _controls[0].timestamp;
if (_mixers != nullptr) {
/* do mixing */
_outputs.noutputs = _mixers->mix(_outputs.output, 4);
}
// Set last throttle for battery calculations
g_dev->set_last_throttle(_controls[0].control[3]);
/* disable unused ports by setting their output to NaN */
for (size_t i = _outputs.noutputs; i < sizeof(_outputs.output) / sizeof(_outputs.output[0]);
i++) {
_outputs.output[i] = NAN;
}
// Check if the outputs are in range and rescale
for (size_t i = 0; i < _outputs.noutputs; ++i) {
if (i < _outputs.noutputs &&
PX4_ISFINITE(_outputs.output[i]) &&
_outputs.output[i] >= -1.0f &&
_outputs.output[i] <= 1.0f) {
/* scale for Bebop output 0.0 - 1.0 */
_outputs.output[i] = (_outputs.output[i] + 1.0f) / 2.0f;
} else {
/*
* Value is NaN, INF or out of band - set to the minimum value.
* This will be clearly visible on the servo status and will limit the risk of accidentally
* spinning motors. It would be deadly in flight.
*/
_outputs.output[i] = 0.0;
}
}
// Adjust order of BLDCs from PX4 to Bebop
float motor_out[4];
motor_out[0] = _outputs.output[2];
motor_out[1] = _outputs.output[0];
motor_out[2] = _outputs.output[3];
motor_out[3] = _outputs.output[1];
g_dev->set_esc_speeds(motor_out);
if (_outputs_pub != nullptr) {
orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &_outputs);
} else {
_outputs_pub = orb_advertise(ORB_ID(actuator_outputs), &_outputs);
}
}
bool updated;
orb_check(_armed_sub, &updated);
if (updated) {
orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
}
const bool lockdown = _armed.manual_lockdown || _armed.lockdown || _armed.force_failsafe;
// Start the motors if armed but not alreay running
if (_armed.armed && !lockdown && !_motors_running) {
g_dev->start_motors();
_motors_running = true;
}
// Stop motors if not armed or killed, but running
if ((!_armed.armed || lockdown) && _motors_running) {
g_dev->stop_motors();
_motors_running = false;
}
}
orb_unsubscribe(_controls_sub);
orb_unsubscribe(_armed_sub);
_is_running = false;
}
int start()
{
g_dev = new DfBebopBusWrapper();
if (g_dev == nullptr) {
PX4_ERR("failed instantiating DfBebopBusWrapper object");
return -1;
}
int ret = g_dev->start();
if (ret != 0) {
PX4_ERR("DfBebopBusWrapper start failed");
return ret;
}
// Open the Bebop dirver
DevHandle h;
DevMgr::getHandle(BEBOP_BUS_DEVICE_PATH, h);
if (!h.isValid()) {
DF_LOG_INFO("Error: unable to obtain a valid handle for the receiver at: %s (%d)",
BEBOP_BUS_DEVICE_PATH, h.getError());
return -1;
}
DevMgr::releaseHandle(h);
// Start the task to forward the motor control commands
_task_handle = px4_task_spawn_cmd("bebop_bus_esc_main",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX,
2000,
(px4_main_t)&task_main,
nullptr);
if (_task_handle < 0) {
PX4_ERR("task start failed");
return -1;
}
_is_running = true;
return 0;
}
int stop()
{
// Stop bebop motor control task
_task_should_exit = true;
while (_is_running) {
usleep(200000);
PX4_INFO(".");
}
_task_handle = -1;
if (g_dev == nullptr) {
PX4_ERR("driver not running");
return 1;
}
// Stop DF device
int ret = g_dev->stop();
if (ret != 0) {
PX4_ERR("driver could not be stopped");
return ret;
}
delete g_dev;
g_dev = nullptr;
return 0;
}
/**
* Print a little info about the driver.
*/
int
info()
{
if (g_dev == nullptr) {
PX4_ERR("driver not running");
return 1;
}
PX4_DEBUG("state @ %p", g_dev);
int ret = g_dev->print_info();
if (ret != 0) {
PX4_ERR("Unable to print info");
return ret;
}
return 0;
}
/**
* Clear errors present on the Bebop hardware
*/
int
clear_errors()
{
if (g_dev == nullptr) {
PX4_ERR("driver not running");
return 1;
}
PX4_DEBUG("state @ %p", g_dev);
int ret = g_dev->clear_errors();
if (ret != 0) {
PX4_ERR("Unable to clear errors");
return ret;
}
return 0;
}
void
usage()
{
PX4_INFO("Usage: df_bebop_bus_wrapper 'start', 'info', 'clear_errors', 'stop'");
}
} /* df_bebop_bus_wrapper */
int
df_bebop_bus_wrapper_main(int argc, char *argv[])
{
int ret = 0;
int myoptind = 1;
if (argc <= 1) {
df_bebop_bus_wrapper::usage();
return 1;
}
const char *verb = argv[myoptind];
if (!strcmp(verb, "start")) {
ret = df_bebop_bus_wrapper::start();
}
else if (!strcmp(verb, "stop")) {
ret = df_bebop_bus_wrapper::stop();
}
else if (!strcmp(verb, "info")) {
ret = df_bebop_bus_wrapper::info();
}
else if (!strcmp(verb, "clear_errors")) {
ret = df_bebop_bus_wrapper::clear_errors();
}
else {
df_bebop_bus_wrapper::usage();
return 1;
}
return ret;
}

45
src/drivers/driver_framework_wrapper/df_bebop_rangefinder_wrapper/CMakeLists.txt
View File

@ -1,45 +0,0 @@
############################################################################
#
# Copyright (c) 2016 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_directories(../../../lib/DriverFramework/drivers)
px4_add_module(
MODULE platforms__posix__drivers__df_bebop_rangefinder_wrapper
MAIN df_bebop_rangefinder_wrapper
SRCS
df_bebop_rangefinder_wrapper.cpp
DEPENDS
git_driverframework
df_driver_framework
df_bebop_rangefinder
)

308
src/drivers/driver_framework_wrapper/df_bebop_rangefinder_wrapper/df_bebop_rangefinder_wrapper.cpp
View File

@ -1,308 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2016 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 df_bebop_rangefinder_wrapper.cpp
* Driver to access the Bebop rangefinder of the DriverFramework.
*/
#include <px4_platform_common/px4_config.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <stdint.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <unistd.h>
#include <px4_platform_common/getopt.h>
#include <errno.h>
#include <string>
#include <perf/perf_counter.h>
#include <systemlib/err.h>
#include <drivers/drv_range_finder.h>
#include <drivers/drv_hrt.h>
#include <uORB/uORB.h>
#include <uORB/topics/distance_sensor.h>
#include <board_config.h>
#include <bebop_rangefinder/BebopRangeFinder.hpp>
#include <DevMgr.hpp>
extern "C" { __EXPORT int df_bebop_rangefinder_wrapper_main(int argc, char *argv[]); }
using namespace DriverFramework;
class DfBebopRangeFinderWrapper : public BebopRangeFinder
{
public:
DfBebopRangeFinderWrapper();
~DfBebopRangeFinderWrapper() = default;
/**
* Start automatic measurement.
*
* @return 0 on success
*/
int start();
/**
* Stop automatic measurement.
*
* @return 0 on success
*/
int stop();
private:
int _publish(struct bebop_range &data);
orb_advert_t _range_topic;
int _orb_class_instance;
// perf_counter_t _range_sample_perf;
};
DfBebopRangeFinderWrapper::DfBebopRangeFinderWrapper(/*enum Rotation rotation*/) :
BebopRangeFinder(BEBOP_RANGEFINDER_DEVICE_PATH),
_range_topic(nullptr),
_orb_class_instance(-1)
{
}
int DfBebopRangeFinderWrapper::start()
{
/* Init device and start sensor. */
int ret = init();
if (ret != 0) {
PX4_ERR("BebopRangeFinder init fail: %d", ret);
return ret;
}
ret = BebopRangeFinder::start();
if (ret != 0) {
PX4_ERR("BebopRangeFinder start fail: %d", ret);
return ret;
}
return 0;
}
int DfBebopRangeFinderWrapper::stop()
{
/* Stop sensor. */
int ret = BebopRangeFinder::stop();
if (ret != 0) {
PX4_ERR("BebopRangeFinder stop fail: %d", ret);
return ret;
}
return 0;
}
int DfBebopRangeFinderWrapper::_publish(struct bebop_range &data)
{
struct distance_sensor_s distance_data;
memset(&distance_data, 0, sizeof(distance_sensor_s));
distance_data.timestamp = hrt_absolute_time();
distance_data.min_distance = float(BEBOP_RANGEFINDER_MIN_DISTANCE_M); /* m */
distance_data.max_distance = float(BEBOP_RANGEFINDER_MAX_DISTANCE_M); /* m */
distance_data.current_distance = float(data.height_m);
distance_data.type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND;
distance_data.id = 0; // TODO set proper ID
distance_data.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
distance_data.variance = 1.0f; // TODO set correct value
distance_data.signal_quality = -1;
if (_range_topic == nullptr) {
_range_topic = orb_advertise_multi(ORB_ID(distance_sensor), &distance_data,
&_orb_class_instance, ORB_PRIO_DEFAULT);
} else {
orb_publish(ORB_ID(distance_sensor), _range_topic, &distance_data);
}
return 0;
};
namespace df_bebop_rangefinder_wrapper
{
DfBebopRangeFinderWrapper *g_dev = nullptr;
int start();
int stop();
int info();
void usage();
int start()
{
g_dev = new DfBebopRangeFinderWrapper();
if (g_dev == nullptr) {
PX4_ERR("failed instantiating DfBebopRangeFinderWrapper object");
return -1;
}
int ret = g_dev->start();
if (ret != 0) {
PX4_ERR("DfBebopRangeFinderWrapper start failed");
return ret;
}
// Open the range sensor
DevHandle h;
DevMgr::getHandle(BEBOP_RANGEFINDER_DEVICE_PATH, h);
if (!h.isValid()) {
DF_LOG_INFO("Error: unable to obtain a valid handle for the receiver at: %s (%d)",
BEBOP_RANGEFINDER_DEVICE_PATH, h.getError());
return -1;
}
DevMgr::releaseHandle(h);
return 0;
}
int stop()
{
if (g_dev == nullptr) {
PX4_ERR("driver not running");
return 1;
}
int ret = g_dev->stop();
if (ret != 0) {
PX4_ERR("driver could not be stopped");
return ret;
}
delete g_dev;
g_dev = nullptr;
return 0;
}
/**
* Print a little info about the driver.
*/
int
info()
{
if (g_dev == nullptr) {
PX4_ERR("driver not running");
return 1;
}
PX4_DEBUG("state @ %p", g_dev);
return 0;
}
void
usage()
{
PX4_WARN("Usage: df_bebop_rangefinder_wrapper 'start', 'info', 'stop'");
}
} // namespace df_bebop_rangefinder_wrapper
int
df_bebop_rangefinder_wrapper_main(int argc, char *argv[])
{
int ch;
int ret = 0;
int myoptind = 1;
const char *myoptarg = NULL;
/* jump over start/off/etc and look at options first */
while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
// Add rotation if necessary
default:
df_bebop_rangefinder_wrapper::usage();
return 0;
}
}
if (argc <= 1) {
df_bebop_rangefinder_wrapper::usage();
return 1;
}
const char *verb = argv[myoptind];
if (!strcmp(verb, "start")) {
ret = df_bebop_rangefinder_wrapper::start(/*rotation*/);
}
else if (!strcmp(verb, "stop")) {
ret = df_bebop_rangefinder_wrapper::stop();
}
else if (!strcmp(verb, "info")) {
ret = df_bebop_rangefinder_wrapper::info();
}
else {
df_bebop_rangefinder_wrapper::usage();
return 1;
}
return ret;
}