zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

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.
master
Julian Oes 3 years ago committed by Daniel Agar
parent
f2216dff55
commit
39f0e97245
21 changed files with 1234 additions and 1369 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      msg/vehicle_command.msg
  2. 24
      src/modules/mavlink/mavlink_main.cpp
  3. 1
      src/modules/mavlink/mavlink_main.h
  4. 53
      src/modules/vmount/common.h
  5. 55
      src/modules/vmount/input.cpp
  6. 61
      src/modules/vmount/input.h
  7. 1030
      src/modules/vmount/input_mavlink.cpp
  8. 86
      src/modules/vmount/input_mavlink.h
  9. 98
      src/modules/vmount/input_rc.cpp
  10. 47
      src/modules/vmount/input_rc.h
  11. 75
      src/modules/vmount/input_test.cpp
  12. 42
      src/modules/vmount/input_test.h
  13. 71
      src/modules/vmount/output.cpp
  14. 61
      src/modules/vmount/output.h
  15. 90
      src/modules/vmount/output_mavlink.cpp
  16. 27
      src/modules/vmount/output_mavlink.h
  17. 45
      src/modules/vmount/output_rc.cpp
  18. 21
      src/modules/vmount/output_rc.h
  19. 597
      src/modules/vmount/vmount.cpp
  20. 26
      src/modules/vmount/vmount_params.c
  21. 91
      src/modules/vmount/vmount_params.h

2
msg/vehicle_command.msg
Unescape View File

@ -175,3 +175,5 @@ uint8 source_system # System sending the command @@ -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

24
src/modules/mavlink/mavlink_main.cpp
Unescape View File

@ -2394,6 +2394,30 @@ Mavlink::task_main(int argc, char *argv[]) @@ -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) {

1
src/modules/mavlink/mavlink_main.h
Unescape View File

@ -544,6 +544,7 @@ private: @@ -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;

53
src/modules/vmount/common.h
Unescape View File

@ -31,11 +31,6 @@ @@ -31,11 +31,6 @@
*
****************************************************************************/
/**
* @file common.h
* @author Beat Küng <beat-kueng@gmx.net>
*
*/
#pragma once
@ -52,44 +47,44 @@ namespace vmount @@ -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 */

55
src/modules/vmount/input.cpp
Unescape View File

@ -1,6 +1,6 @@ @@ -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 @@ @@ -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();
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;
}
InputBase::InputBase(Parameters &parameters) :
_parameters(parameters)
{}
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 */

61
src/modules/vmount/input.h
Unescape View File

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

1030
src/modules/vmount/input_mavlink.cpp
View File

File diff suppressed because it is too large Load Diff

86
src/modules/vmount/input_mavlink.h
Unescape View File

