mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-17 23:53:34 +08:00
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
@@ -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
|
||||
)
|
||||
@@ -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;
|
||||
}
|
||||
@@ -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
@@ -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;
|
||||
}
|
||||
Reference in New Issue
Block a user