vmount: refactor for v2 auto input, test command

This is a bigger refactor of vmount to clean it up and plug some holes
in functionality.

The changes include:
- Fixing/simplifying the test command.
- Changing the dependencies of the input and output classes to just the
  parameter list.
- Adding a separate topic to publish gimbal v1 commands to avoid
  polluting the vehicle_command topic.
- Removing outdated comments and author lists.
- Extracting the gimbal v2 "in control" notion outside into control_data
  rather than only the v2 input.
This commit is contained in:
Julian Oes 2022-01-12 10:42:18 +01:00 committed by Daniel Agar
parent f2216dff55
commit 39f0e97245
21 changed files with 1343 additions and 1478 deletions

View File

@ -175,3 +175,5 @@ uint8 source_system # System sending the command
uint8 source_component # Component sending the command
uint8 confirmation # 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command)
bool from_external
# TOPICS vehicle_command gimbal_v1_command

View File

@ -2394,6 +2394,30 @@ Mavlink::task_main(int argc, char *argv[])
}
}
// For legacy gimbals using the mavlink gimbal v1 protocol, we need to send out commands.
// We don't care about acks for these though.
if (_gimbal_v1_command_sub.updated()) {
vehicle_command_s cmd;
_gimbal_v1_command_sub.copy(&cmd);
// FIXME: filter for target system/component
mavlink_command_long_t msg{};
msg.param1 = cmd.param1;
msg.param2 = cmd.param2;
msg.param3 = cmd.param3;
msg.param4 = cmd.param4;
msg.param5 = cmd.param5;
msg.param6 = cmd.param6;
msg.param7 = cmd.param7;
msg.command = cmd.command;
msg.target_system = cmd.target_system;
msg.target_component = cmd.target_component;
msg.confirmation = 0;
mavlink_msg_command_long_send_struct(get_channel(), &msg);
}
/* check for shell output */
if (_mavlink_shell && _mavlink_shell->available() > 0) {
if (get_free_tx_buf() >= MAVLINK_MSG_ID_SERIAL_CONTROL_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) {

View File

@ -544,6 +544,7 @@ private:
uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
uORB::Subscription _vehicle_command_ack_sub{ORB_ID(vehicle_command_ack)};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
uORB::Subscription _gimbal_v1_command_sub{ORB_ID(gimbal_v1_command)};
static bool _boot_complete;

View File

@ -31,11 +31,6 @@
*
****************************************************************************/
/**
* @file common.h
* @author Beat Küng <beat-kueng@gmx.net>
*
*/
#pragma once
@ -52,44 +47,44 @@ namespace vmount
*/
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 */
enum class Type {
Neutral = 0,
Angle,
LonLat
};
Type type = Type::Neutral;
union TypeData {
struct TypeAngle {
float q[4]; /**< attitude quaternion */
float angular_velocity[3]; // angular velocity
float q[4];
float angular_velocity[3];
// according to DO_MOUNT_CONFIGURE
enum class Frame : uint8_t {
AngleBodyFrame = 0, /**< Follow mode, angle relative to vehicle (usually default for yaw axis). */
AngularRate = 1, /**< Angular rate set only, for compatibility with MAVLink v1 protocol. */
AngleAbsoluteFrame = 2/**< Lock mode, angle relative to horizon/world, lock mode. (usually default for roll and pitch). */
} frames[3]; /**< Mode. */
AngleBodyFrame = 0, // Also called follow mode, angle relative to vehicle forward (usually default for yaw axis).
AngularRate = 1, // Angular rate set only.
AngleAbsoluteFrame = 2 // Also called lock mode, angle relative to horizon/world, lock mode. (usually default for roll and pitch).
} frames[3];
} angle;
struct TypeLonLat {
double lon; /**< longitude in [deg] */
double lat; /**< latitude in [deg] */
float altitude; /**< altitude in [m] */
float roll_angle; /**< roll is set to a fixed angle. Set to 0 for level horizon [rad] */
float pitch_angle_offset; /**< angular offset for pitch [rad] */
float yaw_angle_offset; /**< angular offset for yaw [rad] */
float pitch_fixed_angle; /**< ignored if < -pi, otherwise use a fixed pitch angle instead of the altitude */
double lon; // longitude in deg
double lat; // latitude in deg
float altitude; // altitude in m
float roll_offset; // roll offset in rad
float pitch_offset; // pitch offset in rad
float yaw_offset; // yaw offset in rad
float pitch_fixed_angle; // ignored if < -pi, otherwise use a fixed pitch angle instead of the altitude
} lonlat;
} type_data;
Type type = Type::Neutral;
bool stabilize_axis[3] = { false, false, false }; /**< whether the vmount driver should stabilize an axis
(if the output supports it, this can also be done externally) */
bool gimbal_shutter_retract = false; /**< whether to lock the gimbal (only in RC output mode) */
bool gimbal_shutter_retract = false; // whether to lock the gimbal (only in RC output mode)
uint8_t sysid_primary_control = 0; // The MAVLink system ID selected to be in control, 0 for no one.
uint8_t compid_primary_control = 0; // The MAVLink component ID selected to be in control, 0 for no one.
// uint8_t sysid_secondary_control = 0; // The MAVLink system ID selected for additional input, not implemented yet.
// uint8_t compid_secondary_control = 0; // The MAVLink component ID selected for additional input, not implemented yet.
};
} /* namespace vmount */

View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2016-2017 PX4 Development Team. All rights reserved.
* Copyright (c) 2016-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -31,56 +31,29 @@
*
****************************************************************************/
/**
* @file input.cpp
* @author Beat Küng <beat-kueng@gmx.net>
*
*/
#include "input.h"
namespace vmount
{
int InputBase::update(unsigned int timeout_ms, ControlData **control_data, bool already_active)
{
if (!_initialized) {
int ret = initialize();
InputBase::InputBase(Parameters &parameters) :
_parameters(parameters)
{}
if (ret) {
return ret;
}
//on startup, set the mount to a neutral position
_control_data.type = ControlData::Type::Neutral;
_control_data.gimbal_shutter_retract = true;
*control_data = &_control_data;
_initialized = true;
return 0;
}
return update_impl(timeout_ms, control_data, already_active);
}
void InputBase::control_data_set_lon_lat(double lon, double lat, float altitude, float roll_angle,
void InputBase::control_data_set_lon_lat(ControlData &control_data, double lon, double lat, float altitude,
float roll_angle,
float pitch_fixed_angle)
{
_control_data.type = ControlData::Type::LonLat;
_control_data.type_data.lonlat.lon = lon;
_control_data.type_data.lonlat.lat = lat;
_control_data.type_data.lonlat.altitude = altitude;
_control_data.type_data.lonlat.roll_angle = roll_angle;
_control_data.type_data.lonlat.pitch_fixed_angle = pitch_fixed_angle;
_control_data.type_data.lonlat.pitch_angle_offset = 0.f;
_control_data.type_data.lonlat.yaw_angle_offset = 0.f;
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_offset = roll_angle;
control_data.type_data.lonlat.pitch_fixed_angle = pitch_fixed_angle;
control_data.type_data.lonlat.pitch_offset = 0.f;
control_data.type_data.lonlat.yaw_offset = 0.f;
}
void InputBase::set_stabilize(bool roll_stabilize, bool pitch_stabilize, bool yaw_stabilize)
{
_control_data.stabilize_axis[0] = roll_stabilize;
_control_data.stabilize_axis[1] = pitch_stabilize;
_control_data.stabilize_axis[2] = yaw_stabilize;
}
} /* namespace vmount */

View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2016-2017 PX4 Development Team. All rights reserved.
* Copyright (c) 2016-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -31,62 +31,39 @@
*
****************************************************************************/
/**
* @file input.h
* @author Beat Küng <beat-kueng@gmx.net>
*
*/
#pragma once
#include "common.h"
#include "math.h"
#include "vmount_params.h"
namespace vmount
{
struct Parameters;
/**
** class InputBase
* Base class for all driver input classes
*/
class InputBase
{
public:
InputBase() = default;
enum class UpdateResult {
NoUpdate,
UpdatedActive,
UpdatedActiveOnce,
UpdatedNotActive,
};
InputBase() = delete;
explicit InputBase(Parameters &parameters);
virtual ~InputBase() = default;
/**
* Wait for an input update, with a timeout.
* @param timeout_ms timeout in ms
* @param control_data unchanged on error. On success it is nullptr if no new
* data is available, otherwise set to an object.
* If it is set, the returned object will not be changed for
* subsequent calls to update() that return no new data
* (in other words: if (some) control_data values change,
* non-null will be returned).
* @param already_active true if the mode was already active last time, false if it was not and "major"
* change is necessary such as big stick movement for RC.
* @return 0 on success, <0 otherwise
*/
virtual int update(unsigned int timeout_ms, ControlData **control_data, bool already_active);
/** report status to stdout */
virtual void print_status() = 0;
void set_stabilize(bool roll_stabilize, bool pitch_stabilize, bool yaw_stabilize);
virtual int initialize() = 0;
virtual UpdateResult update(unsigned int timeout_ms, ControlData &control_data, bool already_active) = 0;
virtual void print_status() const = 0;
protected:
virtual int update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active) = 0;
void control_data_set_lon_lat(ControlData &control_data, double lon, double lat, float altitude, float roll_angle = NAN,
float pitch_fixed_angle = NAN);
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;
Parameters &_parameters;
};

File diff suppressed because it is too large Load Diff

View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2016-2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2016-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -31,11 +31,6 @@
*
****************************************************************************/
/**
* @file input_mavlink.h
* @author Beat Küng <beat-kueng@gmx.net>
*
*/
#pragma once
@ -48,32 +43,30 @@
#include <uORB/topics/gimbal_device_attitude_status.h>
#include <uORB/topics/gimbal_manager_information.h>
#include <uORB/topics/gimbal_manager_status.h>
#include <uORB/topics/gimbal_manager_set_attitude.h>
#include <uORB/topics/gimbal_manager_set_manual_control.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_roi.h>
#include <uORB/topics/vehicle_global_position.h>
#include <lib/geo/geo.h>
namespace vmount
{
/**
** class InputMavlinkROI
** Input based on the vehicle_roi topic
*/
class InputMavlinkROI : public InputBase
{
public:
InputMavlinkROI() = default;
InputMavlinkROI() = delete;
explicit InputMavlinkROI(Parameters &parameters);
virtual ~InputMavlinkROI();
virtual void print_status();
protected:
virtual int update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active);
virtual int initialize();
void print_status() const override;
UpdateResult update(unsigned int timeout_ms, ControlData &control_data, bool already_active) override;
int initialize() override;
private:
void _read_control_data_from_position_setpoint_sub();
void _read_control_data_from_position_setpoint_sub(ControlData &control_data);
int _vehicle_roi_sub = -1;
int _position_setpoint_triplet_sub = -1;
@ -81,50 +74,49 @@ private:
};
/**
** class InputMavlinkCmdMount
** Input based on the VEHICLE_CMD_DO_MOUNT_CONTROL mavlink command
*/
class InputMavlinkCmdMount : public InputBase
{
public:
InputMavlinkCmdMount();
InputMavlinkCmdMount() = delete;
explicit InputMavlinkCmdMount(Parameters &parameters);
virtual ~InputMavlinkCmdMount();
virtual void print_status();
protected:
virtual int update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active);
virtual int initialize();
void print_status() const override;
UpdateResult update(unsigned int timeout_ms, ControlData &control_data, bool already_active) override;
int initialize() override;
private:
void _ack_vehicle_command(vehicle_command_s *cmd);
UpdateResult _process_command(ControlData &control_data, const vehicle_command_s &vehicle_command);
void _ack_vehicle_command(const vehicle_command_s &cmd);
int _vehicle_command_sub = -1;
int32_t _mav_sys_id{1}; ///< our mavlink system id
int32_t _mav_comp_id{1}; ///< our mavlink component id
};
class InputMavlinkGimbalV2 : public InputBase
{
public:
InputMavlinkGimbalV2(uint8_t mav_sys_id, uint8_t mav_comp_id, float &mnt_rate_pitch, float &mnt_rate_yaw);
InputMavlinkGimbalV2() = delete;
explicit InputMavlinkGimbalV2(Parameters &parameters);
virtual ~InputMavlinkGimbalV2();
virtual void print_status();
protected:
virtual int update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active);
virtual int initialize();
UpdateResult update(unsigned int timeout_ms, ControlData &control_data, bool already_active) override;
int initialize() override;
void print_status() const override;
private:
void _set_control_data_from_set_attitude(const uint32_t flags, const matrix::Quatf &q,
UpdateResult _process_set_attitude(ControlData &control_data, const gimbal_manager_set_attitude_s &set_attitude);
UpdateResult _process_vehicle_roi(ControlData &control_data, const vehicle_roi_s &vehicle_roi);
UpdateResult _process_position_setpoint_triplet(ControlData &control_data,
const position_setpoint_triplet_s &position_setpoint_triplet);
UpdateResult _process_command(ControlData &control_data, const vehicle_command_s &vehicle_command);
UpdateResult _process_set_manual_control(ControlData &control_data,
const gimbal_manager_set_manual_control_s &set_manual_control);
void _set_control_data_from_set_attitude(ControlData &control_data, const uint32_t flags, const matrix::Quatf &q,
const matrix::Vector3f &angular_velocity);
void _ack_vehicle_command(vehicle_command_s *cmd, uint8_t result);
void _ack_vehicle_command(const vehicle_command_s &cmd, uint8_t result);
void _stream_gimbal_manager_information();
void _stream_gimbal_manager_status();
void _read_control_data_from_position_setpoint_sub();
void _stream_gimbal_manager_status(const ControlData &control_data);
void _read_control_data_from_position_setpoint_sub(ControlData &control_data);
int _vehicle_roi_sub = -1;
int _gimbal_manager_set_attitude_sub = -1;
@ -132,17 +124,7 @@ private:
int _position_setpoint_triplet_sub = -1;
int _vehicle_command_sub = -1;
uint8_t _mav_sys_id{1}; ///< our mavlink system id
uint8_t _mav_comp_id{1}; ///< our mavlink component id
uint8_t _sys_id_primary_control{0};
uint8_t _comp_id_primary_control{0};
float &_mnt_rate_pitch;
float &_mnt_rate_yaw;
uORB::Subscription _gimbal_device_attitude_status_sub{ORB_ID(gimbal_device_attitude_status)};
uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)};
uORB::Publication<gimbal_manager_information_s> _gimbal_manager_info_pub{ORB_ID(gimbal_manager_information)};
uORB::Publication<gimbal_manager_status_s> _gimbal_manager_status_pub{ORB_ID(gimbal_manager_status)};
uint8_t _cur_roi_mode = vehicle_roi_s::ROI_NONE;

