mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-17 04:23:34 +08:00
move vmount to modules (it's not a driver)
This commit is contained in:
Daniel Agar
@@ -0,0 +1,51 @@
|
||||
############################################################################
|
||||
#
|
||||
# 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.
|
||||
#
|
||||
############################################################################
|
||||
px4_add_module(
|
||||
MODULE drivers__vmount
|
||||
MAIN vmount
|
||||
COMPILE_FLAGS
|
||||
STACK_MAIN 1024
|
||||
SRCS
|
||||
input.cpp
|
||||
input_mavlink.cpp
|
||||
input_rc.cpp
|
||||
input_test.cpp
|
||||
output.cpp
|
||||
output_mavlink.cpp
|
||||
output_rc.cpp
|
||||
vmount.cpp
|
||||
DEPENDS
|
||||
git_ecl
|
||||
ecl_geo
|
||||
)
|
||||
|
||||
@@ -0,0 +1,93 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 common.h
|
||||
* @author Beat Küng <beat-kueng@gmx.net>
|
||||
*
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
|
||||
namespace vmount
|
||||
{
|
||||
|
||||
|
||||
/**
|
||||
* @struct ControlData
|
||||
* This defines the common API between an input and an output of the vmount driver.
|
||||
* Each output must support the (full) set of the commands, and an input can create all
|
||||
* or a subset of the types.
|
||||
*/
|
||||
struct ControlData {
|
||||
|
||||
enum class Type : uint8_t {
|
||||
Neutral = 0, /**< move to neutral position */
|
||||
Angle, /**< control the roll, pitch & yaw angle directly */
|
||||
LonLat /**< control via lon, lat */
|
||||
//TODO: add more, like smooth curve, ... ?
|
||||
};
|
||||
|
||||
Type type = Type::Neutral;
|
||||
|
||||
union TypeData {
|
||||
struct TypeAngle {
|
||||
float angles[3]; /**< attitude angles (roll, pitch, yaw) in rad, [-pi, +pi] if is_speed[i] == false */
|
||||
|
||||
bool is_speed[3]; /**< if true, the angle is the angular speed in rad/s */
|
||||
} angle;
|
||||
|
||||
struct TypeLonLat {
|
||||
double lon; /**< longitude in [deg] */
|
||||
double lat; /**< latitude in [deg] */
|
||||
float altitude; /**< altitude in [m] */
|
||||
float roll_angle; /**< roll is set to a fixed angle. Set to 0 for level horizon [rad] */
|
||||
float pitch_angle_offset; /**< angular offset for pitch [rad] */
|
||||
float yaw_angle_offset; /**< angular offset for yaw [rad] */
|
||||
float pitch_fixed_angle; /**< ignored if < -pi, otherwise use a fixed pitch angle instead of the altitude */
|
||||
} lonlat;
|
||||
} type_data;
|
||||
|
||||
|
||||
bool stabilize_axis[3] = { false, false, false }; /**< whether the vmount driver should stabilize an axis
|
||||
(if the output supports it, this can also be done externally) */
|
||||
|
||||
bool gimbal_shutter_retract = false; /**< whether to lock the gimbal (only in RC output mode) */
|
||||
|
||||
};
|
||||
|
||||
|
||||
} /* namespace vmount */
|
||||
@@ -0,0 +1,80 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2016-2017 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file input.cpp
|
||||
* @author Beat Küng <beat-kueng@gmx.net>
|
||||
*
|
||||
*/
|
||||
|
||||
#include "input.h"
|
||||
|
||||
|
||||
namespace vmount
|
||||
{
|
||||
|
||||
int InputBase::update(unsigned int timeout_ms, ControlData **control_data, bool already_active)
|
||||
{
|
||||
if (!_initialized) {
|
||||
int ret = initialize();
|
||||
|
||||
if (ret) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
//on startup, set the mount to a neutral position
|
||||
_control_data.type = ControlData::Type::Neutral;
|
||||
_control_data.gimbal_shutter_retract = true;
|
||||
*control_data = &_control_data;
|
||||
_initialized = true;
|
||||
return 0;
|
||||
}
|
||||
|
||||
return update_impl(timeout_ms, control_data, already_active);
|
||||
}
|
||||
|
||||
void InputBase::control_data_set_lon_lat(double lon, double lat, float altitude, float roll_angle,
|
||||
float pitch_fixed_angle)
|
||||
{
|
||||
_control_data.type = ControlData::Type::LonLat;
|
||||
_control_data.type_data.lonlat.lon = lon;
|
||||
_control_data.type_data.lonlat.lat = lat;
|
||||
_control_data.type_data.lonlat.altitude = altitude;
|
||||
_control_data.type_data.lonlat.roll_angle = roll_angle;
|
||||
_control_data.type_data.lonlat.pitch_fixed_angle = pitch_fixed_angle;
|
||||
_control_data.type_data.lonlat.pitch_angle_offset = 0.f;
|
||||
_control_data.type_data.lonlat.yaw_angle_offset = 0.f;
|
||||
}
|
||||
|
||||
} /* namespace vmount */
|
||||
|
||||
@@ -0,0 +1,90 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2016-2017 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file input.h
|
||||
* @author Beat Küng <beat-kueng@gmx.net>
|
||||
*
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "common.h"
|
||||
|
||||
namespace vmount
|
||||
{
|
||||
|
||||
|
||||
/**
|
||||
** class InputBase
|
||||
* Base class for all driver input classes
|
||||
*/
|
||||
class InputBase
|
||||
{
|
||||
public:
|
||||
virtual ~InputBase() {}
|
||||
|
||||
/**
|
||||
* Wait for an input update, with a timeout.
|
||||
* @param timeout_ms timeout in ms
|
||||
* @param control_data unchanged on error. On success it is nullptr if no new
|
||||
* data is available, otherwise set to an object.
|
||||
* If it is set, the returned object will not be changed for
|
||||
* subsequent calls to update() that return no new data
|
||||
* (in other words: if (some) control_data values change,
|
||||
* non-null will be returned).
|
||||
* @param already_active true if the mode was already active last time, false if it was not and "major"
|
||||
* change is necessary such as big stick movement for RC.
|
||||
* @return 0 on success, <0 otherwise
|
||||
*/
|
||||
virtual int update(unsigned int timeout_ms, ControlData **control_data, bool already_active);
|
||||
|
||||
/** report status to stdout */
|
||||
virtual void print_status() = 0;
|
||||
|
||||
protected:
|
||||
virtual int update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active) = 0;
|
||||
|
||||
virtual int initialize() { return 0; }
|
||||
|
||||
void control_data_set_lon_lat(double lon, double lat, float altitude, float roll_angle = 0.f,
|
||||
float pitch_fixed_angle = -10.f);
|
||||
|
||||
ControlData _control_data;
|
||||
|
||||
private:
|
||||
bool _initialized = false;
|
||||
};
|
||||
|
||||
|
||||
} /* namespace vmount */
|
||||
@@ -0,0 +1,358 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2016-2017 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file input_mavlink.cpp
|
||||
* @author Leon Müller (thedevleon)
|
||||
* @author Beat Küng <beat-kueng@gmx.net>
|
||||
*
|
||||
*/
|
||||
|
||||
#include "input_mavlink.h"
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/vehicle_roi.h>
|
||||
#include <uORB/topics/vehicle_command_ack.h>
|
||||
#include <uORB/topics/position_setpoint_triplet.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
#include <px4_defines.h>
|
||||
#include <px4_posix.h>
|
||||
#include <errno.h>
|
||||
|
||||
namespace vmount
|
||||
{
|
||||
|
||||
InputMavlinkROI::~InputMavlinkROI()
|
||||
{
|
||||
if (_vehicle_roi_sub >= 0) {
|
||||
orb_unsubscribe(_vehicle_roi_sub);
|
||||
}
|
||||
|
||||
if (_position_setpoint_triplet_sub >= 0) {
|
||||
orb_unsubscribe(_position_setpoint_triplet_sub);
|
||||
}
|
||||
}
|
||||
|
||||
int InputMavlinkROI::initialize()
|
||||
{
|
||||
_vehicle_roi_sub = orb_subscribe(ORB_ID(vehicle_roi));
|
||||
|
||||
if (_vehicle_roi_sub < 0) {
|
||||
return -errno;
|
||||
}
|
||||
|
||||
_position_setpoint_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
|
||||
|
||||
if (_position_setpoint_triplet_sub < 0) {
|
||||
return -errno;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int InputMavlinkROI::update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active)
|
||||
{
|
||||
// already_active is unused, we don't care what happened previously.
|
||||
|
||||
// Default to no change, set if we receive anything.
|
||||
*control_data = nullptr;
|
||||
|
||||
const int num_poll = 2;
|
||||
px4_pollfd_struct_t polls[num_poll];
|
||||
polls[0].fd = _vehicle_roi_sub;
|
||||
polls[0].events = POLLIN;
|
||||
polls[1].fd = _position_setpoint_triplet_sub;
|
||||
polls[1].events = POLLIN;
|
||||
|
||||
int ret = px4_poll(polls, num_poll, timeout_ms);
|
||||
|
||||
if (ret < 0) {
|
||||
return -errno;
|
||||
}
|
||||
|
||||
if (ret == 0) {
|
||||
// Timeout, _control_data is already null
|
||||
|
||||
} else {
|
||||
if (polls[0].revents & POLLIN) {
|
||||
vehicle_roi_s vehicle_roi;
|
||||
orb_copy(ORB_ID(vehicle_roi), _vehicle_roi_sub, &vehicle_roi);
|
||||
|
||||
_control_data.gimbal_shutter_retract = false;
|
||||
|
||||
if (vehicle_roi.mode == vehicle_roi_s::ROI_NONE) {
|
||||
|
||||
_control_data.type = ControlData::Type::Neutral;
|
||||
*control_data = &_control_data;
|
||||
|
||||
} else if (vehicle_roi.mode == vehicle_roi_s::ROI_WPNEXT) {
|
||||
_control_data.type = ControlData::Type::LonLat;
|
||||
_read_control_data_from_position_setpoint_sub();
|
||||
_control_data.type_data.lonlat.pitch_fixed_angle = -10.f;
|
||||
|
||||
_control_data.type_data.lonlat.roll_angle = vehicle_roi.roll_offset;
|
||||
_control_data.type_data.lonlat.pitch_angle_offset = vehicle_roi.pitch_offset;
|
||||
_control_data.type_data.lonlat.yaw_angle_offset = vehicle_roi.yaw_offset;
|
||||
|
||||
*control_data = &_control_data;
|
||||
|
||||
} else if (vehicle_roi.mode == vehicle_roi_s::ROI_LOCATION) {
|
||||
control_data_set_lon_lat(vehicle_roi.lon, vehicle_roi.lat, vehicle_roi.alt);
|
||||
|
||||
*control_data = &_control_data;
|
||||
|
||||
} else if (vehicle_roi.mode == vehicle_roi_s::ROI_TARGET) {
|
||||
//TODO is this even suported?
|
||||
}
|
||||
|
||||
_cur_roi_mode = vehicle_roi.mode;
|
||||
|
||||
//set all other control data fields to defaults
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
_control_data.stabilize_axis[i] = false;
|
||||
}
|
||||
}
|
||||
|
||||
// check whether the position setpoint got updated
|
||||
if (polls[1].revents & POLLIN) {
|
||||
if (_cur_roi_mode == vehicle_roi_s::ROI_WPNEXT) {
|
||||
_read_control_data_from_position_setpoint_sub();
|
||||
*control_data = &_control_data;
|
||||
|
||||
} else { // must do an orb_copy() in *every* case
|
||||
position_setpoint_triplet_s position_setpoint_triplet;
|
||||
orb_copy(ORB_ID(position_setpoint_triplet), _position_setpoint_triplet_sub, &position_setpoint_triplet);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
void InputMavlinkROI::_read_control_data_from_position_setpoint_sub()
|
||||
{
|
||||
position_setpoint_triplet_s position_setpoint_triplet;
|
||||
orb_copy(ORB_ID(position_setpoint_triplet), _position_setpoint_triplet_sub, &position_setpoint_triplet);
|
||||
_control_data.type_data.lonlat.lon = position_setpoint_triplet.current.lon;
|
||||
_control_data.type_data.lonlat.lat = position_setpoint_triplet.current.lat;
|
||||
_control_data.type_data.lonlat.altitude = position_setpoint_triplet.current.alt;
|
||||
}
|
||||
|
||||
void InputMavlinkROI::print_status()
|
||||
{
|
||||
PX4_INFO("Input: Mavlink (ROI)");
|
||||
}
|
||||
|
||||
|
||||
InputMavlinkCmdMount::InputMavlinkCmdMount(bool stabilize)
|
||||
: _stabilize {stabilize, stabilize, stabilize}
|
||||
{
|
||||
param_t handle = param_find("MAV_SYS_ID");
|
||||
|
||||
if (handle != PARAM_INVALID) {
|
||||
param_get(handle, &_mav_sys_id);
|
||||
}
|
||||
|
||||
handle = param_find("MAV_COMP_ID");
|
||||
|
||||
if (handle != PARAM_INVALID) {
|
||||
param_get(handle, &_mav_comp_id);
|
||||
}
|
||||
}
|
||||
|
||||
InputMavlinkCmdMount::~InputMavlinkCmdMount()
|
||||
{
|
||||
if (_vehicle_command_sub >= 0) {
|
||||
orb_unsubscribe(_vehicle_command_sub);
|
||||
}
|
||||
}
|
||||
|
||||
int InputMavlinkCmdMount::initialize()
|
||||
{
|
||||
if ((_vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command))) < 0) {
|
||||
return -errno;
|
||||
}
|
||||
|
||||
// rate-limit inputs to 100Hz. If we don't do this and the output is configured to mavlink mode,
|
||||
// it will publish vehicle_command's as well, causing the input poll() in here to return
|
||||
// immediately, which in turn will cause an output update and thus a busy loop.
|
||||
orb_set_interval(_vehicle_command_sub, 10);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
int InputMavlinkCmdMount::update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active)
|
||||
{
|
||||
// Default to notify that there was no change.
|
||||
*control_data = nullptr;
|
||||
|
||||
const int num_poll = 1;
|
||||
px4_pollfd_struct_t polls[num_poll];
|
||||
polls[0].fd = _vehicle_command_sub;
|
||||
polls[0].events = POLLIN;
|
||||
|
||||
int poll_timeout = (int)timeout_ms;
|
||||
|
||||
bool exit_loop = false;
|
||||
|
||||
while (!exit_loop && poll_timeout >= 0) {
|
||||
hrt_abstime poll_start = hrt_absolute_time();
|
||||
|
||||
int ret = px4_poll(polls, num_poll, poll_timeout);
|
||||
|
||||
if (ret < 0) {
|
||||
return -errno;
|
||||
}
|
||||
|
||||
poll_timeout -= (hrt_absolute_time() - poll_start) / 1000;
|
||||
|
||||
// if we get a command that we need to handle, we exit the loop, otherwise we poll until we reach the timeout
|
||||
exit_loop = true;
|
||||
|
||||
if (ret == 0) {
|
||||
// Timeout control_data already null.
|
||||
|
||||
} else {
|
||||
if (polls[0].revents & POLLIN) {
|
||||
vehicle_command_s vehicle_command;
|
||||
orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &vehicle_command);
|
||||
|
||||
// Process only if the command is for us or for anyone (component id 0).
|
||||
const bool sysid_correct = (vehicle_command.target_system == _mav_sys_id);
|
||||
const bool compid_correct = ((vehicle_command.target_component == _mav_comp_id) ||
|
||||
(vehicle_command.target_component == 0));
|
||||
|
||||
if (!sysid_correct || !compid_correct) {
|
||||
exit_loop = false;
|
||||
continue;
|
||||
}
|
||||
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
_control_data.stabilize_axis[i] = _stabilize[i];
|
||||
}
|
||||
|
||||
_control_data.gimbal_shutter_retract = 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:
|
||||
_control_data.gimbal_shutter_retract = true;
|
||||
|
||||
/* FALLTHROUGH */
|
||||
|
||||
case vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL:
|
||||
_control_data.type = ControlData::Type::Neutral;
|
||||
|
||||
*control_data = &_control_data;
|
||||
break;
|
||||
|
||||
case vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING:
|
||||
_control_data.type = ControlData::Type::Angle;
|
||||
_control_data.type_data.angle.is_speed[0] = false;
|
||||
_control_data.type_data.angle.is_speed[1] = false;
|
||||
_control_data.type_data.angle.is_speed[2] = false;
|
||||
// vmount spec has roll on channel 0, MAVLink spec has pitch on channel 0
|
||||
_control_data.type_data.angle.angles[0] = vehicle_command.param2 * M_DEG_TO_RAD_F;
|
||||
// vmount spec has pitch on channel 1, MAVLink spec has roll on channel 1
|
||||
_control_data.type_data.angle.angles[1] = vehicle_command.param1 * M_DEG_TO_RAD_F;
|
||||
// both specs have yaw on channel 2
|
||||
_control_data.type_data.angle.angles[2] = vehicle_command.param3 * M_DEG_TO_RAD_F;
|
||||
|
||||
// We expect angle of [-pi..+pi]. If the input range is [0..2pi] we can fix that.
|
||||
if (_control_data.type_data.angle.angles[2] > M_PI_F) {
|
||||
_control_data.type_data.angle.angles[2] -= 2 * M_PI_F;
|
||||
}
|
||||
|
||||
*control_data = &_control_data;
|
||||
break;
|
||||
|
||||
case vehicle_command_s::VEHICLE_MOUNT_MODE_RC_TARGETING:
|
||||
break;
|
||||
|
||||
case vehicle_command_s::VEHICLE_MOUNT_MODE_GPS_POINT:
|
||||
control_data_set_lon_lat((double)vehicle_command.param2, (double)vehicle_command.param1, vehicle_command.param3);
|
||||
|
||||
*control_data = &_control_data;
|
||||
break;
|
||||
}
|
||||
|
||||
_ack_vehicle_command(&vehicle_command);
|
||||
|
||||
} else if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE) {
|
||||
_stabilize[0] = (uint8_t) vehicle_command.param2 == 1;
|
||||
_stabilize[1] = (uint8_t) vehicle_command.param3 == 1;
|
||||
_stabilize[2] = (uint8_t) vehicle_command.param4 == 1;
|
||||
_control_data.type = ControlData::Type::Neutral; //always switch to neutral position
|
||||
|
||||
*control_data = &_control_data;
|
||||
_ack_vehicle_command(&vehicle_command);
|
||||
|
||||
} else {
|
||||
exit_loop = false;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
void InputMavlinkCmdMount::_ack_vehicle_command(vehicle_command_s *cmd)
|
||||
{
|
||||
vehicle_command_ack_s vehicle_command_ack = {};
|
||||
|
||||
vehicle_command_ack.timestamp = hrt_absolute_time();
|
||||
vehicle_command_ack.command = cmd->command;
|
||||
vehicle_command_ack.result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
|
||||
vehicle_command_ack.target_system = cmd->source_system;
|
||||
vehicle_command_ack.target_component = cmd->source_component;
|
||||
|
||||
if (_vehicle_command_ack_pub == nullptr) {
|
||||
_vehicle_command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &vehicle_command_ack,
|
||||
vehicle_command_ack_s::ORB_QUEUE_LENGTH);
|
||||
|
||||
} else {
|
||||
orb_publish(ORB_ID(vehicle_command_ack), _vehicle_command_ack_pub, &vehicle_command_ack);
|
||||
}
|
||||
}
|
||||
|
||||
void InputMavlinkCmdMount::print_status()
|
||||
{
|
||||
PX4_INFO("Input: Mavlink (CMD_MOUNT)");
|
||||
}
|
||||
|
||||
|
||||
} /* namespace vmount */
|
||||
@@ -0,0 +1,106 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2016-2017 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file input_mavlink.h
|
||||
* @author Beat Küng <beat-kueng@gmx.net>
|
||||
*
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "input.h"
|
||||
#include "input_rc.h"
|
||||
#include <cstdint>
|
||||
|
||||
#include <uORB/topics/vehicle_roi.h>
|
||||
#include <uORB/topics/vehicle_command.h>
|
||||
|
||||
namespace vmount
|
||||
{
|
||||
|
||||
|
||||
/**
|
||||
** class InputMavlinkROI
|
||||
** Input based on the vehicle_roi topic
|
||||
*/
|
||||
class InputMavlinkROI : public InputBase
|
||||
{
|
||||
public:
|
||||
InputMavlinkROI() = default;
|
||||
virtual ~InputMavlinkROI();
|
||||
|
||||
virtual void print_status();
|
||||
|
||||
protected:
|
||||
virtual int update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active);
|
||||
virtual int initialize();
|
||||
|
||||
private:
|
||||
void _read_control_data_from_position_setpoint_sub();
|
||||
|
||||
int _vehicle_roi_sub = -1;
|
||||
int _position_setpoint_triplet_sub = -1;
|
||||
uint8_t _cur_roi_mode = vehicle_roi_s::ROI_NONE;
|
||||
};
|
||||
|
||||
|
||||
/**
|
||||
** class InputMavlinkCmdMount
|
||||
** Input based on the VEHICLE_CMD_DO_MOUNT_CONTROL mavlink command
|
||||
*/
|
||||
class InputMavlinkCmdMount : public InputBase
|
||||
{
|
||||
public:
|
||||
InputMavlinkCmdMount(bool stabilize);
|
||||
virtual ~InputMavlinkCmdMount();
|
||||
|
||||
virtual void print_status();
|
||||
|
||||
protected:
|
||||
virtual int update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active);
|
||||
virtual int initialize();
|
||||
|
||||
private:
|
||||
void _ack_vehicle_command(vehicle_command_s *cmd);
|
||||
|
||||
int _vehicle_command_sub = -1;
|
||||
orb_advert_t _vehicle_command_ack_pub = nullptr;
|
||||
bool _stabilize[3] = { false, false, false };
|
||||
|
||||
int32_t _mav_sys_id{1}; ///< our mavlink system id
|
||||
int32_t _mav_comp_id{1}; ///< our mavlink component id
|
||||
};
|
||||
|
||||
|
||||
} /* namespace vmount */
|
||||
@@ -0,0 +1,178 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2016-2017 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file input_rc.cpp
|
||||
* @author Beat Küng <beat-kueng@gmx.net>
|
||||
*
|
||||
*/
|
||||
|
||||
#include "input_rc.h"
|
||||
|
||||
#include <errno.h>
|
||||
#include <px4_posix.h>
|
||||
#include <px4_defines.h>
|
||||
|
||||
|
||||
namespace vmount
|
||||
{
|
||||
|
||||
|
||||
InputRC::InputRC(bool do_stabilization, int aux_channel_roll, int aux_channel_pitch, int aux_channel_yaw)
|
||||
: _do_stabilization(do_stabilization)
|
||||
{
|
||||
_aux_channels[0] = aux_channel_roll;
|
||||
_aux_channels[1] = aux_channel_pitch;
|
||||
_aux_channels[2] = aux_channel_yaw;
|
||||
}
|
||||
|
||||
InputRC::~InputRC()
|
||||
{
|
||||
if (_manual_control_setpoint_sub >= 0) {
|
||||
orb_unsubscribe(_manual_control_setpoint_sub);
|
||||
}
|
||||
}
|
||||
|
||||
int InputRC::initialize()
|
||||
{
|
||||
_manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
|
||||
|
||||
if (_manual_control_setpoint_sub < 0) {
|
||||
return -errno;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int InputRC::update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active)
|
||||
{
|
||||
// Default to no change signalled by NULL.
|
||||
*control_data = nullptr;
|
||||
|
||||
px4_pollfd_struct_t polls[1];
|
||||
polls[0].fd = _manual_control_setpoint_sub;
|
||||
polls[0].events = POLLIN;
|
||||
|
||||
int ret = px4_poll(polls, 1, timeout_ms);
|
||||
|
||||
if (ret < 0) {
|
||||
return -errno;
|
||||
}
|
||||
|
||||
if (ret == 0) {
|
||||
// Timeout, leave NULL
|
||||
} else {
|
||||
if (polls[0].revents & POLLIN) {
|
||||
// Only if there was a change, we update the control data, otherwise leave it NULL.
|
||||
if (_read_control_data_from_subscription(_control_data, already_active)) {
|
||||
*control_data = &_control_data;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
bool InputRC::_read_control_data_from_subscription(ControlData &control_data, bool already_active)
|
||||
{
|
||||
manual_control_setpoint_s manual_control_setpoint;
|
||||
orb_copy(ORB_ID(manual_control_setpoint), _manual_control_setpoint_sub, &manual_control_setpoint);
|
||||
control_data.type = ControlData::Type::Angle;
|
||||
|
||||
float new_aux_values[3];
|
||||
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
new_aux_values[i] = _get_aux_value(manual_control_setpoint, i);
|
||||
}
|
||||
|
||||
// If we were already active previously, we just update normally. Otherwise, there needs to be
|
||||
// a major stick movement to re-activate manual (or it's running for the very first time).
|
||||
bool major_movement = false;
|
||||
|
||||
// Detect a big stick movement
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
if (fabsf(_last_set_aux_values[i] - new_aux_values[i]) > 0.25f) {
|
||||
major_movement = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (already_active || major_movement || _first_time) {
|
||||
|
||||
_first_time = false;
|
||||
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
control_data.type_data.angle.is_speed[i] = false;
|
||||
control_data.type_data.angle.angles[i] = new_aux_values[i] * M_PI_F;
|
||||
control_data.stabilize_axis[i] = _do_stabilization;
|
||||
|
||||
_last_set_aux_values[i] = new_aux_values[i];
|
||||
}
|
||||
|
||||
control_data.gimbal_shutter_retract = false;
|
||||
return true;
|
||||
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
float InputRC::_get_aux_value(const manual_control_setpoint_s &manual_control_setpoint, int channel_idx)
|
||||
{
|
||||
switch (_aux_channels[channel_idx]) {
|
||||
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
void InputRC::print_status()
|
||||
{
|
||||
PX4_INFO("Input: RC (channels: roll=%i, pitch=%i, yaw=%i)", _aux_channels[0], _aux_channels[1], _aux_channels[2]);
|
||||
}
|
||||
|
||||
} /* namespace vmount */
|
||||
@@ -0,0 +1,93 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2016-2017 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file input_rc.h
|
||||
* @author Beat Küng <beat-kueng@gmx.net>
|
||||
*
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "input.h"
|
||||
#include <uORB/topics/manual_control_setpoint.h>
|
||||
|
||||
namespace vmount
|
||||
{
|
||||
|
||||
class InputMavlinkROI;
|
||||
class InputMavlinkCmdMount;
|
||||
|
||||
/**
|
||||
** class InputRC
|
||||
* RC input class using manual_control_setpoint topic
|
||||
*/
|
||||
class InputRC : public InputBase
|
||||
{
|
||||
public:
|
||||
|
||||
/**
|
||||
* @param do_stabilization
|
||||
* @param aux_channel_roll which aux channel to use for roll (set to 0 to use a fixed angle of 0)
|
||||
* @param aux_channel_pitch
|
||||
* @param aux_channel_yaw
|
||||
*/
|
||||
InputRC(bool do_stabilization, int aux_channel_roll, int aux_channel_pitch, int aux_channel_yaw);
|
||||
virtual ~InputRC();
|
||||
|
||||
virtual void print_status();
|
||||
|
||||
protected:
|
||||
virtual int update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active);
|
||||
virtual int initialize();
|
||||
|
||||
/**
|
||||
* @return true if there was a change in control data
|
||||
*/
|
||||
virtual bool _read_control_data_from_subscription(ControlData &control_data, bool already_active);
|
||||
|
||||
int _get_subscription_fd() const { return _manual_control_setpoint_sub; }
|
||||
|
||||
float _get_aux_value(const manual_control_setpoint_s &manual_control_setpoint, int channel_idx);
|
||||
|
||||
private:
|
||||
const bool _do_stabilization;
|
||||
int _aux_channels[3];
|
||||
int _manual_control_setpoint_sub = -1;
|
||||
|
||||
bool _first_time = true;
|
||||
float _last_set_aux_values[3] = {};
|
||||
};
|
||||
|
||||
|
||||
} /* namespace vmount */
|
||||
@@ -0,0 +1,88 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2016-2017 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file input_test.cpp
|
||||
* @author Beat Küng <beat-kueng@gmx.net>
|
||||
*
|
||||
*/
|
||||
|
||||
#include "input_test.h"
|
||||
|
||||
#include <px4_posix.h>
|
||||
|
||||
|
||||
namespace vmount
|
||||
{
|
||||
|
||||
InputTest::InputTest(float roll_deg, float pitch_deg, float yaw_deg)
|
||||
{
|
||||
_angles[0] = roll_deg;
|
||||
_angles[1] = pitch_deg;
|
||||
_angles[2] = yaw_deg;
|
||||
}
|
||||
|
||||
bool InputTest::finished()
|
||||
{
|
||||
return true; /* only a single-shot test (for now) */
|
||||
}
|
||||
|
||||
int InputTest::update(unsigned int timeout_ms, ControlData **control_data, bool already_active)
|
||||
{
|
||||
//we directly override the update() here, since we don't need the initialization from the base class
|
||||
|
||||
_control_data.type = ControlData::Type::Angle;
|
||||
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
_control_data.type_data.angle.is_speed[i] = false;
|
||||
_control_data.type_data.angle.angles[i] = _angles[i] * M_DEG_TO_RAD_F;
|
||||
|
||||
_control_data.stabilize_axis[i] = false;
|
||||
}
|
||||
|
||||
_control_data.gimbal_shutter_retract = false;
|
||||
*control_data = &_control_data;
|
||||
return 0;
|
||||
}
|
||||
|
||||
int InputTest::initialize()
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
void InputTest::print_status()
|
||||
{
|
||||
PX4_INFO("Input: Test");
|
||||
}
|
||||
|
||||
} /* namespace vmount */
|
||||
@@ -0,0 +1,78 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2016-2017 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file input_test.h
|
||||
* @author Beat Küng <beat-kueng@gmx.net>
|
||||
*
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "input.h"
|
||||
|
||||
namespace vmount
|
||||
{
|
||||
|
||||
/**
|
||||
** class InputTest
|
||||
* Send a single control command, configured via constructor arguments
|
||||
*/
|
||||
class InputTest : public InputBase
|
||||
{
|
||||
public:
|
||||
|
||||
/**
|
||||
* set to a fixed angle
|
||||
*/
|
||||
InputTest(float roll_deg, float pitch_deg, float yaw_deg);
|
||||
virtual ~InputTest() {}
|
||||
|
||||
/** check whether the test finished, and thus the main thread can quit */
|
||||
bool finished();
|
||||
|
||||
virtual int update(unsigned int timeout_ms, ControlData **control_data, bool already_active);
|
||||
|
||||
protected:
|
||||
virtual int update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active) { return 0; } //not needed
|
||||
|
||||
virtual int initialize();
|
||||
|
||||
virtual void print_status();
|
||||
|
||||
private:
|
||||
float _angles[3]; /**< desired angles in [deg] */
|
||||
};
|
||||
|
||||
|
||||
} /* namespace vmount */
|
||||
@@ -0,0 +1,233 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 output.cpp
|
||||
* @author Leon Müller (thedevleon)
|
||||
* @author Beat Küng <beat-kueng@gmx.net>
|
||||
*
|
||||
*/
|
||||
|
||||
#include "output.h"
|
||||
#include <errno.h>
|
||||
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/mount_orientation.h>
|
||||
#include <px4_defines.h>
|
||||
#include <lib/ecl/geo/geo.h>
|
||||
#include <math.h>
|
||||
#include <mathlib/mathlib.h>
|
||||
#include <matrix/math.hpp>
|
||||
|
||||
namespace vmount
|
||||
{
|
||||
|
||||
OutputBase::OutputBase(const OutputConfig &output_config)
|
||||
: _config(output_config)
|
||||
{
|
||||
_last_update = hrt_absolute_time();
|
||||
}
|
||||
|
||||
OutputBase::~OutputBase()
|
||||
{
|
||||
if (_vehicle_attitude_sub >= 0) {
|
||||
orb_unsubscribe(_vehicle_attitude_sub);
|
||||
}
|
||||
|
||||
if (_vehicle_global_position_sub >= 0) {
|
||||
orb_unsubscribe(_vehicle_global_position_sub);
|
||||
}
|
||||
|
||||
if (_mount_orientation_pub) {
|
||||
orb_unadvertise(_mount_orientation_pub);
|
||||
}
|
||||
}
|
||||
|
||||
int OutputBase::initialize()
|
||||
{
|
||||
if ((_vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude))) < 0) {
|
||||
return -errno;
|
||||
}
|
||||
|
||||
if ((_vehicle_global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position))) < 0) {
|
||||
return -errno;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
void OutputBase::publish()
|
||||
{
|
||||
int instance;
|
||||
mount_orientation_s mount_orientation;
|
||||
|
||||
for (unsigned i = 0; i < 3; ++i) {
|
||||
mount_orientation.attitude_euler_angle[i] = _angle_outputs[i];
|
||||
}
|
||||
|
||||
//PX4_INFO("roll: %.2f, pitch: %.2f, yaw: %.2f",
|
||||
// (double)_angle_outputs[0],
|
||||
// (double)_angle_outputs[1],
|
||||
// (double)_angle_outputs[2]);
|
||||
|
||||
orb_publish_auto(ORB_ID(mount_orientation), &_mount_orientation_pub, &mount_orientation, &instance, ORB_PRIO_DEFAULT);
|
||||
}
|
||||
|
||||
float OutputBase::_calculate_pitch(double lon, double lat, float altitude,
|
||||
const vehicle_global_position_s &global_position)
|
||||
{
|
||||
if (!map_projection_initialized(&_projection_reference)) {
|
||||
map_projection_init(&_projection_reference, global_position.lat, global_position.lon);
|
||||
}
|
||||
|
||||
float x1, y1, x2, y2;
|
||||
map_projection_project(&_projection_reference, lat, lon, &x1, &y1);
|
||||
map_projection_project(&_projection_reference, global_position.lat, global_position.lon, &x2, &y2);
|
||||
float dx = x1 - x2, dy = y1 - y2;
|
||||
float target_distance = sqrtf(dx * dx + dy * dy);
|
||||
float z = altitude - global_position.alt;
|
||||
|
||||
return atan2f(z, target_distance);
|
||||
}
|
||||
|
||||
void OutputBase::_set_angle_setpoints(const ControlData *control_data)
|
||||
{
|
||||
_cur_control_data = control_data;
|
||||
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
_stabilize[i] = control_data->stabilize_axis[i];
|
||||
_angle_speeds[i] = 0.f;
|
||||
}
|
||||
|
||||
switch (control_data->type) {
|
||||
case ControlData::Type::Angle:
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
if (control_data->type_data.angle.is_speed[i]) {
|
||||
_angle_speeds[i] = control_data->type_data.angle.angles[i];
|
||||
|
||||
} else {
|
||||
_angle_setpoints[i] = control_data->type_data.angle.angles[i];
|
||||
}
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case ControlData::Type::LonLat:
|
||||
_handle_position_update(true);
|
||||
break;
|
||||
|
||||
case ControlData::Type::Neutral:
|
||||
_angle_setpoints[0] = 0.f;
|
||||
_angle_setpoints[1] = 0.f;
|
||||
_angle_setpoints[2] = 0.f;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void OutputBase::_handle_position_update(bool force_update)
|
||||
{
|
||||
bool need_update = force_update;
|
||||
|
||||
if (!_cur_control_data || _cur_control_data->type != ControlData::Type::LonLat) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (!force_update) {
|
||||
orb_check(_vehicle_global_position_sub, &need_update);
|
||||
}
|
||||
|
||||
if (!need_update) {
|
||||
return;
|
||||
}
|
||||
|
||||
vehicle_global_position_s vehicle_global_position;
|
||||
orb_copy(ORB_ID(vehicle_global_position), _vehicle_global_position_sub, &vehicle_global_position);
|
||||
|
||||
float pitch;
|
||||
const double &lon = _cur_control_data->type_data.lonlat.lon;
|
||||
const double &lat = _cur_control_data->type_data.lonlat.lat;
|
||||
const float &alt = _cur_control_data->type_data.lonlat.altitude;
|
||||
|
||||
if (_cur_control_data->type_data.lonlat.pitch_fixed_angle >= -M_PI_F) {
|
||||
pitch = _cur_control_data->type_data.lonlat.pitch_fixed_angle;
|
||||
|
||||
} else {
|
||||
pitch = _calculate_pitch(lon, lat, alt, vehicle_global_position);
|
||||
}
|
||||
|
||||
float roll = _cur_control_data->type_data.lonlat.roll_angle;
|
||||
float yaw = get_bearing_to_next_waypoint(vehicle_global_position.lat, vehicle_global_position.lon, lat, lon)
|
||||
- vehicle_global_position.yaw;
|
||||
|
||||
_angle_setpoints[0] = roll;
|
||||
_angle_setpoints[1] = pitch + _cur_control_data->type_data.lonlat.pitch_angle_offset;
|
||||
_angle_setpoints[2] = yaw + _cur_control_data->type_data.lonlat.yaw_angle_offset;
|
||||
}
|
||||
|
||||
void OutputBase::_calculate_output_angles(const hrt_abstime &t)
|
||||
{
|
||||
//take speed into account
|
||||
float dt = (t - _last_update) / 1.e6f;
|
||||
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
_angle_setpoints[i] += dt * _angle_speeds[i];
|
||||
}
|
||||
|
||||
//get the output angles and stabilize if necessary
|
||||
vehicle_attitude_s vehicle_attitude;
|
||||
|
||||
if (_stabilize[0] || _stabilize[1] || _stabilize[2]) {
|
||||
orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &vehicle_attitude);
|
||||
}
|
||||
|
||||
matrix::Eulerf euler = matrix::Quatf(vehicle_attitude.q);
|
||||
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
if (_stabilize[i]) {
|
||||
_angle_outputs[i] = _angle_setpoints[i] - euler(i);
|
||||
|
||||
} else {
|
||||
_angle_outputs[i] = _angle_setpoints[i];
|
||||
}
|
||||
|
||||
//bring angles into proper range [-pi, pi]
|
||||
while (_angle_outputs[i] > M_PI_F) { _angle_outputs[i] -= 2.f * M_PI_F; }
|
||||
|
||||
while (_angle_outputs[i] < -M_PI_F) { _angle_outputs[i] += 2.f * M_PI_F; }
|
||||
}
|
||||
}
|
||||
|
||||
} /* namespace vmount */
|
||||
|
||||
@@ -0,0 +1,132 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 output.h
|
||||
* @author Beat Küng <beat-kueng@gmx.net>
|
||||
*
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "common.h"
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <lib/ecl/geo/geo.h>
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
|
||||
|
||||
namespace vmount
|
||||
{
|
||||
|
||||
struct OutputConfig {
|
||||
float gimbal_retracted_mode_value; /**< Mixer output value for selecting gimbal retracted mode */
|
||||
float gimbal_normal_mode_value; /**< Mixer output value for selecting gimbal normal mode */
|
||||
|
||||
/** Scale factor for pitch channel (maps from angle in radians to actuator output in [-1,1]). OutputRC only. */
|
||||
float pitch_scale;
|
||||
/** Scale factor for roll channel (maps from angle in radians to actuator output in [-1,1]). OutputRC only. */
|
||||
float roll_scale;
|
||||
/** Scale factor for yaw channel (maps from angle in radians to actuator output in [-1,1]). OutputRC only. */
|
||||
float yaw_scale;
|
||||
|
||||
float pitch_offset; /**< Offset for pitch channel in radians */
|
||||
float roll_offset; /**< Offset for roll channel in radians */
|
||||
float yaw_offset; /**< Offset for yaw channel in radians */
|
||||
|
||||
uint32_t mavlink_sys_id; /**< Mavlink target system id for mavlink output */
|
||||
uint32_t mavlink_comp_id;
|
||||
};
|
||||
|
||||
|
||||
/**
|
||||
** class OutputBase
|
||||
* Base class for all driver output classes
|
||||
*/
|
||||
class OutputBase
|
||||
{
|
||||
public:
|
||||
OutputBase(const OutputConfig &output_config);
|
||||
virtual ~OutputBase();
|
||||
|
||||
virtual int initialize();
|
||||
|
||||
/**
|
||||
* Update the output.
|
||||
* @param data new command if non null
|
||||
* @return 0 on success, <0 otherwise
|
||||
*/
|
||||
virtual int update(const ControlData *control_data) = 0;
|
||||
|
||||
/** report status to stdout */
|
||||
virtual void print_status() = 0;
|
||||
|
||||
/** Publish _angle_outputs as a mount_orientation message. */
|
||||
void publish();
|
||||
|
||||
protected:
|
||||
float _calculate_pitch(double lon, double lat, float altitude,
|
||||
const vehicle_global_position_s &global_position);
|
||||
|
||||
map_projection_reference_s _projection_reference = {}; ///< reference to convert (lon, lat) to local [m]
|
||||
|
||||
const OutputConfig &_config;
|
||||
|
||||
/** set angle setpoints, speeds & stabilize flags */
|
||||
void _set_angle_setpoints(const ControlData *control_data);
|
||||
|
||||
/** check if vehicle position changed and update the setpoint angles if in gps mode */
|
||||
void _handle_position_update(bool force_update = false);
|
||||
|
||||
const ControlData *_cur_control_data = nullptr;
|
||||
float _angle_setpoints[3] = { 0.f, 0.f, 0.f }; ///< [rad]
|
||||
float _angle_speeds[3] = { 0.f, 0.f, 0.f };
|
||||
bool _stabilize[3] = { false, false, false };
|
||||
|
||||
/** calculate the _angle_outputs (with speed) and stabilize if needed */
|
||||
void _calculate_output_angles(const hrt_abstime &t);
|
||||
|
||||
float _angle_outputs[3] = { 0.f, 0.f, 0.f }; ///< calculated output angles (roll, pitch, yaw) [rad]
|
||||
hrt_abstime _last_update;
|
||||
|
||||
int _get_vehicle_attitude_sub() const { return _vehicle_attitude_sub; }
|
||||
|
||||
private:
|
||||
int _vehicle_attitude_sub = -1;
|
||||
int _vehicle_global_position_sub = -1;
|
||||
|
||||
orb_advert_t _mount_orientation_pub = nullptr;
|
||||
};
|
||||
|
||||
|
||||
} /* namespace vmount */
|
||||
@@ -0,0 +1,124 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 output_mavlink.cpp
|
||||
* @author Leon Müller (thedevleon)
|
||||
* @author Beat Küng <beat-kueng@gmx.net>
|
||||
*
|
||||
*/
|
||||
|
||||
#include "output_mavlink.h"
|
||||
|
||||
#include <uORB/topics/vehicle_command.h>
|
||||
#include <px4_defines.h>
|
||||
|
||||
|
||||
namespace vmount
|
||||
{
|
||||
|
||||
OutputMavlink::OutputMavlink(const OutputConfig &output_config)
|
||||
: OutputBase(output_config)
|
||||
{
|
||||
}
|
||||
|
||||
OutputMavlink::~OutputMavlink()
|
||||
{
|
||||
if (_vehicle_command_pub) {
|
||||
orb_unadvertise(_vehicle_command_pub);
|
||||
}
|
||||
}
|
||||
|
||||
int OutputMavlink::update(const ControlData *control_data)
|
||||
{
|
||||
vehicle_command_s vehicle_command = {};
|
||||
vehicle_command.timestamp = hrt_absolute_time();
|
||||
vehicle_command.target_system = (uint8_t)_config.mavlink_sys_id;
|
||||
vehicle_command.target_component = (uint8_t)_config.mavlink_comp_id;
|
||||
|
||||
if (control_data) {
|
||||
//got new command
|
||||
_set_angle_setpoints(control_data);
|
||||
|
||||
vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE;
|
||||
vehicle_command.timestamp = hrt_absolute_time();
|
||||
|
||||
if (control_data->type == ControlData::Type::Neutral) {
|
||||
vehicle_command.param1 = vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL;
|
||||
|
||||
} else {
|
||||
vehicle_command.param1 = vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING;
|
||||
}
|
||||
|
||||
if (_vehicle_command_pub) {
|
||||
orb_publish(ORB_ID(vehicle_command), _vehicle_command_pub, &vehicle_command);
|
||||
|
||||
} else {
|
||||
_vehicle_command_pub = orb_advertise_queue(ORB_ID(vehicle_command), &vehicle_command,
|
||||
vehicle_command_s::ORB_QUEUE_LENGTH);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
if (!_vehicle_command_pub) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
_handle_position_update();
|
||||
|
||||
hrt_abstime t = hrt_absolute_time();
|
||||
_calculate_output_angles(t);
|
||||
|
||||
vehicle_command.timestamp = t;
|
||||
vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL;
|
||||
|
||||
// vmount spec has roll, pitch on channels 0, 1, respectively; MAVLink spec has roll, pitch on channels 1, 0, respectively
|
||||
// vmount uses radians, MAVLink uses degrees
|
||||
vehicle_command.param1 = (_angle_outputs[1] + _config.pitch_offset) * M_RAD_TO_DEG_F;
|
||||
vehicle_command.param2 = (_angle_outputs[0] + _config.roll_offset) * M_RAD_TO_DEG_F;
|
||||
vehicle_command.param3 = (_angle_outputs[2] + _config.yaw_offset) * M_RAD_TO_DEG_F;
|
||||
|
||||
orb_publish(ORB_ID(vehicle_command), _vehicle_command_pub, &vehicle_command);
|
||||
|
||||
_last_update = t;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
void OutputMavlink::print_status()
|
||||
{
|
||||
PX4_INFO("Output: Mavlink");
|
||||
}
|
||||
|
||||
} /* namespace vmount */
|
||||
|
||||
@@ -0,0 +1,70 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 output_mavlink.h
|
||||
* @author Beat Küng <beat-kueng@gmx.net>
|
||||
*
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "output.h"
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
|
||||
|
||||
namespace vmount
|
||||
{
|
||||
|
||||
|
||||
/**
|
||||
** class OutputMavlink
|
||||
* Output via vehicle_command topic
|
||||
*/
|
||||
class OutputMavlink : public OutputBase
|
||||
{
|
||||
public:
|
||||
OutputMavlink(const OutputConfig &output_config);
|
||||
virtual ~OutputMavlink();
|
||||
|
||||
virtual int update(const ControlData *control_data);
|
||||
|
||||
virtual void print_status();
|
||||
|
||||
private:
|
||||
orb_advert_t _vehicle_command_pub = nullptr;
|
||||
};
|
||||
|
||||
|
||||
} /* namespace vmount */
|
||||
@@ -0,0 +1,97 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 output_rc.cpp
|
||||
* @author Leon Müller (thedevleon)
|
||||
* @author Beat Küng <beat-kueng@gmx.net>
|
||||
*
|
||||
*/
|
||||
|
||||
#include "output_rc.h"
|
||||
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <px4_defines.h>
|
||||
|
||||
|
||||
namespace vmount
|
||||
{
|
||||
|
||||
OutputRC::OutputRC(const OutputConfig &output_config)
|
||||
: OutputBase(output_config)
|
||||
{
|
||||
}
|
||||
OutputRC::~OutputRC()
|
||||
{
|
||||
if (_actuator_controls_pub) {
|
||||
orb_unadvertise(_actuator_controls_pub);
|
||||
}
|
||||
}
|
||||
|
||||
int OutputRC::update(const ControlData *control_data)
|
||||
{
|
||||
if (control_data) {
|
||||
//got new command
|
||||
_retract_gimbal = control_data->gimbal_shutter_retract;
|
||||
_set_angle_setpoints(control_data);
|
||||
}
|
||||
|
||||
_handle_position_update();
|
||||
|
||||
hrt_abstime t = hrt_absolute_time();
|
||||
_calculate_output_angles(t);
|
||||
|
||||
actuator_controls_s actuator_controls;
|
||||
actuator_controls.timestamp = hrt_absolute_time();
|
||||
// _angle_outputs are in radians, actuator_controls are in [-1, 1]
|
||||
actuator_controls.control[0] = (_angle_outputs[0] + _config.roll_offset) * _config.roll_scale;
|
||||
actuator_controls.control[1] = (_angle_outputs[1] + _config.pitch_offset) * _config.pitch_scale;
|
||||
actuator_controls.control[2] = (_angle_outputs[2] + _config.yaw_offset) * _config.yaw_scale;
|
||||
actuator_controls.control[3] = _retract_gimbal ? _config.gimbal_retracted_mode_value : _config.gimbal_normal_mode_value;
|
||||
|
||||
int instance;
|
||||
orb_publish_auto(ORB_ID(actuator_controls_2), &_actuator_controls_pub, &actuator_controls,
|
||||
&instance, ORB_PRIO_DEFAULT);
|
||||
|
||||
_last_update = t;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
void OutputRC::print_status()
|
||||
{
|
||||
PX4_INFO("Output: AUX");
|
||||
}
|
||||
|
||||
} /* namespace vmount */
|
||||
|
||||
@@ -0,0 +1,71 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 output_rc.h
|
||||
* @author Beat Küng <beat-kueng@gmx.net>
|
||||
*
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "output.h"
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
|
||||
|
||||
namespace vmount
|
||||
{
|
||||
|
||||
|
||||
/**
|
||||
** class OutputRC
|
||||
* Output via actuator_controls_2 topic
|
||||
*/
|
||||
class OutputRC : public OutputBase
|
||||
{
|
||||
public:
|
||||
OutputRC(const OutputConfig &output_config);
|
||||
virtual ~OutputRC();
|
||||
|
||||
virtual int update(const ControlData *control_data);
|
||||
|
||||
virtual void print_status();
|
||||
|
||||
private:
|
||||
orb_advert_t _actuator_controls_pub = nullptr;
|
||||
bool _retract_gimbal = true;
|
||||
};
|
||||
|
||||
|
||||
} /* namespace vmount */
|
||||
@@ -0,0 +1,607 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-2017 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file vmount.cpp
|
||||
* @author Leon Müller (thedevleon)
|
||||
* @author Beat Küng <beat-kueng@gmx.net>
|
||||
* @author Julian Oes <julian@oes.ch>
|
||||
* @author Matthew Edwards (mje-nz)
|
||||
*
|
||||
* Driver for to control mounts such as gimbals or servos.
|
||||
* Inputs for the mounts can RC and/or mavlink commands.
|
||||
* Outputs to the mounts can be RC (PWM) output or mavlink.
|
||||
*/
|
||||
|
||||
#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 <px4_defines.h>
|
||||
#include <px4_tasks.h>
|
||||
|
||||
#include "input_mavlink.h"
|
||||
#include "input_rc.h"
|
||||
#include "input_test.h"
|
||||
#include "output_rc.h"
|
||||
#include "output_mavlink.h"
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
|
||||
#include <px4_config.h>
|
||||
#include <px4_module.h>
|
||||
|
||||
using namespace vmount;
|
||||
|
||||
/* thread state */
|
||||
static volatile bool thread_should_exit = false;
|
||||
static volatile bool thread_running = false;
|
||||
|
||||
static constexpr int input_objs_len_max = 3;
|
||||
|
||||
struct ThreadData {
|
||||
InputBase *input_objs[input_objs_len_max] = {nullptr, nullptr, nullptr};
|
||||
int input_objs_len = 0;
|
||||
OutputBase *output_obj = nullptr;
|
||||
};
|
||||
static volatile ThreadData *g_thread_data = nullptr;
|
||||
|
||||
struct Parameters {
|
||||
int32_t mnt_mode_in;
|
||||
int32_t mnt_mode_out;
|
||||
int32_t mnt_mav_sysid;
|
||||
int32_t mnt_mav_compid;
|
||||
float mnt_ob_lock_mode;
|
||||
float mnt_ob_norm_mode;
|
||||
int32_t mnt_man_pitch;
|
||||
int32_t mnt_man_roll;
|
||||
int32_t mnt_man_yaw;
|
||||
int32_t mnt_do_stab;
|
||||
float mnt_range_pitch;
|
||||
float mnt_range_roll;
|
||||
float mnt_range_yaw;
|
||||
float mnt_off_pitch;
|
||||
float mnt_off_roll;
|
||||
float mnt_off_yaw;
|
||||
|
||||
bool operator!=(const Parameters &p)
|
||||
{
|
||||
#pragma GCC diagnostic push
|
||||
#pragma GCC diagnostic ignored "-Wfloat-equal"
|
||||
return mnt_mode_in != p.mnt_mode_in ||
|
||||
mnt_mode_out != p.mnt_mode_out ||
|
||||
mnt_mav_sysid != p.mnt_mav_sysid ||
|
||||
mnt_mav_compid != p.mnt_mav_compid ||
|
||||
fabsf(mnt_ob_lock_mode - p.mnt_ob_lock_mode) > 1e-6f ||
|
||||
fabsf(mnt_ob_norm_mode - p.mnt_ob_norm_mode) > 1e-6f ||
|
||||
mnt_man_pitch != p.mnt_man_pitch ||
|
||||
mnt_man_roll != p.mnt_man_roll ||
|
||||
mnt_man_yaw != p.mnt_man_yaw ||
|
||||
mnt_do_stab != p.mnt_do_stab ||
|
||||
mnt_range_pitch != p.mnt_range_pitch ||
|
||||
mnt_range_roll != p.mnt_range_roll ||
|
||||
mnt_range_yaw != p.mnt_range_yaw ||
|
||||
mnt_off_pitch != p.mnt_off_pitch ||
|
||||
mnt_off_roll != p.mnt_off_roll ||
|
||||
mnt_off_yaw != p.mnt_off_yaw;
|
||||
#pragma GCC diagnostic pop
|
||||
|
||||
}
|
||||
};
|
||||
|
||||
struct ParameterHandles {
|
||||
param_t mnt_mode_in;
|
||||
param_t mnt_mode_out;
|
||||
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_pitch;
|
||||
param_t mnt_man_roll;
|
||||
param_t mnt_man_yaw;
|
||||
param_t mnt_do_stab;
|
||||
param_t mnt_range_pitch;
|
||||
param_t mnt_range_roll;
|
||||
param_t mnt_range_yaw;
|
||||
param_t mnt_off_pitch;
|
||||
param_t mnt_off_roll;
|
||||
param_t mnt_off_yaw;
|
||||
};
|
||||
|
||||
|
||||
/* functions */
|
||||
static void usage();
|
||||
static void update_params(ParameterHandles ¶m_handles, Parameters ¶ms, bool &got_changes);
|
||||
static bool get_params(ParameterHandles ¶m_handles, Parameters ¶ms);
|
||||
|
||||
static int vmount_thread_main(int argc, char *argv[]);
|
||||
extern "C" __EXPORT int vmount_main(int argc, char *argv[]);
|
||||
|
||||
static void usage()
|
||||
{
|
||||
PRINT_MODULE_DESCRIPTION(
|
||||
R"DESCR_STR(
|
||||
### Description
|
||||
Mount (Gimbal) control driver. It maps several different input methods (eg. RC or MAVLink) to a configured
|
||||
output (eg. AUX channels or MAVLink).
|
||||
|
||||
Documentation how to use it is on the [gimbal_control](https://dev.px4.io/en/advanced/gimbal_control.html) page.
|
||||
|
||||
### Implementation
|
||||
Each method is implemented in its own class, and there is a common base class for inputs and outputs.
|
||||
They are connected via an API, defined by the `ControlData` data structure. This makes sure that each input method
|
||||
can be used with each output method and new inputs/outputs can be added with minimal effort.
|
||||
|
||||
### Examples
|
||||
Test the output by setting a fixed yaw angle (and the other axes to 0):
|
||||
$ vmount stop
|
||||
$ vmount test yaw 30
|
||||
)DESCR_STR");
|
||||
|
||||
PRINT_MODULE_USAGE_NAME("vmount", "driver");
|
||||
PRINT_MODULE_USAGE_COMMAND("start");
|
||||
PRINT_MODULE_USAGE_COMMAND_DESCR("test", "Test the output: set a fixed angle for one axis (vmount must not be running)");
|
||||
PRINT_MODULE_USAGE_ARG("roll|pitch|yaw <angle>", "Specify an axis and an angle in degrees", false);
|
||||
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
|
||||
}
|
||||
|
||||
static int vmount_thread_main(int argc, char *argv[])
|
||||
{
|
||||
ParameterHandles param_handles;
|
||||
Parameters params;
|
||||
OutputConfig output_config;
|
||||
ThreadData thread_data;
|
||||
memset(¶ms, 0, sizeof(params));
|
||||
|
||||
|
||||
InputTest *test_input = nullptr;
|
||||
|
||||
#ifdef __PX4_NUTTX
|
||||
/* the NuttX optarg handler does not
|
||||
* ignore argv[0] like the POSIX handler
|
||||
* does, nor does it deal with non-flag
|
||||
* verbs well. So we Remove the application
|
||||
* name and the verb.
|
||||
*/
|
||||
argc -= 1;
|
||||
argv += 1;
|
||||
#endif
|
||||
|
||||
if (argc > 0 && !strcmp(argv[0], "test")) {
|
||||
PX4_INFO("Starting in test mode");
|
||||
|
||||
const char *axis_names[3] = {"roll", "pitch", "yaw"};
|
||||
float angles[3] = { 0.f, 0.f, 0.f };
|
||||
|
||||
if (argc == 3) {
|
||||
bool found_axis = false;
|
||||
|
||||
for (int i = 0 ; i < 3; ++i) {
|
||||
if (!strcmp(argv[1], axis_names[i])) {
|
||||
long angle_deg = strtol(argv[2], nullptr, 0);
|
||||
angles[i] = (float)angle_deg;
|
||||
found_axis = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (!found_axis) {
|
||||
usage();
|
||||
return -1;
|
||||
}
|
||||
|
||||
test_input = new InputTest(angles[0], angles[1], angles[2]);
|
||||
|
||||
if (!test_input) {
|
||||
PX4_ERR("memory allocation failed");
|
||||
return -1;
|
||||
}
|
||||
|
||||
} else {
|
||||
usage();
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
if (!get_params(param_handles, params)) {
|
||||
PX4_ERR("could not get mount parameters!");
|
||||
delete test_input;
|
||||
return -1;
|
||||
}
|
||||
|
||||
int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
|
||||
thread_running = true;
|
||||
ControlData *control_data = nullptr;
|
||||
g_thread_data = &thread_data;
|
||||
|
||||
int last_active = 0;
|
||||
|
||||
while (!thread_should_exit) {
|
||||
|
||||
if (!thread_data.input_objs[0] && (params.mnt_mode_in >= 0 || test_input)) { //need to initialize
|
||||
|
||||
output_config.gimbal_normal_mode_value = params.mnt_ob_norm_mode;
|
||||
output_config.gimbal_retracted_mode_value = params.mnt_ob_lock_mode;
|
||||
output_config.pitch_scale = 1.0f / ((params.mnt_range_pitch / 2.0f) * M_DEG_TO_RAD_F);
|
||||
output_config.roll_scale = 1.0f / ((params.mnt_range_roll / 2.0f) * M_DEG_TO_RAD_F);
|
||||
output_config.yaw_scale = 1.0f / ((params.mnt_range_yaw / 2.0f) * M_DEG_TO_RAD_F);
|
||||
output_config.pitch_offset = params.mnt_off_pitch * M_DEG_TO_RAD_F;
|
||||
output_config.roll_offset = params.mnt_off_roll * M_DEG_TO_RAD_F;
|
||||
output_config.yaw_offset = params.mnt_off_yaw * M_DEG_TO_RAD_F;
|
||||
output_config.mavlink_sys_id = params.mnt_mav_sysid;
|
||||
output_config.mavlink_comp_id = params.mnt_mav_compid;
|
||||
|
||||
bool alloc_failed = false;
|
||||
thread_data.input_objs_len = 1;
|
||||
|
||||
if (test_input) {
|
||||
thread_data.input_objs[0] = test_input;
|
||||
|
||||
} else {
|
||||
switch (params.mnt_mode_in) {
|
||||
case 0:
|
||||
|
||||
// Automatic
|
||||
thread_data.input_objs[0] = new InputMavlinkCmdMount(params.mnt_do_stab);
|
||||
thread_data.input_objs[1] = new InputMavlinkROI();
|
||||
|
||||
// RC is on purpose last here so that if there are any mavlink
|
||||
// messages, they will take precedence over RC.
|
||||
// This logic is done further below while update() is called.
|
||||
thread_data.input_objs[2] = new InputRC(params.mnt_do_stab, params.mnt_man_roll, params.mnt_man_pitch, params.mnt_man_yaw);
|
||||
thread_data.input_objs_len = 3;
|
||||
|
||||
break;
|
||||
|
||||
case 1: //RC
|
||||
thread_data.input_objs[0] = new InputRC(params.mnt_do_stab, params.mnt_man_roll, params.mnt_man_pitch, params.mnt_man_yaw);
|
||||
break;
|
||||
|
||||
case 2: //MAVLINK_ROI
|
||||
thread_data.input_objs[0] = new InputMavlinkROI();
|
||||
break;
|
||||
|
||||
case 3: //MAVLINK_DO_MOUNT
|
||||
thread_data.input_objs[0] = new InputMavlinkCmdMount(params.mnt_do_stab);
|
||||
break;
|
||||
|
||||
default:
|
||||
PX4_ERR("invalid input mode %i", params.mnt_mode_in);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
for (int i = 0; i < thread_data.input_objs_len; ++i) {
|
||||
if (!thread_data.input_objs[i]) {
|
||||
alloc_failed = true;
|
||||
}
|
||||
}
|
||||
|
||||
switch (params.mnt_mode_out) {
|
||||
case 0: //AUX
|
||||
thread_data.output_obj = new OutputRC(output_config);
|
||||
|
||||
if (!thread_data.output_obj) { alloc_failed = true; }
|
||||
|
||||
break;
|
||||
|
||||
case 1: //MAVLINK
|
||||
thread_data.output_obj = new OutputMavlink(output_config);
|
||||
|
||||
if (!thread_data.output_obj) { alloc_failed = true; }
|
||||
|
||||
break;
|
||||
|
||||
default:
|
||||
PX4_ERR("invalid output mode %i", params.mnt_mode_out);
|
||||
thread_should_exit = true;
|
||||
break;
|
||||
}
|
||||
|
||||
if (alloc_failed) {
|
||||
thread_data.input_objs_len = 0;
|
||||
PX4_ERR("memory allocation failed");
|
||||
thread_should_exit = true;
|
||||
}
|
||||
|
||||
if (thread_should_exit) {
|
||||
break;
|
||||
}
|
||||
|
||||
int ret = thread_data.output_obj->initialize();
|
||||
|
||||
if (ret) {
|
||||
PX4_ERR("failed to initialize output mode (%i)", ret);
|
||||
thread_should_exit = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (thread_data.input_objs_len > 0) {
|
||||
|
||||
//get input: we cannot make the timeout too large, because the output needs to update
|
||||
//periodically for stabilization and angle updates.
|
||||
|
||||
for (int i = 0; i < thread_data.input_objs_len; ++i) {
|
||||
|
||||
bool already_active = (last_active == i);
|
||||
|
||||
ControlData *control_data_to_check = nullptr;
|
||||
unsigned int poll_timeout = already_active ? 50 : 0; // poll only on active input to reduce latency
|
||||
int ret = thread_data.input_objs[i]->update(poll_timeout, &control_data_to_check, already_active);
|
||||
|
||||
if (ret) {
|
||||
PX4_ERR("failed to read input %i (ret: %i)", i, ret);
|
||||
continue;
|
||||
}
|
||||
|
||||
if (control_data_to_check != nullptr || already_active) {
|
||||
control_data = control_data_to_check;
|
||||
last_active = i;
|
||||
}
|
||||
}
|
||||
|
||||
//update output
|
||||
int ret = thread_data.output_obj->update(control_data);
|
||||
|
||||
if (ret) {
|
||||
PX4_ERR("failed to write output (%i)", ret);
|
||||
break;
|
||||
}
|
||||
|
||||
thread_data.output_obj->publish();
|
||||
|
||||
} else {
|
||||
//wait for parameter changes. We still need to wake up regularily to check for thread exit requests
|
||||
usleep(1e6);
|
||||
}
|
||||
|
||||
if (test_input && test_input->finished()) {
|
||||
thread_should_exit = true;
|
||||
break;
|
||||
}
|
||||
|
||||
|
||||
//check for parameter changes
|
||||
bool updated;
|
||||
|
||||
if (orb_check(parameter_update_sub, &updated) == 0 && updated) {
|
||||
parameter_update_s param_update;
|
||||
orb_copy(ORB_ID(parameter_update), parameter_update_sub, ¶m_update);
|
||||
update_params(param_handles, params, updated);
|
||||
|
||||
if (updated) {
|
||||
//re-init objects
|
||||
for (int i = 0; i < input_objs_len_max; ++i) {
|
||||
if (thread_data.input_objs[i]) {
|
||||
delete (thread_data.input_objs[i]);
|
||||
thread_data.input_objs[i] = nullptr;
|
||||
}
|
||||
}
|
||||
|
||||
thread_data.input_objs_len = 0;
|
||||
|
||||
last_active = 0;
|
||||
|
||||
if (thread_data.output_obj) {
|
||||
delete (thread_data.output_obj);
|
||||
thread_data.output_obj = nullptr;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
g_thread_data = nullptr;
|
||||
|
||||
orb_unsubscribe(parameter_update_sub);
|
||||
|
||||
for (int i = 0; i < input_objs_len_max; ++i) {
|
||||
if (thread_data.input_objs[i]) {
|
||||
delete (thread_data.input_objs[i]);
|
||||
thread_data.input_objs[i] = nullptr;
|
||||
}
|
||||
}
|
||||
|
||||
thread_data.input_objs_len = 0;
|
||||
|
||||
if (thread_data.output_obj) {
|
||||
delete (thread_data.output_obj);
|
||||
thread_data.output_obj = nullptr;
|
||||
}
|
||||
|
||||
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 < 2) {
|
||||
PX4_ERR("missing command");
|
||||
usage();
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "start") || !strcmp(argv[1], "test")) {
|
||||
|
||||
/* this is not an error */
|
||||
if (thread_running) {
|
||||
PX4_WARN("mount driver already running");
|
||||
return 0;
|
||||
}
|
||||
|
||||
thread_should_exit = false;
|
||||
int vmount_task = px4_task_spawn_cmd("vmount",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_DEFAULT,
|
||||
1900,
|
||||
vmount_thread_main,
|
||||
(char *const *)argv + 1);
|
||||
|
||||
int counter = 0;
|
||||
|
||||
while (!thread_running && vmount_task >= 0) {
|
||||
usleep(5000);
|
||||
|
||||
if (++counter >= 100) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (vmount_task < 0) {
|
||||
PX4_ERR("failed to start");
|
||||
return -1;
|
||||
}
|
||||
|
||||
return counter < 100 || thread_should_exit ? 0 : -1;
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "stop")) {
|
||||
|
||||
/* this is not an error */
|
||||
if (!thread_running) {
|
||||
PX4_WARN("mount driver not running");
|
||||
return 0;
|
||||
}
|
||||
|
||||
thread_should_exit = true;
|
||||
|
||||
while (thread_running) {
|
||||
usleep(100000);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "status")) {
|
||||
if (thread_running && g_thread_data) {
|
||||
|
||||
for (int i = 0; i < g_thread_data->input_objs_len; ++i) {
|
||||
g_thread_data->input_objs[i]->print_status();
|
||||
}
|
||||
|
||||
if (g_thread_data->input_objs_len == 0) {
|
||||
PX4_INFO("Input: None");
|
||||
}
|
||||
|
||||
if (g_thread_data->output_obj) {
|
||||
g_thread_data->output_obj->print_status();
|
||||
|
||||
} else {
|
||||
PX4_INFO("Output: None");
|
||||
}
|
||||
|
||||
} else {
|
||||
PX4_INFO("not running");
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
PX4_ERR("unrecognized command");
|
||||
usage();
|
||||
return -1;
|
||||
}
|
||||
|
||||
void update_params(ParameterHandles ¶m_handles, Parameters ¶ms, bool &got_changes)
|
||||
{
|
||||
Parameters prev_params = params;
|
||||
param_get(param_handles.mnt_mode_in, ¶ms.mnt_mode_in);
|
||||
param_get(param_handles.mnt_mode_out, ¶ms.mnt_mode_out);
|
||||
param_get(param_handles.mnt_mav_sysid, ¶ms.mnt_mav_sysid);
|
||||
param_get(param_handles.mnt_mav_compid, ¶ms.mnt_mav_compid);
|
||||
param_get(param_handles.mnt_ob_lock_mode, ¶ms.mnt_ob_lock_mode);
|
||||
param_get(param_handles.mnt_ob_norm_mode, ¶ms.mnt_ob_norm_mode);
|
||||
param_get(param_handles.mnt_man_pitch, ¶ms.mnt_man_pitch);
|
||||
param_get(param_handles.mnt_man_roll, ¶ms.mnt_man_roll);
|
||||
param_get(param_handles.mnt_man_yaw, ¶ms.mnt_man_yaw);
|
||||
param_get(param_handles.mnt_do_stab, ¶ms.mnt_do_stab);
|
||||
param_get(param_handles.mnt_range_pitch, ¶ms.mnt_range_pitch);
|
||||
param_get(param_handles.mnt_range_roll, ¶ms.mnt_range_roll);
|
||||
param_get(param_handles.mnt_range_yaw, ¶ms.mnt_range_yaw);
|
||||
param_get(param_handles.mnt_off_pitch, ¶ms.mnt_off_pitch);
|
||||
param_get(param_handles.mnt_off_roll, ¶ms.mnt_off_roll);
|
||||
param_get(param_handles.mnt_off_yaw, ¶ms.mnt_off_yaw);
|
||||
|
||||
got_changes = prev_params != params;
|
||||
}
|
||||
|
||||
bool get_params(ParameterHandles ¶m_handles, Parameters ¶ms)
|
||||
{
|
||||
param_handles.mnt_mode_in = param_find("MNT_MODE_IN");
|
||||
param_handles.mnt_mode_out = param_find("MNT_MODE_OUT");
|
||||
param_handles.mnt_mav_sysid = param_find("MNT_MAV_SYSID");
|
||||
param_handles.mnt_mav_compid = param_find("MNT_MAV_COMPID");
|
||||
param_handles.mnt_ob_lock_mode = param_find("MNT_OB_LOCK_MODE");
|
||||
param_handles.mnt_ob_norm_mode = param_find("MNT_OB_NORM_MODE");
|
||||
param_handles.mnt_man_pitch = param_find("MNT_MAN_PITCH");
|
||||
param_handles.mnt_man_roll = param_find("MNT_MAN_ROLL");
|
||||
param_handles.mnt_man_yaw = param_find("MNT_MAN_YAW");
|
||||
param_handles.mnt_do_stab = param_find("MNT_DO_STAB");
|
||||
param_handles.mnt_range_pitch = param_find("MNT_RANGE_PITCH");
|
||||
param_handles.mnt_range_roll = param_find("MNT_RANGE_ROLL");
|
||||
param_handles.mnt_range_yaw = param_find("MNT_RANGE_YAW");
|
||||
param_handles.mnt_off_pitch = param_find("MNT_OFF_PITCH");
|
||||
param_handles.mnt_off_roll = param_find("MNT_OFF_ROLL");
|
||||
param_handles.mnt_off_yaw = param_find("MNT_OFF_YAW");
|
||||
|
||||
if (param_handles.mnt_mode_in == PARAM_INVALID ||
|
||||
param_handles.mnt_mode_out == PARAM_INVALID ||
|
||||
param_handles.mnt_mav_sysid == PARAM_INVALID ||
|
||||
param_handles.mnt_mav_compid == PARAM_INVALID ||
|
||||
param_handles.mnt_ob_lock_mode == PARAM_INVALID ||
|
||||
param_handles.mnt_ob_norm_mode == PARAM_INVALID ||
|
||||
param_handles.mnt_man_pitch == PARAM_INVALID ||
|
||||
param_handles.mnt_man_roll == PARAM_INVALID ||
|
||||
param_handles.mnt_man_yaw == PARAM_INVALID ||
|
||||
param_handles.mnt_do_stab == PARAM_INVALID ||
|
||||
param_handles.mnt_range_pitch == PARAM_INVALID ||
|
||||
param_handles.mnt_range_roll == PARAM_INVALID ||
|
||||
param_handles.mnt_range_yaw == PARAM_INVALID ||
|
||||
param_handles.mnt_off_pitch == PARAM_INVALID ||
|
||||
param_handles.mnt_off_roll == PARAM_INVALID ||
|
||||
param_handles.mnt_off_yaw == PARAM_INVALID) {
|
||||
return false;
|
||||
}
|
||||
|
||||
bool dummy;
|
||||
update_params(param_handles, params, dummy);
|
||||
return true;
|
||||
}
|
||||
@@ -0,0 +1,227 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-2017 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file vmount_params.c
|
||||
* @author Leon Müller (thedevleon)
|
||||
* @author Matthew Edwards (mje-nz)
|
||||
*
|
||||
*/
|
||||
|
||||
/**
|
||||
* Mount input mode
|
||||
*
|
||||
* RC uses the AUX input channels (see MNT_MAN_* parameters),
|
||||
* MAVLINK_ROI uses the MAV_CMD_DO_SET_ROI Mavlink message, and MAVLINK_DO_MOUNT the
|
||||
* MAV_CMD_DO_MOUNT_CONFIGURE and MAV_CMD_DO_MOUNT_CONTROL messages to control a mount.
|
||||
*
|
||||
* @value -1 DISABLED
|
||||
* @value 0 AUTO
|
||||
* @value 1 RC
|
||||
* @value 2 MAVLINK_ROI
|
||||
* @value 3 MAVLINK_DO_MOUNT
|
||||
* @min -1
|
||||
* @max 3
|
||||
* @group Mount
|
||||
* @reboot_required true
|
||||
*/
|
||||
PARAM_DEFINE_INT32(MNT_MODE_IN, -1);
|
||||
|
||||
/**
|
||||
* Mount output mode
|
||||
*
|
||||
* AUX uses the mixer output Control Group #2.
|
||||
* MAVLINK uses the MAV_CMD_DO_MOUNT_CONFIGURE and MAV_CMD_DO_MOUNT_CONTROL MavLink messages
|
||||
* to control a mount (set MNT_MAV_SYSID & MNT_MAV_COMPID)
|
||||
*
|
||||
* @value 0 AUX
|
||||
* @value 1 MAVLINK
|
||||
* @min 0
|
||||
* @max 1
|
||||
* @group Mount
|
||||
*/
|
||||
PARAM_DEFINE_INT32(MNT_MODE_OUT, 0);
|
||||
|
||||
/**
|
||||
* Mavlink System ID of the mount
|
||||
*
|
||||
* If MNT_MODE_OUT is MAVLINK, mount configure/control commands will be sent with this target ID.
|
||||
*
|
||||
* @group Mount
|
||||
*/
|
||||
PARAM_DEFINE_INT32(MNT_MAV_SYSID, 1);
|
||||
|
||||
/**
|
||||
* Mavlink Component ID of the mount
|
||||
*
|
||||
* If MNT_MODE_OUT is MAVLINK, mount configure/control commands will be sent with this component ID.
|
||||
*
|
||||
* @group Mount
|
||||
*/
|
||||
PARAM_DEFINE_INT32(MNT_MAV_COMPID, 154);
|
||||
|
||||
/**
|
||||
* Mixer value for selecting normal mode
|
||||
* if required by the gimbal (only in AUX output 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 AUX output mode)
|
||||
*
|
||||
* @min -1.0
|
||||
* @max 1.0
|
||||
* @decimal 3
|
||||
* @group Mount
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MNT_OB_LOCK_MODE, 0.0f);
|
||||
|
||||
/**
|
||||
* Auxiliary channel to control roll (in AUX input or manual mode).
|
||||
*
|
||||
* @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 (in AUX input or manual mode).
|
||||
*
|
||||
* @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 (in AUX input or manual mode).
|
||||
*
|
||||
* @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);
|
||||
|
||||
/**
|
||||
* Stabilize the mount (set to true for servo gimbal, false for passthrough).
|
||||
* Does not affect MAVLINK_ROI input.
|
||||
*
|
||||
* @boolean
|
||||
* @group Mount
|
||||
*/
|
||||
PARAM_DEFINE_INT32(MNT_DO_STAB, 0);
|
||||
|
||||
/**
|
||||
* Range of pitch channel output in degrees (only in AUX output mode).
|
||||
*
|
||||
* @min 1.0
|
||||
* @max 720.0
|
||||
* @decimal 1
|
||||
* @group Mount
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MNT_RANGE_PITCH, 360.0f);
|
||||
|
||||
/**
|
||||
* Range of roll channel output in degrees (only in AUX output mode).
|
||||
*
|
||||
* @min 1.0
|
||||
* @max 720.0
|
||||
* @decimal 1
|
||||
* @group Mount
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MNT_RANGE_ROLL, 360.0f);
|
||||
|
||||
/**
|
||||
* Range of yaw channel output in degrees (only in AUX output mode).
|
||||
*
|
||||
* @min 1.0
|
||||
* @max 720.0
|
||||
* @decimal 1
|
||||
* @group Mount
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MNT_RANGE_YAW, 360.0f);
|
||||
|
||||
/**
|
||||
* Offset for pitch channel output in degrees.
|
||||
*
|
||||
* @min -360.0
|
||||
* @max 360.0
|
||||
* @decimal 1
|
||||
* @group Mount
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MNT_OFF_PITCH, 0.0f);
|
||||
|
||||
/**
|
||||
* Offset for roll channel output in degrees.
|
||||
*
|
||||
* @min -360.0
|
||||
* @max 360.0
|
||||
* @decimal 1
|
||||
* @group Mount
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MNT_OFF_ROLL, 0.0f);
|
||||
|
||||
/**
|
||||
* Offset for yaw channel output in degrees.
|
||||
*
|
||||
* @min -360.0
|
||||
* @max 360.0
|
||||
* @decimal 1
|
||||
* @group Mount
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MNT_OFF_YAW, 0.0f);
|
||||
Reference in New Issue
Block a user