@ -1,6 +1,6 @@ @@ -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 @@ @@ -31,11 +31,6 @@
*
****************************************************************************/
/**
* @file input_mavlink.h
* @author Beat Küng <beat-kueng@gmx.net>
*
*/
#pragma once
@ -48,32 +43,30 @@ @@ -48,32 +43,30 @@
#include <uORB/topics/gimbal_device_attitude_status.h>
#include <uORB/topics/gimbal_manager_information.h>
#include <uORB/topics/gimbal_manager_status.h>
#include <uORB/topics/gimbal_manager_set_attitude.h>
#include <uORB/topics/gimbal_manager_set_manual_control.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_roi.h>
#include <uORB/topics/vehicle_global_position.h>
#include <lib/geo/geo.h>
namespace vmount
{
/**
** class InputMavlinkROI
** Input based on the vehicle_roi topic
*/
class InputMavlinkROI : public InputBase
{
public:
InputMavlinkROI() = default;
InputMavlinkROI() = delete;
explicit InputMavlinkROI(Parameters &parameters);
virtual ~InputMavlinkROI();
virtual void print_status();
protected:
virtual int update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active);
virtual int initialize();
void print_status() const override;
UpdateResult update(unsigned int timeout_ms, ControlData &control_data, bool already_active) override;
int initialize() override;
private:
void _read_control_data_from_position_setpoint_sub();
void _read_control_data_from_position_setpoint_sub(ControlData &control_data);
int _vehicle_roi_sub = -1;
int _position_setpoint_triplet_sub = -1;
@ -81,50 +74,49 @@ private: @@ -81,50 +74,49 @@ private:
};
/**
** class InputMavlinkCmdMount
** Input based on the VEHICLE_CMD_DO_MOUNT_CONTROL mavlink command
*/
class InputMavlinkCmdMount : public InputBase
{
public:
InputMavlinkCmdMount();
InputMavlinkCmdMount() = delete;
explicit InputMavlinkCmdMount(Parameters &parameters);
virtual ~InputMavlinkCmdMount();
virtual void print_status();
protected:
virtual int update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active);
virtual int initialize();
void print_status() const override;
UpdateResult update(unsigned int timeout_ms, ControlData &control_data, bool already_active) override;
int initialize() override;
private:
void _ack_vehicle_command(vehicle_command_s *cmd);
UpdateResult _process_command(ControlData &control_data, const vehicle_command_s &vehicle_command);
void _ack_vehicle_command(const vehicle_command_s &cmd);
int _vehicle_command_sub = -1;
int32_t _mav_sys_id{1}; ///< our mavlink system id
int32_t _mav_comp_id{1}; ///< our mavlink component id
};
class InputMavlinkGimbalV2 : public InputBase
{
public:
InputMavlinkGimbalV2(uint8_t mav_sys_id, uint8_t mav_comp_id, float &mnt_rate_pitch, float &mnt_rate_yaw);
InputMavlinkGimbalV2() = delete;
explicit InputMavlinkGimbalV2(Parameters &parameters);
virtual ~InputMavlinkGimbalV2();
virtual void print_status();
protected:
virtual int update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active);
virtual int initialize();
UpdateResult update(unsigned int timeout_ms, ControlData &control_data, bool already_active) override;
int initialize() override;
void print_status() const override;
private:
void _set_control_data_from_set_attitude(const uint32_t flags, const matrix::Quatf &q,
UpdateResult _process_set_attitude(ControlData &control_data, const gimbal_manager_set_attitude_s &set_attitude);
UpdateResult _process_vehicle_roi(ControlData &control_data, const vehicle_roi_s &vehicle_roi);
UpdateResult _process_position_setpoint_triplet(ControlData &control_data,
const position_setpoint_triplet_s &position_setpoint_triplet);
UpdateResult _process_command(ControlData &control_data, const vehicle_command_s &vehicle_command);
UpdateResult _process_set_manual_control(ControlData &control_data,
const gimbal_manager_set_manual_control_s &set_manual_control);
void _set_control_data_from_set_attitude(ControlData &control_data, const uint32_t flags, const matrix::Quatf &q,
const matrix::Vector3f &angular_velocity);
void _ack_vehicle_command(vehicle_command_s *cmd, uint8_t result);
void _ack_vehicle_command(const vehicle_command_s &cmd, uint8_t result);
void _stream_gimbal_manager_information();
void _stream_gimbal_manager_status();
void _read_control_data_from_position_setpoint_sub();
void _stream_gimbal_manager_status(const ControlData &control_data);
void _read_control_data_from_position_setpoint_sub(ControlData &control_data);
int _vehicle_roi_sub = -1;
int _gimbal_manager_set_attitude_sub = -1;
@ -132,17 +124,7 @@ private: @@ -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;

98
src/modules/vmount/input_rc.cpp
Unescape View File

@ -1,6 +1,6 @@ @@ -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 @@ @@ -31,11 +31,6 @@
*
****************************************************************************/
/**
* @file input_rc.cpp
* @author Beat Küng <beat-kueng@gmx.net>
*
*/
#include "input_rc.h"
@ -50,17 +45,9 @@ @@ -50,17 +45,9 @@
namespace vmount
{
InputRC::InputRC(int aux_channel_roll, int aux_channel_pitch, int aux_channel_yaw, float mnt_rate_pitch,
float mnt_rate_yaw, int rc_in_mode)
{
_aux_channels[0] = aux_channel_roll;
_aux_channels[1] = aux_channel_pitch;
_aux_channels[2] = aux_channel_yaw;
_mnt_rate_pitch = mnt_rate_pitch;
_mnt_rate_yaw = mnt_rate_yaw;
_rc_in_mode = rc_in_mode;
}
InputRC::InputRC(Parameters &parameters) :
InputBase(parameters)
{}
InputRC::~InputRC()
{
@ -80,11 +67,8 @@ int InputRC::initialize() @@ -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 @@ -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 @@ -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 @@ -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 @@ -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 @@ -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 */

47
src/modules/vmount/input_rc.h
Unescape View File

@ -1,6 +1,6 @@ @@ -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 @@ @@ -31,66 +31,37 @@
*
****************************************************************************/
/**
* @file input_rc.h
* @author Beat Küng <beat-kueng@gmx.net>
*
*/
#pragma once
#include "input.h"
#include "vmount_params.h"
#include <uORB/topics/manual_control_setpoint.h>
namespace vmount
{
class InputMavlinkROI;
class InputMavlinkCmdMount;
/**
** class InputRC
* RC input class using manual_control_setpoint topic
*/
class InputRC : public InputBase
{
public:
InputRC() = delete;
explicit InputRC(Parameters &parameters);
/**
* @param aux_channel_roll which aux channel to use for roll (set to 0 to use a fixed angle of 0)
* @param aux_channel_pitch
* @param aux_channel_yaw
*/
InputRC(int aux_channel_roll, int aux_channel_pitch, int aux_channel_yaw, float mnt_rate_pitch, float mnt_rate_yaw,
int rc_in_mode);
virtual ~InputRC();
virtual void print_status();
protected:
virtual int update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active);
virtual int initialize();
virtual void print_status() const;
/**
* @return true if there was a change in control data
*/
virtual bool _read_control_data_from_subscription(ControlData &control_data, bool already_active);
virtual UpdateResult update(unsigned int timeout_ms, ControlData &control_data, bool already_active);
float _get_aux_value(const manual_control_setpoint_s &manual_control_setpoint, int channel_idx);
virtual int initialize();
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 */

75
src/modules/vmount/input_test.cpp
Unescape View File

@ -1,6 +1,6 @@ @@ -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 @@ @@ -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 @@ @@ -49,39 +41,42 @@
namespace vmount
{
InputTest::InputTest(float roll_deg, float pitch_deg, float yaw_deg)
{
_angles[0] = roll_deg;
_angles[1] = pitch_deg;
_angles[2] = yaw_deg;
}
bool InputTest::finished()
{
return true; /* only a single-shot test (for now) */
}
InputTest::InputTest(Parameters &parameters) :
InputBase(parameters)
{}
int InputTest::update(unsigned int timeout_ms, ControlData **control_data, bool already_active)
InputTest::UpdateResult 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
if (!_has_been_set.load()) {
return UpdateResult::NoUpdate;
}
_control_data.type = ControlData::Type::Angle;
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() @@ -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 */

42
src/modules/vmount/input_test.h
Unescape View File

@ -1,6 +1,6 @@ @@ -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 @@ @@ -31,47 +31,33 @@
*
****************************************************************************/
/**
* @file input_test.h
* @author Beat Küng <beat-kueng@gmx.net>
*
*/
#pragma once
#include "input.h"
#include <px4_platform_common/atomic.h>
namespace vmount
{
/**
** class InputTest
* Send a single control command, configured via constructor arguments
*/
class InputTest : public InputBase
{
public:
InputTest() = delete;
explicit InputTest(Parameters &parameters);
virtual ~InputTest() = default;
/**
* set to a fixed angle
*/
InputTest(float roll_deg, float pitch_deg, float yaw_deg);
virtual ~InputTest() {}
/** 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);
UpdateResult update(unsigned int timeout_ms, ControlData &control_data, bool already_active) override;
int initialize() override;
void print_status() const override;
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};
};

71
src/modules/vmount/output.cpp
Unescape View File

@ -1,6 +1,6 @@ @@ -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 @@ @@ -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 @@ @@ -53,8 +46,8 @@
namespace vmount
{
OutputBase::OutputBase(const OutputConfig &output_config)
: _config(output_config)
OutputBase::OutputBase(const Parameters &parameters)
: _parameters(parameters)
{
_last_update = hrt_absolute_time();
}
@ -88,16 +81,14 @@ float OutputBase::_calculate_pitch(double lon, double lat, float altitude, @@ -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) @@ -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) @@ -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) @@ -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) @@ -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 */

61
src/modules/vmount/output.h
Unescape View File

@ -1,6 +1,6 @@ @@ -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 @@ @@ -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 @@ @@ -47,71 +43,37 @@
#include <uORB/topics/mount_orientation.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_local_position.h>
namespace vmount
{
struct OutputConfig {
float gimbal_retracted_mode_value; /**< Mixer output value for selecting gimbal retracted mode */
float gimbal_normal_mode_value; /**< Mixer output value for selecting gimbal normal mode */
/** Scale factor for pitch channel (maps from angle in radians to actuator output in [-1,1]). OutputRC only. */
float pitch_scale;
/** Scale factor for roll channel (maps from angle in radians to actuator output in [-1,1]). OutputRC only. */
float roll_scale;
/** Scale factor for yaw channel (maps from angle in radians to actuator output in [-1,1]). OutputRC only. */
float yaw_scale;
float pitch_offset; /**< Offset for pitch channel in radians */
float roll_offset; /**< Offset for roll channel in radians */
float yaw_offset; /**< Offset for yaw channel in radians */
uint32_t mavlink_sys_id_v1; /**< Mavlink target system id for mavlink output only for v1 */
uint32_t mavlink_comp_id_v1;
};
/**
** class OutputBase
* Base class for all driver output classes
*/
class OutputBase
{
public:
OutputBase(const OutputConfig &output_config);
OutputBase() = delete;
explicit OutputBase(const Parameters &parameters);
virtual ~OutputBase() = default;
virtual int initialize() { return 0; }
/**
* 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 update(const ControlData &control_data, bool new_setpoints) = 0;
/** report status to stdout */
virtual void print_status() = 0;
virtual void print_status() const = 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);
/** check if vehicle position changed and update the setpoint angles if in gps mode */
void _handle_position_update(bool force_update = false);
void _set_angle_setpoints(const ControlData &control_data);
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: @@ -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)};
};

90
src/modules/vmount/output_mavlink.cpp
Unescape View File

@ -1,6 +1,6 @@ @@ -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 @@ @@ -31,51 +31,42 @@
*
****************************************************************************/
/**
* @file output_mavlink.cpp
* @author Leon Müller (thedevleon)
* @author Beat Küng <beat-kueng@gmx.net>
*
*/
#include "output_mavlink.h"
#include <math.h>
#include <matrix/matrix/math.hpp>
#include <uORB/topics/vehicle_command.h>
#include <px4_platform_common/defines.h>
namespace vmount
{
OutputMavlinkV1::OutputMavlinkV1(const OutputConfig &output_config)
: OutputBase(output_config)
{
}
OutputMavlinkV1::OutputMavlinkV1(const Parameters &parameters)
: OutputBase(parameters)
{}
int OutputMavlinkV1::update(const ControlData *control_data)
void OutputMavlinkV1::update(const ControlData &control_data, bool new_setpoints)
{
vehicle_command_s vehicle_command{};
vehicle_command.timestamp = hrt_absolute_time();
vehicle_command.target_system = (uint8_t)_config.mavlink_sys_id_v1;
vehicle_command.target_component = (uint8_t)_config.mavlink_comp_id_v1;
vehicle_command.target_system = (uint8_t)_parameters.mnt_mav_sysid_v1;
vehicle_command.target_component = (uint8_t)_parameters.mnt_mav_compid_v1;
if (control_data) {
if (new_setpoints) {
//got new command
_set_angle_setpoints(control_data);
const bool configuration_changed =
(control_data->type != _previous_control_data_type);
_previous_control_data_type = control_data->type;
(control_data.type != _previous_control_data_type);
_previous_control_data_type = control_data.type;
if (configuration_changed) {
vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE;
vehicle_command.timestamp = hrt_absolute_time();
if (control_data->type == ControlData::Type::Neutral) {
if (control_data.type == ControlData::Type::Neutral) {
vehicle_command.param1 = vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL;
vehicle_command.param5 = 0.0;
@ -85,20 +76,20 @@ int OutputMavlinkV1::update(const ControlData *control_data) @@ -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) @@ -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() @@ -143,52 +132,51 @@ void OutputMavlinkV1::_stream_device_attitude_status()
_attitude_status_pub.publish(attitude_status);
}
void OutputMavlinkV1::print_status()
void OutputMavlinkV1::print_status() const
{
PX4_INFO("Output: MAVLink gimbal protocol v1");
}
OutputMavlinkV2::OutputMavlinkV2(int32_t mav_sys_id, int32_t mav_comp_id, const OutputConfig &output_config)
: OutputBase(output_config),
_mav_sys_id(mav_sys_id),
_mav_comp_id(mav_comp_id)
OutputMavlinkV2::OutputMavlinkV2(const Parameters &parameters)
: OutputBase(parameters)
{
}
int OutputMavlinkV2::update(const ControlData *control_data)
void OutputMavlinkV2::update(const ControlData &control_data, bool new_setpoints)
{
_check_for_gimbal_device_information();
hrt_abstime t = hrt_absolute_time();
if (!_gimbal_device_found && t - _last_gimbal_device_checked > 1000000) {
_request_gimbal_device_information();
_last_gimbal_device_checked = t;
} else {
if (control_data) {
if (new_setpoints) {
//got new command
_set_angle_setpoints(control_data);
_handle_position_update(control_data);
_last_update = t;
}
_handle_position_update();
_publish_gimbal_device_set_attitude();
_last_update = t;
}
return 0;
}
void OutputMavlinkV2::_request_gimbal_device_information()
{
printf("request gimbal device\n");
vehicle_command_s vehicle_cmd{};
vehicle_cmd.timestamp = hrt_absolute_time();
vehicle_cmd.command = vehicle_command_s::VEHICLE_CMD_REQUEST_MESSAGE;
vehicle_cmd.param1 = vehicle_command_s::VEHICLE_CMD_GIMBAL_DEVICE_INFORMATION;
vehicle_cmd.target_system = 0;
vehicle_cmd.target_component = 0;
vehicle_cmd.source_system = _mav_sys_id;
vehicle_cmd.source_component = _mav_comp_id;
vehicle_cmd.source_system = _parameters.mav_sysid;
vehicle_cmd.source_component = _parameters.mav_compid;
vehicle_cmd.confirmation = 0;
vehicle_cmd.from_external = false;
@ -206,16 +194,26 @@ void OutputMavlinkV2::_check_for_gimbal_device_information() @@ -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];

27
src/modules/vmount/output_mavlink.h
Unescape View File

@ -1,6 +1,6 @@ @@ -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 @@ @@ -31,11 +31,6 @@
*
****************************************************************************/
/**
* @file output_mavlink.h
* @author Beat Küng <beat-kueng@gmx.net>
*
*/
#pragma once
@ -50,23 +45,19 @@ @@ -50,23 +45,19 @@
namespace vmount
{
/**
** class OutputMavlink
* Output via vehicle_command topic
*/
class OutputMavlinkV1 : public OutputBase
{
public:
OutputMavlinkV1(const OutputConfig &output_config);
OutputMavlinkV1(const Parameters &parameters);
virtual ~OutputMavlinkV1() = default;
virtual int update(const ControlData *control_data);
virtual void update(const ControlData &control_data, bool new_setpoints);
virtual void print_status();
virtual void print_status() const;
private:
void _stream_device_attitude_status();
uORB::Publication<vehicle_command_s> _vehicle_command_pub{ORB_ID(vehicle_command)};
uORB::Publication<vehicle_command_s> _gimbal_v1_command_pub{ORB_ID(gimbal_v1_command)};
uORB::Publication <gimbal_device_attitude_status_s> _attitude_status_pub{ORB_ID(gimbal_device_attitude_status)};
ControlData::Type _previous_control_data_type {ControlData::Type::Neutral};
@ -75,12 +66,12 @@ private: @@ -75,12 +66,12 @@ private:
class OutputMavlinkV2 : public OutputBase
{
public:
OutputMavlinkV2(int32_t mav_sys_id, int32_t mav_comp_id, const OutputConfig &output_config);
OutputMavlinkV2(const Parameters &parameters);
virtual ~OutputMavlinkV2() = default;
virtual int update(const ControlData *control_data);
virtual void update(const ControlData &control_data, bool new_setpoints);
virtual void print_status();
virtual void print_status() const;
private:
void _publish_gimbal_device_set_attitude();
@ -90,8 +81,6 @@ private: @@ -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};

45
src/modules/vmount/output_rc.cpp
Unescape View File

@ -1,6 +1,6 @@ @@ -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 @@ @@ -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; @@ -49,20 +43,19 @@ using math::constrain;
namespace vmount
{
OutputRC::OutputRC(const OutputConfig &output_config)
: OutputBase(output_config)
OutputRC::OutputRC(const Parameters &parameters)
: OutputBase(parameters)
{
}
int OutputRC::update(const ControlData *control_data)
void OutputRC::update(const ControlData &control_data, bool new_setpoints)
{
if (control_data) {
//got new command
_retract_gimbal = control_data->gimbal_shutter_retract;
if (new_setpoints) {
_retract_gimbal = control_data.gimbal_shutter_retract;
_set_angle_setpoints(control_data);
}
_handle_position_update();
_handle_position_update(control_data);
hrt_abstime t = hrt_absolute_time();
_calculate_angle_output(t);
@ -71,20 +64,28 @@ int OutputRC::update(const ControlData *control_data) @@ -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");
}

21
src/modules/vmount/output_rc.h
Unescape View File

@ -1,6 +1,6 @@ @@ -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 @@ @@ -31,11 +31,6 @@
*
****************************************************************************/
/**
* @file output_rc.h
* @author Beat Küng <beat-kueng@gmx.net>
*
*/
#pragma once
@ -48,20 +43,15 @@ @@ -48,20 +43,15 @@
namespace vmount
{
/**
** class OutputRC
* Output via actuator_controls_2 topic
*/
class OutputRC : public OutputBase
{
public:
OutputRC(const OutputConfig &output_config);
OutputRC() = delete;
explicit OutputRC(const Parameters &parameters);
virtual ~OutputRC() = default;
virtual int update(const ControlData *control_data);
virtual void print_status();
virtual void update(const ControlData &control_data, bool new_setpoints);
virtual void print_status() const;
private:
void _stream_device_attitude_status();
@ -72,5 +62,4 @@ private: @@ -72,5 +62,4 @@ private:
bool _retract_gimbal = true;
};
} /* namespace vmount */

597
src/modules/vmount/vmount.cpp
Unescape View File

@ -1,6 +1,6 @@ @@ -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 @@ @@ -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 @@ @@ -66,105 +60,30 @@
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/topics/parameter_update.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/atomic.h>
using namespace time_literals;
using namespace vmount;
/* thread state */
static volatile bool thread_should_exit = false;
static volatile bool thread_running = false;
static px4::atomic<bool> thread_should_exit {false};
static px4::atomic<bool> thread_running {false};
static constexpr int input_objs_len_max = 3;
struct ThreadData {
InputBase *input_objs[input_objs_len_max] = {nullptr, nullptr, nullptr};
int input_objs_len = 0;
int last_input_active = -1;
OutputBase *output_obj = nullptr;
InputTest *test_input = nullptr;
};
static volatile ThreadData *g_thread_data = nullptr;
struct Parameters {
int32_t mnt_mode_in;
int32_t mnt_mode_out;
int32_t mnt_mav_sys_id_v1;
int32_t mnt_mav_comp_id_v1;
float mnt_ob_lock_mode;
float mnt_ob_norm_mode;
int32_t mnt_man_pitch;
int32_t mnt_man_roll;
int32_t mnt_man_yaw;
int32_t mnt_do_stab;
float mnt_range_pitch;
float mnt_range_roll;
float mnt_range_yaw;
float mnt_off_pitch;
float mnt_off_roll;
float mnt_off_yaw;
int32_t mav_sys_id;
int32_t mav_comp_id;
float mnt_rate_pitch;
float mnt_rate_yaw;
int32_t mnt_rc_in_mode;
bool operator!=(const Parameters &p)
{
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wfloat-equal"
return mnt_mode_in != p.mnt_mode_in ||
mnt_mode_out != p.mnt_mode_out ||
mnt_mav_sys_id_v1 != p.mnt_mav_sys_id_v1 ||
mnt_mav_comp_id_v1 != p.mnt_mav_comp_id_v1 ||
fabsf(mnt_ob_lock_mode - p.mnt_ob_lock_mode) > 1e-6f ||
fabsf(mnt_ob_norm_mode - p.mnt_ob_norm_mode) > 1e-6f ||
mnt_man_pitch != p.mnt_man_pitch ||
mnt_man_roll != p.mnt_man_roll ||
mnt_man_yaw != p.mnt_man_yaw ||
mnt_do_stab != p.mnt_do_stab ||
mnt_range_pitch != p.mnt_range_pitch ||
mnt_range_roll != p.mnt_range_roll ||
mnt_range_yaw != p.mnt_range_yaw ||
mnt_off_pitch != p.mnt_off_pitch ||
mnt_off_roll != p.mnt_off_roll ||
mnt_off_yaw != p.mnt_off_yaw ||
mav_sys_id != p.mav_sys_id ||
mav_comp_id != p.mav_comp_id ||
mnt_rc_in_mode != p.mnt_rc_in_mode;
#pragma GCC diagnostic pop
}
};
struct ParameterHandles {
param_t mnt_mode_in;
param_t mnt_mode_out;
param_t mnt_mav_sys_id_v1;
param_t mnt_mav_comp_id_v1;
param_t mnt_ob_lock_mode;
param_t mnt_ob_norm_mode;
param_t mnt_man_pitch;
param_t mnt_man_roll;
param_t mnt_man_yaw;
param_t mnt_do_stab;
param_t mnt_range_pitch;
param_t mnt_range_roll;
param_t mnt_range_yaw;
param_t mnt_off_pitch;
param_t mnt_off_roll;
param_t mnt_off_yaw;
param_t mav_sys_id;
param_t mav_comp_id;
param_t mnt_rate_pitch;
param_t mnt_rate_yaw;
param_t mnt_rc_in_mode;
};
static ThreadData *g_thread_data = nullptr;
/* functions */
static void usage();
static void update_params(ParameterHandles &param_handles, Parameters &params, bool &got_changes);
static bool get_params(ParameterHandles &param_handles, Parameters &params);
static void update_params(ParameterHandles &param_handles, Parameters &params);
static bool initialize_params(ParameterHandles &param_handles, Parameters &params);
static int vmount_thread_main(int argc, char *argv[]);
extern "C" __EXPORT int vmount_main(int argc, char *argv[]);
@ -173,282 +92,199 @@ static int vmount_thread_main(int argc, char *argv[]) @@ -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:
// Automatic
thread_data.input_objs[0] = new InputMavlinkCmdMount();
thread_data.input_objs[1] = new InputMavlinkROI();
// RC is on purpose last here so that if there are any mavlink
// messages, they will take precedence over RC.
// This logic is done further below while update() is called.
thread_data.input_objs[2] = new InputRC(params.mnt_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 2: // MAVLINK_ROI commands only (to be deprecated)
thread_data.input_objs[thread_data.input_objs_len++] = new InputMavlinkROI(params);
break;
break;
case 3: // MAVLINK_DO_MOUNT commands only (to be deprecated)
thread_data.input_objs[thread_data.input_objs_len++] = new InputMavlinkCmdMount(params);
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;
case 4: //MAVLINK_V2
thread_data.input_objs[thread_data.input_objs_len++] = new InputMavlinkGimbalV2(params);
break;
case 2: //MAVLINK_ROI
thread_data.input_objs[0] = new InputMavlinkROI();
break;
default:
PX4_ERR("invalid input mode %" PRId32, params.mnt_mode_in);
break;
}
case 3: //MAVLINK_DO_MOUNT
thread_data.input_objs[0] = new InputMavlinkCmdMount();
break;
for (int i = 0; i < thread_data.input_objs_len; ++i) {
if (!thread_data.input_objs[i]) {
alloc_failed = true;
}
}
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;
if (alloc_failed) {
PX4_ERR("input objs memory allocation failed");
thread_should_exit.store(true);
}
default:
PX4_ERR("invalid input mode %" PRId32, params.mnt_mode_in);
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);
}
}
}
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(params);
switch (params.mnt_mode_out) {
case 0: //AUX
thread_data.output_obj = new OutputRC(output_config);
if (!thread_data.output_obj) { alloc_failed = true; }
if (!thread_data.output_obj) { alloc_failed = true; }
break;
break;
case 1: //MAVLink gimbal v1 protocol
thread_data.output_obj = new OutputMavlinkV1(params);
case 1: //MAVLink v1 gimbal protocol
thread_data.output_obj = new OutputMavlinkV1(output_config);
if (!thread_data.output_obj) { alloc_failed = true; }
if (!thread_data.output_obj) { alloc_failed = true; }
break;
break;
case 2: //MAVLink gimbal v2 protocol
thread_data.output_obj = new OutputMavlinkV2(params);
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; }
if (!thread_data.output_obj) { alloc_failed = true; }
break;
break;
default:
PX4_ERR("invalid output mode %" PRId32, params.mnt_mode_out);
thread_should_exit.store(true);
break;
}
default:
PX4_ERR("invalid output mode %" PRId32, params.mnt_mode_out);
thread_should_exit = true;
break;
}
if (alloc_failed) {
PX4_ERR("output memory allocation failed");
thread_should_exit.store(true);
}
if (alloc_failed) {
thread_data.input_objs_len = 0;
PX4_ERR("memory allocation failed");
thread_should_exit = true;
}
while (!thread_should_exit.load()) {
if (thread_should_exit) {
break;
}
const bool updated = parameter_update_sub.updated();
int ret = thread_data.output_obj->initialize();
if (updated) {
parameter_update_s pupdate;
parameter_update_sub.copy(&pupdate);
update_params(param_handles, params);
}
if (ret) {
PX4_ERR("failed to initialize output mode (%i)", ret);
thread_should_exit = true;
break;
}
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[]) @@ -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[]) @@ -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 (found_start || found_test) {
if (!strcmp(argv[1], "start")) {
/* 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[]) @@ -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[]) @@ -569,13 +438,12 @@ int vmount_main(int argc, char *argv[])
return -1;
}
void update_params(ParameterHandles &param_handles, Parameters &params, bool &got_changes)
void update_params(ParameterHandles &param_handles, Parameters &params)
{
Parameters prev_params = params;
param_get(param_handles.mnt_mode_in, &params.mnt_mode_in);
param_get(param_handles.mnt_mode_out, &params.mnt_mode_out);
param_get(param_handles.mnt_mav_sys_id_v1, &params.mnt_mav_sys_id_v1);
param_get(param_handles.mnt_mav_comp_id_v1, &params.mnt_mav_comp_id_v1);
param_get(param_handles.mnt_mav_sysid_v1, &params.mnt_mav_sysid_v1);
param_get(param_handles.mnt_mav_compid_v1, &params.mnt_mav_compid_v1);
param_get(param_handles.mnt_ob_lock_mode, &params.mnt_ob_lock_mode);
param_get(param_handles.mnt_ob_norm_mode, &params.mnt_ob_norm_mode);
param_get(param_handles.mnt_man_pitch, &params.mnt_man_pitch);
@ -588,21 +456,19 @@ void update_params(ParameterHandles &param_handles, Parameters &params, bool &go @@ -588,21 +456,19 @@ void update_params(ParameterHandles &param_handles, Parameters &params, bool &go
param_get(param_handles.mnt_off_pitch, &params.mnt_off_pitch);
param_get(param_handles.mnt_off_roll, &params.mnt_off_roll);
param_get(param_handles.mnt_off_yaw, &params.mnt_off_yaw);
param_get(param_handles.mav_sys_id, &params.mav_sys_id);
param_get(param_handles.mav_comp_id, &params.mav_comp_id);
param_get(param_handles.mav_sysid, &params.mav_sysid);
param_get(param_handles.mav_compid, &params.mav_compid);
param_get(param_handles.mnt_rate_pitch, &params.mnt_rate_pitch);
param_get(param_handles.mnt_rate_yaw, &params.mnt_rate_yaw);
param_get(param_handles.mnt_rc_in_mode, &params.mnt_rc_in_mode);
got_changes = prev_params != params;
}
bool get_params(ParameterHandles &param_handles, Parameters &params)
bool initialize_params(ParameterHandles &param_handles, Parameters &params)
{
param_handles.mnt_mode_in = param_find("MNT_MODE_IN");
param_handles.mnt_mode_out = param_find("MNT_MODE_OUT");
param_handles.mnt_mav_sys_id_v1 = param_find("MNT_MAV_SYSID");
param_handles.mnt_mav_comp_id_v1 = param_find("MNT_MAV_COMPID");
param_handles.mnt_mav_sysid_v1 = param_find("MNT_MAV_SYSID");
param_handles.mnt_mav_compid_v1 = param_find("MNT_MAV_COMPID");
param_handles.mnt_ob_lock_mode = param_find("MNT_OB_LOCK_MODE");
param_handles.mnt_ob_norm_mode = param_find("MNT_OB_NORM_MODE");
param_handles.mnt_man_pitch = param_find("MNT_MAN_PITCH");
@ -615,16 +481,16 @@ bool get_params(ParameterHandles &param_handles, Parameters &params) @@ -615,16 +481,16 @@ bool get_params(ParameterHandles &param_handles, Parameters &params)
param_handles.mnt_off_pitch = param_find("MNT_OFF_PITCH");
param_handles.mnt_off_roll = param_find("MNT_OFF_ROLL");
param_handles.mnt_off_yaw = param_find("MNT_OFF_YAW");
param_handles.mav_sys_id = param_find("MAV_SYS_ID");
param_handles.mav_comp_id = param_find("MAV_COMP_ID");
param_handles.mav_sysid = param_find("MAV_SYS_ID");
param_handles.mav_compid = param_find("MAV_COMP_ID");
param_handles.mnt_rate_pitch = param_find("MNT_RATE_PITCH");
param_handles.mnt_rate_yaw = param_find("MNT_RATE_YAW");
param_handles.mnt_rc_in_mode = param_find("MNT_RC_IN_MODE");
if (param_handles.mnt_mode_in == PARAM_INVALID ||
param_handles.mnt_mode_out == PARAM_INVALID ||
param_handles.mnt_mav_sys_id_v1 == PARAM_INVALID ||
param_handles.mnt_mav_comp_id_v1 == PARAM_INVALID ||
param_handles.mnt_mav_sysid_v1 == PARAM_INVALID ||
param_handles.mnt_mav_compid_v1 == PARAM_INVALID ||
param_handles.mnt_ob_lock_mode == PARAM_INVALID ||
param_handles.mnt_ob_norm_mode == PARAM_INVALID ||
param_handles.mnt_man_pitch == PARAM_INVALID ||
@ -637,8 +503,8 @@ bool get_params(ParameterHandles &param_handles, Parameters &params) @@ -637,8 +503,8 @@ bool get_params(ParameterHandles &param_handles, Parameters &params)
param_handles.mnt_off_pitch == PARAM_INVALID ||
param_handles.mnt_off_roll == PARAM_INVALID ||
param_handles.mnt_off_yaw == PARAM_INVALID ||
param_handles.mav_sys_id == PARAM_INVALID ||
param_handles.mav_comp_id == PARAM_INVALID ||
param_handles.mav_sysid == PARAM_INVALID ||
param_handles.mav_compid == PARAM_INVALID ||
param_handles.mnt_rate_pitch == PARAM_INVALID ||
param_handles.mnt_rate_yaw == PARAM_INVALID ||
param_handles.mnt_rc_in_mode == PARAM_INVALID
@ -646,8 +512,7 @@ bool get_params(ParameterHandles &param_handles, Parameters &params) @@ -646,8 +512,7 @@ bool get_params(ParameterHandles &param_handles, Parameters &params)
return false;
}
bool dummy;
update_params(param_handles, params, dummy);
update_params(param_handles, params);
return true;
}
@ -656,25 +521,19 @@ static void usage() @@ -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();
}

26
src/modules/vmount/vmount_params.c
Unescape View File

@ -31,25 +31,20 @@ @@ -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); @@ -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); @@ -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);

91
src/modules/vmount/vmount_params.h
Unescape View File

@ -0,0 +1,91 @@ @@ -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 */
Write Preview
Loading…
Cancel
Save