View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2016-2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2016-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -31,11 +31,6 @@
*
****************************************************************************/
/**
* @file input_rc.cpp
* @author Beat Küng <beat-kueng@gmx.net>
*
*/
#include "input_rc.h"
@ -50,17 +45,9 @@
namespace vmount
{
InputRC::InputRC(int aux_channel_roll, int aux_channel_pitch, int aux_channel_yaw, float mnt_rate_pitch,
float mnt_rate_yaw, int rc_in_mode)
{
_aux_channels[0] = aux_channel_roll;
_aux_channels[1] = aux_channel_pitch;
_aux_channels[2] = aux_channel_yaw;
_mnt_rate_pitch = mnt_rate_pitch;
_mnt_rate_yaw = mnt_rate_yaw;
_rc_in_mode = rc_in_mode;
}
InputRC::InputRC(Parameters &parameters) :
InputBase(parameters)
{}
InputRC::~InputRC()
{
@ -80,11 +67,8 @@ int InputRC::initialize()
return 0;
}
int InputRC::update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active)
InputRC::UpdateResult InputRC::update(unsigned int timeout_ms, ControlData &control_data, bool already_active)
{
// Default to no change signalled by NULL.
*control_data = nullptr;
px4_pollfd_struct_t polls[1];
polls[0].fd = _manual_control_setpoint_sub;
polls[0].events = POLLIN;
@ -92,24 +76,25 @@ int InputRC::update_impl(unsigned int timeout_ms, ControlData **control_data, bo
int ret = px4_poll(polls, 1, timeout_ms);
if (ret < 0) {
return -errno;
return UpdateResult::NoUpdate;
}
if (ret == 0) {
// Timeout, leave NULL
} else {
if (polls[0].revents & POLLIN) {
// Only if there was a change, we update the control data, otherwise leave it NULL.
if (_read_control_data_from_subscription(_control_data, already_active)) {
*control_data = &_control_data;
}
// If we have been active before, we stay active, unless someone steals
// the control away.
if (already_active) {
return UpdateResult::UpdatedActive;
}
}
return 0;
if (polls[0].revents & POLLIN) {
return _read_control_data_from_subscription(control_data, already_active);
}
return UpdateResult::NoUpdate;
}
bool InputRC::_read_control_data_from_subscription(ControlData &control_data, bool already_active)
InputRC::UpdateResult InputRC::_read_control_data_from_subscription(ControlData &control_data, bool already_active)
{
manual_control_setpoint_s manual_control_setpoint{};
orb_copy(ORB_ID(manual_control_setpoint), _manual_control_setpoint_sub, &manual_control_setpoint);
@ -123,20 +108,23 @@ bool InputRC::_read_control_data_from_subscription(ControlData &control_data, bo
// If we were already active previously, we just update normally. Otherwise, there needs to be
// a major stick movement to re-activate manual (or it's running for the very first time).
bool major_movement = false;
// Detect a big stick movement
for (int i = 0; i < 3; ++i) {
if (fabsf(_last_set_aux_values[i] - new_aux_values[i]) > 0.25f) {
major_movement = true;
const bool major_movement = [&]() {
for (int i = 0; i < 3; ++i) {
if (fabsf(_last_set_aux_values[i] - new_aux_values[i]) > 0.25f) {
return true;
}
}
}
if (already_active || major_movement || _first_time) {
return false;
}();
_first_time = false;
if (already_active || major_movement) {
control_data.sysid_primary_control = _parameters.mav_sysid;
control_data.compid_primary_control = _parameters.mav_compid;
if (_rc_in_mode == 0) {
if (_parameters.mnt_rc_in_mode == 0) {
// We scale manual input from roll -180..180, pitch -90..90, yaw, -180..180 degrees.
matrix::Eulerf euler(new_aux_values[0] * math::radians(180.f),
new_aux_values[1] * math::radians(90.f),
@ -160,8 +148,8 @@ bool InputRC::_read_control_data_from_subscription(ControlData &control_data, bo
control_data.type_data.angle.q[3] = NAN;
control_data.type_data.angle.angular_velocity[0] = 0.f;
control_data.type_data.angle.angular_velocity[1] = math::radians(_mnt_rate_pitch) * new_aux_values[1];
control_data.type_data.angle.angular_velocity[2] = math::radians(_mnt_rate_yaw) * new_aux_values[2];
control_data.type_data.angle.angular_velocity[1] = math::radians(_parameters.mnt_rate_pitch) * new_aux_values[1];
control_data.type_data.angle.angular_velocity[2] = math::radians(_parameters.mnt_rate_yaw) * new_aux_values[2];
control_data.type_data.angle.frames[0] = ControlData::TypeData::TypeAngle::Frame::AngularRate;
control_data.type_data.angle.frames[1] = ControlData::TypeData::TypeAngle::Frame::AngularRate;
@ -174,16 +162,33 @@ bool InputRC::_read_control_data_from_subscription(ControlData &control_data, bo
}
control_data.gimbal_shutter_retract = false;
return true;
return UpdateResult::UpdatedActive;
} else {
return false;
return UpdateResult::NoUpdate;
}
}
float InputRC::_get_aux_value(const manual_control_setpoint_s &manual_control_setpoint, int channel_idx)
{
switch (_aux_channels[channel_idx]) {
int32_t aux_channel = [&]() {
switch (channel_idx) {
case 0:
return _parameters.mnt_man_roll;
case 1:
return _parameters.mnt_man_pitch;
case 2:
return _parameters.mnt_man_yaw;
default:
return int32_t(0);
}
}();
switch (aux_channel) {
case 1:
return manual_control_setpoint.aux1;
@ -208,9 +213,10 @@ float InputRC::_get_aux_value(const manual_control_setpoint_s &manual_control_se
}
}
void InputRC::print_status()
void InputRC::print_status() const
{
PX4_INFO("Input: RC (channels: roll=%i, pitch=%i, yaw=%i)", _aux_channels[0], _aux_channels[1], _aux_channels[2]);
PX4_INFO("Input: RC (channels: roll=%" PRIi32 ", pitch=%" PRIi32 ", yaw=%" PRIi32 ")", _parameters.mnt_man_roll,
_parameters.mnt_man_pitch, _parameters.mnt_man_yaw);
}
} /* namespace vmount */

View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2016-2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2016-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -31,66 +31,37 @@
*
****************************************************************************/
/**
* @file input_rc.h
* @author Beat Küng <beat-kueng@gmx.net>
*
*/
#pragma once
#include "input.h"
#include "vmount_params.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:
InputRC() = delete;
explicit InputRC(Parameters &parameters);
/**
* @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, float mnt_rate_pitch, float mnt_rate_yaw,
int rc_in_mode);
virtual ~InputRC();
virtual void print_status();
virtual void print_status() const;
virtual UpdateResult update(unsigned int timeout_ms, ControlData &control_data, bool already_active);
protected:
virtual int update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active);
virtual int initialize();
/**
* @return true if there was a change in control data
*/
virtual bool _read_control_data_from_subscription(ControlData &control_data, bool already_active);
float _get_aux_value(const manual_control_setpoint_s &manual_control_setpoint, int channel_idx);
private:
int _aux_channels[3] {};
float _mnt_rate_pitch{0.f};
float _mnt_rate_yaw{0.f};
int _rc_in_mode{0};
virtual UpdateResult _read_control_data_from_subscription(ControlData &control_data, bool already_active);
float _get_aux_value(const manual_control_setpoint_s &manual_control_setpoint, int channel_idx);
int _manual_control_setpoint_sub{-1};
bool _first_time{true};
float _last_set_aux_values[3] {};
};
} /* namespace vmount */

View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2016-2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2016-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -31,16 +31,8 @@
*
****************************************************************************/
/**
* @file input_test.cpp
* @author Beat Küng <beat-kueng@gmx.net>
*
*/
#include "input_test.h"
#include <math.h>
#include <px4_platform_common/posix.h>
#include <lib/matrix/matrix/math.hpp>
#include <lib/mathlib/mathlib.h>
@ -49,39 +41,42 @@
namespace vmount
{
InputTest::InputTest(float roll_deg, float pitch_deg, float yaw_deg)
InputTest::InputTest(Parameters &parameters) :
InputBase(parameters)
{}
InputTest::UpdateResult InputTest::update(unsigned int timeout_ms, ControlData &control_data, bool already_active)
{
_angles[0] = roll_deg;
_angles[1] = pitch_deg;
_angles[2] = yaw_deg;
}
if (!_has_been_set.load()) {
return UpdateResult::NoUpdate;
}
bool InputTest::finished()
{
return true; /* only a single-shot test (for now) */
}
control_data.type = ControlData::Type::Angle;
int InputTest::update(unsigned int timeout_ms, ControlData **control_data, bool already_active)
{
//we directly override the update() here, since we don't need the initialization from the base class
_control_data.type = ControlData::Type::Angle;
_control_data.type_data.angle.frames[0] = ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame;
_control_data.type_data.angle.frames[1] = ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame;
_control_data.type_data.angle.frames[2] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame;
control_data.type_data.angle.frames[0] = ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame;
control_data.type_data.angle.frames[1] = ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame;
control_data.type_data.angle.frames[2] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame;
matrix::Eulerf euler(
math::radians(_angles[0]),
math::radians(_angles[1]),
math::radians(_angles[2]));
math::radians((float)_roll_deg),
math::radians((float)_pitch_deg),
math::radians((float)_yaw_deg));
matrix::Quatf q(euler);
q.copyTo(_control_data.type_data.angle.q);
q.copyTo(control_data.type_data.angle.q);
_control_data.gimbal_shutter_retract = false;
*control_data = &_control_data;
return 0;
control_data.gimbal_shutter_retract = false;
control_data.type_data.angle.angular_velocity[0] = NAN;
control_data.type_data.angle.angular_velocity[1] = NAN;
control_data.type_data.angle.angular_velocity[2] = NAN;
// For testing we mark ourselves as in control.
control_data.sysid_primary_control = _parameters.mav_sysid;
control_data.compid_primary_control = _parameters.mav_compid;
_has_been_set.store(false);
return UpdateResult::UpdatedActive;
}
int InputTest::initialize()
@ -89,9 +84,21 @@ int InputTest::initialize()
return 0;
}
void InputTest::print_status()
void InputTest::print_status() const
{
PX4_INFO("Input: Test");
PX4_INFO_RAW(" roll : % 3d deg\n", _roll_deg);
PX4_INFO_RAW(" pitch: % 3d deg\n", _pitch_deg);
PX4_INFO_RAW(" yaw : % 3d deg\n", _yaw_deg);
}
void InputTest::set_test_input(int roll_deg, int pitch_deg, int yaw_deg)
{
_roll_deg = roll_deg;
_pitch_deg = pitch_deg;
_yaw_deg = yaw_deg;
_has_been_set.store(true);
}
} /* namespace vmount */

View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2016-2017 PX4 Development Team. All rights reserved.
* Copyright (c) 2016-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -31,47 +31,33 @@
*
****************************************************************************/
/**
* @file input_test.h
* @author Beat Küng <beat-kueng@gmx.net>
*
*/
#pragma once
#include "input.h"
#include <px4_platform_common/atomic.h>
namespace vmount
{
/**
** class InputTest
* Send a single control command, configured via constructor arguments
*/
class InputTest : public InputBase
{
public:
InputTest() = delete;
explicit InputTest(Parameters &parameters);
virtual ~InputTest() = default;
/**
* set to a fixed angle
*/
InputTest(float roll_deg, float pitch_deg, float yaw_deg);
virtual ~InputTest() {}
UpdateResult update(unsigned int timeout_ms, ControlData &control_data, bool already_active) override;
int initialize() override;
void print_status() const override;
/** check whether the test finished, and thus the main thread can quit */
bool finished();
virtual int update(unsigned int timeout_ms, ControlData **control_data, bool already_active);
protected:
virtual int update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active) { return 0; } //not needed
virtual int initialize();
virtual void print_status();
void set_test_input(int roll_deg, int pitch_deg, int yaw_deg);
private:
float _angles[3]; /**< desired angles in [deg] */
int _roll_deg {0};
int _pitch_deg {0};
int _yaw_deg {0};
px4::atomic<bool> _has_been_set {false};
};

View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2016-2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2016-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -31,15 +31,8 @@
*
****************************************************************************/
/**
* @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>
@ -53,8 +46,8 @@
namespace vmount
{
OutputBase::OutputBase(const OutputConfig &output_config)
: _config(output_config)
OutputBase::OutputBase(const Parameters &parameters)
: _parameters(parameters)
{
_last_update = hrt_absolute_time();
}
@ -88,16 +81,14 @@ float OutputBase::_calculate_pitch(double lon, double lat, float altitude,
return atan2f(z, target_distance);
}
void OutputBase::_set_angle_setpoints(const ControlData *control_data)
void OutputBase::_set_angle_setpoints(const ControlData &control_data)
{
_cur_control_data = control_data;
switch (control_data->type) {
switch (control_data.type) {
case ControlData::Type::Angle:
{
for (int i = 0; i < 3; ++i) {
switch (control_data->type_data.angle.frames[i]) {
switch (control_data.type_data.angle.frames[i]) {
case ControlData::TypeData::TypeAngle::Frame::AngularRate:
break;
@ -110,18 +101,18 @@ void OutputBase::_set_angle_setpoints(const ControlData *control_data)
break;
}
_angle_velocity[i] = control_data->type_data.angle.angular_velocity[i];
_angle_velocity[i] = control_data.type_data.angle.angular_velocity[i];
}
for (int i = 0; i < 4; ++i) {
_q_setpoint[i] = control_data->type_data.angle.q[i];
_q_setpoint[i] = control_data.type_data.angle.q[i];
}
}
break;
case ControlData::Type::LonLat:
_handle_position_update(true);
_handle_position_update(control_data, true);
break;
case ControlData::Type::Neutral:
@ -134,47 +125,39 @@ void OutputBase::_set_angle_setpoints(const ControlData *control_data)
_angle_velocity[2] = NAN;
break;
}
for (int i = 0; i < 3; ++i) {
_stabilize[i] = control_data->stabilize_axis[i];
}
}
void OutputBase::_handle_position_update(bool force_update)
void OutputBase::_handle_position_update(const ControlData &control_data, bool force_update)
{
if (!_cur_control_data || _cur_control_data->type != ControlData::Type::LonLat) {
if (control_data.type != ControlData::Type::LonLat) {
return;
}
vehicle_global_position_s vehicle_global_position{};
vehicle_local_position_s vehicle_local_position{};
if (force_update) {
_vehicle_global_position_sub.copy(&vehicle_global_position);
_vehicle_local_position_sub.copy(&vehicle_local_position);
} else {
if (!_vehicle_global_position_sub.update(&vehicle_global_position)) {
return;
}
if (!_vehicle_local_position_sub.update(&vehicle_local_position)) {
return;
}
}
const double &vlat = vehicle_global_position.lat;
const double &vlon = vehicle_global_position.lon;
const double &lat = _cur_control_data->type_data.lonlat.lat;
const double &lon = _cur_control_data->type_data.lonlat.lon;
const float &alt = _cur_control_data->type_data.lonlat.altitude;
const double &lat = control_data.type_data.lonlat.lat;
const double &lon = control_data.type_data.lonlat.lon;
const float &alt = control_data.type_data.lonlat.altitude;
float roll = _cur_control_data->type_data.lonlat.roll_angle;
float roll = PX4_ISFINITE(control_data.type_data.lonlat.roll_offset)
? control_data.type_data.lonlat.roll_offset
: 0.0f;
// interface: use fixed pitch value > -pi otherwise consider ROI altitude
float pitch = (_cur_control_data->type_data.lonlat.pitch_fixed_angle >= -M_PI_F) ?
_cur_control_data->type_data.lonlat.pitch_fixed_angle :
float pitch = (control_data.type_data.lonlat.pitch_fixed_angle >= -M_PI_F) ?
control_data.type_data.lonlat.pitch_fixed_angle :
_calculate_pitch(lon, lat, alt, vehicle_global_position);
float yaw = get_bearing_to_next_waypoint(vlat, vlon, lat, lon);
@ -182,8 +165,13 @@ void OutputBase::_handle_position_update(bool force_update)
_absolute_angle[2] = true;
// add offsets from VEHICLE_CMD_DO_SET_ROI_WPNEXT_OFFSET
pitch += _cur_control_data->type_data.lonlat.pitch_angle_offset;
yaw += _cur_control_data->type_data.lonlat.yaw_angle_offset;
if (PX4_ISFINITE(control_data.type_data.lonlat.pitch_offset)) {
pitch += control_data.type_data.lonlat.pitch_offset;
}
if (PX4_ISFINITE(control_data.type_data.lonlat.yaw_offset)) {
yaw += control_data.type_data.lonlat.yaw_offset;
}
matrix::Quatf(matrix::Eulerf(roll, pitch, yaw)).copyTo(_q_setpoint);
@ -246,5 +234,12 @@ void OutputBase::_calculate_angle_output(const hrt_abstime &t)
}
}
void OutputBase::set_stabilize(bool roll_stabilize, bool pitch_stabilize, bool yaw_stabilize)
{
_stabilize[0] = roll_stabilize;
_stabilize[1] = pitch_stabilize;
_stabilize[2] = yaw_stabilize;
}
} /* namespace vmount */

View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2016-2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2016-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -31,15 +31,11 @@
*
****************************************************************************/
/**
* @file output.h
* @author Beat Küng <beat-kueng@gmx.net>
*
*/
#pragma once
#include "common.h"
#include "vmount_params.h"
#include <drivers/drv_hrt.h>
#include <lib/geo/geo.h>
#include <uORB/Publication.hpp>
@ -47,71 +43,37 @@
#include <uORB/topics/mount_orientation.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_local_position.h>
namespace vmount
{
struct OutputConfig {
float gimbal_retracted_mode_value; /**< Mixer output value for selecting gimbal retracted mode */
float gimbal_normal_mode_value; /**< Mixer output value for selecting gimbal normal mode */
/** Scale factor for pitch channel (maps from angle in radians to actuator output in [-1,1]). OutputRC only. */
float pitch_scale;
/** Scale factor for roll channel (maps from angle in radians to actuator output in [-1,1]). OutputRC only. */
float roll_scale;
/** Scale factor for yaw channel (maps from angle in radians to actuator output in [-1,1]). OutputRC only. */
float yaw_scale;
float pitch_offset; /**< Offset for pitch channel in radians */
float roll_offset; /**< Offset for roll channel in radians */
float yaw_offset; /**< Offset for yaw channel in radians */
uint32_t mavlink_sys_id_v1; /**< Mavlink target system id for mavlink output only for v1 */
uint32_t mavlink_comp_id_v1;
};
/**
** class OutputBase
* Base class for all driver output classes
*/
class OutputBase
{
public:
OutputBase(const OutputConfig &output_config);
OutputBase() = delete;
explicit OutputBase(const Parameters &parameters);
virtual ~OutputBase() = default;
virtual int initialize() { return 0; }
virtual void update(const ControlData &control_data, bool new_setpoints) = 0;
/**
* Update the output.
* @param data new command if non null
* @return 0 on success, <0 otherwise
*/
virtual int update(const ControlData *control_data) = 0;
virtual void print_status() const = 0;
/** report status to stdout */
virtual void print_status() = 0;
/** Publish _angle_outputs as a mount_orientation message. */
void publish();
void set_stabilize(bool roll_stabilize, bool pitch_stabilize, bool yaw_stabilize);
protected:
float _calculate_pitch(double lon, double lat, float altitude,
const vehicle_global_position_s &global_position);
MapProjection _projection_reference{}; ///< class to convert (lon, lat) to local [m]
const OutputConfig &_config;
const Parameters &_parameters;
/** set angle setpoints, speeds & stabilize flags */
void _set_angle_setpoints(const ControlData *control_data);
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;
void _handle_position_update(const ControlData &control_data, bool force_update = false);
float _q_setpoint[4] = { NAN, NAN, NAN, NAN }; ///< can be NAN if not specifically set
float _angle_velocity[3] = { NAN, NAN, NAN }; //< [rad/s], can be NAN if not specifically set
@ -131,7 +93,6 @@ protected:
private:
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)};
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
uORB::Publication<mount_orientation_s> _mount_orientation_pub{ORB_ID(mount_orientation)};
};

View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2016-2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2016-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -31,51 +31,42 @@
*
****************************************************************************/
/**
* @file output_mavlink.cpp
* @author Leon Müller (thedevleon)
* @author Beat Küng <beat-kueng@gmx.net>
*
*/
#include "output_mavlink.h"
#include <math.h>
#include <matrix/matrix/math.hpp>
#include <uORB/topics/vehicle_command.h>
#include <px4_platform_common/defines.h>
namespace vmount
{
OutputMavlinkV1::OutputMavlinkV1(const OutputConfig &output_config)
: OutputBase(output_config)
{
}
OutputMavlinkV1::OutputMavlinkV1(const Parameters &parameters)
: OutputBase(parameters)
{}
int OutputMavlinkV1::update(const ControlData *control_data)
void OutputMavlinkV1::update(const ControlData &control_data, bool new_setpoints)
{
vehicle_command_s vehicle_command{};
vehicle_command.timestamp = hrt_absolute_time();
vehicle_command.target_system = (uint8_t)_config.mavlink_sys_id_v1;
vehicle_command.target_component = (uint8_t)_config.mavlink_comp_id_v1;
vehicle_command.target_system = (uint8_t)_parameters.mnt_mav_sysid_v1;
vehicle_command.target_component = (uint8_t)_parameters.mnt_mav_compid_v1;
if (control_data) {
if (new_setpoints) {
//got new command
_set_angle_setpoints(control_data);
const bool configuration_changed =
(control_data->type != _previous_control_data_type);
_previous_control_data_type = control_data->type;
(control_data.type != _previous_control_data_type);
_previous_control_data_type = control_data.type;
if (configuration_changed) {
vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE;
vehicle_command.timestamp = hrt_absolute_time();
if (control_data->type == ControlData::Type::Neutral) {
if (control_data.type == ControlData::Type::Neutral) {
vehicle_command.param1 = vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL;
vehicle_command.param5 = 0.0;
@ -85,20 +76,20 @@ int OutputMavlinkV1::update(const ControlData *control_data)
} else {
vehicle_command.param1 = vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING;
vehicle_command.param5 = static_cast<double>(control_data->type_data.angle.frames[0]);
vehicle_command.param6 = static_cast<double>(control_data->type_data.angle.frames[1]);
vehicle_command.param7 = static_cast<float>(control_data->type_data.angle.frames[2]);
vehicle_command.param5 = static_cast<double>(control_data.type_data.angle.frames[0]);
vehicle_command.param6 = static_cast<double>(control_data.type_data.angle.frames[1]);
vehicle_command.param7 = static_cast<float>(control_data.type_data.angle.frames[2]);
}
vehicle_command.param2 = _stabilize[0] ? 1.0f : 0.0f;
vehicle_command.param3 = _stabilize[1] ? 1.0f : 0.0f;
vehicle_command.param4 = _stabilize[2] ? 1.0f : 0.0f;
_vehicle_command_pub.publish(vehicle_command);
_gimbal_v1_command_pub.publish(vehicle_command);
}
}
_handle_position_update();
_handle_position_update(control_data);
hrt_abstime t = hrt_absolute_time();
_calculate_angle_output(t);
@ -108,18 +99,16 @@ int OutputMavlinkV1::update(const ControlData *control_data)
// vmount spec has roll, pitch on channels 0, 1, respectively; MAVLink spec has roll, pitch on channels 1, 0, respectively
// vmount uses radians, MAVLink uses degrees
vehicle_command.param1 = math::degrees(_angle_outputs[1] + _config.pitch_offset);
vehicle_command.param2 = math::degrees(_angle_outputs[0] + _config.roll_offset);
vehicle_command.param3 = math::degrees(_angle_outputs[2] + _config.yaw_offset);
vehicle_command.param1 = math::degrees(_angle_outputs[1] + math::radians(_parameters.mnt_off_pitch));
vehicle_command.param2 = math::degrees(_angle_outputs[0] + math::radians(_parameters.mnt_off_roll));
vehicle_command.param3 = math::degrees(_angle_outputs[2] + math::radians(_parameters.mnt_off_yaw));
vehicle_command.param7 = 2.0f; // MAV_MOUNT_MODE_MAVLINK_TARGETING;
_vehicle_command_pub.publish(vehicle_command);
_gimbal_v1_command_pub.publish(vehicle_command);
_stream_device_attitude_status();
_last_update = t;
return 0;
}
void OutputMavlinkV1::_stream_device_attitude_status()
@ -143,52 +132,51 @@ void OutputMavlinkV1::_stream_device_attitude_status()
_attitude_status_pub.publish(attitude_status);
}
void OutputMavlinkV1::print_status()
void OutputMavlinkV1::print_status() const
{
PX4_INFO("Output: MAVLink gimbal protocol v1");
}
OutputMavlinkV2::OutputMavlinkV2(int32_t mav_sys_id, int32_t mav_comp_id, const OutputConfig &output_config)
: OutputBase(output_config),
_mav_sys_id(mav_sys_id),
_mav_comp_id(mav_comp_id)
OutputMavlinkV2::OutputMavlinkV2(const Parameters &parameters)
: OutputBase(parameters)
{
}
int OutputMavlinkV2::update(const ControlData *control_data)
void OutputMavlinkV2::update(const ControlData &control_data, bool new_setpoints)
{
_check_for_gimbal_device_information();
hrt_abstime t = hrt_absolute_time();
if (!_gimbal_device_found && t - _last_gimbal_device_checked > 1000000) {
_request_gimbal_device_information();
_last_gimbal_device_checked = t;
} else {
if (control_data) {
if (new_setpoints) {
//got new command
_set_angle_setpoints(control_data);
_handle_position_update(control_data);
_last_update = t;
}
_handle_position_update();
_publish_gimbal_device_set_attitude();
_last_update = t;
}
return 0;
}
void OutputMavlinkV2::_request_gimbal_device_information()
{
printf("request gimbal device\n");
vehicle_command_s vehicle_cmd{};
vehicle_cmd.timestamp = hrt_absolute_time();
vehicle_cmd.command = vehicle_command_s::VEHICLE_CMD_REQUEST_MESSAGE;
vehicle_cmd.param1 = vehicle_command_s::VEHICLE_CMD_GIMBAL_DEVICE_INFORMATION;
vehicle_cmd.target_system = 0;
vehicle_cmd.target_component = 0;
vehicle_cmd.source_system = _mav_sys_id;
vehicle_cmd.source_component = _mav_comp_id;
vehicle_cmd.source_system = _parameters.mav_sysid;
vehicle_cmd.source_component = _parameters.mav_compid;
vehicle_cmd.confirmation = 0;
vehicle_cmd.from_external = false;
@ -206,16 +194,26 @@ void OutputMavlinkV2::_check_for_gimbal_device_information()
}
}
void OutputMavlinkV2::print_status()
void OutputMavlinkV2::print_status() const
{
PX4_INFO("Output: MAVLink gimbal protocol v2");
PX4_INFO_RAW(" quaternion: [%.1f %.1f %.1f %.1f]\n",
(double)_q_setpoint[0],
(double)_q_setpoint[1],
(double)_q_setpoint[2],
(double)_q_setpoint[3]);
PX4_INFO_RAW(" angular velocity: [%.1f %.1f %.1f]\n",
(double)_angle_velocity[0],
(double)_angle_velocity[1],
(double)_angle_velocity[2]);
}
void OutputMavlinkV2::_publish_gimbal_device_set_attitude()
{
gimbal_device_set_attitude_s set_attitude{};
set_attitude.timestamp = hrt_absolute_time();
set_attitude.target_system = (uint8_t)_mav_sys_id;
set_attitude.target_system = (uint8_t)_parameters.mav_sysid;
set_attitude.target_component = _gimbal_device_compid;
set_attitude.angular_velocity_x = _angle_velocity[0];

View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2016-2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2016-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -31,11 +31,6 @@
*
****************************************************************************/
/**
* @file output_mavlink.h
* @author Beat Küng <beat-kueng@gmx.net>
*
*/
#pragma once
@ -50,23 +45,19 @@
namespace vmount
{
/**
** class OutputMavlink
* Output via vehicle_command topic
*/
class OutputMavlinkV1 : public OutputBase
{
public:
OutputMavlinkV1(const OutputConfig &output_config);
OutputMavlinkV1(const Parameters &parameters);
virtual ~OutputMavlinkV1() = default;
virtual int update(const ControlData *control_data);
virtual void update(const ControlData &control_data, bool new_setpoints);
virtual void print_status();
virtual void print_status() const;
private:
void _stream_device_attitude_status();
uORB::Publication<vehicle_command_s> _vehicle_command_pub{ORB_ID(vehicle_command)};
uORB::Publication<vehicle_command_s> _gimbal_v1_command_pub{ORB_ID(gimbal_v1_command)};
uORB::Publication <gimbal_device_attitude_status_s> _attitude_status_pub{ORB_ID(gimbal_device_attitude_status)};
ControlData::Type _previous_control_data_type {ControlData::Type::Neutral};
@ -75,12 +66,12 @@ private:
class OutputMavlinkV2 : public OutputBase
{
public:
OutputMavlinkV2(int32_t mav_sys_id, int32_t mav_comp_id, const OutputConfig &output_config);
OutputMavlinkV2(const Parameters &parameters);
virtual ~OutputMavlinkV2() = default;
virtual int update(const ControlData *control_data);
virtual void update(const ControlData &control_data, bool new_setpoints);
virtual void print_status();
virtual void print_status() const;
private:
void _publish_gimbal_device_set_attitude();
@ -90,8 +81,6 @@ private:
uORB::Publication<gimbal_device_set_attitude_s> _gimbal_device_set_attitude_pub{ORB_ID(gimbal_device_set_attitude)};
uORB::Subscription _gimbal_device_information_sub{ORB_ID(gimbal_device_information)};
int32_t _mav_sys_id{1}; ///< our mavlink system id
int32_t _mav_comp_id{1}; ///< our mavlink component id
uint8_t _gimbal_device_compid{0};
hrt_abstime _last_gimbal_device_checked{0};
bool _gimbal_device_found {false};

View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2016 PX4 Development Team. All rights reserved.
* Copyright (c) 2016-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -31,12 +31,6 @@
*
****************************************************************************/
/**
* @file output_rc.cpp
* @author Leon Müller (thedevleon)
* @author Beat Küng <beat-kueng@gmx.net>
*
*/
#include "output_rc.h"
@ -49,20 +43,19 @@ using math::constrain;
namespace vmount
{
OutputRC::OutputRC(const OutputConfig &output_config)
: OutputBase(output_config)
OutputRC::OutputRC(const Parameters &parameters)
: OutputBase(parameters)
{
}
int OutputRC::update(const ControlData *control_data)
void OutputRC::update(const ControlData &control_data, bool new_setpoints)
{
if (control_data) {
//got new command
_retract_gimbal = control_data->gimbal_shutter_retract;
if (new_setpoints) {
_retract_gimbal = control_data.gimbal_shutter_retract;
_set_angle_setpoints(control_data);
}
_handle_position_update();
_handle_position_update(control_data);
hrt_abstime t = hrt_absolute_time();
_calculate_angle_output(t);
@ -71,20 +64,28 @@ int OutputRC::update(const ControlData *control_data)
// _angle_outputs are in radians, actuator_controls are in [-1, 1]
actuator_controls_s actuator_controls{};
actuator_controls.control[0] = constrain((_angle_outputs[0] + _config.roll_offset) * _config.roll_scale, -1.f, 1.f);
actuator_controls.control[1] = constrain((_angle_outputs[1] + _config.pitch_offset) * _config.pitch_scale, -1.f, 1.f);
actuator_controls.control[2] = constrain((_angle_outputs[2] + _config.yaw_offset) * _config.yaw_scale, -1.f, 1.f);
actuator_controls.control[3] = constrain(_retract_gimbal ? _config.gimbal_retracted_mode_value :
_config.gimbal_normal_mode_value, -1.f, 1.f);
actuator_controls.control[0] = constrain(
(_angle_outputs[0] + math::radians(_parameters.mnt_off_roll)) *
(1.0f / (math::radians(_parameters.mnt_range_roll / 2.0f))),
-1.f, 1.f);
actuator_controls.control[1] = constrain(
(_angle_outputs[1] + math::radians(_parameters.mnt_off_pitch)) *
(1.0f / (math::radians(_parameters.mnt_range_pitch / 2.0f))),
-1.f, 1.f);
actuator_controls.control[2] = constrain(
(_angle_outputs[2] + math::radians(_parameters.mnt_off_yaw)) *
(1.0f / (math::radians(_parameters.mnt_range_yaw / 2.0f))),
-1.f, 1.f);
actuator_controls.control[3] = constrain(
_retract_gimbal ? _parameters.mnt_ob_lock_mode : _parameters.mnt_ob_norm_mode,
-1.f, 1.f);
actuator_controls.timestamp = hrt_absolute_time();
_actuator_controls_pub.publish(actuator_controls);
_last_update = t;
return 0;
}
void OutputRC::print_status()
void OutputRC::print_status() const
{
PX4_INFO("Output: AUX");
}

View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2016 PX4 Development Team. All rights reserved.
* Copyright (c) 2016-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -31,11 +31,6 @@
*
****************************************************************************/
/**
* @file output_rc.h
* @author Beat Küng <beat-kueng@gmx.net>
*
*/
#pragma once
@ -48,20 +43,15 @@
namespace vmount
{
/**
** class OutputRC
* Output via actuator_controls_2 topic
*/
class OutputRC : public OutputBase
{
public:
OutputRC(const OutputConfig &output_config);
OutputRC() = delete;
explicit OutputRC(const Parameters &parameters);
virtual ~OutputRC() = default;
virtual int update(const ControlData *control_data);
virtual void print_status();
virtual void update(const ControlData &control_data, bool new_setpoints);
virtual void print_status() const;
private:
void _stream_device_attitude_status();
@ -72,5 +62,4 @@ private:
bool _retract_gimbal = true;
};
} /* namespace vmount */

View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013-2020, 2021 PX4 Development Team. All rights reserved.
* Copyright (c) 2013-2022 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,30 +32,24 @@
****************************************************************************/
/**
* @file vmount.cpp
* @author Leon Müller (thedevleon)
* @author Beat Küng <beat-kueng@gmx.net>
* @author Julian Oes <julian@oes.ch>
* @author Matthew Edwards (mje-nz)
*
* Driver for to control mounts such as gimbals or servos.
* Inputs for the mounts can RC and/or mavlink commands.
* Outputs to the mounts can be RC (PWM) output or mavlink.
* Gimbal/mount driver.
* Supported inputs:
* - RC
* - MAVLink gimbal protocol v1
* - MAVLink gimbal protocol v2
* - Test CLI commands
* Supported outputs:
* - PWM
* - MAVLink gimbal protocol v1
* - MAVLink gimbal protocol v2
*/
#include <math.h>
#include <stdlib.h>
#include <stdio.h>
#include <stdbool.h>
#include <string.h>
#include <sys/types.h>
#include <fcntl.h>
#include <unistd.h>
#include <systemlib/err.h>
#include <lib/parameters/param.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/tasks.h>
#include "vmount_params.h"
#include "input_mavlink.h"
#include "input_rc.h"
#include "input_test.h"
@ -66,105 +60,30 @@
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/topics/parameter_update.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/atomic.h>
using namespace time_literals;
using namespace vmount;
/* thread state */
static volatile bool thread_should_exit = false;
static volatile bool thread_running = false;
static px4::atomic<bool> thread_should_exit {false};
static px4::atomic<bool> thread_running {false};
static constexpr int input_objs_len_max = 3;
struct ThreadData {
InputBase *input_objs[input_objs_len_max] = {nullptr, nullptr, nullptr};
int input_objs_len = 0;
int last_input_active = -1;
OutputBase *output_obj = nullptr;
InputTest *test_input = nullptr;
};
static volatile ThreadData *g_thread_data = nullptr;
struct Parameters {
int32_t mnt_mode_in;
int32_t mnt_mode_out;
int32_t mnt_mav_sys_id_v1;
int32_t mnt_mav_comp_id_v1;
float mnt_ob_lock_mode;
float mnt_ob_norm_mode;
int32_t mnt_man_pitch;
int32_t mnt_man_roll;
int32_t mnt_man_yaw;
int32_t mnt_do_stab;
float mnt_range_pitch;
float mnt_range_roll;
float mnt_range_yaw;
float mnt_off_pitch;
float mnt_off_roll;
float mnt_off_yaw;
int32_t mav_sys_id;
int32_t mav_comp_id;
float mnt_rate_pitch;
float mnt_rate_yaw;
int32_t mnt_rc_in_mode;
bool operator!=(const Parameters &p)
{
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wfloat-equal"
return mnt_mode_in != p.mnt_mode_in ||
mnt_mode_out != p.mnt_mode_out ||
mnt_mav_sys_id_v1 != p.mnt_mav_sys_id_v1 ||
mnt_mav_comp_id_v1 != p.mnt_mav_comp_id_v1 ||
fabsf(mnt_ob_lock_mode - p.mnt_ob_lock_mode) > 1e-6f ||
fabsf(mnt_ob_norm_mode - p.mnt_ob_norm_mode) > 1e-6f ||
mnt_man_pitch != p.mnt_man_pitch ||
mnt_man_roll != p.mnt_man_roll ||
mnt_man_yaw != p.mnt_man_yaw ||
mnt_do_stab != p.mnt_do_stab ||
mnt_range_pitch != p.mnt_range_pitch ||
mnt_range_roll != p.mnt_range_roll ||
mnt_range_yaw != p.mnt_range_yaw ||
mnt_off_pitch != p.mnt_off_pitch ||
mnt_off_roll != p.mnt_off_roll ||
mnt_off_yaw != p.mnt_off_yaw ||
mav_sys_id != p.mav_sys_id ||
mav_comp_id != p.mav_comp_id ||
mnt_rc_in_mode != p.mnt_rc_in_mode;
#pragma GCC diagnostic pop
}
};
struct ParameterHandles {
param_t mnt_mode_in;
param_t mnt_mode_out;
param_t mnt_mav_sys_id_v1;
param_t mnt_mav_comp_id_v1;
param_t mnt_ob_lock_mode;
param_t mnt_ob_norm_mode;
param_t mnt_man_pitch;
param_t mnt_man_roll;
param_t mnt_man_yaw;
param_t mnt_do_stab;
param_t mnt_range_pitch;
param_t mnt_range_roll;
param_t mnt_range_yaw;
param_t mnt_off_pitch;
param_t mnt_off_roll;
param_t mnt_off_yaw;
param_t mav_sys_id;
param_t mav_comp_id;
param_t mnt_rate_pitch;
param_t mnt_rate_yaw;
param_t mnt_rc_in_mode;
};
static ThreadData *g_thread_data = nullptr;
/* functions */
static void usage();
static void update_params(ParameterHandles &param_handles, Parameters &params, bool &got_changes);
static bool get_params(ParameterHandles &param_handles, Parameters &params);
static void update_params(ParameterHandles &param_handles, Parameters &params);
static bool initialize_params(ParameterHandles &param_handles, Parameters &params);
static int vmount_thread_main(int argc, char *argv[]);
extern "C" __EXPORT int vmount_main(int argc, char *argv[]);
@ -173,282 +92,199 @@ static int vmount_thread_main(int argc, char *argv[])
{
ParameterHandles param_handles;
Parameters params {};
OutputConfig output_config;
ThreadData thread_data;
InputTest *test_input = nullptr;
// We don't need the task name.
argc -= 1;
argv += 1;
if (argc > 0 && !strcmp(argv[0], "test")) {
PX4_INFO("Starting in test mode");
const char *axis_names[3] = {"roll", "pitch", "yaw"};
float angles[3] = { 0.f, 0.f, 0.f };
if (argc == 3) {
bool found_axis = false;
for (int i = 0 ; i < 3; ++i) {
if (!strcmp(argv[1], axis_names[i])) {
long angle_deg = strtol(argv[2], nullptr, 0);
angles[i] = (float)angle_deg;
found_axis = true;
}
}
if (!found_axis) {
usage();
return -1;
}
test_input = new InputTest(angles[0], angles[1], angles[2]);
if (!test_input) {
PX4_ERR("memory allocation failed");
return -1;
}
} else {
usage();
return -1;
}
}
if (!get_params(param_handles, params)) {
if (!initialize_params(param_handles, params)) {
PX4_ERR("could not get mount parameters!");
delete test_input;
delete g_thread_data->test_input;
return -1;
}
uORB::SubscriptionInterval parameter_update_sub{ORB_ID(parameter_update), 1_s};
thread_running = true;
ControlData *control_data = nullptr;
thread_running.store(true);
ControlData control_data {};
g_thread_data = &thread_data;
int last_active = -1;
thread_data.test_input = new InputTest(params);
while (!thread_should_exit) {
bool alloc_failed = false;
if (!thread_data.input_objs[0] && (params.mnt_mode_in >= 0 || test_input)) { //need to initialize
thread_data.input_objs[thread_data.input_objs_len++] = thread_data.test_input;
output_config.gimbal_normal_mode_value = params.mnt_ob_norm_mode;
output_config.gimbal_retracted_mode_value = params.mnt_ob_lock_mode;
output_config.pitch_scale = 1.0f / (math::radians(params.mnt_range_pitch / 2.0f));
output_config.roll_scale = 1.0f / (math::radians(params.mnt_range_roll / 2.0f));
output_config.yaw_scale = 1.0f / (math::radians(params.mnt_range_yaw / 2.0f));
output_config.pitch_offset = math::radians(params.mnt_off_pitch);
output_config.roll_offset = math::radians(params.mnt_off_roll);
output_config.yaw_offset = math::radians(params.mnt_off_yaw);
output_config.mavlink_sys_id_v1 = params.mnt_mav_sys_id_v1;
output_config.mavlink_comp_id_v1 = params.mnt_mav_comp_id_v1;
switch (params.mnt_mode_in) {
case 0:
// Automatic
// MAVLINK_V2 as well as RC input are supported together.
// Whichever signal is updated last, gets control, for RC there is a deadzone
// to avoid accidental activation.
thread_data.input_objs[thread_data.input_objs_len++] = new InputMavlinkGimbalV2(params);
bool alloc_failed = false;
thread_data.input_objs_len = 1;
thread_data.input_objs[thread_data.input_objs_len++] = new InputRC(params);
break;
if (test_input) {
thread_data.input_objs[0] = test_input;
case 1: // RC only
thread_data.input_objs[thread_data.input_objs_len++] = new InputRC(params);
break;
} else {
switch (params.mnt_mode_in) {
case 0:
case 2: // MAVLINK_ROI commands only (to be deprecated)
thread_data.input_objs[thread_data.input_objs_len++] = new InputMavlinkROI(params);
break;
// Automatic
thread_data.input_objs[0] = new InputMavlinkCmdMount();
thread_data.input_objs[1] = new InputMavlinkROI();
case 3: // MAVLINK_DO_MOUNT commands only (to be deprecated)
thread_data.input_objs[thread_data.input_objs_len++] = new InputMavlinkCmdMount(params);
break;
// RC is on purpose last here so that if there are any mavlink
// messages, they will take precedence over RC.
// This logic is done further below while update() is called.
thread_data.input_objs[2] = new InputRC(params.mnt_man_roll,
params.mnt_man_pitch,
params.mnt_man_yaw,
params.mnt_rate_pitch,
params.mnt_rate_yaw,
params.mnt_rc_in_mode);
thread_data.input_objs_len = 3;
case 4: //MAVLINK_V2
thread_data.input_objs[thread_data.input_objs_len++] = new InputMavlinkGimbalV2(params);
break;
break;
default:
PX4_ERR("invalid input mode %" PRId32, params.mnt_mode_in);
break;
}
case 1: //RC
thread_data.input_objs[0] = new InputRC(params.mnt_man_roll,
params.mnt_man_pitch,
params.mnt_man_yaw,
params.mnt_rate_pitch,
params.mnt_rate_yaw,
params.mnt_rc_in_mode);
break;
for (int i = 0; i < thread_data.input_objs_len; ++i) {
if (!thread_data.input_objs[i]) {
alloc_failed = true;
}
}
case 2: //MAVLINK_ROI
thread_data.input_objs[0] = new InputMavlinkROI();
break;
if (alloc_failed) {
PX4_ERR("input objs memory allocation failed");
thread_should_exit.store(true);
}
case 3: //MAVLINK_DO_MOUNT
thread_data.input_objs[0] = new InputMavlinkCmdMount();
break;
case 4: //MAVLINK_V2
thread_data.input_objs[0] = new InputMavlinkGimbalV2(
params.mav_sys_id,
params.mav_comp_id,
params.mnt_rate_pitch,
params.mnt_rate_yaw);
break;
default:
PX4_ERR("invalid input mode %" PRId32, params.mnt_mode_in);
break;
}
}
for (int i = 0; i < thread_data.input_objs_len; ++i) {
if (!thread_data.input_objs[i]) {
alloc_failed = true;
}
}
switch (params.mnt_mode_out) {
case 0: //AUX
thread_data.output_obj = new OutputRC(output_config);
if (!thread_data.output_obj) { alloc_failed = true; }
break;
case 1: //MAVLink v1 gimbal protocol
thread_data.output_obj = new OutputMavlinkV1(output_config);
if (!thread_data.output_obj) { alloc_failed = true; }
break;
case 2: //MAVLink v2 gimbal protocol
thread_data.output_obj = new OutputMavlinkV2(params.mav_sys_id, params.mav_comp_id, output_config);
if (!thread_data.output_obj) { alloc_failed = true; }
break;
default:
PX4_ERR("invalid output mode %" PRId32, params.mnt_mode_out);
thread_should_exit = true;
break;
}
if (alloc_failed) {
thread_data.input_objs_len = 0;
PX4_ERR("memory allocation failed");
thread_should_exit = true;
}
if (thread_should_exit) {
break;
}
int ret = thread_data.output_obj->initialize();
if (ret) {
PX4_ERR("failed to initialize output mode (%i)", ret);
thread_should_exit = true;
break;
if (!alloc_failed) {
for (int i = 0; i < thread_data.input_objs_len; ++i) {
if (thread_data.input_objs[i]->initialize() != 0) {
PX4_ERR("Input %d failed", i);
thread_should_exit.store(true);
}
}
}
switch (params.mnt_mode_out) {
case 0: //AUX
thread_data.output_obj = new OutputRC(params);
if (!thread_data.output_obj) { alloc_failed = true; }
break;
case 1: //MAVLink gimbal v1 protocol
thread_data.output_obj = new OutputMavlinkV1(params);
if (!thread_data.output_obj) { alloc_failed = true; }
break;
case 2: //MAVLink gimbal v2 protocol
thread_data.output_obj = new OutputMavlinkV2(params);
if (!thread_data.output_obj) { alloc_failed = true; }
break;
default:
PX4_ERR("invalid output mode %" PRId32, params.mnt_mode_out);
thread_should_exit.store(true);
break;
}
if (alloc_failed) {
PX4_ERR("output memory allocation failed");
thread_should_exit.store(true);
}
while (!thread_should_exit.load()) {
const bool updated = parameter_update_sub.updated();
if (updated) {
parameter_update_s pupdate;
parameter_update_sub.copy(&pupdate);
update_params(param_handles, params);
}
if (thread_data.last_input_active == -1) {
// Reset control as no one is active anymore, or yet.
control_data.sysid_primary_control = 0;
control_data.compid_primary_control = 0;
}
InputBase::UpdateResult update_result = InputBase::UpdateResult::NoUpdate;
if (thread_data.input_objs_len > 0) {
//get input: we cannot make the timeout too large, because the output needs to update
//periodically for stabilization and angle updates.
// get input: we cannot make the timeout too large, because the output needs to update
// periodically for stabilization and angle updates.
for (int i = 0; i < thread_data.input_objs_len; ++i) {
if (params.mnt_do_stab == 1) {
thread_data.input_objs[i]->set_stabilize(true, true, true);
const bool already_active = (thread_data.last_input_active == i);
const unsigned int poll_timeout = already_active ? 20 : 0; // poll only on active input to reduce latency
} else if (params.mnt_do_stab == 2) {
thread_data.input_objs[i]->set_stabilize(false, false, true);
update_result = thread_data.input_objs[i]->update(poll_timeout, control_data, already_active);
} else {
thread_data.input_objs[i]->set_stabilize(false, false, false);
}
switch (update_result) {
case InputBase::UpdateResult::NoUpdate:
if (already_active) {
// No longer active.
thread_data.last_input_active = -1;
}
break;
bool already_active = (last_active == i);
case InputBase::UpdateResult::UpdatedActive:
thread_data.last_input_active = i;
break;
ControlData *control_data_to_check = nullptr;
unsigned int poll_timeout = already_active ? 20 : 0; // poll only on active input to reduce latency
int ret = thread_data.input_objs[i]->update(poll_timeout, &control_data_to_check, already_active);
case InputBase::UpdateResult::UpdatedActiveOnce:
thread_data.last_input_active = -1;
break;
if (ret) {
PX4_ERR("failed to read input %i (ret: %i)", i, ret);
continue;
}
case InputBase::UpdateResult::UpdatedNotActive:
if (already_active) {
// No longer active
thread_data.last_input_active = -1;
}
if (control_data_to_check != nullptr || already_active) {
control_data = control_data_to_check;
last_active = i;
break;
}
}
//update output
int ret = thread_data.output_obj->update(control_data);
if (params.mnt_do_stab == 1) {
thread_data.output_obj->set_stabilize(true, true, true);
if (ret) {
PX4_ERR("failed to write output (%i)", ret);
break;
} else if (params.mnt_do_stab == 2) {
thread_data.output_obj->set_stabilize(false, false, true);
} else {
thread_data.output_obj->set_stabilize(false, false, false);
}
// Update output
thread_data.output_obj->update(
control_data,
update_result != InputBase::UpdateResult::NoUpdate);
// Only publish the mount orientation if the mode is not mavlink v1 or v2
// If the gimbal speaks mavlink it publishes its own orientation.
if (params.mnt_mode_out != 1 && params.mnt_mode_out != 2) { // 1 = MAVLink v1, 2 = MAVLink v2
thread_data.output_obj->publish();
}
} else {
//wait for parameter changes. We still need to wake up regularily to check for thread exit requests
// We still need to wake up regularly to check for thread exit requests
px4_usleep(1e6);
}
if (test_input && test_input->finished()) {
thread_should_exit = true;
break;
}
// check for parameter updates
if (parameter_update_sub.updated()) {
// clear update
parameter_update_s pupdate;
parameter_update_sub.copy(&pupdate);
// update parameters from storage
bool updated = false;
update_params(param_handles, params, updated);
if (updated) {
//re-init objects
for (int i = 0; i < input_objs_len_max; ++i) {
if (thread_data.input_objs[i]) {
delete (thread_data.input_objs[i]);
thread_data.input_objs[i] = nullptr;
}
}
thread_data.input_objs_len = 0;
last_active = -1;
if (thread_data.output_obj) {
delete (thread_data.output_obj);
thread_data.output_obj = nullptr;
}
}
}
}
g_thread_data = nullptr;
delete thread_data.test_input;
thread_data.test_input = nullptr;
for (int i = 0; i < input_objs_len_max; ++i) {
if (thread_data.input_objs[i]) {
delete (thread_data.input_objs[i]);
@ -463,14 +299,10 @@ static int vmount_thread_main(int argc, char *argv[])
thread_data.output_obj = nullptr;
}
thread_running = false;
thread_running.store(false);
return 0;
}
/**
* The main command function.
* Processes command line arguments and starts the daemon.
*/
int vmount_main(int argc, char *argv[])
{
if (argc < 2) {
@ -479,34 +311,23 @@ int vmount_main(int argc, char *argv[])
return -1;
}
const bool found_start = !strcmp(argv[1], "start");
const bool found_test = !strcmp(argv[1], "test");
if (!strcmp(argv[1], "start")) {
if (found_start || found_test) {
/* this is not an error */
if (thread_running) {
if (found_start) {
PX4_WARN("mount driver already running");
return 0;
} else {
PX4_WARN("mount driver already running, run vmount stop before 'vmount test'");
return 1;
}
if (thread_running.load()) {
PX4_WARN("mount driver already running");
return 1;
}
thread_should_exit = false;
int vmount_task = px4_task_spawn_cmd("vmount",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
2100,
vmount_thread_main,
(char *const *)argv + 1);
nullptr);
int counter = 0;
while (!thread_running && vmount_task >= 0) {
while (!thread_running.load() && vmount_task >= 0) {
px4_usleep(5000);
if (++counter >= 100) {
@ -519,35 +340,83 @@ int vmount_main(int argc, char *argv[])
return -1;
}
return counter < 100 || thread_should_exit ? 0 : -1;
return counter < 100 || thread_should_exit.load() ? 0 : -1;
}
if (!strcmp(argv[1], "stop")) {
else if (!strcmp(argv[1], "stop")) {
/* this is not an error */
if (!thread_running) {
if (!thread_running.load()) {
PX4_WARN("mount driver not running");
return 0;
}
thread_should_exit = true;
thread_should_exit.store(true);
while (thread_running) {
while (thread_running.load()) {
px4_usleep(100000);
}
return 0;
}
if (!strcmp(argv[1], "status")) {
if (thread_running && g_thread_data) {
else if (!strcmp(argv[1], "test")) {
for (int i = 0; i < g_thread_data->input_objs_len; ++i) {
g_thread_data->input_objs[i]->print_status();
if (thread_running.load() && g_thread_data && g_thread_data->test_input) {
if (argc >= 4) {
bool found_axis = false;
const char *axis_names[3] = {"roll", "pitch", "yaw"};
int angles[3] = { 0, 0, 0 };
for (int arg_i = 2 ; arg_i < (argc - 1); ++arg_i) {
for (int axis_i = 0; axis_i < 3; ++axis_i) {
if (!strcmp(argv[arg_i], axis_names[axis_i])) {
int angle_deg = (int)strtol(argv[arg_i + 1], nullptr, 0);
angles[axis_i] = angle_deg;
found_axis = true;
}
}
}
if (!found_axis) {
usage();
return -1;
}
g_thread_data->test_input->set_test_input(angles[0], angles[1], angles[2]);
return 0;
}
} else {
PX4_WARN("not running");
usage();
return 1;
}
}
else if (!strcmp(argv[1], "status")) {
if (thread_running.load() && g_thread_data && g_thread_data->test_input) {
if (g_thread_data->input_objs_len == 0) {
PX4_INFO("Input: None");
} else {
PX4_INFO("Input Selected");
for (int i = 0; i < g_thread_data->input_objs_len; ++i) {
if (i == g_thread_data->last_input_active) {
g_thread_data->input_objs[i]->print_status();
}
}
PX4_INFO("Input not selected");
for (int i = 0; i < g_thread_data->input_objs_len; ++i) {
if (i != g_thread_data->last_input_active) {
g_thread_data->input_objs[i]->print_status();
}
}
}
if (g_thread_data->output_obj) {
@ -569,13 +438,12 @@ int vmount_main(int argc, char *argv[])
return -1;
}
void update_params(ParameterHandles &param_handles, Parameters &params, bool &got_changes)
void update_params(ParameterHandles &param_handles, Parameters &params)
{
Parameters prev_params = params;
param_get(param_handles.mnt_mode_in, &params.mnt_mode_in);
param_get(param_handles.mnt_mode_out, &params.mnt_mode_out);
param_get(param_handles.mnt_mav_sys_id_v1, &params.mnt_mav_sys_id_v1);
param_get(param_handles.mnt_mav_comp_id_v1, &params.mnt_mav_comp_id_v1);
param_get(param_handles.mnt_mav_sysid_v1, &params.mnt_mav_sysid_v1);
param_get(param_handles.mnt_mav_compid_v1, &params.mnt_mav_compid_v1);
param_get(param_handles.mnt_ob_lock_mode, &params.mnt_ob_lock_mode);
param_get(param_handles.mnt_ob_norm_mode, &params.mnt_ob_norm_mode);
param_get(param_handles.mnt_man_pitch, &params.mnt_man_pitch);
@ -588,21 +456,19 @@ void update_params(ParameterHandles &param_handles, Parameters &params, bool &go
param_get(param_handles.mnt_off_pitch, &params.mnt_off_pitch);
param_get(param_handles.mnt_off_roll, &params.mnt_off_roll);
param_get(param_handles.mnt_off_yaw, &params.mnt_off_yaw);
param_get(param_handles.mav_sys_id, &params.mav_sys_id);
param_get(param_handles.mav_comp_id, &params.mav_comp_id);
param_get(param_handles.mav_sysid, &params.mav_sysid);
param_get(param_handles.mav_compid, &params.mav_compid);
param_get(param_handles.mnt_rate_pitch, &params.mnt_rate_pitch);
param_get(param_handles.mnt_rate_yaw, &params.mnt_rate_yaw);
param_get(param_handles.mnt_rc_in_mode, &params.mnt_rc_in_mode);
got_changes = prev_params != params;
}
bool get_params(ParameterHandles &param_handles, Parameters &params)
bool initialize_params(ParameterHandles &param_handles, Parameters &params)
{
param_handles.mnt_mode_in = param_find("MNT_MODE_IN");
param_handles.mnt_mode_out = param_find("MNT_MODE_OUT");
param_handles.mnt_mav_sys_id_v1 = param_find("MNT_MAV_SYSID");
param_handles.mnt_mav_comp_id_v1 = param_find("MNT_MAV_COMPID");
param_handles.mnt_mav_sysid_v1 = param_find("MNT_MAV_SYSID");
param_handles.mnt_mav_compid_v1 = param_find("MNT_MAV_COMPID");
param_handles.mnt_ob_lock_mode = param_find("MNT_OB_LOCK_MODE");
param_handles.mnt_ob_norm_mode = param_find("MNT_OB_NORM_MODE");
param_handles.mnt_man_pitch = param_find("MNT_MAN_PITCH");
@ -615,16 +481,16 @@ bool get_params(ParameterHandles &param_handles, Parameters &params)
param_handles.mnt_off_pitch = param_find("MNT_OFF_PITCH");
param_handles.mnt_off_roll = param_find("MNT_OFF_ROLL");
param_handles.mnt_off_yaw = param_find("MNT_OFF_YAW");
param_handles.mav_sys_id = param_find("MAV_SYS_ID");
param_handles.mav_comp_id = param_find("MAV_COMP_ID");
param_handles.mav_sysid = param_find("MAV_SYS_ID");
param_handles.mav_compid = param_find("MAV_COMP_ID");
param_handles.mnt_rate_pitch = param_find("MNT_RATE_PITCH");
param_handles.mnt_rate_yaw = param_find("MNT_RATE_YAW");
param_handles.mnt_rc_in_mode = param_find("MNT_RC_IN_MODE");
if (param_handles.mnt_mode_in == PARAM_INVALID ||
param_handles.mnt_mode_out == PARAM_INVALID ||
param_handles.mnt_mav_sys_id_v1 == PARAM_INVALID ||
param_handles.mnt_mav_comp_id_v1 == PARAM_INVALID ||
param_handles.mnt_mav_sysid_v1 == PARAM_INVALID ||
param_handles.mnt_mav_compid_v1 == PARAM_INVALID ||
param_handles.mnt_ob_lock_mode == PARAM_INVALID ||
param_handles.mnt_ob_norm_mode == PARAM_INVALID ||
param_handles.mnt_man_pitch == PARAM_INVALID ||
@ -637,8 +503,8 @@ bool get_params(ParameterHandles &param_handles, Parameters &params)
param_handles.mnt_off_pitch == PARAM_INVALID ||
param_handles.mnt_off_roll == PARAM_INVALID ||
param_handles.mnt_off_yaw == PARAM_INVALID ||
param_handles.mav_sys_id == PARAM_INVALID ||
param_handles.mav_comp_id == PARAM_INVALID ||
param_handles.mav_sysid == PARAM_INVALID ||
param_handles.mav_compid == PARAM_INVALID ||
param_handles.mnt_rate_pitch == PARAM_INVALID ||
param_handles.mnt_rate_yaw == PARAM_INVALID ||
param_handles.mnt_rc_in_mode == PARAM_INVALID
@ -646,8 +512,7 @@ bool get_params(ParameterHandles &param_handles, Parameters &params)
return false;
}
bool dummy;
update_params(param_handles, params, dummy);
update_params(param_handles, params);
return true;
}
@ -656,25 +521,19 @@ static void usage()
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(
### Description
Mount (Gimbal) control driver. It maps several different input methods (eg. RC or MAVLink) to a configured
Mount/gimbal Gimbal control driver. It maps several different input methods (eg. RC or MAVLink) to a configured
output (eg. AUX channels or MAVLink).
Documentation how to use it is on the [gimbal_control](https://dev.px4.io/master/en/advanced/gimbal_control.html) page.
### Implementation
Each method is implemented in its own class, and there is a common base class for inputs and outputs.
They are connected via an API, defined by the `ControlData` data structure. This makes sure that each input method
can be used with each output method and new inputs/outputs can be added with minimal effort.
Documentation how to use it is on the [gimbal_control](https://docs.px4.io/master/en/advanced/gimbal_control.html) page.
### Examples
Test the output by setting a fixed yaw angle (and the other axes to 0):
$ vmount stop
$ vmount test yaw 30
Test the output by setting a angles (all omitted axes are set to 0):
$ vmount test pitch -45 yaw 30
)DESCR_STR");
PRINT_MODULE_USAGE_NAME("vmount", "driver");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_COMMAND_DESCR("test", "Test the output: set a fixed angle for one axis (vmount must not be running)");
PRINT_MODULE_USAGE_COMMAND_DESCR("test", "Test the output: set a fixed angle for one or multiple axes (vmount must be running)");
PRINT_MODULE_USAGE_ARG("roll|pitch|yaw <angle>", "Specify an axis and an angle in degrees", false);
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
}

View File

@ -31,25 +31,20 @@
*
****************************************************************************/
/**
* @file vmount_params.c
* @author Leon Müller (thedevleon)
* @author Matthew Edwards (mje-nz)
*
*/
/**
* Mount input mode
*
* RC uses the AUX input channels (see MNT_MAN_* parameters),
* MAVLINK_ROI uses the MAV_CMD_DO_SET_ROI Mavlink message, and MAVLINK_DO_MOUNT the
* MAV_CMD_DO_MOUNT_CONFIGURE and MAV_CMD_DO_MOUNT_CONTROL messages to control a mount.
* This is the protocol used between the ground station and the autopilot.
*
* Recommended is Auto, RC only or MAVLink gimbal protocol v2.
* The rest will be deprecated.
*
* @value -1 DISABLED
* @value 0 AUTO
* @value 0 Auto (RC and MAVLink gimbal protocol v2)
* @value 1 RC
* @value 2 MAVLINK_ROI (protocol v1)
* @value 3 MAVLINK_DO_MOUNT (protocol v1)
* @value 2 MAVLINK_ROI (protocol v1, to be deprecated)
* @value 3 MAVLINK_DO_MOUNT (protocol v1, to be deprecated)
* @value 4 MAVlink gimbal protocol v2
* @min -1
* @max 4
@ -61,9 +56,9 @@ PARAM_DEFINE_INT32(MNT_MODE_IN, -1);
/**
* Mount output mode
*
* AUX uses the mixer output Control Group #2.
* MAVLINK uses the MAV_CMD_DO_MOUNT_CONFIGURE and MAV_CMD_DO_MOUNT_CONTROL MavLink messages
* to control a mount (set MNT_MAV_SYSID & MNT_MAV_COMPID)
* This is the protocol used between the autopilot and a connected gimbal.
*
* Recommended is the MAVLink gimbal protocol v2 if the gimbal supports it.
*
* @value 0 AUX
* @value 1 MAVLink gimbal protocol v1
@ -71,6 +66,7 @@ PARAM_DEFINE_INT32(MNT_MODE_IN, -1);
* @min 0
* @max 2
* @group Mount
* @reboot_required true
*/
PARAM_DEFINE_INT32(MNT_MODE_OUT, 0);

View File

@ -0,0 +1,91 @@
/****************************************************************************
*
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include <stdint.h>
#include <lib/parameters/param.h>
namespace vmount
{
struct Parameters {
int32_t mnt_mode_in;
int32_t mnt_mode_out;
int32_t mnt_mav_sysid_v1;
int32_t mnt_mav_compid_v1;
float mnt_ob_lock_mode;
float mnt_ob_norm_mode;
int32_t mnt_man_pitch;
int32_t mnt_man_roll;
int32_t mnt_man_yaw;
int32_t mnt_do_stab;
float mnt_range_pitch;
float mnt_range_roll;
float mnt_range_yaw;
float mnt_off_pitch;
float mnt_off_roll;
float mnt_off_yaw;
int32_t mav_sysid;
int32_t mav_compid;
float mnt_rate_pitch;
float mnt_rate_yaw;
int32_t mnt_rc_in_mode;
};
struct ParameterHandles {
param_t mnt_mode_in;
param_t mnt_mode_out;
param_t mnt_mav_sysid_v1;
param_t mnt_mav_compid_v1;
param_t mnt_ob_lock_mode;
param_t mnt_ob_norm_mode;
param_t mnt_man_pitch;
param_t mnt_man_roll;
param_t mnt_man_yaw;
param_t mnt_do_stab;
param_t mnt_range_pitch;
param_t mnt_range_roll;
param_t mnt_range_yaw;
param_t mnt_off_pitch;
param_t mnt_off_roll;
param_t mnt_off_yaw;
param_t mav_sysid;
param_t mav_compid;
param_t mnt_rate_pitch;
param_t mnt_rate_yaw;
param_t mnt_rc_in_mode;
};
} /* namespace vmount */