mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
vmount: refactor architecture & use C++
This splits vmount into inputs and outputs modules with a small API in between. It allows for greater flexibility, as any input method can be combined with any output method. At the same time it is easy to add a new input or output module.
This commit is contained in:
Beat Küng
parent
8757b9a9a5
commit
94dbf358bd
@ -37,11 +37,14 @@ px4_add_module(
|
||||
COMPILE_FLAGS
|
||||
-Os
|
||||
SRCS
|
||||
vmount.cpp
|
||||
vmount_params.c
|
||||
vmount_rc.cpp
|
||||
vmount_mavlink.cpp
|
||||
vmount_onboard.cpp
|
||||
input.cpp
|
||||
input_mavlink.cpp
|
||||
input_rc.cpp
|
||||
output.cpp
|
||||
output_mavlink.cpp
|
||||
output_rc.cpp
|
||||
vmount.cpp
|
||||
vmount_params.c
|
||||
DEPENDS
|
||||
platforms__common
|
||||
)
|
||||
|
||||
90
src/drivers/vmount/common.h
Normal file
90
src/drivers/vmount/common.h
Normal file
@ -0,0 +1,90 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 */
|
||||
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 */
|
||||
|
||||
bool gimbal_shutter_retract = false; /**< whether to lock the gimbal (only in RC output mode) */
|
||||
|
||||
};
|
||||
|
||||
|
||||
} /* namespace vmount */
|
||||
@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-2016 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2016 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
@ -32,28 +32,47 @@
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file vmount_onboard
|
||||
* @author Leon Müller (thedevleon)
|
||||
* @file input.cpp
|
||||
* @author Beat Küng <beat-kueng@gmx.net>
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef _VMOUNT_ONBOARD_H
|
||||
#define _VMOUNT_ONBOARD_H
|
||||
#include "input.h"
|
||||
|
||||
#include <sys/types.h>
|
||||
#include <stdbool.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
|
||||
// Public functions
|
||||
bool vmount_onboard_init(void);
|
||||
void vmount_onboard_deinit(void);
|
||||
void vmount_onboard_configure(int new_mount_mode, bool new_stab_roll, bool new_stab_pitch, bool new_stab_yaw);
|
||||
void vmount_onboard_set_location(double lat, double lon, float alt);
|
||||
void vmount_onboard_set_manual(double pitch_new, double roll_new, float yaw_new);
|
||||
void vmount_onboard_set_mode(int new_mount_mode);
|
||||
void vmount_onboard_point(double global_lat, double global_lon, float global_alt);
|
||||
void vmount_onboard_point_manual(float pitch_target, float roll_target, float yaw_target);
|
||||
void vmount_onboard_update_attitude(vehicle_attitude_s vehicle_attitude_new);
|
||||
float vmount_onboard_calculate_pitch(double global_lat, double global_lon, float global_alt);
|
||||
namespace vmount
|
||||
{
|
||||
|
||||
int InputBase::update(unsigned int timeout_ms, ControlData **control_data)
|
||||
{
|
||||
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);
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
} /* namespace vmount */
|
||||
|
||||
#endif /* _VMOUNT_ONBOARD_H */
|
||||
88
src/drivers/vmount/input.h
Normal file
88
src/drivers/vmount/input.h
Normal file
@ -0,0 +1,88 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 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).
|
||||
* @return 0 on success, <0 otherwise
|
||||
*/
|
||||
virtual int update(unsigned int timeout_ms, ControlData **control_data);
|
||||
|
||||
/** report status to stdout */
|
||||
virtual void print_status() = 0;
|
||||
|
||||
protected:
|
||||
virtual int update_impl(unsigned int timeout_ms, ControlData **control_data) = 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 */
|
||||
317
src/drivers/vmount/input_mavlink.cpp
Normal file
317
src/drivers/vmount/input_mavlink.cpp
Normal file
@ -0,0 +1,317 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 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.h>
|
||||
#include <uORB/topics/vehicle_command_ack.h>
|
||||
#include <uORB/topics/position_setpoint_triplet.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
#include <px4_posix.h>
|
||||
#include <errno.h>
|
||||
|
||||
|
||||
namespace vmount
|
||||
{
|
||||
|
||||
|
||||
InputMavlinkROI::InputMavlinkROI(InputRC *manual_override)
|
||||
: _manual_control(manual_override)
|
||||
{
|
||||
}
|
||||
|
||||
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)
|
||||
{
|
||||
px4_pollfd_struct_t polls[3];
|
||||
polls[0].fd = _vehicle_roi_sub;
|
||||
polls[0].events = POLLIN;
|
||||
polls[1].fd = _position_setpoint_triplet_sub;
|
||||
polls[1].events = POLLIN;
|
||||
int num_poll = 2;
|
||||
|
||||
if (_manual_control && _allow_manual_control) {
|
||||
polls[num_poll].fd = _manual_control->_get_subscription_fd();
|
||||
polls[num_poll].events = POLLIN;
|
||||
++num_poll;
|
||||
}
|
||||
|
||||
int ret = px4_poll(polls, num_poll, timeout_ms);
|
||||
|
||||
if (ret < 0) {
|
||||
return -errno;
|
||||
}
|
||||
|
||||
if (ret == 0) {
|
||||
*control_data = nullptr;
|
||||
|
||||
} else {
|
||||
if (polls[0].revents & POLLIN) {
|
||||
vehicle_roi_s vehicle_roi;
|
||||
orb_copy(ORB_ID(vehicle_roi), _vehicle_roi_sub, &vehicle_roi);
|
||||
|
||||
if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_NONE) {
|
||||
_allow_manual_control = true;
|
||||
|
||||
if (!_manual_control) {
|
||||
_control_data.type = ControlData::Type::Neutral;
|
||||
}
|
||||
|
||||
} else if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_WPNEXT) {
|
||||
_allow_manual_control = false;
|
||||
_read_control_data_from_position_setpoint_sub();
|
||||
_control_data.type_data.lonlat.roll_angle = 0.f;
|
||||
_control_data.type_data.lonlat.pitch_fixed_angle = -10.f;
|
||||
|
||||
} else if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_WPINDEX) {
|
||||
_allow_manual_control = false;
|
||||
//TODO how to do this?
|
||||
|
||||
} else if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_LOCATION) {
|
||||
_allow_manual_control = false;
|
||||
control_data_set_lon_lat(vehicle_roi.lon, vehicle_roi.lat, vehicle_roi.alt);
|
||||
|
||||
} else if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_TARGET) {
|
||||
_allow_manual_control = false;
|
||||
//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;
|
||||
}
|
||||
|
||||
_control_data.gimbal_shutter_retract = false;
|
||||
|
||||
} else if (num_poll > 2 && (polls[2].revents & POLLIN)) {
|
||||
_manual_control->_read_control_data_from_subscription(_control_data);
|
||||
}
|
||||
|
||||
// check whether the position setpoint got updated
|
||||
if ((polls[1].revents & POLLIN) && _cur_roi_mode == vehicle_roi_s::VEHICLE_ROI_WPNEXT) {
|
||||
_read_control_data_from_position_setpoint_sub();
|
||||
}
|
||||
|
||||
*control_data = &_control_data;
|
||||
}
|
||||
|
||||
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.next.lon;
|
||||
_control_data.type_data.lonlat.lat = position_setpoint_triplet.next.lat;
|
||||
_control_data.type_data.lonlat.altitude = position_setpoint_triplet.next.alt;
|
||||
}
|
||||
|
||||
void InputMavlinkROI::print_status()
|
||||
{
|
||||
PX4_INFO("Input: Mavlink (ROI)%s", _manual_control ? " (with manual)" : "");
|
||||
}
|
||||
|
||||
InputMavlinkCmdMount::InputMavlinkCmdMount(InputRC *manual_override)
|
||||
: _manual_control(manual_override)
|
||||
{
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
int InputMavlinkCmdMount::update_impl(unsigned int timeout_ms, ControlData **control_data)
|
||||
{
|
||||
px4_pollfd_struct_t polls[2];
|
||||
polls[0].fd = _vehicle_command_sub;
|
||||
polls[0].events = POLLIN;
|
||||
int num_poll = 1;
|
||||
|
||||
if (_manual_control && _allow_manual_control) {
|
||||
polls[num_poll].fd = _manual_control->_get_subscription_fd();
|
||||
polls[num_poll].events = POLLIN;
|
||||
++num_poll;
|
||||
}
|
||||
|
||||
int ret = px4_poll(polls, num_poll, timeout_ms);
|
||||
|
||||
if (ret < 0) {
|
||||
return -errno;
|
||||
}
|
||||
|
||||
if (ret == 0) {
|
||||
*control_data = nullptr;
|
||||
|
||||
} else {
|
||||
*control_data = &_control_data;
|
||||
|
||||
if (polls[0].revents & POLLIN) {
|
||||
vehicle_command_s vehicle_command;
|
||||
orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &vehicle_command);
|
||||
|
||||
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) {
|
||||
_allow_manual_control = false;
|
||||
|
||||
switch ((int)vehicle_command.param7) {
|
||||
case vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT:
|
||||
_control_data.gimbal_shutter_retract = true;
|
||||
|
||||
//no break
|
||||
case vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL:
|
||||
_control_data.type = ControlData::Type::Neutral;
|
||||
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;
|
||||
_control_data.type_data.angle.angles[0] = vehicle_command.param1;
|
||||
_control_data.type_data.angle.angles[1] = vehicle_command.param2;
|
||||
_control_data.type_data.angle.angles[2] = vehicle_command.param3;
|
||||
|
||||
break;
|
||||
|
||||
case vehicle_command_s::VEHICLE_MOUNT_MODE_RC_TARGETING:
|
||||
_allow_manual_control = true;
|
||||
*control_data = nullptr;
|
||||
|
||||
break;
|
||||
|
||||
case vehicle_command_s::VEHICLE_MOUNT_MODE_GPS_POINT:
|
||||
control_data_set_lon_lat(vehicle_command.param2, vehicle_command.param1, vehicle_command.param3);
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
_ack_vehicle_command(vehicle_command.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
|
||||
|
||||
_ack_vehicle_command(vehicle_command.command);
|
||||
}
|
||||
|
||||
} else if (num_poll > 1 && (polls[1].revents & POLLIN)) {
|
||||
_manual_control->_read_control_data_from_subscription(_control_data);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
void InputMavlinkCmdMount::_ack_vehicle_command(uint16_t command)
|
||||
{
|
||||
vehicle_command_ack_s vehicle_command_ack;
|
||||
vehicle_command_ack.timestamp = hrt_absolute_time();
|
||||
vehicle_command_ack.command = command;
|
||||
vehicle_command_ack.result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
|
||||
|
||||
int instance;
|
||||
orb_publish_auto(ORB_ID(vehicle_command_ack), &_vehicle_command_ack_pub, &vehicle_command_ack,
|
||||
&instance, ORB_PRIO_DEFAULT);
|
||||
}
|
||||
|
||||
void InputMavlinkCmdMount::print_status()
|
||||
{
|
||||
PX4_INFO("Input: Mavlink (CMD_MOUNT)%s", _manual_control ? " (with manual)" : "");
|
||||
}
|
||||
|
||||
|
||||
} /* namespace vmount */
|
||||
|
||||
114
src/drivers/vmount/input_mavlink.h
Normal file
114
src/drivers/vmount/input_mavlink.h
Normal file
@ -0,0 +1,114 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 input_mavlink.h
|
||||
* @author Beat Küng <beat-kueng@gmx.net>
|
||||
*
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "input.h"
|
||||
#include "input_rc.h"
|
||||
#include <uORB/topics/vehicle_roi.h>
|
||||
|
||||
namespace vmount
|
||||
{
|
||||
|
||||
|
||||
/**
|
||||
** class InputMavlinkROI
|
||||
** Input based on the vehicle_roi topic
|
||||
*/
|
||||
class InputMavlinkROI : public InputBase
|
||||
{
|
||||
public:
|
||||
|
||||
/**
|
||||
* @param manual_control if non-null allow manual input as long as we have not received any mavlink
|
||||
* command yet or ROI mode is set to NONE.
|
||||
*/
|
||||
InputMavlinkROI(InputRC *manual_control = nullptr);
|
||||
virtual ~InputMavlinkROI();
|
||||
|
||||
virtual void print_status();
|
||||
|
||||
protected:
|
||||
virtual int update_impl(unsigned int timeout_ms, ControlData **control_data);
|
||||
virtual int initialize();
|
||||
|
||||
private:
|
||||
void _read_control_data_from_position_setpoint_sub();
|
||||
|
||||
int _vehicle_roi_sub = -1;
|
||||
int _position_setpoint_triplet_sub = -1;
|
||||
bool _allow_manual_control = true;
|
||||
InputRC *_manual_control;
|
||||
uint8_t _cur_roi_mode = vehicle_roi_s::VEHICLE_ROI_NONE;
|
||||
};
|
||||
|
||||
|
||||
/**
|
||||
** class InputMavlinkCmdMount
|
||||
** Input based on the VEHICLE_CMD_DO_MOUNT_CONTROL mavlink command
|
||||
*/
|
||||
class InputMavlinkCmdMount : public InputBase
|
||||
{
|
||||
public:
|
||||
|
||||
/**
|
||||
* @param manual_control if non-null allow manual input as long as we have not received any mavlink
|
||||
* command yet.
|
||||
*/
|
||||
InputMavlinkCmdMount(InputRC *manual_control = nullptr);
|
||||
virtual ~InputMavlinkCmdMount();
|
||||
|
||||
virtual void print_status();
|
||||
|
||||
protected:
|
||||
virtual int update_impl(unsigned int timeout_ms, ControlData **control_data);
|
||||
virtual int initialize();
|
||||
|
||||
private:
|
||||
void _ack_vehicle_command(uint16_t command);
|
||||
|
||||
int _vehicle_command_sub = -1;
|
||||
bool _allow_manual_control = true;
|
||||
InputRC *_manual_control;
|
||||
orb_advert_t _vehicle_command_ack_pub = nullptr;
|
||||
bool _stabilize[3] = { false, false, false };
|
||||
};
|
||||
|
||||
|
||||
} /* namespace vmount */
|
||||
146
src/drivers/vmount/input_rc.cpp
Normal file
146
src/drivers/vmount/input_rc.cpp
Normal file
@ -0,0 +1,146 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 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(int aux_channel_roll, int aux_channel_pitch, int aux_channel_yaw)
|
||||
{
|
||||
_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)
|
||||
{
|
||||
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) {
|
||||
*control_data = nullptr;
|
||||
|
||||
} else {
|
||||
if (polls[0].revents & POLLIN) {
|
||||
_read_control_data_from_subscription(_control_data);
|
||||
*control_data = &_control_data;
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
void InputRC::_read_control_data_from_subscription(ControlData &control_data)
|
||||
{
|
||||
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;
|
||||
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
control_data.type_data.angle.is_speed[i] = false;
|
||||
control_data.type_data.angle.angles[i] = _get_aux_value(manual_control_setpoint, i) * M_PI_F;
|
||||
|
||||
control_data.stabilize_axis[i] = false;
|
||||
}
|
||||
|
||||
control_data.gimbal_shutter_retract = 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 */
|
||||
89
src/drivers/vmount/input_rc.h
Normal file
89
src/drivers/vmount/input_rc.h
Normal file
@ -0,0 +1,89 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 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 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(int aux_channel_roll, int aux_channel_pitch, int aux_channel_yaw);
|
||||
virtual ~InputRC();
|
||||
|
||||
|
||||
protected:
|
||||
virtual int update_impl(unsigned int timeout_ms, ControlData **control_data);
|
||||
virtual int initialize();
|
||||
|
||||
virtual void print_status();
|
||||
|
||||
private:
|
||||
float _get_aux_value(const manual_control_setpoint_s &manual_control_setpoint, int channel_idx);
|
||||
|
||||
int _get_subscription_fd() const { return _manual_control_setpoint_sub; }
|
||||
|
||||
void _read_control_data_from_subscription(ControlData &control_data);
|
||||
|
||||
int _aux_channels[3];
|
||||
int _manual_control_setpoint_sub = -1;
|
||||
|
||||
friend class InputMavlinkROI;
|
||||
friend class InputMavlinkCmdMount;
|
||||
};
|
||||
|
||||
|
||||
} /* namespace vmount */
|
||||
205
src/drivers/vmount/output.cpp
Normal file
205
src/drivers/vmount/output.cpp
Normal file
@ -0,0 +1,205 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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_global_position.h>
|
||||
#include <px4_defines.h>
|
||||
#include <geo/geo.h>
|
||||
#include <math.h>
|
||||
|
||||
#define LATLON_TO_M 0.01113195
|
||||
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
float OutputBase::_calculate_pitch(double lon, double lat, float altitude,
|
||||
const vehicle_global_position_s &global_position)
|
||||
{
|
||||
double scale = cos(M_DEG_TO_RAD * ((global_position.lat + lat) * 0.00000005));
|
||||
float x = (float)((lon - global_position.lon) * scale * LATLON_TO_M);
|
||||
float y = (float)((lat - global_position.lat) * LATLON_TO_M);
|
||||
float z = altitude - global_position.alt;
|
||||
float target_distance = sqrtf(x * x + y * y);
|
||||
|
||||
return atan2f(z, target_distance) * (float)M_RAD_TO_DEG;
|
||||
}
|
||||
|
||||
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) * (float)M_RAD_TO_DEG;
|
||||
|
||||
_angle_setpoints[0] = roll;
|
||||
_angle_setpoints[1] = pitch;
|
||||
_angle_setpoints[2] = yaw;
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
float att[3] = { vehicle_attitude.roll, vehicle_attitude.pitch, vehicle_attitude.yaw };
|
||||
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
if (_stabilize[i]) {
|
||||
_angle_outputs[i] = _angle_setpoints[i] - att[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 */
|
||||
|
||||
111
src/drivers/vmount/output.h
Normal file
111
src/drivers/vmount/output.h
Normal file
@ -0,0 +1,111 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 <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 */
|
||||
|
||||
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;
|
||||
|
||||
protected:
|
||||
static float _calculate_pitch(double lon, double lat, float altitude,
|
||||
const vehicle_global_position_s &global_position);
|
||||
|
||||
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 };
|
||||
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 };
|
||||
hrt_abstime _last_update;
|
||||
|
||||
private:
|
||||
int _vehicle_attitude_sub = -1;
|
||||
int _vehicle_global_position_sub = -1;
|
||||
|
||||
};
|
||||
|
||||
|
||||
} /* namespace vmount */
|
||||
113
src/drivers/vmount/output_mavlink.cpp
Normal file
113
src/drivers/vmount/output_mavlink.cpp
Normal file
@ -0,0 +1,113 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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)
|
||||
{
|
||||
}
|
||||
|
||||
int OutputMavlink::update(const ControlData *control_data)
|
||||
{
|
||||
vehicle_command_s vehicle_command;
|
||||
|
||||
if (control_data) {
|
||||
//got new command
|
||||
_set_angle_setpoints(control_data);
|
||||
|
||||
vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE;
|
||||
vehicle_command.target_system = _config.mavlink_sys_id;
|
||||
vehicle_command.target_component = _config.mavlink_comp_id;
|
||||
|
||||
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, 3);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
if (!_vehicle_command_pub) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
_handle_position_update();
|
||||
|
||||
hrt_abstime t = hrt_absolute_time();
|
||||
_calculate_output_angles(t);
|
||||
|
||||
vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL;
|
||||
vehicle_command.target_system = _config.mavlink_sys_id;
|
||||
vehicle_command.target_component = _config.mavlink_comp_id;
|
||||
|
||||
vehicle_command.param1 = _angle_outputs[0];
|
||||
vehicle_command.param2 = _angle_outputs[1];
|
||||
vehicle_command.param3 = _angle_outputs[2];
|
||||
|
||||
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 */
|
||||
|
||||
@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-2016 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2016 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
@ -32,24 +32,39 @@
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file vmount_rc
|
||||
* @author Leon Müller (thedevleon)
|
||||
* @file output_mavlink.h
|
||||
* @author Beat Küng <beat-kueng@gmx.net>
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef _VMOUNT_RC_H
|
||||
#define _VMOUNT_RC_H
|
||||
#pragma once
|
||||
|
||||
#include <sys/types.h>
|
||||
#include <stdbool.h>
|
||||
#include "output.h"
|
||||
|
||||
// Public functions
|
||||
bool vmount_rc_init(void);
|
||||
void vmount_rc_deinit(void);
|
||||
void vmount_rc_configure(int roi_mode, bool man_control, int normal_mode_new, int locked_mode_new);
|
||||
void vmount_rc_set_location(double lat_new, double lon_new, float alt_new);
|
||||
void vmount_rc_point(double global_lat, double global_lon, float global_alt);
|
||||
void vmount_rc_point_manual(float pitch_new, float roll_new, float yaw_new);
|
||||
float vmount_rc_calculate_pitch(double global_lat, double global_lon, float global_alt);
|
||||
#include <uORB/uORB.h>
|
||||
|
||||
#endif /* _VMOUNT_RC_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 */
|
||||
90
src/drivers/vmount/output_rc.cpp
Normal file
90
src/drivers/vmount/output_rc.cpp
Normal file
@ -0,0 +1,90 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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)
|
||||
{
|
||||
}
|
||||
|
||||
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();
|
||||
actuator_controls.control[0] = _angle_outputs[0] / M_PI_F;
|
||||
actuator_controls.control[1] = _angle_outputs[1] / M_PI_F;
|
||||
actuator_controls.control[2] = _angle_outputs[2] / M_PI_F;
|
||||
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 */
|
||||
|
||||
@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-2016 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2016 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
@ -32,25 +32,40 @@
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file vmount_mavlink.h
|
||||
* @author Leon Müller (thedevleon)
|
||||
* @file output_rc.h
|
||||
* @author Beat Küng <beat-kueng@gmx.net>
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef _VMOUNT_MAVLINK_H
|
||||
#define _VMOUNT_MAVLINK_H
|
||||
#pragma once
|
||||
|
||||
#include <sys/types.h>
|
||||
#include <stdbool.h>
|
||||
#include "output.h"
|
||||
|
||||
// Public functions
|
||||
bool vmount_mavlink_init();
|
||||
void vmount_mavlink_deinit(void);
|
||||
void vmount_mavlink_configure(int roi_mode, bool man_control, int sysid, int compid);
|
||||
void vmount_mavlink_set_location(double lat_new, double lon_new, float alt_new);
|
||||
void vmount_mavlink_point(double global_lat, double global_lon, float global_alt);
|
||||
void vmount_mavlink_point_manual(float pitch_new, float roll_new, float yaw_new);
|
||||
float vmount_mavlink_calculate_pitch(double global_lat, double global_lon, float global_alt);
|
||||
#include <uORB/uORB.h>
|
||||
|
||||
|
||||
#endif /* _VMOUNT_MAVLINK_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 */
|
||||
@ -34,6 +34,7 @@
|
||||
/**
|
||||
* @file vmount.cpp
|
||||
* @author Leon Müller (thedevleon)
|
||||
* @author Beat Küng <beat-kueng@gmx.net>
|
||||
* MAV_MOUNT driver for controlling mavlink gimbals, rc gimbals/servors and
|
||||
* future kinds of mounts.
|
||||
*
|
||||
@ -49,82 +50,26 @@
|
||||
#include <systemlib/err.h>
|
||||
#include <systemlib/systemlib.h>
|
||||
|
||||
#include "vmount_mavlink.h"
|
||||
#include "vmount_rc.h"
|
||||
#include "vmount_onboard.h"
|
||||
#include "input_rc.h"
|
||||
#include "input_mavlink.h"
|
||||
#include "output_rc.h"
|
||||
#include "output_mavlink.h"
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/vehicle_roi.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_command.h>
|
||||
#include <uORB/topics/vehicle_command_ack.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/position_setpoint_triplet.h>
|
||||
#include <uORB/topics/manual_control_setpoint.h>
|
||||
|
||||
#include <px4_config.h>
|
||||
#include <px4_posix.h>
|
||||
#include <poll.h>
|
||||
|
||||
using namespace vmount;
|
||||
|
||||
/* thread state */
|
||||
static volatile bool thread_should_exit = false;
|
||||
static volatile bool thread_running = false;
|
||||
static volatile bool thread_should_restart = false;
|
||||
static int vmount_task;
|
||||
typedef enum { IDLE = -1, MAVLINK = 0, RC = 1, ONBOARD = 2 } vmount_state_t;
|
||||
static vmount_state_t vmount_state = IDLE;
|
||||
static volatile bool thread_do_report_status = false;
|
||||
|
||||
/* polling */
|
||||
px4_pollfd_struct_t polls[7];
|
||||
|
||||
/* functions */
|
||||
static void usage(void);
|
||||
static void vmount_update_topics(void);
|
||||
static void update_params(void);
|
||||
static bool get_params(void);
|
||||
static float get_aux_value(int);
|
||||
static void ack_mount_command(uint16_t command);
|
||||
static int vmount_thread_main(int argc, char *argv[]);
|
||||
extern "C" __EXPORT int vmount_main(int argc, char *argv[]);
|
||||
|
||||
/* uORB subscriptions */
|
||||
static int vehicle_roi_sub = -1;
|
||||
static int vehicle_global_position_sub = -1;
|
||||
static int vehicle_command_sub = -1;
|
||||
static int vehicle_attitude_sub = -1;
|
||||
static int position_setpoint_triplet_sub = -1;
|
||||
static int manual_control_setpoint_sub = -1;
|
||||
static int parameter_update_sub = -1;
|
||||
|
||||
/* uORB publication */
|
||||
static orb_advert_t vehicle_command_ack_pub;
|
||||
|
||||
static struct vehicle_roi_s vehicle_roi;
|
||||
static bool vehicle_roi_updated;
|
||||
|
||||
static struct vehicle_global_position_s vehicle_global_position;
|
||||
static bool vehicle_global_position_updated;
|
||||
|
||||
static struct vehicle_command_s vehicle_command;
|
||||
static bool vehicle_command_updated;
|
||||
|
||||
static struct vehicle_attitude_s vehicle_attitude;
|
||||
static bool vehicle_attitude_updated;
|
||||
|
||||
static struct position_setpoint_triplet_s position_setpoint_triplet;
|
||||
static bool position_setpoint_triplet_updated;
|
||||
|
||||
static struct manual_control_setpoint_s manual_control_setpoint;
|
||||
static bool manual_control_setpoint_updated;
|
||||
|
||||
static struct parameter_update_s parameter_update;
|
||||
static bool parameter_update_updated;
|
||||
|
||||
static struct vehicle_command_ack_s vehicle_command_ack;
|
||||
|
||||
static struct {
|
||||
int mnt_mode;
|
||||
struct Parameters {
|
||||
int mnt_mode_in;
|
||||
int mnt_mode_out;
|
||||
int mnt_mav_sysid;
|
||||
int mnt_mav_compid;
|
||||
int mnt_ob_lock_mode;
|
||||
@ -133,11 +78,25 @@ static struct {
|
||||
int mnt_man_pitch;
|
||||
int mnt_man_roll;
|
||||
int mnt_man_yaw;
|
||||
int mnt_mode_ovr;
|
||||
} params;
|
||||
|
||||
static struct {
|
||||
param_t mnt_mode;
|
||||
bool operator!=(const Parameters &p)
|
||||
{
|
||||
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 ||
|
||||
mnt_ob_lock_mode != p.mnt_ob_lock_mode ||
|
||||
mnt_ob_norm_mode != p.mnt_ob_norm_mode ||
|
||||
mnt_man_control != p.mnt_man_control ||
|
||||
mnt_man_pitch != p.mnt_man_pitch ||
|
||||
mnt_man_roll != p.mnt_man_roll ||
|
||||
mnt_man_yaw != p.mnt_man_yaw;
|
||||
}
|
||||
};
|
||||
|
||||
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;
|
||||
@ -146,314 +105,193 @@ static struct {
|
||||
param_t mnt_man_pitch;
|
||||
param_t mnt_man_roll;
|
||||
param_t mnt_man_yaw;
|
||||
param_t mnt_mode_ovr;
|
||||
} params_handels;
|
||||
};
|
||||
|
||||
static bool manual_control_desired;
|
||||
|
||||
/**
|
||||
* Print command usage information
|
||||
*/
|
||||
/* functions */
|
||||
static void usage(void);
|
||||
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()
|
||||
{
|
||||
fprintf(stderr,
|
||||
"usage: vmount start\n"
|
||||
" vmount stop\n"
|
||||
" vmount status\n");
|
||||
exit(1);
|
||||
PX4_INFO("usage: vmount {start|stop|status}");
|
||||
}
|
||||
|
||||
/**
|
||||
* The daemon thread.
|
||||
*/
|
||||
static int vmount_thread_main(int argc, char *argv[])
|
||||
{
|
||||
if (!get_params()) {
|
||||
warnx("could not get mount parameters!");
|
||||
ParameterHandles param_handles;
|
||||
Parameters params;
|
||||
memset(¶ms, 0, sizeof(params));
|
||||
|
||||
if (!get_params(param_handles, params)) {
|
||||
PX4_ERR("could not get mount parameters!");
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (params.mnt_mode == 0) { vmount_state = IDLE;}
|
||||
|
||||
else if (params.mnt_mode == 1) { vmount_state = MAVLINK;}
|
||||
|
||||
else if (params.mnt_mode == 2) { vmount_state = RC;}
|
||||
|
||||
else if (params.mnt_mode == 3) { vmount_state = ONBOARD;}
|
||||
|
||||
//TODO is this needed?
|
||||
memset(&vehicle_roi, 0, sizeof(vehicle_roi));
|
||||
memset(&vehicle_global_position, 0, sizeof(vehicle_global_position));
|
||||
memset(&vehicle_command, 0, sizeof(vehicle_command));
|
||||
memset(&vehicle_attitude, 0, sizeof(vehicle_attitude));
|
||||
memset(&position_setpoint_triplet, 0, sizeof(position_setpoint_triplet));
|
||||
memset(&manual_control_setpoint, 0, sizeof(manual_control_setpoint));
|
||||
memset(¶meter_update, 0, sizeof(parameter_update));
|
||||
memset(&vehicle_command_ack, 0, sizeof(vehicle_command_ack));
|
||||
|
||||
|
||||
vehicle_roi_sub = orb_subscribe(ORB_ID(vehicle_roi));
|
||||
vehicle_global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
|
||||
vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command));
|
||||
vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
|
||||
position_setpoint_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
|
||||
manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
|
||||
parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
|
||||
|
||||
vehicle_command_ack_pub = orb_advertise(ORB_ID(vehicle_command_ack), &vehicle_command_ack);
|
||||
|
||||
if (!vehicle_roi_sub || !position_setpoint_triplet_sub ||
|
||||
!manual_control_setpoint_sub || !vehicle_global_position_sub ||
|
||||
!vehicle_command_sub || !parameter_update_sub || !vehicle_command_ack_pub ||
|
||||
!vehicle_attitude_sub) {
|
||||
err(1, "could not subscribe to uORB topics");
|
||||
}
|
||||
|
||||
polls[0].fd = vehicle_roi_sub;
|
||||
polls[0].events = POLLIN;
|
||||
polls[1].fd = vehicle_global_position_sub;
|
||||
polls[1].events = POLLIN;
|
||||
polls[2].fd = vehicle_attitude_sub;
|
||||
polls[2].events = POLLIN;
|
||||
polls[3].fd = vehicle_command_sub;
|
||||
polls[3].events = POLLIN;
|
||||
polls[4].fd = position_setpoint_triplet_sub;
|
||||
polls[4].events = POLLIN;
|
||||
polls[5].fd = manual_control_setpoint_sub;
|
||||
polls[5].events = POLLIN;
|
||||
polls[6].fd = parameter_update_sub;
|
||||
polls[6].events = POLLIN;
|
||||
|
||||
int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
|
||||
thread_running = true;
|
||||
OutputConfig output_config;
|
||||
ControlData *control_data = nullptr;
|
||||
InputBase *input_obj = nullptr;
|
||||
OutputBase *output_obj = nullptr;
|
||||
InputRC *manual_input = nullptr;
|
||||
|
||||
run: {
|
||||
if (vmount_state == MAVLINK) {
|
||||
if (!vmount_mavlink_init()) {
|
||||
err(1, "could not initiate vmount_mavlink");
|
||||
}
|
||||
while (!thread_should_exit) {
|
||||
|
||||
warnx("running mount driver in mavlink mode");
|
||||
if (!input_obj && params.mnt_mode_in != 0) { //need to initialize
|
||||
|
||||
if (params.mnt_man_control) manual_control_desired = true;
|
||||
|
||||
while (!thread_should_exit && !thread_should_restart) {
|
||||
vmount_update_topics();
|
||||
|
||||
if (vehicle_roi_updated) {
|
||||
vehicle_roi_updated = false;
|
||||
vmount_mavlink_configure(vehicle_roi.mode, (params.mnt_man_control == 1), params.mnt_mav_sysid, params.mnt_mav_compid);
|
||||
|
||||
if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_NONE) {
|
||||
if (params.mnt_man_control) manual_control_desired = true;
|
||||
|
||||
} else if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_WPNEXT) {
|
||||
manual_control_desired = false;
|
||||
vmount_mavlink_set_location(
|
||||
position_setpoint_triplet.next.lat,
|
||||
position_setpoint_triplet.next.lon,
|
||||
position_setpoint_triplet.next.alt
|
||||
);
|
||||
|
||||
} else if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_WPINDEX) {
|
||||
manual_control_desired = false;
|
||||
//TODO how to do this?
|
||||
|
||||
} else if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_LOCATION) {
|
||||
manual_control_desired = false;
|
||||
vmount_mavlink_set_location(
|
||||
vehicle_roi.lat,
|
||||
vehicle_roi.lon,
|
||||
vehicle_roi.alt
|
||||
);
|
||||
|
||||
} else if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_TARGET) {
|
||||
manual_control_desired = false;
|
||||
//TODO is this even suported?
|
||||
}
|
||||
}
|
||||
|
||||
else if (manual_control_desired && manual_control_setpoint_updated)
|
||||
{
|
||||
manual_control_setpoint_updated = false;
|
||||
vmount_mavlink_point_manual(get_aux_value(params.mnt_man_pitch), get_aux_value(params.mnt_man_roll), get_aux_value(params.mnt_man_yaw));
|
||||
}
|
||||
|
||||
else if (!manual_control_desired && vehicle_global_position_updated)
|
||||
{
|
||||
vehicle_global_position_updated = false;
|
||||
vmount_mavlink_point(vehicle_global_position.lat, vehicle_global_position.lon, vehicle_global_position.alt);
|
||||
}
|
||||
|
||||
vmount_mavlink_deinit();
|
||||
}
|
||||
|
||||
} else if (vmount_state == RC) {
|
||||
if (!vmount_rc_init()) {
|
||||
err(1, "could not initiate vmount_rc");
|
||||
}
|
||||
|
||||
warnx("running mount driver in rc mode");
|
||||
output_config.gimbal_normal_mode_value = params.mnt_ob_norm_mode;
|
||||
output_config.gimbal_retracted_mode_value = params.mnt_ob_lock_mode;
|
||||
output_config.mavlink_sys_id = params.mnt_mav_sysid;
|
||||
output_config.mavlink_comp_id = params.mnt_mav_compid;
|
||||
|
||||
if (params.mnt_man_control) {
|
||||
manual_control_desired = true;
|
||||
}
|
||||
manual_input = new InputRC(params.mnt_man_roll, params.mnt_man_pitch, params.mnt_man_yaw);
|
||||
|
||||
while (!thread_should_exit && !thread_should_restart) {
|
||||
vmount_update_topics();
|
||||
|
||||
if (vehicle_roi_updated) {
|
||||
vehicle_roi_updated = false;
|
||||
vmount_rc_configure(vehicle_roi.mode, (params.mnt_man_control == 1), params.mnt_ob_norm_mode, params.mnt_ob_lock_mode);
|
||||
|
||||
if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_NONE) {
|
||||
if (params.mnt_man_control) manual_control_desired = true;
|
||||
|
||||
} else if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_WPNEXT) {
|
||||
manual_control_desired = false;
|
||||
vmount_rc_set_location(
|
||||
position_setpoint_triplet.next.lat,
|
||||
position_setpoint_triplet.next.lon,
|
||||
position_setpoint_triplet.next.alt
|
||||
);
|
||||
|
||||
} else if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_WPINDEX) {
|
||||
manual_control_desired = false;
|
||||
//TODO how to do this?
|
||||
} else if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_LOCATION) {
|
||||
manual_control_desired = false;
|
||||
vmount_rc_set_location(
|
||||
vehicle_roi.lat,
|
||||
vehicle_roi.lon,
|
||||
vehicle_roi.alt
|
||||
);
|
||||
|
||||
} else if (vehicle_roi.mode == vehicle_roi_s::VEHICLE_ROI_TARGET) {
|
||||
manual_control_desired = false;
|
||||
//TODO is this even suported?
|
||||
}
|
||||
}
|
||||
|
||||
else if (manual_control_desired && manual_control_setpoint_updated) {
|
||||
manual_control_setpoint_updated = false;
|
||||
vmount_rc_point_manual(get_aux_value(params.mnt_man_pitch), get_aux_value(params.mnt_man_roll), get_aux_value(params.mnt_man_yaw));
|
||||
}
|
||||
|
||||
else if (!manual_control_desired && vehicle_global_position_updated)
|
||||
{
|
||||
vehicle_global_position_updated = false;
|
||||
vmount_rc_point(vehicle_global_position.lat, vehicle_global_position.lon, vehicle_global_position.alt);
|
||||
if (!manual_input) {
|
||||
PX4_ERR("memory allocation failed");
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
vmount_rc_deinit();
|
||||
switch (params.mnt_mode_in) {
|
||||
case 1: //RC
|
||||
if (manual_input) {
|
||||
input_obj = manual_input;
|
||||
manual_input = nullptr;
|
||||
|
||||
} else if (vmount_state == ONBOARD) {
|
||||
if (!vmount_onboard_init()) {
|
||||
err(1, "could not initiate vmount_onboard");
|
||||
} else {
|
||||
input_obj = new InputRC(params.mnt_man_roll, params.mnt_man_pitch, params.mnt_man_yaw);
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case 2: //MAVLINK_ROI
|
||||
input_obj = new InputMavlinkROI(manual_input);
|
||||
break;
|
||||
|
||||
case 3: //MAVLINK_DO_MOUNT
|
||||
input_obj = new InputMavlinkCmdMount(manual_input);
|
||||
break;
|
||||
|
||||
default:
|
||||
PX4_ERR("invalid input mode %i", params.mnt_mode_in);
|
||||
break;
|
||||
}
|
||||
|
||||
warnx("running mount driver in onboard mode");
|
||||
switch (params.mnt_mode_out) {
|
||||
case 0: //AUX
|
||||
output_obj = new OutputRC(output_config);
|
||||
break;
|
||||
|
||||
if (params.mnt_man_control) manual_control_desired = true;
|
||||
case 1: //MAVLINK
|
||||
output_obj = new OutputMavlink(output_config);
|
||||
break;
|
||||
|
||||
while (!thread_should_exit && !thread_should_restart) {
|
||||
vmount_update_topics();
|
||||
default:
|
||||
PX4_ERR("invalid output mode %i", params.mnt_mode_out);
|
||||
break;
|
||||
}
|
||||
|
||||
if(vehicle_attitude_updated)
|
||||
{
|
||||
vmount_onboard_update_attitude(vehicle_attitude);
|
||||
vehicle_attitude_updated = false;
|
||||
if (!input_obj || !output_obj) {
|
||||
PX4_ERR("memory allocation failed");
|
||||
break;
|
||||
}
|
||||
|
||||
int ret = output_obj->initialize();
|
||||
|
||||
if (ret) {
|
||||
PX4_ERR("failed to initialize output mode (%i)", ret);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (params.mnt_mode_in != 0) {
|
||||
|
||||
//get input: we cannot make the timeout too large, because the output needs to update
|
||||
//periodically for stabilization and angle updates.
|
||||
int ret = input_obj->update(50, &control_data);
|
||||
|
||||
if (ret) {
|
||||
PX4_ERR("failed to read input (%i)", ret);
|
||||
break;
|
||||
}
|
||||
|
||||
//update output
|
||||
ret = output_obj->update(control_data);
|
||||
|
||||
if (ret) {
|
||||
PX4_ERR("failed to write output (%i)", ret);
|
||||
break;
|
||||
}
|
||||
|
||||
} else {
|
||||
//wait for parameter changes. We still need to wake up regularily to check for thread exit requests
|
||||
usleep(1e6);
|
||||
}
|
||||
|
||||
//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
|
||||
if (input_obj) {
|
||||
delete(input_obj);
|
||||
input_obj = nullptr;
|
||||
}
|
||||
|
||||
if (params.mnt_mode_ovr && manual_control_setpoint_updated) {
|
||||
manual_control_setpoint_updated = false;
|
||||
|
||||
float ovr_value = get_aux_value(params.mnt_mode_ovr);
|
||||
|
||||
if(ovr_value < 0.0f)
|
||||
{
|
||||
vmount_onboard_set_mode(vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT);
|
||||
}
|
||||
else if (ovr_value > 0.0f)
|
||||
{
|
||||
vmount_onboard_set_mode(vehicle_command_s::VEHICLE_MOUNT_MODE_RC_TARGETING);
|
||||
}
|
||||
|
||||
if (output_obj) {
|
||||
delete(output_obj);
|
||||
output_obj = nullptr;
|
||||
}
|
||||
|
||||
else if (vehicle_command_updated) {
|
||||
vehicle_command_updated = false;
|
||||
|
||||
if(vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL)
|
||||
{
|
||||
switch ((int)vehicle_command.param7) {
|
||||
case vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT:
|
||||
case vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL:
|
||||
manual_control_desired = false;
|
||||
break;
|
||||
|
||||
case vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING:
|
||||
manual_control_desired = false;
|
||||
vmount_onboard_set_mode(vehicle_command.param7);
|
||||
vmount_onboard_set_manual(vehicle_command.param1,
|
||||
vehicle_command.param2, vehicle_command.param3);
|
||||
|
||||
case vehicle_command_s::VEHICLE_MOUNT_MODE_RC_TARGETING:
|
||||
if (params.mnt_man_control) {
|
||||
manual_control_desired = true;
|
||||
vmount_onboard_set_mode(vehicle_command.param7);
|
||||
}
|
||||
|
||||
case vehicle_command_s::VEHICLE_MOUNT_MODE_GPS_POINT:
|
||||
manual_control_desired = false;
|
||||
vmount_onboard_set_mode(vehicle_command.param7);
|
||||
vmount_onboard_set_location(
|
||||
vehicle_command.param1,
|
||||
vehicle_command.param2,
|
||||
vehicle_command.param3);
|
||||
|
||||
default:
|
||||
manual_control_desired = false;
|
||||
break;
|
||||
}
|
||||
|
||||
ack_mount_command(vehicle_command.command);
|
||||
}
|
||||
|
||||
else if(vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE)
|
||||
{
|
||||
vmount_onboard_configure(vehicle_command.param1,
|
||||
((uint8_t) vehicle_command.param2 == 1), ((uint8_t) vehicle_command.param3 == 1), ((uint8_t) vehicle_command.param4 == 1));
|
||||
|
||||
ack_mount_command(vehicle_command.command);
|
||||
}
|
||||
}
|
||||
|
||||
else if (manual_control_desired && manual_control_setpoint_updated)
|
||||
{
|
||||
manual_control_setpoint_updated = false;
|
||||
vmount_onboard_point_manual(get_aux_value(params.mnt_man_pitch), get_aux_value(params.mnt_man_roll), get_aux_value(params.mnt_man_yaw));
|
||||
}
|
||||
else if (!manual_control_desired && vehicle_global_position_updated)
|
||||
{
|
||||
vehicle_global_position_updated = false;
|
||||
vmount_onboard_point(vehicle_global_position.lat, vehicle_global_position.lon, vehicle_global_position.alt);
|
||||
if (manual_input) {
|
||||
delete(manual_input);
|
||||
manual_input = nullptr;
|
||||
}
|
||||
}
|
||||
} else if (vmount_state == IDLE)
|
||||
{
|
||||
warnx("running mount driver in idle mode");
|
||||
}
|
||||
|
||||
while (!thread_should_exit && !thread_should_restart) {
|
||||
vmount_update_topics();
|
||||
if (thread_do_report_status) {
|
||||
if (input_obj) {
|
||||
input_obj->print_status();
|
||||
|
||||
} else {
|
||||
PX4_INFO("Input: None");
|
||||
}
|
||||
|
||||
if (output_obj) {
|
||||
output_obj->print_status();
|
||||
|
||||
} else {
|
||||
PX4_INFO("Output: None");
|
||||
}
|
||||
|
||||
thread_do_report_status = false;
|
||||
}
|
||||
}
|
||||
|
||||
if (thread_should_restart)
|
||||
{
|
||||
thread_should_restart = false;
|
||||
warnx("parameter update, restarting mount driver!");
|
||||
goto run;
|
||||
orb_unsubscribe(parameter_update_sub);
|
||||
|
||||
if (input_obj) {
|
||||
delete(input_obj);
|
||||
}
|
||||
|
||||
if (output_obj) {
|
||||
delete(output_obj);
|
||||
}
|
||||
|
||||
if (manual_input) {
|
||||
delete(manual_input);
|
||||
}
|
||||
|
||||
thread_running = false;
|
||||
@ -466,227 +304,116 @@ static int vmount_thread_main(int argc, char *argv[])
|
||||
*/
|
||||
int vmount_main(int argc, char *argv[])
|
||||
{
|
||||
if (argc < 1) {
|
||||
warnx("missing command");
|
||||
if (argc < 2) {
|
||||
PX4_ERR("missing command");
|
||||
usage();
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "start")) {
|
||||
|
||||
/* this is not an error */
|
||||
if (thread_running) {
|
||||
errx(0, "mount driver already running");
|
||||
PX4_WARN("mount driver already running");
|
||||
return 0;
|
||||
}
|
||||
|
||||
thread_should_exit = false;
|
||||
vmount_task = px4_task_spawn_cmd("vmount",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_DEFAULT + 40, //TODO we might want a higher priority?
|
||||
1100,
|
||||
vmount_thread_main,
|
||||
(char *const *)argv);
|
||||
int vmount_task = px4_task_spawn_cmd("vmount",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_DEFAULT + 40,
|
||||
1200,
|
||||
vmount_thread_main,
|
||||
(char *const *)argv);
|
||||
|
||||
while (!thread_running) {
|
||||
while (!thread_running && vmount_task >= 0) {
|
||||
usleep(200);
|
||||
}
|
||||
|
||||
exit(0);
|
||||
return 0;
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "stop")) {
|
||||
|
||||
/* this is not an error */
|
||||
if (!thread_running) {
|
||||
errx(0, "mount driver already stopped");
|
||||
PX4_WARN("mount driver not running");
|
||||
return 0;
|
||||
}
|
||||
|
||||
thread_should_exit = true;
|
||||
|
||||
while (thread_running) {
|
||||
usleep(1000000);
|
||||
warnx(".");
|
||||
usleep(100000);
|
||||
}
|
||||
|
||||
warnx("terminated.");
|
||||
exit(0);
|
||||
return 0;
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "status")) {
|
||||
if (thread_running) {
|
||||
switch (vmount_state) {
|
||||
case IDLE:
|
||||
errx(0, "running: IDLE");
|
||||
break;
|
||||
|
||||
case MAVLINK:
|
||||
errx(0, "running: MAVLINK - Manual Control? %d", manual_control_desired);
|
||||
break;
|
||||
|
||||
case RC:
|
||||
errx(0, "running: RC - Manual Control? %d", manual_control_desired);
|
||||
break;
|
||||
|
||||
case ONBOARD:
|
||||
errx(0, "running: ONBOARD");
|
||||
break;
|
||||
thread_do_report_status = true;
|
||||
|
||||
while (thread_do_report_status) {
|
||||
usleep(10000);
|
||||
}
|
||||
|
||||
} else {
|
||||
errx(1, "not running");
|
||||
PX4_INFO("not running");
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
warnx("unrecognized command");
|
||||
PX4_ERR("unrecognized command");
|
||||
usage();
|
||||
/* not getting here */
|
||||
return 0;
|
||||
return -1;
|
||||
}
|
||||
|
||||
/* Update oURB topics */
|
||||
void vmount_update_topics()
|
||||
void update_params(ParameterHandles ¶m_handles, Parameters ¶ms, bool &got_changes)
|
||||
{
|
||||
/*
|
||||
polls = {
|
||||
{ .fd = vehicle_roi_sub, .events = POLLIN },
|
||||
{ .fd = vehicle_global_position_sub, .events = POLLIN },
|
||||
{ .fd = vehicle_attitude_sub, .events = POLLIN },
|
||||
{ .fd = vehicle_command_sub, .events = POLLIN },
|
||||
{ .fd = position_setpoint_triplet_sub, .events = POLLIN },
|
||||
{ .fd = manual_control_setpoint_sub, .events = POLLIN },
|
||||
{ .fd = parameter_update_sub, .events = POLLIN },
|
||||
};
|
||||
*/
|
||||
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_control, ¶ms.mnt_man_control);
|
||||
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);
|
||||
|
||||
/* wait for sensor update of 7 file descriptors for 100 ms */
|
||||
int poll_ret = px4_poll(polls, 7, 100);
|
||||
|
||||
//Nothing updated.
|
||||
if(poll_ret == 0) return;
|
||||
|
||||
if (polls[0].revents & POLLIN) {
|
||||
orb_copy(ORB_ID(vehicle_roi), vehicle_roi_sub, &vehicle_roi);
|
||||
vehicle_roi_updated = true;
|
||||
}
|
||||
|
||||
if (polls[1].revents & POLLIN) {
|
||||
orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &vehicle_global_position);
|
||||
vehicle_global_position_updated = true;
|
||||
}
|
||||
|
||||
if (polls[2].revents & POLLIN) {
|
||||
orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &vehicle_attitude);
|
||||
vehicle_attitude_updated = true;
|
||||
}
|
||||
|
||||
if (polls[3].revents & POLLIN) {
|
||||
orb_copy(ORB_ID(vehicle_command), vehicle_command_sub, &vehicle_command);
|
||||
vehicle_command_updated = true;
|
||||
}
|
||||
|
||||
if (polls[4].revents & POLLIN) {
|
||||
orb_copy(ORB_ID(position_setpoint_triplet), position_setpoint_triplet_sub, &position_setpoint_triplet);
|
||||
position_setpoint_triplet_updated = true;
|
||||
}
|
||||
|
||||
if (polls[5].revents & POLLIN) {
|
||||
orb_copy(ORB_ID(manual_control_setpoint), manual_control_setpoint_sub, &manual_control_setpoint);
|
||||
manual_control_setpoint_updated = true;
|
||||
}
|
||||
|
||||
if (polls[6].revents & POLLIN) {
|
||||
orb_copy(ORB_ID(parameter_update), parameter_update_sub, ¶meter_update);
|
||||
parameter_update_updated = true;
|
||||
update_params();
|
||||
}
|
||||
got_changes = prev_params != params;
|
||||
}
|
||||
|
||||
void update_params()
|
||||
bool get_params(ParameterHandles ¶m_handles, Parameters ¶ms)
|
||||
{
|
||||
param_get(params_handels.mnt_mode, ¶ms.mnt_mode);
|
||||
param_get(params_handels.mnt_mav_sysid, ¶ms.mnt_mav_sysid);
|
||||
param_get(params_handels.mnt_mav_compid, ¶ms.mnt_mav_compid);
|
||||
param_get(params_handels.mnt_ob_lock_mode, ¶ms.mnt_ob_lock_mode);
|
||||
param_get(params_handels.mnt_ob_norm_mode, ¶ms.mnt_ob_norm_mode);
|
||||
param_get(params_handels.mnt_man_control, ¶ms.mnt_man_control);
|
||||
param_get(params_handels.mnt_man_pitch, ¶ms.mnt_man_pitch);
|
||||
param_get(params_handels.mnt_man_roll, ¶ms.mnt_man_roll);
|
||||
param_get(params_handels.mnt_man_yaw, ¶ms.mnt_man_yaw);
|
||||
param_get(params_handels.mnt_mode_ovr, ¶ms.mnt_mode_ovr);
|
||||
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_control = param_find("MNT_MAN_CONTROL");
|
||||
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");
|
||||
|
||||
if (vmount_state != params.mnt_mode)
|
||||
{
|
||||
thread_should_restart = true;
|
||||
}
|
||||
else if (vmount_state == MAVLINK)
|
||||
{
|
||||
vmount_mavlink_configure(vehicle_roi.mode, (params.mnt_man_control == 1), params.mnt_mav_sysid, params.mnt_mav_compid);
|
||||
}
|
||||
else if (vmount_state == RC)
|
||||
{
|
||||
vmount_rc_configure(vehicle_roi.mode, (params.mnt_man_control == 1), params.mnt_ob_norm_mode, params.mnt_ob_lock_mode);
|
||||
}
|
||||
else if(vmount_state == ONBOARD)
|
||||
{
|
||||
//None of the parameter changes require a reconfiguration of the onboard mount.
|
||||
}
|
||||
}
|
||||
|
||||
bool get_params()
|
||||
{
|
||||
params_handels.mnt_mode = param_find("MNT_MODE");
|
||||
params_handels.mnt_mav_sysid = param_find("MNT_MAV_SYSID");
|
||||
params_handels.mnt_mav_compid = param_find("MNT_MAV_COMPID");
|
||||
params_handels.mnt_ob_lock_mode = param_find("MNT_OB_LOCK_MODE");
|
||||
params_handels.mnt_ob_norm_mode = param_find("MNT_OB_NORM_MODE");
|
||||
params_handels.mnt_man_control = param_find("MNT_MAN_CONTROL");
|
||||
params_handels.mnt_man_pitch = param_find("MNT_MAN_PITCH");
|
||||
params_handels.mnt_man_roll = param_find("MNT_MAN_ROLL");
|
||||
params_handels.mnt_man_yaw = param_find("MNT_MAN_YAW");
|
||||
params_handels.mnt_mode_ovr = param_find("MNT_MODE_OVR");
|
||||
|
||||
if (param_get(params_handels.mnt_mode, ¶ms.mnt_mode) ||
|
||||
param_get(params_handels.mnt_mav_sysid, ¶ms.mnt_mav_sysid) ||
|
||||
param_get(params_handels.mnt_mav_compid, ¶ms.mnt_mav_compid) ||
|
||||
param_get(params_handels.mnt_ob_lock_mode, ¶ms.mnt_ob_lock_mode) ||
|
||||
param_get(params_handels.mnt_ob_norm_mode, ¶ms.mnt_ob_norm_mode) ||
|
||||
param_get(params_handels.mnt_man_control, ¶ms.mnt_man_control) ||
|
||||
param_get(params_handels.mnt_man_pitch, ¶ms.mnt_man_pitch) ||
|
||||
param_get(params_handels.mnt_man_roll, ¶ms.mnt_man_roll) ||
|
||||
param_get(params_handels.mnt_man_yaw, ¶ms.mnt_man_yaw) ||
|
||||
param_get(params_handels.mnt_mode_ovr, ¶ms.mnt_mode_ovr)) {
|
||||
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_control == PARAM_INVALID ||
|
||||
param_handles.mnt_man_pitch == PARAM_INVALID ||
|
||||
param_handles.mnt_man_roll == PARAM_INVALID ||
|
||||
param_handles.mnt_man_yaw == PARAM_INVALID) {
|
||||
return false;
|
||||
}
|
||||
|
||||
bool dummy;
|
||||
update_params(param_handles, params, dummy);
|
||||
return true;
|
||||
|
||||
}
|
||||
|
||||
void ack_mount_command(uint16_t command)
|
||||
{
|
||||
vehicle_command_ack.command = command;
|
||||
vehicle_command_ack.result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
|
||||
|
||||
orb_publish(ORB_ID(vehicle_command_ack), vehicle_command_ack_pub, &vehicle_command_ack);
|
||||
}
|
||||
|
||||
float get_aux_value(int aux)
|
||||
{
|
||||
switch (aux)
|
||||
{
|
||||
case 0:
|
||||
return 0.0f;
|
||||
case 1:
|
||||
return manual_control_setpoint.aux1;
|
||||
case 2:
|
||||
return manual_control_setpoint.aux2;
|
||||
case 3:
|
||||
return manual_control_setpoint.aux3;
|
||||
case 4:
|
||||
return manual_control_setpoint.aux4;
|
||||
case 5:
|
||||
return manual_control_setpoint.aux5;
|
||||
default:
|
||||
return 0.0f;
|
||||
}
|
||||
}
|
||||
|
||||
@ -1,178 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-2016 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file vmount_mavlink.cpp
|
||||
* @author Leon Müller (thedevleon)
|
||||
*
|
||||
*/
|
||||
|
||||
#include "vmount_mavlink.h"
|
||||
|
||||
#include <stdlib.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <arch/math.h>
|
||||
#include <geo/geo.h>
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/vehicle_command.h>
|
||||
#include <uORB/topics/vehicle_roi.h>
|
||||
|
||||
#include <px4_posix.h>
|
||||
|
||||
/* uORB advertising */
|
||||
static struct vehicle_command_s *vehicle_command;
|
||||
static orb_advert_t vehicle_command_pub;
|
||||
|
||||
static int sys_id;
|
||||
static int comp_id;
|
||||
static double lat;
|
||||
static double lon;
|
||||
static float alt;
|
||||
|
||||
bool vmount_mavlink_init()
|
||||
{
|
||||
memset(&vehicle_command, 0, sizeof(vehicle_command));
|
||||
vehicle_command_pub = orb_advertise(ORB_ID(vehicle_command), &vehicle_command);
|
||||
|
||||
if (!vehicle_command_pub) { return false; }
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
void vmount_mavlink_deinit()
|
||||
{
|
||||
orb_unadvertise(vehicle_command_pub);
|
||||
free(vehicle_command);
|
||||
}
|
||||
|
||||
/*
|
||||
* MAV_CMD_DO_MOUNT_CONFIGURE (#204)
|
||||
* param1 = mount_mode (1 = MAV_MOUNT_MODE_NEUTRAL recenters camera)
|
||||
*/
|
||||
|
||||
void vmount_mavlink_configure(int roi_mode, bool man_control, int sysid, int compid)
|
||||
{
|
||||
sys_id = sysid;
|
||||
comp_id = compid;
|
||||
|
||||
vehicle_command->command = vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE;
|
||||
vehicle_command->target_system = sys_id;
|
||||
vehicle_command->target_component = comp_id;
|
||||
|
||||
switch (roi_mode) {
|
||||
case vehicle_roi_s::VEHICLE_ROI_NONE:
|
||||
if (man_control) { vehicle_command->param1 = vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING; }
|
||||
|
||||
else { vehicle_command->param1 = vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL; }
|
||||
|
||||
break;
|
||||
|
||||
case vehicle_roi_s::VEHICLE_ROI_WPNEXT:
|
||||
vehicle_command->param1 = vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING;
|
||||
break;
|
||||
|
||||
case vehicle_roi_s::VEHICLE_ROI_WPINDEX:
|
||||
vehicle_command->param1 = vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING;
|
||||
break;
|
||||
|
||||
case vehicle_roi_s::VEHICLE_ROI_LOCATION:
|
||||
vehicle_command->param1 = vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING;
|
||||
break;
|
||||
|
||||
case vehicle_roi_s::VEHICLE_ROI_TARGET:
|
||||
vehicle_command->param1 = vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING;
|
||||
break;
|
||||
|
||||
default:
|
||||
vehicle_command->param1 = vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL;
|
||||
break;
|
||||
}
|
||||
|
||||
orb_publish(ORB_ID(vehicle_command), vehicle_command_pub, vehicle_command);
|
||||
}
|
||||
|
||||
|
||||
/* MAV_CMD_DO_MOUNT_CONTROL (#205)
|
||||
* param1 = pitch, angle in degree or pwm input
|
||||
* param2 = roll, angle in degree or pwm input
|
||||
* param3 = yaw, angle in degree or pwm input
|
||||
* param6 = typeflags (uint16_t)
|
||||
* 0x0001: pitch, 0: unlimited, 1: limited, see CMD_SETANGLE
|
||||
* 0x0002: roll, 0: unlimited, 1: limited, see CMD_SETANGLE
|
||||
* 0x0004: yaw, 0: unlimited, 1: limited, see CMD_SETANGLE
|
||||
* 0x0100: input type, 0: angle input (see CMD_SETANGLE), 1: pwm input (see CMD_SETPITCHROLLYAW)
|
||||
*/
|
||||
|
||||
|
||||
void vmount_mavlink_set_location(double lat_new, double lon_new, float alt_new)
|
||||
{
|
||||
lat = lat_new;
|
||||
lon = lon_new;
|
||||
alt = alt_new;
|
||||
}
|
||||
|
||||
void vmount_mavlink_point(double global_lat, double global_lon, float global_alt)
|
||||
{
|
||||
float pitch = vmount_mavlink_calculate_pitch(global_lat, global_lon, global_alt);
|
||||
float roll = 0.0f; // We want a level horizon, so leave roll at 0 degrees.
|
||||
float yaw = get_bearing_to_next_waypoint(global_lat, global_lon, lat, lon) * (float)M_RAD_TO_DEG;
|
||||
|
||||
vmount_mavlink_point_manual(pitch, roll, yaw);
|
||||
}
|
||||
|
||||
|
||||
void vmount_mavlink_point_manual(float pitch_new, float roll_new, float yaw_new)
|
||||
{
|
||||
vehicle_command->command = vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL;
|
||||
vehicle_command->target_system = sys_id;
|
||||
vehicle_command->target_component = comp_id;
|
||||
|
||||
vehicle_command->param1 = pitch_new;
|
||||
vehicle_command->param2 = roll_new;
|
||||
vehicle_command->param3 = yaw_new;
|
||||
|
||||
orb_publish(ORB_ID(vehicle_command), vehicle_command_pub, vehicle_command);
|
||||
}
|
||||
|
||||
float vmount_mavlink_calculate_pitch(double global_lat, double global_lon, float global_alt)
|
||||
{
|
||||
float x = (lon - global_lon) * cos(M_DEG_TO_RAD * ((global_lat + lat) * 0.00000005)) * 0.01113195;
|
||||
float y = (lat - global_lat) * 0.01113195;
|
||||
float z = (alt - global_alt);
|
||||
float target_distance = sqrtf(powf(x, 2) + powf(y, 2));
|
||||
|
||||
return atan2(z, target_distance) * M_RAD_TO_DEG;
|
||||
}
|
||||
@ -1,193 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-2016 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file vmount_onboard.cpp
|
||||
* @author Leon Müller (thedevleon)
|
||||
*
|
||||
*/
|
||||
|
||||
#include "vmount_onboard.h"
|
||||
|
||||
#include <stdlib.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <arch/math.h>
|
||||
#include <geo/geo.h>
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_command.h>
|
||||
|
||||
|
||||
/* uORB topics */
|
||||
static struct actuator_controls_s actuator_controls;
|
||||
static orb_advert_t actuator_controls_pub;
|
||||
|
||||
static struct vehicle_attitude_s vehicle_attitude;
|
||||
|
||||
static int mount_mode;
|
||||
static float pitch;
|
||||
static float roll;
|
||||
static float yaw;
|
||||
static float retracts;
|
||||
static int stab_pitch;
|
||||
static int stab_roll;
|
||||
static int stab_yaw;
|
||||
static double lat;
|
||||
static double lon;
|
||||
static float alt;
|
||||
|
||||
bool vmount_onboard_init()
|
||||
{
|
||||
memset(&actuator_controls, 0, sizeof(actuator_controls));
|
||||
memset(&vehicle_attitude, 0, sizeof(vehicle_attitude));
|
||||
actuator_controls_pub = orb_advertise(ORB_ID(actuator_controls_2), &actuator_controls);
|
||||
|
||||
if (!actuator_controls_pub) { return false; }
|
||||
|
||||
vmount_onboard_configure(vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT, 0.0f, 0.0f, 0.0f);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void vmount_onboard_deinit()
|
||||
{
|
||||
orb_unadvertise(actuator_controls_pub);
|
||||
//free(actuator_controls);
|
||||
//free(vehicle_attitude);
|
||||
}
|
||||
|
||||
void vmount_onboard_configure(int new_mount_mode, bool new_stab_roll, bool new_stab_pitch, bool new_stab_yaw)
|
||||
{
|
||||
mount_mode = new_mount_mode;
|
||||
stab_roll = new_stab_roll;
|
||||
stab_pitch = new_stab_pitch;
|
||||
stab_yaw = new_stab_yaw;
|
||||
|
||||
switch (mount_mode) {
|
||||
case vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT:
|
||||
retracts = -1.0f;
|
||||
vmount_onboard_set_manual(0.0f, 0.0f, 0.0f);
|
||||
break;
|
||||
|
||||
case vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL:
|
||||
retracts = 0.0f;
|
||||
vmount_onboard_set_manual(0.0f, 0.0f, 0.0f);
|
||||
break;
|
||||
|
||||
case vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING:
|
||||
case vehicle_command_s::VEHICLE_MOUNT_MODE_RC_TARGETING:
|
||||
case vehicle_command_s::VEHICLE_MOUNT_MODE_GPS_POINT:
|
||||
retracts = 0.0f;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void vmount_onboard_set_location(double lat_new, double lon_new, float alt_new)
|
||||
{
|
||||
lat = lat_new;
|
||||
lon = lon_new;
|
||||
alt = alt_new;
|
||||
}
|
||||
|
||||
void vmount_onboard_set_manual(double pitch_new, double roll_new, float yaw_new)
|
||||
{
|
||||
pitch = pitch_new;
|
||||
roll = roll_new;
|
||||
yaw = yaw_new;
|
||||
}
|
||||
|
||||
void vmount_onboard_set_mode(int new_mount_mode)
|
||||
{
|
||||
vmount_onboard_configure(new_mount_mode, stab_roll, stab_pitch, stab_yaw);
|
||||
}
|
||||
|
||||
void vmount_onboard_point(double global_lat, double global_lon, float global_alt)
|
||||
{
|
||||
if (mount_mode == vehicle_command_s::VEHICLE_MOUNT_MODE_GPS_POINT) {
|
||||
pitch = vmount_onboard_calculate_pitch(global_lat, global_lon, global_alt);
|
||||
roll = 0.0f; // We want a level horizon, so leave roll at 0 degrees.
|
||||
yaw = get_bearing_to_next_waypoint(global_lat, global_lon, lat, lon) * (float)M_RAD_TO_DEG;
|
||||
}
|
||||
|
||||
vmount_onboard_point_manual(pitch, roll, yaw);
|
||||
}
|
||||
|
||||
void vmount_onboard_point_manual(float pitch_target, float roll_target, float yaw_target)
|
||||
{
|
||||
float pitch_new = pitch_target;
|
||||
float roll_new = roll_target;
|
||||
float yaw_new = yaw_target;
|
||||
|
||||
if (mount_mode != vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT &&
|
||||
mount_mode != vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL) {
|
||||
if (stab_roll) {
|
||||
roll_new += 1.0f / M_PI_F * -vehicle_attitude.roll;
|
||||
}
|
||||
|
||||
if (stab_pitch) {
|
||||
pitch_new += 1.0f / M_PI_F * -vehicle_attitude.pitch;
|
||||
}
|
||||
|
||||
if (stab_yaw) {
|
||||
yaw_new += 1.0f / M_PI_F * vehicle_attitude.yaw;
|
||||
}
|
||||
}
|
||||
|
||||
actuator_controls.timestamp = hrt_absolute_time();
|
||||
actuator_controls.control[0] = pitch_new;
|
||||
actuator_controls.control[1] = roll_new;
|
||||
actuator_controls.control[2] = yaw_new;
|
||||
actuator_controls.control[4] = retracts;
|
||||
|
||||
orb_publish(ORB_ID(actuator_controls_2), actuator_controls_pub, &actuator_controls);
|
||||
}
|
||||
|
||||
void vmount_onboard_update_attitude(vehicle_attitude_s vehicle_attitude_new)
|
||||
{
|
||||
vehicle_attitude = vehicle_attitude_new;
|
||||
}
|
||||
|
||||
float vmount_onboard_calculate_pitch(double global_lat, double global_lon, float global_alt)
|
||||
{
|
||||
float x = (lon - global_lon) * cos(M_DEG_TO_RAD * ((global_lat + lat) * 0.00000005)) * 0.01113195;
|
||||
float y = (lat - global_lat) * 0.01113195;
|
||||
float z = (alt - global_alt);
|
||||
float target_distance = sqrtf(powf(x, 2) + powf(y, 2));
|
||||
|
||||
return atan2(z, target_distance) * M_RAD_TO_DEG;
|
||||
}
|
||||
@ -41,46 +41,42 @@
|
||||
#include <systemlib/param/param.h>
|
||||
|
||||
/**
|
||||
* Mount operation mode
|
||||
* MAVLINK and RC use the ROI (or RC input if enabled and no ROI set) to control a mount.
|
||||
* ONBOARD uses MAV_CMD_DO_MOUNT_CONFIGURE and MAV_CMD_DO_MOUNT_CONTROL MavLink messages
|
||||
* to control a mount.
|
||||
* 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 0 DISABLE
|
||||
* @value 1 MAVLINK
|
||||
* @value 2 RC
|
||||
* @value 3 ONBOARD
|
||||
* @value 1 RC
|
||||
* @value 2 MAVLINK_ROI
|
||||
* @value 3 MAVLINK_DO_MOUNT
|
||||
* @min 0
|
||||
* @max 3
|
||||
* @group Mount
|
||||
*/
|
||||
PARAM_DEFINE_INT32(MNT_MODE, 0);
|
||||
PARAM_DEFINE_INT32(MNT_MODE_IN, 0);
|
||||
|
||||
/**
|
||||
* Auxiliary channel to override current mount mode (only in ONBOARD mode)
|
||||
* if <0.0f then MODE_RETRACT (retracts not retracted)
|
||||
* if =0.0f then don't override
|
||||
* if >0.0f then MODE_RC_TARGETING (retracts retracted)
|
||||
* @value 0 Disable
|
||||
* @value 1 AUX1
|
||||
* @value 2 AUX2
|
||||
* @value 3 AUX3
|
||||
* @value 4 AUX4
|
||||
* @value 5 AUX5
|
||||
* 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 5
|
||||
* @max 1
|
||||
* @group Mount
|
||||
*/
|
||||
PARAM_DEFINE_INT32(MNT_MODE_OVR, 0);
|
||||
PARAM_DEFINE_INT32(MNT_MODE_OUT, 0);
|
||||
|
||||
/**
|
||||
* Mavlink System ID
|
||||
* Mavlink System ID (if MNT_MODE_OUT is MAVLINK)
|
||||
*
|
||||
* @group Mount
|
||||
*/
|
||||
PARAM_DEFINE_INT32(MNT_MAV_SYSID, 71);
|
||||
|
||||
/**
|
||||
* Mavlink Component ID
|
||||
* Mavlink Component ID (if MNT_MODE_OUT is MAVLINK)
|
||||
*
|
||||
* @group Mount
|
||||
*/
|
||||
@ -88,7 +84,7 @@ PARAM_DEFINE_INT32(MNT_MAV_COMPID, 67);
|
||||
|
||||
/**
|
||||
* Mixer value for selecting normal mode
|
||||
* if required by the gimbal (only in RC mode)
|
||||
* if required by the gimbal (only in AUX output mode)
|
||||
* @min -1.0
|
||||
* @max 1.0
|
||||
* @decimal 3
|
||||
@ -98,7 +94,7 @@ PARAM_DEFINE_FLOAT(MNT_OB_NORM_MODE, -1.0f);
|
||||
|
||||
/**
|
||||
* Mixer value for selecting a locking mode
|
||||
* if required for the gimbal (only in RC mode)
|
||||
* if required for the gimbal (only in AUX output mode)
|
||||
* @min -1.0
|
||||
* @max 1.0
|
||||
* @decimal 3
|
||||
@ -108,7 +104,7 @@ PARAM_DEFINE_FLOAT(MNT_OB_LOCK_MODE, 0.0f);
|
||||
|
||||
|
||||
/**
|
||||
* This enables the mount to be controlled when no ROI is set.
|
||||
* This enables the mount to be manually controlled when no ROI is set.
|
||||
*
|
||||
* If set to 1, the mount will be controlled by the AUX channels below
|
||||
* when no ROI is set.
|
||||
@ -119,7 +115,7 @@ PARAM_DEFINE_FLOAT(MNT_OB_LOCK_MODE, 0.0f);
|
||||
PARAM_DEFINE_INT32(MNT_MAN_CONTROL, 0);
|
||||
|
||||
/**
|
||||
* Auxiliary channel to control roll.
|
||||
* Auxiliary channel to control roll (in AUX input or manual mode).
|
||||
*
|
||||
* @value 0 Disable
|
||||
* @value 1 AUX1
|
||||
@ -134,7 +130,7 @@ PARAM_DEFINE_INT32(MNT_MAN_CONTROL, 0);
|
||||
PARAM_DEFINE_INT32(MNT_MAN_ROLL, 0);
|
||||
|
||||
/**
|
||||
* Auxiliary channel to control pitch.
|
||||
* Auxiliary channel to control pitch (in AUX input or manual mode).
|
||||
*
|
||||
* @value 0 Disable
|
||||
* @value 1 AUX1
|
||||
@ -149,7 +145,7 @@ PARAM_DEFINE_INT32(MNT_MAN_ROLL, 0);
|
||||
PARAM_DEFINE_INT32(MNT_MAN_PITCH, 0);
|
||||
|
||||
/**
|
||||
* Auxiliary channel to control yaw.
|
||||
* Auxiliary channel to control yaw (in AUX input or manual mode).
|
||||
*
|
||||
* @value 0 Disable
|
||||
* @value 1 AUX1
|
||||
@ -162,3 +158,4 @@ PARAM_DEFINE_INT32(MNT_MAN_PITCH, 0);
|
||||
* @group Mount
|
||||
*/
|
||||
PARAM_DEFINE_INT32(MNT_MAN_YAW, 0);
|
||||
|
||||
|
||||
@ -1,155 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-2016 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file vmount_rc.c
|
||||
* @author Leon Müller (thedevleon)
|
||||
*
|
||||
*/
|
||||
|
||||
#include "vmount_rc.h"
|
||||
|
||||
#include <stdlib.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <arch/math.h>
|
||||
#include <geo/geo.h>
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/vehicle_roi.h>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
|
||||
|
||||
/* uORB advertising */
|
||||
static struct actuator_controls_s actuator_controls;
|
||||
static orb_advert_t actuator_controls_pub;
|
||||
|
||||
static double lon;
|
||||
static double lat;
|
||||
static float alt;
|
||||
static float normal_mode;
|
||||
static float locked_mode;
|
||||
static bool locked;
|
||||
|
||||
bool vmount_rc_init()
|
||||
{
|
||||
memset(&actuator_controls, 0, sizeof(actuator_controls));
|
||||
actuator_controls_pub = orb_advertise(ORB_ID(actuator_controls_2), &actuator_controls);
|
||||
|
||||
if (!actuator_controls_pub) { return false; }
|
||||
|
||||
locked = false;
|
||||
normal_mode = -1.0f;
|
||||
locked_mode = 0.0f;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void vmount_rc_deinit()
|
||||
{
|
||||
orb_unadvertise(actuator_controls_pub);
|
||||
//free(&actuator_controls);
|
||||
}
|
||||
|
||||
void vmount_rc_configure(int roi_mode, bool man_control, int normal_mode_new, int locked_mode_new)
|
||||
{
|
||||
normal_mode = normal_mode_new;
|
||||
locked_mode = locked_mode_new;
|
||||
|
||||
switch (roi_mode) {
|
||||
case vehicle_roi_s::VEHICLE_ROI_NONE:
|
||||
locked = false;
|
||||
|
||||
if (!man_control) {vmount_rc_point_manual(0.0f, 0.0f, 0.0f);}
|
||||
|
||||
break;
|
||||
|
||||
case vehicle_roi_s::VEHICLE_ROI_WPNEXT:
|
||||
case vehicle_roi_s::VEHICLE_ROI_WPINDEX:
|
||||
case vehicle_roi_s::VEHICLE_ROI_LOCATION:
|
||||
case vehicle_roi_s::VEHICLE_ROI_TARGET:
|
||||
locked = true;
|
||||
break;
|
||||
|
||||
default:
|
||||
locked = false;
|
||||
vmount_rc_point_manual(0.0f, 0.0f, 0.0f);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void vmount_rc_set_location(double lat_new, double lon_new, float alt_new)
|
||||
{
|
||||
lat = lat_new;
|
||||
lon = lon_new;
|
||||
alt = alt_new;
|
||||
}
|
||||
|
||||
void vmount_rc_point(double global_lat, double global_lon, float global_alt)
|
||||
{
|
||||
float pitch = vmount_rc_calculate_pitch(global_lat, global_lon, global_alt);
|
||||
float roll = 0.0f; // We want a level horizon, so leave roll at 0 degrees.
|
||||
float yaw = get_bearing_to_next_waypoint(global_lat, global_lon, lat, lon) * (float)M_RAD_TO_DEG;
|
||||
|
||||
vmount_rc_point_manual(pitch, roll, yaw);
|
||||
}
|
||||
|
||||
void vmount_rc_point_manual(float pitch_new, float roll_new, float yaw_new)
|
||||
{
|
||||
actuator_controls.timestamp = hrt_absolute_time();
|
||||
actuator_controls.control[0] = pitch_new;
|
||||
actuator_controls.control[1] = roll_new;
|
||||
actuator_controls.control[2] = yaw_new;
|
||||
actuator_controls.control[3] = (locked) ? locked_mode : normal_mode;
|
||||
|
||||
/** for debugging purposes
|
||||
warnx("actuator_controls_2 values:\t%8.4f\t%8.4f\t%8.4f\t%8.4f\t%8.4f",
|
||||
(double)actuator_controls.control[0],
|
||||
(double)actuator_controls.control[1],
|
||||
(double)actuator_controls.control[2],
|
||||
(double)actuator_controls.control[3],
|
||||
(double)actuator_controls.control[4]);
|
||||
**/
|
||||
|
||||
orb_publish(ORB_ID(actuator_controls_2), actuator_controls_pub, &actuator_controls);
|
||||
}
|
||||
|
||||
float vmount_rc_calculate_pitch(double global_lat, double global_lon, float global_alt)
|
||||
{
|
||||
float x = (lon - global_lon) * cos(M_DEG_TO_RAD * ((global_lat + lat) * 0.00000005)) * 0.01113195;
|
||||
float y = (lat - global_lat) * 0.01113195;
|
||||
float z = (alt - global_alt);
|
||||
float target_distance = sqrtf(powf(x, 2) + powf(y, 2));
|
||||
|
||||
return atan2(z, target_distance) * M_RAD_TO_DEG;
|
||||
}
|
||||
Loading…
x
Reference in New Issue
Block a user