fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-16 12:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

vmount: add mount and ROI implementation

Browse Source 
MavLink spec implementation

implemented vehicle_roi topic

rename old gimbal to rc_gimbal

little changes

corrected RC Gimbal group

Starting ROI implementation in commander

implementation done, needs to be tested

uhm..

add todo

Change to float32 for x,y and z

remove mission topic again, not needed

change roi coordinates to lat, lon and alt

adjust to float64

starting mount implementation

correcting small mistakes, compiles now

add todos

further progress

implementing parameters

adjust default parameters

started implementation of mavlink

fix typo

change to lat, lon and alt

fix typo :D

change to double (to represent float64)

add global_position_

add mount topic

commander mount implementation done

cleanup

almost finished

little fix

codestyle fixes

leave pitch at 0 degrees

added pitch calculation

codestyle changes

Undo vehicle_mount, react to updated parameters, parsing of CMD_DO_MOUNT_* in mount.cpp

start implementing mode override

forgot a semikolon.

add debug

Finish implementation of mount override and manual control.

fix codestyle

correct cleanup

rename to vmount

works now

fix rebase error

fix polling

refactoring and custom airframe for gimbal

couple changes

remove warnx

almost done

finally

What is going on?

change back to actuator_controls_2

working

bump parameter version number and some clarification

