forked from Archive/PX4-Autopilot
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:
parent
f2216dff55
commit
39f0e97245
|
@ -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
|
||||
|
|
|
@ -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) {
|
||||
|
|
|
@ -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;
|
||||
|
||||
|
|
|
@ -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 */
|
||||
|
|
|
@ -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 ¶meters) :
|
||||
_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 */
|
||||
|
|
|
@ -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 ¶meters);
|
||||
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
|
@ -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 ¶meters);
|
||||
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 ¶meters);
|
||||
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 ¶meters);
|
||||
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;
|
||||
|
|
|
@ -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 ¶meters) :
|
||||
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 */
|
||||
|
|
|
@ -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 ¶meters);
|
||||
|
||||
/**
|
||||
* @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 */
|
||||
|
|
|
@ -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 ¶meters) :
|
||||
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 */
|
||||
|
|
|
@ -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 ¶meters);
|
||||
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};
|
||||
};
|
||||
|
||||
|
||||
|
|
|
@ -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 ¶meters)
|
||||
: _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 */
|
||||
|
||||
|
|
|
@ -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 ¶meters);
|
||||
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)};
|
||||
};
|
||||
|
|
|
@ -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 ¶meters)
|
||||
: 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 ¶meters)
|
||||
: 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];
|
||||
|
|
|
@ -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 ¶meters);
|
||||
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 ¶meters);
|
||||
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};
|
||||
|
|
|
@ -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 ¶meters)
|
||||
: 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");
|
||||
}
|
||||
|
|
|
@ -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 ¶meters);
|
||||
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 */
|
||||
|
|
|
@ -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 ¶m_handles, Parameters ¶ms, bool &got_changes);
|
||||
static bool get_params(ParameterHandles ¶m_handles, Parameters ¶ms);
|
||||
static void update_params(ParameterHandles ¶m_handles, Parameters ¶ms);
|
||||
static bool initialize_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[]);
|
||||
|
@ -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 ¶m_handles, Parameters ¶ms, bool &got_changes)
|
||||
void update_params(ParameterHandles ¶m_handles, Parameters ¶ms)
|
||||
{
|
||||
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_sys_id_v1, ¶ms.mnt_mav_sys_id_v1);
|
||||
param_get(param_handles.mnt_mav_comp_id_v1, ¶ms.mnt_mav_comp_id_v1);
|
||||
param_get(param_handles.mnt_mav_sysid_v1, ¶ms.mnt_mav_sysid_v1);
|
||||
param_get(param_handles.mnt_mav_compid_v1, ¶ms.mnt_mav_compid_v1);
|
||||
param_get(param_handles.mnt_ob_lock_mode, ¶ms.mnt_ob_lock_mode);
|
||||
param_get(param_handles.mnt_ob_norm_mode, ¶ms.mnt_ob_norm_mode);
|
||||
param_get(param_handles.mnt_man_pitch, ¶ms.mnt_man_pitch);
|
||||
|
@ -588,21 +456,19 @@ void update_params(ParameterHandles ¶m_handles, Parameters ¶ms, bool &go
|
|||
param_get(param_handles.mnt_off_pitch, ¶ms.mnt_off_pitch);
|
||||
param_get(param_handles.mnt_off_roll, ¶ms.mnt_off_roll);
|
||||
param_get(param_handles.mnt_off_yaw, ¶ms.mnt_off_yaw);
|
||||
param_get(param_handles.mav_sys_id, ¶ms.mav_sys_id);
|
||||
param_get(param_handles.mav_comp_id, ¶ms.mav_comp_id);
|
||||
param_get(param_handles.mav_sysid, ¶ms.mav_sysid);
|
||||
param_get(param_handles.mav_compid, ¶ms.mav_compid);
|
||||
param_get(param_handles.mnt_rate_pitch, ¶ms.mnt_rate_pitch);
|
||||
param_get(param_handles.mnt_rate_yaw, ¶ms.mnt_rate_yaw);
|
||||
param_get(param_handles.mnt_rc_in_mode, ¶ms.mnt_rc_in_mode);
|
||||
|
||||
got_changes = prev_params != params;
|
||||
}
|
||||
|
||||
bool get_params(ParameterHandles ¶m_handles, Parameters ¶ms)
|
||||
bool initialize_params(ParameterHandles ¶m_handles, Parameters ¶ms)
|
||||
{
|
||||
param_handles.mnt_mode_in = param_find("MNT_MODE_IN");
|
||||
param_handles.mnt_mode_out = param_find("MNT_MODE_OUT");
|
||||
param_handles.mnt_mav_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 ¶m_handles, Parameters ¶ms)
|
|||
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 ¶m_handles, Parameters ¶ms)
|
|||
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 ¶m_handles, Parameters ¶ms)
|
|||
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();
|
||||
}
|
||||
|
|
|
@ -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);
|
||||
|
||||
|
|
|
@ -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 */
|
Loading…
Reference in New Issue