fix submodules
This commit is contained in:
Leon
2016-09-06 11:33:18 +02:00
committed by Beat Küng
parent e6391189bc
commit faebdeedcf
24 changed files with 1757 additions and 843 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/gimbal/gimbal.cpp
-704
View File
@@ -1,704 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2015 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 gimbal.cpp
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Anton Matosov <anton.matosov@gmail.com>
* @author Andreas Antener <andreas@uaventure.com>
*
* Driver to control a gimbal - relies on input via telemetry or RC
* and output via the standardized control group #2 and a mixer.
*/
#include <px4_config.h>
#include <sys/types.h>
#include <stdint.h>
#include <stdlib.h>
#include <stdbool.h>
#include <semaphore.h>
#include <string.h>
#include <fcntl.h>
#include <poll.h>
#include <errno.h>
#include <stdio.h>
#include <math.h>
#include <unistd.h>
#include <termios.h>
#include <nuttx/arch.h>
#include <nuttx/wqueue.h>
#include <nuttx/clock.h>
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_range_finder.h>
#include <drivers/device/device.h>
#include <drivers/device/ringbuffer.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/parameter_update.h>
#include <board_config.h>
#include <mathlib/math/test/test.hpp>
#include <mathlib/math/Quaternion.hpp>
/* Configuration Constants */
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
#ifndef CONFIG_SCHED_WORKQUEUE
# error This requires CONFIG_SCHED_WORKQUEUE.
#endif
#define GIMBAL_DEVICE_PATH "/dev/gimbal"
#define GIMBAL_UPDATE_INTERVAL (5 * 1000)
#define GIMBALIOCATTCOMPENSATE 1
class Gimbal : public device::CDev
{
public:
Gimbal();
virtual ~Gimbal();
virtual int init();
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
/**
* Diagnostics - print some basic information about the driver.
*/
void print_info();
protected:
virtual int probe();
private:
float _min_distance;
float _max_distance;
work_s _work;
int _vehicle_command_sub;
int _att_sub;
int _vehicle_control_mode_sub;
int _manual_control_sub;
int _params_sub;
bool _attitude_compensation_roll;
bool _attitude_compensation_pitch;
bool _attitude_compensation_yaw;
bool _initialized;
bool _control_cmd_set;
bool _config_cmd_set;
int _mount_mode; // see MAV_MOUNT_MODE
orb_advert_t _actuator_controls_2_topic;
struct vehicle_command_s _control_cmd;
struct vehicle_command_s _config_cmd;
struct manual_control_setpoint_s _manual_control;
struct vehicle_control_mode_s _control_mode;
struct {
int aux_mnt_chn;
int use_mnt;
} _parameters;
struct {
param_t aux_mnt_chn;
param_t use_mnt;
} _params_handles;
/**
* Initialise the automatic measurement state machine and start it.
*
* @note This function is called at open and error time. It might make sense
* to make it more aggressive about resetting the bus in case of errors.
*/
void start();
/**
* Stop the automatic measurement state machine.
*/
void stop();
/**
* Update parameters
*/
void update_params();
/**
* Perform a poll cycle; collect from the previous measurement
* and start a new one.
*/
void cycle();
/**
* Static trampoline from the workq context; because we don't have a
* generic workq wrapper yet.
*
* @param arg Instance pointer for the driver that is polling.
*/
static void cycle_trampoline(void *arg);
};
/*
* Driver 'main' command.
*/
extern "C" __EXPORT int gimbal_main(int argc, char *argv[]);
Gimbal::Gimbal() :
CDev("Gimbal", GIMBAL_DEVICE_PATH),
_vehicle_command_sub(-1),
_att_sub(-1),
_vehicle_control_mode_sub(-1),
_manual_control_sub(-1),
_params_sub(-1),
_attitude_compensation_roll(true),
_attitude_compensation_pitch(true),
_attitude_compensation_yaw(true),
_initialized(false),
_actuator_controls_2_topic(nullptr)
{
// disable debug() calls
_debug_enabled = false;
_params_handles.aux_mnt_chn = param_find("GMB_AUX_MNT_CHN");
_params_handles.use_mnt = param_find("GMB_USE_MNT");
update_params();
// work_cancel in the dtor will explode if we don't do this...
memset(&_work, 0, sizeof(_work));
}
Gimbal::~Gimbal()
{
/* make sure we are truly inactive */
stop();
::close(_vehicle_command_sub);
}
int
Gimbal::init()
{
int ret = ERROR;
/* do regular cdev init */
if (CDev::init() != OK) {
goto out;
}
start();
ret = OK;
out:
return ret;
}
int
Gimbal::probe()
{
return OK;
}
int
Gimbal::ioctl(struct file *filp, int cmd, unsigned long arg)
{
switch (cmd) {
case GIMBALIOCATTCOMPENSATE:
_attitude_compensation_roll = (arg != 0);
_attitude_compensation_pitch = (arg != 0);
_attitude_compensation_yaw = (arg != 0);
return OK;
default:
/* give it to the superclass */
return CDev::ioctl(filp, cmd, arg);
}
}
ssize_t
Gimbal::read(struct file *filp, char *buffer, size_t buflen)
{
return 0;
}
void
Gimbal::start()
{
/* schedule a cycle to start things */
work_queue(LPWORK, &_work, (worker_t)&Gimbal::cycle_trampoline, this, 1);
}
void
Gimbal::stop()
{
work_cancel(LPWORK, &_work);
}
void
Gimbal::cycle_trampoline(void *arg)
{
Gimbal *dev = static_cast<Gimbal *>(arg);
dev->cycle();
}
void
Gimbal::update_params()
{
param_get(_params_handles.aux_mnt_chn, &_parameters.aux_mnt_chn);
param_get(_params_handles.use_mnt, &_parameters.use_mnt);
}
void
Gimbal::cycle()
{
if (!_initialized) {
/* get subscription handles */
_vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command));
_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_vehicle_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
/* get a publication handle on actuator output topic */
struct actuator_controls_s zero_report;
memset(&zero_report, 0, sizeof(zero_report));
zero_report.timestamp = hrt_absolute_time();
_actuator_controls_2_topic = orb_advertise(ORB_ID(actuator_controls_2), &zero_report);
if (_actuator_controls_2_topic == nullptr) {
warnx("advert err");
}
_mount_mode = vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT;
_initialized = true;
}
bool updated = false;
float roll = 0.0f;
float pitch = 0.0f;
float yaw = 0.0f;
float out_mount_mode = 0.0f;
/* Parameter update */
bool params_updated;
orb_check(_params_sub, &params_updated);
if (params_updated) {
struct parameter_update_s param_update;
orb_copy(ORB_ID(parameter_update), _params_sub, &param_update); // XXX: is this actually necessary?
update_params();
}
/* Control mode update */
bool vehicle_control_mode_updated;
orb_check(_vehicle_control_mode_sub, &vehicle_control_mode_updated);
if (vehicle_control_mode_updated) {
orb_copy(ORB_ID(vehicle_control_mode), _vehicle_control_mode_sub, &_control_mode);
}
/* Check attitude */
struct vehicle_attitude_s att;
orb_copy(ORB_ID(vehicle_attitude), _att_sub, &att);
if (_attitude_compensation_roll) {
roll = 1.0f / M_PI_F * -att.roll;
updated = true;
}
if (_attitude_compensation_pitch) {
pitch = 1.0f / M_PI_F * -att.pitch;
updated = true;
}
if (_attitude_compensation_yaw) {
yaw = 1.0f / M_PI_F * att.yaw;
updated = true;
}
/* Check manual input */
bool manual_updated;
orb_check(_manual_control_sub, &manual_updated);
if (manual_updated) {
orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sub, &_manual_control);
/* only check manual input for mount mode when not in offboard and aux chan is configured */
if (!_control_mode.flag_control_offboard_enabled && _parameters.aux_mnt_chn > 0) {
float aux = 0.0f;
switch (_parameters.aux_mnt_chn) {
case 1:
aux = _manual_control.aux1;
break;
case 2:
aux = _manual_control.aux2;
break;
case 3:
aux = _manual_control.aux3;
break;
}
if (aux < 0.0f && _mount_mode != vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT) {
_mount_mode = vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT;
updated = true;
}
if (aux > 0.0f && _mount_mode != vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL) {
_mount_mode = vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL;
updated = true;
}
}
}
/* Check command input */
struct vehicle_command_s cmd;
bool cmd_updated;
orb_check(_vehicle_command_sub, &cmd_updated);
if (cmd_updated) {
orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd);
if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL
|| cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT) {
_control_cmd = cmd;
_control_cmd_set = true;
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE) {
_config_cmd = cmd;
_config_cmd_set = true;
}
}
if (_config_cmd_set) {
_config_cmd_set = false;
_attitude_compensation_roll = (int)_config_cmd.param2 == 1;
_attitude_compensation_pitch = (int)_config_cmd.param3 == 1;
_attitude_compensation_yaw = (int)_config_cmd.param4 == 1;
/* only check commanded mount mode when in offboard */
if (_control_mode.flag_control_offboard_enabled &&
fabsf(_config_cmd.param1 - _mount_mode) > FLT_EPSILON) {
_mount_mode = int(_config_cmd.param1 + 0.5f);
updated = true;
}
}
if (_control_cmd_set) {
unsigned mountMode = _control_cmd.param7;
DEVICE_DEBUG("control_cmd: %d, mountMode %d | param1: %8.4f param2: %8.4f", _control_cmd.command, mountMode,
(double)_control_cmd.param1, (double)_control_cmd.param2);
if (_control_cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL &&
mountMode == vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) {
/* Convert to range -1 ... 1, which corresponds to -180deg ... 180deg */
roll += 1.0f / M_PI_F * M_DEG_TO_RAD_F * _control_cmd.param1;
pitch += 1.0f / M_PI_F * M_DEG_TO_RAD_F * _control_cmd.param2;
yaw += 1.0f / M_PI_F * M_DEG_TO_RAD_F * _control_cmd.param3;
updated = true;
}
if (_control_cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT &&
mountMode == vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) {
float gimbalDirectionQuat[] = {_control_cmd.param1, _control_cmd.param2, _control_cmd.param3, _control_cmd.param4};
math::Vector<3> gimablDirectionEuler = math::Quaternion(gimbalDirectionQuat).to_dcm().to_euler();
roll += gimablDirectionEuler(0);
pitch += gimablDirectionEuler(1);
yaw += gimablDirectionEuler(2);
updated = true;
}
}
/* consider mount mode if parameter is set */
if (_parameters.use_mnt > 0) {
switch (_mount_mode) {
case vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT:
out_mount_mode = -1.0f;
roll = 0.0f;
pitch = 0.0f;
yaw = 0.0f;
break;
case vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL:
case vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING:
case vehicle_command_s::VEHICLE_MOUNT_MODE_RC_TARGETING:
case vehicle_command_s::VEHICLE_MOUNT_MODE_GPS_POINT:
out_mount_mode = 1.0f;
break;
default:
out_mount_mode = -1.0f;
}
}
if (updated) {
struct actuator_controls_s controls;
// DEVICE_DEBUG("publishing | roll: %8.4f pitch: %8.4f yaw: %8.4f", (double)roll, (double)pitch, (double)yaw);
/* fill in the final control values */
controls.timestamp = hrt_absolute_time();
controls.control[0] = roll;
controls.control[1] = pitch;
controls.control[2] = yaw;
//controls.control[3] = ; // camera shutter
controls.control[4] = out_mount_mode;
/* publish it */
orb_publish(ORB_ID(actuator_controls_2), _actuator_controls_2_topic, &controls);
}
/* notify anyone waiting for data */
poll_notify(POLLIN);
/* schedule a fresh cycle call when the measurement is done */
work_queue(LPWORK,
&_work,
(worker_t)&Gimbal::cycle_trampoline,
this,
USEC2TICK(GIMBAL_UPDATE_INTERVAL));
}
void
Gimbal::print_info()
{
}
/**
* Local functions in support of the shell command.
*/
namespace gimbal
{
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
const int ERROR = -1;
Gimbal *g_dev;
void start();
void stop();
void test();
void reset();
void info();
/**
* Start the driver.
*/
void
start()
{
if (g_dev != nullptr) {
errx(1, "already started");
}
/* create the driver */
g_dev = new Gimbal();
if (g_dev == nullptr) {
goto fail;
}
if (OK != g_dev->init()) {
goto fail;
}
exit(0);
fail:
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
}
errx(1, "driver start failed");
}
/**
* Stop the driver
*/
void stop()
{
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
} else {
errx(1, "driver not running");
}
exit(0);
}
/**
* Perform some basic functional tests on the driver;
* make sure we can collect data from the sensor in polled
* and automatic modes.
*/
void
test()
{
int fd = open(GIMBAL_DEVICE_PATH, O_RDONLY);
if (ioctl(fd, GIMBALIOCATTCOMPENSATE, 1) < 0) {
err(1, "failed enabling compensation");
}
errx(0, "PASS");
}
/**
* Reset the driver.
*/
void
reset()
{
int fd = open(GIMBAL_DEVICE_PATH, O_RDONLY);
if (fd < 0) {
err(1, "failed ");
}
// if (ioctl(fd, GIMBALIOCATTCOMPENSATE, 1) < 0) {
// err(1, "failed enabling compensation");
// }
exit(0);
}
/**
* Print a little info about the driver.
*/
void
info()
{
if (g_dev == nullptr) {
errx(1, "driver not running");
}
printf("state @ %p\n", g_dev);
g_dev->print_info();
exit(0);
}
} // namespace
int
gimbal_main(int argc, char *argv[])
{
/*
* Start/load the driver.
*/
if (!strcmp(argv[1], "start")) {
gimbal::start();
}
/*
* Stop the driver
*/
if (!strcmp(argv[1], "stop")) {
gimbal::stop();
}
/*
* Test the driver/device.
*/
if (!strcmp(argv[1], "test")) {
gimbal::test();
}
/*
* Reset the driver.
*/
if (!strcmp(argv[1], "reset")) {
gimbal::reset();
}
/*
* Print driver information.
*/
if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) {
gimbal::info();
}
errx(1, "unrecognized command, try 'start', 'stop', 'reset', 'test' or 'info'");
}
src/drivers/gimbal/gimbal_params.c
-74
View File
@@ -1,74 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2015 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 gimbal_params.c
*
* Parameters for the gimbal controller.
*
* @author Andreas Antener <andreas@uaventure.com>
*/
#include <px4_config.h>
#include <systemlib/param/param.h>
/**
* Consider mount operation mode.
*
* If set to 1, mount mode will be enforced.
*
* @boolean
* @group Gimbal
*/
PARAM_DEFINE_INT32(GMB_USE_MNT, 0);
/**
* Auxiliary switch to set mount operation mode.
*
* Set to 0 to disable manual mode control.
*
* If set to an auxiliary switch:
* Switch off means the gimbal is put into safe/locked position.
* Switch on means the gimbal can move freely, and landing gear
* will be retracted if applicable.
*
* @value 0 Disable
* @value 1 AUX1
* @value 2 AUX2
* @value 3 AUX3
* @min 0
* @max 3
* @group Gimbal
*/
PARAM_DEFINE_INT32(GMB_AUX_MNT_CHN, 0);
src/drivers/gimbal/CMakeLists.txt → src/drivers/vmount/CMakeLists.txt
+9 -6
View File
@@ -1,6 +1,6 @@
############################################################################
#
# Copyright (c) 2015 PX4 Development Team. All rights reserved.
# 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
@@ -31,15 +31,18 @@
#
############################################################################
px4_add_module(
MODULE drivers__gimbal
MAIN gimbal
MODULE drivers__vmount
MAIN vmount
STACK_MAIN 1024
COMPILE_FLAGS
-Os
SRCS
gimbal.cpp
gimbal_params.c
vmount.cpp
vmount_params.c
vmount_rc.cpp
vmount_mavlink.cpp
vmount_onboard.cpp
DEPENDS
platforms__common
)
# vim: set noet ft=cmake fenc=utf-8 ff=unix :
# vim: set noet ft=cmake fenc=utf-8 ff=unix :
src/drivers/vmount/vmount.cpp
+692
View File
@@ -0,0 +1,692 @@
/****************************************************************************
*
* Copyright (c) 2013-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 vmount.cpp
* @author Leon Müller (thedevleon)
* MAV_MOUNT driver for controlling mavlink gimbals, rc gimbals/servors and
* future kinds of mounts.
*
*/
#include <stdlib.h>
#include <stdio.h>
#include <stdbool.h>
#include <string.h>
#include <sys/types.h>
#include <fcntl.h>
#include <unistd.h>
#include <systemlib/err.h>
#include <systemlib/systemlib.h>
#include "vmount_mavlink.h"
#include "vmount_rc.h"
#include "vmount_onboard.h"
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_roi.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_command_ack.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <px4_config.h>
#include <px4_posix.h>
#include <poll.h>
/* thread state */
static volatile bool thread_should_exit = false;
static volatile bool thread_running = false;
static volatile bool thread_should_restart = false;
static int vmount_task;
typedef enum { IDLE = -1, MAVLINK = 0, RC = 1, ONBOARD = 2 } vmount_state_t;
static vmount_state_t vmount_state = IDLE;
/* polling */
px4_pollfd_struct_t polls[7];
/* functions */
static void usage(void);
static void vmount_update_topics(void);
static void update_params(void);
static bool get_params(void);
static float get_aux_value(int);
static void ack_mount_command(uint16_t command);
static int vmount_thread_main(int argc, char *argv[]);
extern "C" __EXPORT int vmount_main(int argc, char *argv[]);
/* uORB subscriptions */
static int vehicle_roi_sub = -1;
static int vehicle_global_position_sub = -1;
static int vehicle_command_sub = -1;
static int vehicle_attitude_sub = -1;
static int position_setpoint_triplet_sub = -1;
static int manual_control_setpoint_sub = -1;
static int parameter_update_sub = -1;
/* uORB publication */
static orb_advert_t vehicle_command_ack_pub;
static struct vehicle_roi_s vehicle_roi;
static bool vehicle_roi_updated;
static struct vehicle_global_position_s vehicle_global_position;
static bool vehicle_global_position_updated;
static struct vehicle_command_s vehicle_command;
static bool vehicle_command_updated;
static struct vehicle_attitude_s vehicle_attitude;
static bool vehicle_attitude_updated;
static struct position_setpoint_triplet_s position_setpoint_triplet;
static bool position_setpoint_triplet_updated;
static struct manual_control_setpoint_s manual_control_setpoint;
static bool manual_control_setpoint_updated;
static struct parameter_update_s parameter_update;
static bool parameter_update_updated;
static struct vehicle_command_ack_s vehicle_command_ack;
static struct {
int mnt_mode;
int mnt_mav_sysid;
int mnt_mav_compid;
int mnt_ob_lock_mode;
int mnt_ob_norm_mode;
int mnt_man_control;
int mnt_man_pitch;
int mnt_man_roll;
int mnt_man_yaw;
int mnt_mode_ovr;
} params;
static struct {
param_t mnt_mode;
param_t mnt_mav_sysid;
param_t mnt_mav_compid;
param_t mnt_ob_lock_mode;
param_t mnt_ob_norm_mode;
param_t mnt_man_control;
param_t mnt_man_pitch;
param_t mnt_man_roll;
param_t mnt_man_yaw;
param_t mnt_mode_ovr;
} params_handels;
static bool manual_control_desired;
/**
* Print command usage information
*/
static void usage()
{
fprintf(stderr,
"usage: vmount start\n"
" vmount stop\n"
" vmount status\n");
exit(1);
}
/**
* The daemon thread.
*/
static int vmount_thread_main(int argc, char *argv[])
{
if (!get_params()) {
warnx("could not get mount parameters!");
}
if (params.mnt_mode == 0) { vmount_state = IDLE;}
else if (params.mnt_mode == 1) { vmount_state = MAVLINK;}
else if (params.mnt_mode == 2) { vmount_state = RC;}
else if (params.mnt_mode == 3) { vmount_state = ONBOARD;}
//TODO is this needed?
memset(&vehicle_roi, 0, sizeof(vehicle_roi));
memset(&vehicle_global_position, 0, sizeof(vehicle_global_position));
memset(&vehicle_command, 0, sizeof(vehicle_command));
memset(&vehicle_attitude, 0, sizeof(vehicle_attitude));
memset(&position_setpoint_triplet, 0, sizeof(position_setpoint_triplet));
memset(&manual_control_setpoint, 0, sizeof(manual_control_setpoint));
memset(&parameter_update, 0, sizeof(parameter_update));
memset(&vehicle_command_ack, 0, sizeof(vehicle_command_ack));
vehicle_roi_sub = orb_subscribe(ORB_ID(vehicle_roi));
vehicle_global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command));
vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
position_setpoint_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
vehicle_command_ack_pub = orb_advertise(ORB_ID(vehicle_command_ack), &vehicle_command_ack);
if (!vehicle_roi_sub || !position_setpoint_triplet_sub ||
!manual_control_setpoint_sub || !vehicle_global_position_sub ||
!vehicle_command_sub || !parameter_update_sub || !vehicle_command_ack_pub ||
!vehicle_attitude_sub) {
err(1, "could not subscribe to uORB topics");
}
polls[0].fd = vehicle_roi_sub;
polls[0].events = POLLIN;
polls[1].fd = vehicle_global_position_sub;
polls[1].events = POLLIN;
polls[2].fd = vehicle_attitude_sub;
polls[2].events = POLLIN;
polls[3].fd = vehicle_command_sub;
polls[3].events = POLLIN;
polls[4].fd = position_setpoint_triplet_sub;
polls[4].events = POLLIN;
polls[5].fd = manual_control_setpoint_sub;
polls[5].events = POLLIN;
polls[6].fd = parameter_update_sub;
polls[6].events = POLLIN;
thread_running = true;
run: {
if (vmount_state == MAVLINK) {
if (!vmount_mavlink_init()) {
err(1, "could not initiate vmount_mavlink");
}
warnx("running mount driver in mavlink mode");
if (params.mnt_man_control) manual_control_desired = true;
while (!thread_should_exit && !thread_should_restart) {
vmount_update_topics();
if (vehicle_roi_updated) {
vehicle_roi_updated = false;
vmount_mavlink_configure(vehicle_roi.mode, (params.mnt_man_control == 1), params.mnt_mav_sysid, params.mnt_mav_compid);
if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_NONE) {
if (params.mnt_man_control) manual_control_desired = true;
} else if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_WPNEXT) {
manual_control_desired = false;
vmount_mavlink_set_location(
position_setpoint_triplet.next.lat,
position_setpoint_triplet.next.lon,
position_setpoint_triplet.next.alt
);
} else if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_WPINDEX) {
manual_control_desired = false;
//TODO how to do this?
} else if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_LOCATION) {
manual_control_desired = false;
vmount_mavlink_set_location(
vehicle_roi.lat,
vehicle_roi.lon,
vehicle_roi.alt
);
} else if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_TARGET) {
manual_control_desired = false;
//TODO is this even suported?
}
}
else if (manual_control_desired && manual_control_setpoint_updated)
{
manual_control_setpoint_updated = false;
vmount_mavlink_point_manual(get_aux_value(params.mnt_man_pitch), get_aux_value(params.mnt_man_roll), get_aux_value(params.mnt_man_yaw));
}
else if (!manual_control_desired && vehicle_global_position_updated)
{
vehicle_global_position_updated = false;
vmount_mavlink_point(vehicle_global_position.lat, vehicle_global_position.lon, vehicle_global_position.alt);
}
vmount_mavlink_deinit();
}
} else if (vmount_state == RC) {
if (!vmount_rc_init()) {
err(1, "could not initiate vmount_rc");
}
warnx("running mount driver in rc mode");
if (params.mnt_man_control) {
manual_control_desired = true;
}
while (!thread_should_exit && !thread_should_restart) {
vmount_update_topics();
if (vehicle_roi_updated) {
vehicle_roi_updated = false;
vmount_rc_configure(vehicle_roi.mode, (params.mnt_man_control == 1), params.mnt_ob_norm_mode, params.mnt_ob_lock_mode);
if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_NONE) {
if (params.mnt_man_control) manual_control_desired = true;
} else if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_WPNEXT) {
manual_control_desired = false;
vmount_rc_set_location(
position_setpoint_triplet.next.lat,
position_setpoint_triplet.next.lon,
position_setpoint_triplet.next.alt
);
} else if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_WPINDEX) {
manual_control_desired = false;
//TODO how to do this?
} else if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_LOCATION) {
manual_control_desired = false;
vmount_rc_set_location(
vehicle_roi.lat,
vehicle_roi.lon,
vehicle_roi.alt
);
} else if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_TARGET) {
manual_control_desired = false;
//TODO is this even suported?
}
}
else if (manual_control_desired && manual_control_setpoint_updated) {
manual_control_setpoint_updated = false;
vmount_rc_point_manual(get_aux_value(params.mnt_man_pitch), get_aux_value(params.mnt_man_roll), get_aux_value(params.mnt_man_yaw));
}
else if (!manual_control_desired && vehicle_global_position_updated)
{
vehicle_global_position_updated = false;
vmount_rc_point(vehicle_global_position.lat, vehicle_global_position.lon, vehicle_global_position.alt);
}
}
vmount_rc_deinit();
} else if (vmount_state == ONBOARD) {
if (!vmount_onboard_init()) {
err(1, "could not initiate vmount_onboard");
}
warnx("running mount driver in onboard mode");
if (params.mnt_man_control) manual_control_desired = true;
while (!thread_should_exit && !thread_should_restart) {
vmount_update_topics();
if(vehicle_attitude_updated)
{
vmount_onboard_update_attitude(vehicle_attitude);
vehicle_attitude_updated = false;
}
if (params.mnt_mode_ovr && manual_control_setpoint_updated) {
manual_control_setpoint_updated = false;
float ovr_value = get_aux_value(params.mnt_mode_ovr);
if(ovr_value < 0.0f)
{
vmount_onboard_set_mode(vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT);
}
else if (ovr_value > 0.0f)
{
vmount_onboard_set_mode(vehicle_command_s::VEHICLE_MOUNT_MODE_RC_TARGETING);
}
}
else if (vehicle_command_updated) {
vehicle_command_updated = false;
if(vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL)
{
switch ((int)vehicle_command.param7) {
case vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT:
case vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL:
manual_control_desired = false;
break;
case vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING:
manual_control_desired = false;
vmount_onboard_set_mode(vehicle_command.param7);
vmount_onboard_set_manual(vehicle_command.param1,
vehicle_command.param2, vehicle_command.param3);
case vehicle_command_s::VEHICLE_MOUNT_MODE_RC_TARGETING:
if (params.mnt_man_control) {
manual_control_desired = true;
vmount_onboard_set_mode(vehicle_command.param7);
}
case vehicle_command_s::VEHICLE_MOUNT_MODE_GPS_POINT:
manual_control_desired = false;
vmount_onboard_set_mode(vehicle_command.param7);
vmount_onboard_set_location(
vehicle_command.param1,
vehicle_command.param2,
vehicle_command.param3);
default:
manual_control_desired = false;
break;
}
ack_mount_command(vehicle_command.command);
}
else if(vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE)
{
vmount_onboard_configure(vehicle_command.param1,
((uint8_t) vehicle_command.param2 == 1), ((uint8_t) vehicle_command.param3 == 1), ((uint8_t) vehicle_command.param4 == 1));
ack_mount_command(vehicle_command.command);
}
}
else if (manual_control_desired && manual_control_setpoint_updated)
{
manual_control_setpoint_updated = false;
vmount_onboard_point_manual(get_aux_value(params.mnt_man_pitch), get_aux_value(params.mnt_man_roll), get_aux_value(params.mnt_man_yaw));
}
else if (!manual_control_desired && vehicle_global_position_updated)
{
vehicle_global_position_updated = false;
vmount_onboard_point(vehicle_global_position.lat, vehicle_global_position.lon, vehicle_global_position.alt);
}
}
} else if (vmount_state == IDLE)
{
warnx("running mount driver in idle mode");
while (!thread_should_exit && !thread_should_restart) {
vmount_update_topics();
}
}
}
if (thread_should_restart)
{
thread_should_restart = false;
warnx("parameter update, restarting mount driver!");
goto run;
}
thread_running = false;
return 0;
}
/**
* The main command function.
* Processes command line arguments and starts the daemon.
*/
int vmount_main(int argc, char *argv[])
{
if (argc < 1) {
warnx("missing command");
usage();
}
if (!strcmp(argv[1], "start")) {
/* this is not an error */
if (thread_running) {
errx(0, "mount driver already running");
}
thread_should_exit = false;
vmount_task = px4_task_spawn_cmd("vmount",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT + 40, //TODO we might want a higher priority?
1100,
vmount_thread_main,
(char *const *)argv);
while (!thread_running) {
usleep(200);
}
exit(0);
}
if (!strcmp(argv[1], "stop")) {
/* this is not an error */
if (!thread_running) {
errx(0, "mount driver already stopped");
}
thread_should_exit = true;
while (thread_running) {
usleep(1000000);
warnx(".");
}
warnx("terminated.");
exit(0);
}
if (!strcmp(argv[1], "status")) {
if (thread_running) {
switch (vmount_state) {
case IDLE:
errx(0, "running: IDLE");
break;
case MAVLINK:
errx(0, "running: MAVLINK - Manual Control? %d", manual_control_desired);
break;
case RC:
errx(0, "running: RC - Manual Control? %d", manual_control_desired);
break;
case ONBOARD:
errx(0, "running: ONBOARD");
break;
}
} else {
errx(1, "not running");
}
}
warnx("unrecognized command");
usage();
/* not getting here */
return 0;
}
/* Update oURB topics */
void vmount_update_topics()
{
/*
polls = {
{ .fd = vehicle_roi_sub, .events = POLLIN },
{ .fd = vehicle_global_position_sub, .events = POLLIN },
{ .fd = vehicle_attitude_sub, .events = POLLIN },
{ .fd = vehicle_command_sub, .events = POLLIN },
{ .fd = position_setpoint_triplet_sub, .events = POLLIN },
{ .fd = manual_control_setpoint_sub, .events = POLLIN },
{ .fd = parameter_update_sub, .events = POLLIN },
};
*/
/* wait for sensor update of 7 file descriptors for 100 ms */
int poll_ret = px4_poll(polls, 7, 100);
//Nothing updated.
if(poll_ret == 0) return;
if (polls[0].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_roi), vehicle_roi_sub, &vehicle_roi);
vehicle_roi_updated = true;
}
if (polls[1].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &vehicle_global_position);
vehicle_global_position_updated = true;
}
if (polls[2].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &vehicle_attitude);
vehicle_attitude_updated = true;
}
if (polls[3].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_command), vehicle_command_sub, &vehicle_command);
vehicle_command_updated = true;
}
if (polls[4].revents & POLLIN) {
orb_copy(ORB_ID(position_setpoint_triplet), position_setpoint_triplet_sub, &position_setpoint_triplet);
position_setpoint_triplet_updated = true;
}
if (polls[5].revents & POLLIN) {
orb_copy(ORB_ID(manual_control_setpoint), manual_control_setpoint_sub, &manual_control_setpoint);
manual_control_setpoint_updated = true;
}
if (polls[6].revents & POLLIN) {
orb_copy(ORB_ID(parameter_update), parameter_update_sub, &parameter_update);
parameter_update_updated = true;
update_params();
}
}
void update_params()
{
param_get(params_handels.mnt_mode, &params.mnt_mode);
param_get(params_handels.mnt_mav_sysid, &params.mnt_mav_sysid);
param_get(params_handels.mnt_mav_compid, &params.mnt_mav_compid);
param_get(params_handels.mnt_ob_lock_mode, &params.mnt_ob_lock_mode);
param_get(params_handels.mnt_ob_norm_mode, &params.mnt_ob_norm_mode);
param_get(params_handels.mnt_man_control, &params.mnt_man_control);
param_get(params_handels.mnt_man_pitch, &params.mnt_man_pitch);
param_get(params_handels.mnt_man_roll, &params.mnt_man_roll);
param_get(params_handels.mnt_man_yaw, &params.mnt_man_yaw);
param_get(params_handels.mnt_mode_ovr, &params.mnt_mode_ovr);
if (vmount_state != params.mnt_mode)
{
thread_should_restart = true;
}
else if (vmount_state == MAVLINK)
{
vmount_mavlink_configure(vehicle_roi.mode, (params.mnt_man_control == 1), params.mnt_mav_sysid, params.mnt_mav_compid);
}
else if (vmount_state == RC)
{
vmount_rc_configure(vehicle_roi.mode, (params.mnt_man_control == 1), params.mnt_ob_norm_mode, params.mnt_ob_lock_mode);
}
else if(vmount_state == ONBOARD)
{
//None of the parameter changes require a reconfiguration of the onboard mount.
}
}
bool get_params()
{
params_handels.mnt_mode = param_find("MNT_MODE");
params_handels.mnt_mav_sysid = param_find("MNT_MAV_SYSID");
params_handels.mnt_mav_compid = param_find("MNT_MAV_COMPID");
params_handels.mnt_ob_lock_mode = param_find("MNT_OB_LOCK_MODE");
params_handels.mnt_ob_norm_mode = param_find("MNT_OB_NORM_MODE");
params_handels.mnt_man_control = param_find("MNT_MAN_CONTROL");
params_handels.mnt_man_pitch = param_find("MNT_MAN_PITCH");
params_handels.mnt_man_roll = param_find("MNT_MAN_ROLL");
params_handels.mnt_man_yaw = param_find("MNT_MAN_YAW");
params_handels.mnt_mode_ovr = param_find("MNT_MODE_OVR");
if (param_get(params_handels.mnt_mode, &params.mnt_mode) ||
param_get(params_handels.mnt_mav_sysid, &params.mnt_mav_sysid) ||
param_get(params_handels.mnt_mav_compid, &params.mnt_mav_compid) ||
param_get(params_handels.mnt_ob_lock_mode, &params.mnt_ob_lock_mode) ||
param_get(params_handels.mnt_ob_norm_mode, &params.mnt_ob_norm_mode) ||
param_get(params_handels.mnt_man_control, &params.mnt_man_control) ||
param_get(params_handels.mnt_man_pitch, &params.mnt_man_pitch) ||
param_get(params_handels.mnt_man_roll, &params.mnt_man_roll) ||
param_get(params_handels.mnt_man_yaw, &params.mnt_man_yaw) ||
param_get(params_handels.mnt_mode_ovr, &params.mnt_mode_ovr)) {
return false;
}
return true;
}
void ack_mount_command(uint16_t command)
{
vehicle_command_ack.command = command;
vehicle_command_ack.result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
orb_publish(ORB_ID(vehicle_command_ack), vehicle_command_ack_pub, &vehicle_command_ack);
}
float get_aux_value(int aux)
{
switch (aux)
{
case 0:
return 0.0f;
case 1:
return manual_control_setpoint.aux1;
case 2:
return manual_control_setpoint.aux2;
case 3:
return manual_control_setpoint.aux3;
case 4:
return manual_control_setpoint.aux4;
case 5:
return manual_control_setpoint.aux5;
default:
return 0.0f;
}
}
src/drivers/vmount/vmount_mavlink.cpp
+178
View File
@@ -0,0 +1,178 @@
/****************************************************************************
*
* Copyright (c) 2013-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 vmount_mavlink.cpp
* @author Leon Müller (thedevleon)
*
*/
#include "vmount_mavlink.h"
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <arch/math.h>
#include <geo/geo.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_roi.h>
#include <px4_posix.h>
/* uORB advertising */
static struct vehicle_command_s *vehicle_command;
static orb_advert_t vehicle_command_pub;
static int sys_id;
static int comp_id;
static double lat;
static double lon;
static float alt;
bool vmount_mavlink_init()
{
memset(&vehicle_command, 0, sizeof(vehicle_command));
vehicle_command_pub = orb_advertise(ORB_ID(vehicle_command), &vehicle_command);
if (!vehicle_command_pub) { return false; }
return true;
}
void vmount_mavlink_deinit()
{
orb_unadvertise(vehicle_command_pub);
free(vehicle_command);
}
/*
* MAV_CMD_DO_MOUNT_CONFIGURE (#204)
* param1 = mount_mode (1 = MAV_MOUNT_MODE_NEUTRAL recenters camera)
*/
void vmount_mavlink_configure(int roi_mode, bool man_control, int sysid, int compid)
{
sys_id = sysid;
comp_id = compid;
vehicle_command->command = vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE;
vehicle_command->target_system = sys_id;
vehicle_command->target_component = comp_id;
switch (roi_mode) {
case vehicle_roi_s::VEHICLE_ROI_NONE:
if (man_control) { vehicle_command->param1 = vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING; }
else { vehicle_command->param1 = vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL; }
break;
case vehicle_roi_s::VEHICLE_ROI_WPNEXT:
vehicle_command->param1 = vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING;
break;
case vehicle_roi_s::VEHICLE_ROI_WPINDEX:
vehicle_command->param1 = vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING;
break;
case vehicle_roi_s::VEHICLE_ROI_LOCATION:
vehicle_command->param1 = vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING;
break;
case vehicle_roi_s::VEHICLE_ROI_TARGET:
vehicle_command->param1 = vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING;
break;
default:
vehicle_command->param1 = vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL;
break;
}
orb_publish(ORB_ID(vehicle_command), vehicle_command_pub, vehicle_command);
}
/* MAV_CMD_DO_MOUNT_CONTROL (#205)
* param1 = pitch, angle in degree or pwm input
* param2 = roll, angle in degree or pwm input
* param3 = yaw, angle in degree or pwm input
* param6 = typeflags (uint16_t)
* 0x0001: pitch, 0: unlimited, 1: limited, see CMD_SETANGLE
* 0x0002: roll, 0: unlimited, 1: limited, see CMD_SETANGLE
* 0x0004: yaw, 0: unlimited, 1: limited, see CMD_SETANGLE
* 0x0100: input type, 0: angle input (see CMD_SETANGLE), 1: pwm input (see CMD_SETPITCHROLLYAW)
*/
void vmount_mavlink_set_location(double lat_new, double lon_new, float alt_new)
{
lat = lat_new;
lon = lon_new;
alt = alt_new;
}
void vmount_mavlink_point(double global_lat, double global_lon, float global_alt)
{
float pitch = vmount_mavlink_calculate_pitch(global_lat, global_lon, global_alt);
float roll = 0.0f; // We want a level horizon, so leave roll at 0 degrees.
float yaw = get_bearing_to_next_waypoint(global_lat, global_lon, lat, lon) * (float)M_RAD_TO_DEG;
vmount_mavlink_point_manual(pitch, roll, yaw);
}
void vmount_mavlink_point_manual(float pitch_new, float roll_new, float yaw_new)
{
vehicle_command->command = vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL;
vehicle_command->target_system = sys_id;
vehicle_command->target_component = comp_id;
vehicle_command->param1 = pitch_new;
vehicle_command->param2 = roll_new;
vehicle_command->param3 = yaw_new;
orb_publish(ORB_ID(vehicle_command), vehicle_command_pub, vehicle_command);
}
float vmount_mavlink_calculate_pitch(double global_lat, double global_lon, float global_alt)
{
float x = (lon - global_lon) * cos(M_DEG_TO_RAD * ((global_lat + lat) * 0.00000005)) * 0.01113195;
float y = (lat - global_lat) * 0.01113195;
float z = (alt - global_alt);
float target_distance = sqrtf(powf(x, 2) + powf(y, 2));
return atan2(z, target_distance) * M_RAD_TO_DEG;
}
src/drivers/vmount/vmount_mavlink.h
+56
View File
@@ -0,0 +1,56 @@
/****************************************************************************
*
* Copyright (c) 2013-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 vmount_mavlink.h
* @author Leon Müller (thedevleon)
*
*/
#ifndef _VMOUNT_MAVLINK_H
#define _VMOUNT_MAVLINK_H
#include <sys/types.h>
#include <stdbool.h>
// Public functions
bool vmount_mavlink_init();
void vmount_mavlink_deinit(void);
void vmount_mavlink_configure(int roi_mode, bool man_control, int sysid, int compid);
void vmount_mavlink_set_location(double lat_new, double lon_new, float alt_new);
void vmount_mavlink_point(double global_lat, double global_lon, float global_alt);
void vmount_mavlink_point_manual(float pitch_new, float roll_new, float yaw_new);
float vmount_mavlink_calculate_pitch(double global_lat, double global_lon, float global_alt);
#endif /* _VMOUNT_MAVLINK_H */
src/drivers/vmount/vmount_onboard.cpp
+193
View File
@@ -0,0 +1,193 @@
/****************************************************************************
*
* Copyright (c) 2013-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 vmount_onboard.cpp
* @author Leon Müller (thedevleon)
*
*/
#include "vmount_onboard.h"
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <arch/math.h>
#include <geo/geo.h>
#include <uORB/uORB.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_command.h>
/* uORB topics */
static struct actuator_controls_s actuator_controls;
static orb_advert_t actuator_controls_pub;
static struct vehicle_attitude_s vehicle_attitude;
static int mount_mode;
static float pitch;
static float roll;
static float yaw;
static float retracts;
static int stab_pitch;
static int stab_roll;
static int stab_yaw;
static double lat;
static double lon;
static float alt;
bool vmount_onboard_init()
{
memset(&actuator_controls, 0, sizeof(actuator_controls));
memset(&vehicle_attitude, 0, sizeof(vehicle_attitude));
actuator_controls_pub = orb_advertise(ORB_ID(actuator_controls_2), &actuator_controls);
if (!actuator_controls_pub) { return false; }
vmount_onboard_configure(vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT, 0.0f, 0.0f, 0.0f);
return true;
}
void vmount_onboard_deinit()
{
orb_unadvertise(actuator_controls_pub);
//free(actuator_controls);
//free(vehicle_attitude);
}
void vmount_onboard_configure(int new_mount_mode, bool new_stab_roll, bool new_stab_pitch, bool new_stab_yaw)
{
mount_mode = new_mount_mode;
stab_roll = new_stab_roll;
stab_pitch = new_stab_pitch;
stab_yaw = new_stab_yaw;
switch (mount_mode) {
case vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT:
retracts = -1.0f;
vmount_onboard_set_manual(0.0f, 0.0f, 0.0f);
break;
case vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL:
retracts = 0.0f;
vmount_onboard_set_manual(0.0f, 0.0f, 0.0f);
break;
case vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING:
case vehicle_command_s::VEHICLE_MOUNT_MODE_RC_TARGETING:
case vehicle_command_s::VEHICLE_MOUNT_MODE_GPS_POINT:
retracts = 0.0f;
default:
break;
}
}
void vmount_onboard_set_location(double lat_new, double lon_new, float alt_new)
{
lat = lat_new;
lon = lon_new;
alt = alt_new;
}
void vmount_onboard_set_manual(double pitch_new, double roll_new, float yaw_new)
{
pitch = pitch_new;
roll = roll_new;
yaw = yaw_new;
}
void vmount_onboard_set_mode(int new_mount_mode)
{
vmount_onboard_configure(new_mount_mode, stab_roll, stab_pitch, stab_yaw);
}
void vmount_onboard_point(double global_lat, double global_lon, float global_alt)
{
if (mount_mode == vehicle_command_s::VEHICLE_MOUNT_MODE_GPS_POINT) {
pitch = vmount_onboard_calculate_pitch(global_lat, global_lon, global_alt);
roll = 0.0f; // We want a level horizon, so leave roll at 0 degrees.
yaw = get_bearing_to_next_waypoint(global_lat, global_lon, lat, lon) * (float)M_RAD_TO_DEG;
}
vmount_onboard_point_manual(pitch, roll, yaw);
}
void vmount_onboard_point_manual(float pitch_target, float roll_target, float yaw_target)
{
float pitch_new = pitch_target;
float roll_new = roll_target;
float yaw_new = yaw_target;
if (mount_mode != vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT &&
mount_mode != vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL) {
if (stab_roll) {
roll_new += 1.0f / M_PI_F * -vehicle_attitude.roll;
}
if (stab_pitch) {
pitch_new += 1.0f / M_PI_F * -vehicle_attitude.pitch;
}
if (stab_yaw) {
yaw_new += 1.0f / M_PI_F * vehicle_attitude.yaw;
}
}
actuator_controls.timestamp = hrt_absolute_time();
actuator_controls.control[0] = pitch_new;
actuator_controls.control[1] = roll_new;
actuator_controls.control[2] = yaw_new;
actuator_controls.control[4] = retracts;
orb_publish(ORB_ID(actuator_controls_2), actuator_controls_pub, &actuator_controls);
}
void vmount_onboard_update_attitude(vehicle_attitude_s vehicle_attitude_new)
{
vehicle_attitude = vehicle_attitude_new;
}
float vmount_onboard_calculate_pitch(double global_lat, double global_lon, float global_alt)
{
float x = (lon - global_lon) * cos(M_DEG_TO_RAD * ((global_lat + lat) * 0.00000005)) * 0.01113195;
float y = (lat - global_lat) * 0.01113195;
float z = (alt - global_alt);
float target_distance = sqrtf(powf(x, 2) + powf(y, 2));
return atan2(z, target_distance) * M_RAD_TO_DEG;
}
src/drivers/vmount/vmount_onboard.h
+59
View File
@@ -0,0 +1,59 @@
/****************************************************************************
*
* Copyright (c) 2013-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 vmount_onboard
* @author Leon Müller (thedevleon)
*
*/
#ifndef _VMOUNT_ONBOARD_H
#define _VMOUNT_ONBOARD_H
#include <sys/types.h>
#include <stdbool.h>
#include <uORB/topics/vehicle_attitude.h>
// Public functions
bool vmount_onboard_init(void);
void vmount_onboard_deinit(void);
void vmount_onboard_configure(int new_mount_mode, bool new_stab_roll, bool new_stab_pitch, bool new_stab_yaw);
void vmount_onboard_set_location(double lat, double lon, float alt);
void vmount_onboard_set_manual(double pitch_new, double roll_new, float yaw_new);
void vmount_onboard_set_mode(int new_mount_mode);
void vmount_onboard_point(double global_lat, double global_lon, float global_alt);
void vmount_onboard_point_manual(float pitch_target, float roll_target, float yaw_target);
void vmount_onboard_update_attitude(vehicle_attitude_s vehicle_attitude_new);
float vmount_onboard_calculate_pitch(double global_lat, double global_lon, float global_alt);
#endif /* _VMOUNT_ONBOARD_H */
src/drivers/vmount/vmount_params.c
+164
View File
@@ -0,0 +1,164 @@
/****************************************************************************
*
* Copyright (c) 2013-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 vmount_params.c
* @author Leon Müller (thedevleon)
*
*/
#include <px4_config.h>
#include <systemlib/param/param.h>
/**
* Mount operation mode
* MAVLINK and RC use the ROI (or RC input if enabled and no ROI set) to control a mount.
* ONBOARD uses MAV_CMD_DO_MOUNT_CONFIGURE and MAV_CMD_DO_MOUNT_CONTROL MavLink messages
* to control a mount.
* @value 0 DISABLE
* @value 1 MAVLINK
* @value 2 RC
* @value 3 ONBOARD
* @min 0
* @max 3
* @group Mount
*/
PARAM_DEFINE_INT32(MNT_MODE, 0);
/**
* Auxiliary channel to override current mount mode (only in ONBOARD mode)
* if <0.0f then MODE_RETRACT (retracts not retracted)
* if =0.0f then don't override
* if >0.0f then MODE_RC_TARGETING (retracts retracted)
* @value 0 Disable
* @value 1 AUX1
* @value 2 AUX2
* @value 3 AUX3
* @value 4 AUX4
* @value 5 AUX5
* @min 0
* @max 5
* @group Mount
*/
PARAM_DEFINE_INT32(MNT_MODE_OVR, 0);
/**
* Mavlink System ID
*
* @group Mount
*/
PARAM_DEFINE_INT32(MNT_MAV_SYSID, 71);
/**
* Mavlink Component ID
*
* @group Mount
*/
PARAM_DEFINE_INT32(MNT_MAV_COMPID, 67);
/**
* Mixer value for selecting normal mode
* if required by the gimbal (only in RC mode)
* @min -1.0
* @max 1.0
* @decimal 3
* @group Mount
*/
PARAM_DEFINE_FLOAT(MNT_OB_NORM_MODE, -1.0f);
/**
* Mixer value for selecting a locking mode
* if required for the gimbal (only in RC mode)
* @min -1.0
* @max 1.0
* @decimal 3
* @group Mount
*/
PARAM_DEFINE_FLOAT(MNT_OB_LOCK_MODE, 0.0f);
/**
* This enables the mount to be controlled when no ROI is set.
*
* If set to 1, the mount will be controlled by the AUX channels below
* when no ROI is set.
*
* @boolean
* @group Mount
*/
PARAM_DEFINE_INT32(MNT_MAN_CONTROL, 0);
/**
* Auxiliary channel to control roll.
*
* @value 0 Disable
* @value 1 AUX1
* @value 2 AUX2
* @value 3 AUX3
* @value 4 AUX4
* @value 5 AUX5
* @min 0
* @max 5
* @group Mount
*/
PARAM_DEFINE_INT32(MNT_MAN_ROLL, 0);
/**
* Auxiliary channel to control pitch.
*
* @value 0 Disable
* @value 1 AUX1
* @value 2 AUX2
* @value 3 AUX3
* @value 4 AUX4
* @value 5 AUX5
* @min 0
* @max 5
* @group Mount
*/
PARAM_DEFINE_INT32(MNT_MAN_PITCH, 0);
/**
* Auxiliary channel to control yaw.
*
* @value 0 Disable
* @value 1 AUX1
* @value 2 AUX2
* @value 3 AUX3
* @value 4 AUX4
* @value 5 AUX5
* @min 0
* @max 5
* @group Mount
*/
PARAM_DEFINE_INT32(MNT_MAN_YAW, 0);
src/drivers/vmount/vmount_rc.cpp
+155
View File
@@ -0,0 +1,155 @@
/****************************************************************************
*
* Copyright (c) 2013-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 vmount_rc.c
* @author Leon Müller (thedevleon)
*
*/
#include "vmount_rc.h"
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <arch/math.h>
#include <geo/geo.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_roi.h>
#include <uORB/topics/actuator_controls.h>
/* uORB advertising */
static struct actuator_controls_s actuator_controls;
static orb_advert_t actuator_controls_pub;
static double lon;
static double lat;
static float alt;
static float normal_mode;
static float locked_mode;
static bool locked;
bool vmount_rc_init()
{
memset(&actuator_controls, 0, sizeof(actuator_controls));
actuator_controls_pub = orb_advertise(ORB_ID(actuator_controls_2), &actuator_controls);
if (!actuator_controls_pub) { return false; }
locked = false;
normal_mode = -1.0f;
locked_mode = 0.0f;
return true;
}
void vmount_rc_deinit()
{
orb_unadvertise(actuator_controls_pub);
//free(&actuator_controls);
}
void vmount_rc_configure(int roi_mode, bool man_control, int normal_mode_new, int locked_mode_new)
{
normal_mode = normal_mode_new;
locked_mode = locked_mode_new;
switch (roi_mode) {
case vehicle_roi_s::VEHICLE_ROI_NONE:
locked = false;
if (!man_control) {vmount_rc_point_manual(0.0f, 0.0f, 0.0f);}
break;
case vehicle_roi_s::VEHICLE_ROI_WPNEXT:
case vehicle_roi_s::VEHICLE_ROI_WPINDEX:
case vehicle_roi_s::VEHICLE_ROI_LOCATION:
case vehicle_roi_s::VEHICLE_ROI_TARGET:
locked = true;
break;
default:
locked = false;
vmount_rc_point_manual(0.0f, 0.0f, 0.0f);
break;
}
}
void vmount_rc_set_location(double lat_new, double lon_new, float alt_new)
{
lat = lat_new;
lon = lon_new;
alt = alt_new;
}
void vmount_rc_point(double global_lat, double global_lon, float global_alt)
{
float pitch = vmount_rc_calculate_pitch(global_lat, global_lon, global_alt);
float roll = 0.0f; // We want a level horizon, so leave roll at 0 degrees.
float yaw = get_bearing_to_next_waypoint(global_lat, global_lon, lat, lon) * (float)M_RAD_TO_DEG;
vmount_rc_point_manual(pitch, roll, yaw);
}
void vmount_rc_point_manual(float pitch_new, float roll_new, float yaw_new)
{
actuator_controls.timestamp = hrt_absolute_time();
actuator_controls.control[0] = pitch_new;
actuator_controls.control[1] = roll_new;
actuator_controls.control[2] = yaw_new;
actuator_controls.control[3] = (locked) ? locked_mode : normal_mode;
/** for debugging purposes
warnx("actuator_controls_2 values:\t%8.4f\t%8.4f\t%8.4f\t%8.4f\t%8.4f",
(double)actuator_controls.control[0],
(double)actuator_controls.control[1],
(double)actuator_controls.control[2],
(double)actuator_controls.control[3],
(double)actuator_controls.control[4]);
**/
orb_publish(ORB_ID(actuator_controls_2), actuator_controls_pub, &actuator_controls);
}
float vmount_rc_calculate_pitch(double global_lat, double global_lon, float global_alt)
{
float x = (lon - global_lon) * cos(M_DEG_TO_RAD * ((global_lat + lat) * 0.00000005)) * 0.01113195;
float y = (lat - global_lat) * 0.01113195;
float z = (alt - global_alt);
float target_distance = sqrtf(powf(x, 2) + powf(y, 2));
return atan2(z, target_distance) * M_RAD_TO_DEG;
}
src/drivers/vmount/vmount_rc.h
+55
View File
@@ -0,0 +1,55 @@
/****************************************************************************
*
* Copyright (c) 2013-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 vmount_rc
* @author Leon Müller (thedevleon)
*
*/
#ifndef _VMOUNT_RC_H
#define _VMOUNT_RC_H
#include <sys/types.h>
#include <stdbool.h>
// Public functions
bool vmount_rc_init(void);
void vmount_rc_deinit(void);
void vmount_rc_configure(int roi_mode, bool man_control, int normal_mode_new, int locked_mode_new);
void vmount_rc_set_location(double lat_new, double lon_new, float alt_new);
void vmount_rc_point(double global_lat, double global_lon, float global_alt);
void vmount_rc_point_manual(float pitch_new, float roll_new, float yaw_new);
float vmount_rc_calculate_pitch(double global_lat, double global_lon, float global_alt);
#endif /* _VMOUNT_RC_H */
src/modules/commander/commander.cpp
+44 -7
View File
@@ -89,8 +89,9 @@
#include <uORB/topics/mission.h>
#include <uORB/topics/mission_result.h>
#include <uORB/topics/offboard_control_mode.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_roi.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/safety.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/subsystem_info.h>
@@ -175,6 +176,7 @@ static float eph_threshold = 5.0f;
static float epv_threshold = 10.0f;
static struct vehicle_status_s status = {};
static struct vehicle_roi_s _roi = {};
static struct battery_status_s battery = {};
static struct actuator_armed_s armed = {};
static struct safety_s safety = {};
@@ -228,7 +230,8 @@ void usage(const char *reason);
bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd,
struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos,
struct vehicle_local_position_s *local_pos, struct vehicle_attitude_s *attitude, orb_advert_t *home_pub,
orb_advert_t *command_ack_pub, struct vehicle_command_ack_s *command_ack);
orb_advert_t *command_ack_pub, struct vehicle_command_ack_s *command_ack, struct vehicle_roi_s *roi,
orb_advert_t *roi_pub);
/**
* Mainloop of commander.
@@ -656,7 +659,8 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
struct vehicle_command_s *cmd, struct actuator_armed_s *armed_local,
struct home_position_s *home, struct vehicle_global_position_s *global_pos,
struct vehicle_local_position_s *local_pos, struct vehicle_attitude_s *attitude, orb_advert_t *home_pub,
orb_advert_t *command_ack_pub, struct vehicle_command_ack_s *command_ack)
orb_advert_t *command_ack_pub, struct vehicle_command_ack_s *command_ack,
struct vehicle_roi_s *roi, orb_advert_t *roi_pub)
{
/* only handle commands that are meant to be handled by this system and component */
if (cmd->target_system != status_local->system_id || ((cmd->target_component != status_local->component_id)
@@ -1066,9 +1070,41 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
}
break;
case vehicle_command_s::VEHICLE_CMD_NAV_ROI:
case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI: {
roi->mode = cmd->param1;
if (roi->mode == vehicle_roi_s::VEHICLE_ROI_WPINDEX) {
roi->mission_seq = cmd->param2;
}
else if (roi->mode == vehicle_roi_s::VEHICLE_ROI_LOCATION) {
roi->lat = cmd->param5;
roi->lon = cmd->param6;
roi->alt = cmd->param7;
}
else if (roi->mode == vehicle_roi_s::VEHICLE_ROI_TARGET) {
roi->target_seq = cmd->param2;
}
if (*roi_pub != nullptr) {
orb_publish(ORB_ID(vehicle_roi), *roi_pub, roi);
} else {
*roi_pub = orb_advertise(ORB_ID(vehicle_roi), roi);
}
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
break;
}
case vehicle_command_s::VEHICLE_CMD_CUSTOM_0:
case vehicle_command_s::VEHICLE_CMD_CUSTOM_1:
case vehicle_command_s::VEHICLE_CMD_CUSTOM_2:
case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL:
case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE:
case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT:
case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION:
case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS:
@@ -1079,10 +1115,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
case vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL:
case vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION:
case vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL:
case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE:
case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL:
case vehicle_command_s::VEHICLE_CMD_DO_SET_CAM_TRIGG_DIST:
case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT:
case vehicle_command_s::VEHICLE_CMD_DO_CHANGE_SPEED:
case vehicle_command_s::VEHICLE_CMD_DO_GO_AROUND:
case vehicle_command_s::VEHICLE_CMD_START_RX_PAIR:
@@ -1330,6 +1363,10 @@ int commander_thread_main(int argc, char *argv[])
orb_advert_t home_pub = nullptr;
memset(&_home, 0, sizeof(_home));
/* region of interest */
orb_advert_t roi_pub = nullptr;
memset(&_roi, 0, sizeof(_roi));
/* command ack */
orb_advert_t command_ack_pub = nullptr;
struct vehicle_command_ack_s command_ack;
@@ -2656,7 +2693,7 @@ int commander_thread_main(int argc, char *argv[])
/* handle it */
if (handle_command(&status, &safety, &cmd, &armed, &_home, &global_position, &local_position,
&attitude, &home_pub, &command_ack_pub, &command_ack)) {
&attitude, &home_pub, &command_ack_pub, &command_ack, &_roi, &roi_pub)) {
status_changed = true;
}
}