From 39f0e97245ee03196395976ad43a012082fc0748 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 12 Jan 2022 10:42:18 +0100 Subject: [PATCH] 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. --- msg/vehicle_command.msg | 2 + src/modules/mavlink/mavlink_main.cpp | 24 + src/modules/mavlink/mavlink_main.h | 1 + src/modules/vmount/common.h | 53 +- src/modules/vmount/input.cpp | 55 +- src/modules/vmount/input.h | 61 +- src/modules/vmount/input_mavlink.cpp | 1216 +++++++++++++------------ src/modules/vmount/input_mavlink.h | 86 +- src/modules/vmount/input_rc.cpp | 98 +- src/modules/vmount/input_rc.h | 47 +- src/modules/vmount/input_test.cpp | 77 +- src/modules/vmount/input_test.h | 42 +- src/modules/vmount/output.cpp | 71 +- src/modules/vmount/output.h | 61 +- src/modules/vmount/output_mavlink.cpp | 90 +- src/modules/vmount/output_mavlink.h | 27 +- src/modules/vmount/output_rc.cpp | 45 +- src/modules/vmount/output_rc.h | 21 +- src/modules/vmount/vmount.cpp | 627 +++++-------- src/modules/vmount/vmount_params.c | 26 +- src/modules/vmount/vmount_params.h | 91 ++ 21 files changed, 1343 insertions(+), 1478 deletions(-) create mode 100644 src/modules/vmount/vmount_params.h diff --git a/msg/vehicle_command.msg b/msg/vehicle_command.msg index 1979b9f9db..9ddaad2a92 100644 --- a/msg/vehicle_command.msg +++ b/msg/vehicle_command.msg @@ -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 diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 2e33c63b31..7fb34a583b 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -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) { diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index fc536fe40d..12b078e97b 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -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; diff --git a/src/modules/vmount/common.h b/src/modules/vmount/common.h index d4ecfd8c0c..05e8316271 100644 --- a/src/modules/vmount/common.h +++ b/src/modules/vmount/common.h @@ -31,11 +31,6 @@ * ****************************************************************************/ -/** - * @file common.h - * @author Beat Küng - * - */ #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 */ diff --git a/src/modules/vmount/input.cpp b/src/modules/vmount/input.cpp index f6e544076e..15f849c056 100644 --- a/src/modules/vmount/input.cpp +++ b/src/modules/vmount/input.cpp @@ -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 - * - */ - #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 */ diff --git a/src/modules/vmount/input.h b/src/modules/vmount/input.h index e7b26a697e..b3ce83f644 100644 --- a/src/modules/vmount/input.h +++ b/src/modules/vmount/input.h @@ -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 - * - */ - #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; }; diff --git a/src/modules/vmount/input_mavlink.cpp b/src/modules/vmount/input_mavlink.cpp index b8e1dc20b0..073313c81d 100644 --- a/src/modules/vmount/input_mavlink.cpp +++ b/src/modules/vmount/input_mavlink.cpp @@ -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,23 +31,12 @@ * ****************************************************************************/ -/** - * @file input_mavlink.cpp - * @author Leon Müller (thedevleon) - * @author Beat Küng - * - */ #include "input_mavlink.h" #include #include -#include #include -#include -#include -#include #include -#include #include #include #include @@ -57,6 +46,9 @@ namespace vmount { +InputMavlinkROI::InputMavlinkROI(Parameters ¶meters) : + InputBase(parameters) {} + InputMavlinkROI::~InputMavlinkROI() { if (_vehicle_roi_sub >= 0) { @@ -85,99 +77,97 @@ int InputMavlinkROI::initialize() return 0; } -int InputMavlinkROI::update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active) +InputMavlinkROI::UpdateResult +InputMavlinkROI::update(unsigned int timeout_ms, ControlData &control_data, bool already_active) { - // already_active is unused, we don't care what happened previously. - - // Default to no change, set if we receive anything. - *control_data = nullptr; - const int num_poll = 2; px4_pollfd_struct_t polls[num_poll]; - polls[0].fd = _vehicle_roi_sub; - polls[0].events = POLLIN; - polls[1].fd = _position_setpoint_triplet_sub; - polls[1].events = POLLIN; + polls[0].fd = _vehicle_roi_sub; + polls[0].events = POLLIN; + polls[1].fd = _position_setpoint_triplet_sub; + polls[1].events = POLLIN; int ret = px4_poll(polls, num_poll, timeout_ms); - if (ret < 0) { - return -errno; + if (ret <= 0) { + return UpdateResult::NoUpdate; } - if (ret == 0) { - // Timeout, _control_data is already null + if (polls[0].revents & POLLIN) { + vehicle_roi_s vehicle_roi; + orb_copy(ORB_ID(vehicle_roi), _vehicle_roi_sub, &vehicle_roi); - } else { - if (polls[0].revents & POLLIN) { - vehicle_roi_s vehicle_roi; - orb_copy(ORB_ID(vehicle_roi), _vehicle_roi_sub, &vehicle_roi); + if (vehicle_roi.mode == vehicle_roi_s::ROI_NONE) { - _control_data.gimbal_shutter_retract = false; + control_data.type = ControlData::Type::Neutral; + _cur_roi_mode = vehicle_roi.mode; + control_data.sysid_primary_control = _parameters.mav_sysid; + control_data.compid_primary_control = _parameters.mav_compid; - if (vehicle_roi.mode == vehicle_roi_s::ROI_NONE) { + return UpdateResult::UpdatedActiveOnce; - _control_data.type = ControlData::Type::Neutral; - *control_data = &_control_data; - _cur_roi_mode = vehicle_roi.mode; + } else if (vehicle_roi.mode == vehicle_roi_s::ROI_WPNEXT) { + control_data.type = ControlData::Type::LonLat; + _read_control_data_from_position_setpoint_sub(control_data); + control_data.type_data.lonlat.pitch_fixed_angle = -10.f; - } else if (vehicle_roi.mode == vehicle_roi_s::ROI_WPNEXT) { - _control_data.type = ControlData::Type::LonLat; - _read_control_data_from_position_setpoint_sub(); - _control_data.type_data.lonlat.pitch_fixed_angle = -10.f; + control_data.type_data.lonlat.roll_offset = vehicle_roi.roll_offset; + control_data.type_data.lonlat.pitch_offset = vehicle_roi.pitch_offset; + control_data.type_data.lonlat.yaw_offset = vehicle_roi.yaw_offset; - _control_data.type_data.lonlat.roll_angle = vehicle_roi.roll_offset; - _control_data.type_data.lonlat.pitch_angle_offset = vehicle_roi.pitch_offset; - _control_data.type_data.lonlat.yaw_angle_offset = vehicle_roi.yaw_offset; + _cur_roi_mode = vehicle_roi.mode; - *control_data = &_control_data; - _cur_roi_mode = vehicle_roi.mode; + return UpdateResult::UpdatedActive; - } else if (vehicle_roi.mode == vehicle_roi_s::ROI_LOCATION) { - control_data_set_lon_lat(vehicle_roi.lon, vehicle_roi.lat, vehicle_roi.alt); + } else if (vehicle_roi.mode == vehicle_roi_s::ROI_LOCATION) { + control_data_set_lon_lat(control_data, vehicle_roi.lon, vehicle_roi.lat, vehicle_roi.alt); - *control_data = &_control_data; - _cur_roi_mode = vehicle_roi.mode; + _cur_roi_mode = vehicle_roi.mode; - } else if (vehicle_roi.mode == vehicle_roi_s::ROI_TARGET) { - //TODO is this even suported? - } + return UpdateResult::UpdatedActive; + + } else if (vehicle_roi.mode == vehicle_roi_s::ROI_TARGET) { + PX4_WARN("ROI_TARGET not supported yet"); + return UpdateResult::NoUpdate; } - // check whether the position setpoint got updated - if (polls[1].revents & POLLIN) { - if (_cur_roi_mode == vehicle_roi_s::ROI_WPNEXT) { - _read_control_data_from_position_setpoint_sub(); - *control_data = &_control_data; + return UpdateResult::NoUpdate; + } - } else { // must do an orb_copy() in *every* case - position_setpoint_triplet_s position_setpoint_triplet; - orb_copy(ORB_ID(position_setpoint_triplet), _position_setpoint_triplet_sub, &position_setpoint_triplet); - } + // check whether the position setpoint has been updated + if (polls[1].revents & POLLIN) { + if (_cur_roi_mode == vehicle_roi_s::ROI_WPNEXT) { + _read_control_data_from_position_setpoint_sub(control_data); + + return UpdateResult::UpdatedActive; + + } else { // must do an orb_copy() in *every* case + position_setpoint_triplet_s position_setpoint_triplet; + orb_copy(ORB_ID(position_setpoint_triplet), _position_setpoint_triplet_sub, + &position_setpoint_triplet); } } - return 0; + return UpdateResult::NoUpdate; } -void InputMavlinkROI::_read_control_data_from_position_setpoint_sub() +void InputMavlinkROI::_read_control_data_from_position_setpoint_sub(ControlData &control_data) { position_setpoint_triplet_s position_setpoint_triplet; orb_copy(ORB_ID(position_setpoint_triplet), _position_setpoint_triplet_sub, &position_setpoint_triplet); - _control_data.type_data.lonlat.lon = position_setpoint_triplet.current.lon; - _control_data.type_data.lonlat.lat = position_setpoint_triplet.current.lat; - _control_data.type_data.lonlat.altitude = position_setpoint_triplet.current.alt; + control_data.type_data.lonlat.lon = position_setpoint_triplet.current.lon; + control_data.type_data.lonlat.lat = position_setpoint_triplet.current.lat; + control_data.type_data.lonlat.altitude = position_setpoint_triplet.current.alt; } -void InputMavlinkROI::print_status() +void InputMavlinkROI::print_status() const { PX4_INFO("Input: Mavlink (ROI)"); } - -InputMavlinkCmdMount::InputMavlinkCmdMount() -{ -} +InputMavlinkCmdMount::InputMavlinkCmdMount(Parameters ¶meters) : + InputBase(parameters) +{} InputMavlinkCmdMount::~InputMavlinkCmdMount() { @@ -201,181 +191,190 @@ int InputMavlinkCmdMount::initialize() } -int InputMavlinkCmdMount::update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active) +InputMavlinkCmdMount::UpdateResult +InputMavlinkCmdMount::update(unsigned int timeout_ms, ControlData &control_data, bool already_active) { - // Default to notify that there was no change. - *control_data = nullptr; - const int num_poll = 1; px4_pollfd_struct_t polls[num_poll]; - polls[0].fd = _vehicle_command_sub; - polls[0].events = POLLIN; + polls[0].fd = _vehicle_command_sub; + polls[0].events = POLLIN; - int poll_timeout = (int)timeout_ms; + int poll_timeout = (int) timeout_ms; + + const hrt_abstime poll_start = hrt_absolute_time(); + + // If we get a command that we need to handle we exit the loop, otherwise we poll until we + // reach the timeout. + // We can't return early instead because we need to copy all topics that triggered poll. bool exit_loop = false; + UpdateResult update_result = UpdateResult::NoUpdate; while (!exit_loop && poll_timeout >= 0) { - hrt_abstime poll_start = hrt_absolute_time(); - int ret = px4_poll(polls, num_poll, poll_timeout); + const int ret = px4_poll(polls, num_poll, poll_timeout); - if (ret < 0) { - return -errno; + if (ret <= 0) { + // Error, or timeout, give up. + exit_loop = true; } - poll_timeout -= (hrt_absolute_time() - poll_start) / 1000; + if (polls[0].revents & POLLIN) { - // if we get a command that we need to handle, we exit the loop, otherwise we poll until we reach the timeout - exit_loop = true; + vehicle_command_s vehicle_command; + orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &vehicle_command); - if (ret == 0) { - // Timeout control_data already null. - - } else { - if (polls[0].revents & POLLIN) { - vehicle_command_s vehicle_command; - orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &vehicle_command); - - // Process only if the command is for us or for anyone (component id 0). - const bool sysid_correct = (vehicle_command.target_system == _mav_sys_id); - const bool compid_correct = ((vehicle_command.target_component == _mav_comp_id) || - (vehicle_command.target_component == 0)); - - if (!sysid_correct || !compid_correct) { - exit_loop = false; - continue; - } - - _control_data.gimbal_shutter_retract = false; - - if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL) { - - switch ((int)vehicle_command.param7) { - case vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT: - _control_data.gimbal_shutter_retract = true; - - /* FALLTHROUGH */ - - case vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL: - _control_data.type = ControlData::Type::Neutral; - - *control_data = &_control_data; - break; - - case vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING: { - _control_data.type = ControlData::Type::Angle; - _control_data.type_data.angle.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; - - // vmount spec has roll on channel 0, MAVLink spec has pitch on channel 0 - const float roll = math::radians(vehicle_command.param2); - // vmount spec has pitch on channel 1, MAVLink spec has roll on channel 1 - const float pitch = math::radians(vehicle_command.param1); - // both specs have yaw on channel 2 - float yaw = math::radians(vehicle_command.param3); - - matrix::Eulerf euler(roll, pitch, yaw); - - matrix::Quatf q(euler); - q.copyTo(_control_data.type_data.angle.q); - - _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; - - *control_data = &_control_data; - } - break; - - case vehicle_command_s::VEHICLE_MOUNT_MODE_RC_TARGETING: - break; - - case vehicle_command_s::VEHICLE_MOUNT_MODE_GPS_POINT: - control_data_set_lon_lat((double)vehicle_command.param6, (double)vehicle_command.param5, vehicle_command.param4); - - *control_data = &_control_data; - break; - } - - _ack_vehicle_command(&vehicle_command); - - } else if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE) { - - _control_data.stabilize_axis[0] = (int)(vehicle_command.param2 + 0.5f) == 1; - _control_data.stabilize_axis[1] = (int)(vehicle_command.param3 + 0.5f) == 1; - _control_data.stabilize_axis[2] = (int)(vehicle_command.param4 + 0.5f) == 1; - - - const int params[] = { - (int)((float)vehicle_command.param5 + 0.5f), - (int)((float)vehicle_command.param6 + 0.5f), - (int)(vehicle_command.param7 + 0.5f) - }; - - for (int i = 0; i < 3; ++i) { - - if (params[i] == 0) { - _control_data.type_data.angle.frames[i] = - ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; - - } else if (params[i] == 1) { - _control_data.type_data.angle.frames[i] = - ControlData::TypeData::TypeAngle::Frame::AngularRate; - - } else if (params[i] == 2) { - _control_data.type_data.angle.frames[i] = - ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame; - - } else { - // Not supported, fallback to body angle. - _control_data.type_data.angle.frames[i] = - ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; - } - } - - _control_data.type = ControlData::Type::Neutral; //always switch to neutral position - - *control_data = &_control_data; - _ack_vehicle_command(&vehicle_command); - - } else { - exit_loop = false; - } - } + update_result = _process_command(control_data, vehicle_command); } + + if (update_result != UpdateResult::NoUpdate) { + exit_loop = true; + } + + // Keep going reading commands until timeout is up. + poll_timeout = timeout_ms - (hrt_absolute_time() - poll_start) / 1000; } - return 0; + return update_result; } -void InputMavlinkCmdMount::_ack_vehicle_command(vehicle_command_s *cmd) +InputMavlinkCmdMount::UpdateResult +InputMavlinkCmdMount::_process_command(ControlData &control_data, const vehicle_command_s &vehicle_command) +{ + // Process only if the command is for us or for anyone (component id 0). + const bool sysid_correct = (vehicle_command.target_system == _parameters.mav_sysid); + const bool compid_correct = ((vehicle_command.target_component == _parameters.mav_compid) || + (vehicle_command.target_component == 0)); + + if (!sysid_correct || !compid_correct) { + return UpdateResult::NoUpdate; + } + + if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL) { + + UpdateResult update_result = UpdateResult::NoUpdate; + + switch ((int) vehicle_command.param7) { + case vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT: + control_data.gimbal_shutter_retract = true; + + // fallthrough + case vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL: + control_data.type = ControlData::Type::Neutral; + control_data.sysid_primary_control = vehicle_command.source_system; + control_data.compid_primary_control = vehicle_command.source_component; + update_result = UpdateResult::UpdatedActiveOnce; + break; + + case vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING: { + 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; + + // vmount spec has roll on channel 0, MAVLink spec has pitch on channel 0 + const float roll = math::radians(vehicle_command.param2); + // vmount spec has pitch on channel 1, MAVLink spec has roll on channel 1 + const float pitch = math::radians(vehicle_command.param1); + // both specs have yaw on channel 2 + float yaw = math::radians(vehicle_command.param3); + + matrix::Eulerf euler(roll, pitch, yaw); + + matrix::Quatf q(euler); + q.copyTo(control_data.type_data.angle.q); + + 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; + control_data.sysid_primary_control = vehicle_command.source_system; + control_data.compid_primary_control = vehicle_command.source_component; + update_result = UpdateResult::UpdatedActive; + } + break; + + case vehicle_command_s::VEHICLE_MOUNT_MODE_RC_TARGETING: + // Take over control ourselves, that's supposedly what RC means. + control_data.sysid_primary_control = _parameters.mav_sysid; + control_data.compid_primary_control = _parameters.mav_compid; + update_result = UpdateResult::NoUpdate; + break; + + case vehicle_command_s::VEHICLE_MOUNT_MODE_GPS_POINT: + control_data_set_lon_lat(control_data, (double)vehicle_command.param6, (double)vehicle_command.param5, + vehicle_command.param4); + control_data.sysid_primary_control = vehicle_command.source_system; + control_data.compid_primary_control = vehicle_command.source_component; + update_result = UpdateResult::UpdatedActive; + break; + } + + _ack_vehicle_command(vehicle_command); + return update_result; + + } else if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE) { + + // Stabilization params are ignored. Use MNT_DO_STAB param instead. + + const int params[] = { + (int)((float) vehicle_command.param5 + 0.5f), + (int)((float) vehicle_command.param6 + 0.5f), + (int)(vehicle_command.param7 + 0.5f) + }; + + for (int i = 0; i < 3; ++i) { + + if (params[i] == 0) { + control_data.type_data.angle.frames[i] = + ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; + + } else if (params[i] == 1) { + control_data.type_data.angle.frames[i] = + ControlData::TypeData::TypeAngle::Frame::AngularRate; + + } else if (params[i] == 2) { + control_data.type_data.angle.frames[i] = + ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame; + + } else { + // Not supported, fallback to body angle. + control_data.type_data.angle.frames[i] = + ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; + } + } + + control_data.sysid_primary_control = vehicle_command.source_system; + control_data.compid_primary_control = vehicle_command.source_component; + + _ack_vehicle_command(vehicle_command); + return UpdateResult::UpdatedActive; + } + + return UpdateResult::NoUpdate; +} + +void InputMavlinkCmdMount::_ack_vehicle_command(const vehicle_command_s &cmd) { vehicle_command_ack_s vehicle_command_ack{}; vehicle_command_ack.timestamp = hrt_absolute_time(); - vehicle_command_ack.command = cmd->command; + vehicle_command_ack.command = cmd.command; vehicle_command_ack.result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; - vehicle_command_ack.target_system = cmd->source_system; - vehicle_command_ack.target_component = cmd->source_component; + vehicle_command_ack.target_system = cmd.source_system; + vehicle_command_ack.target_component = cmd.source_component; uORB::Publication cmd_ack_pub{ORB_ID(vehicle_command_ack)}; cmd_ack_pub.publish(vehicle_command_ack); } -void InputMavlinkCmdMount::print_status() +void InputMavlinkCmdMount::print_status() const { PX4_INFO("Input: Mavlink (CMD_MOUNT)"); } -InputMavlinkGimbalV2::InputMavlinkGimbalV2(uint8_t mav_sys_id, uint8_t mav_comp_id, - float &mnt_rate_pitch, float &mnt_rate_yaw) : - _mav_sys_id(mav_sys_id), - _mav_comp_id(mav_comp_id), - _mnt_rate_pitch(mnt_rate_pitch), - _mnt_rate_yaw(mnt_rate_yaw) +InputMavlinkGimbalV2::InputMavlinkGimbalV2(Parameters ¶meters) : + InputBase(parameters) { _stream_gimbal_manager_information(); } @@ -404,7 +403,7 @@ InputMavlinkGimbalV2::~InputMavlinkGimbalV2() } -void InputMavlinkGimbalV2::print_status() +void InputMavlinkGimbalV2::print_status() const { PX4_INFO("Input: Mavlink (Gimbal V2)"); } @@ -423,7 +422,7 @@ int InputMavlinkGimbalV2::initialize() return -errno; } - _gimbal_manager_set_attitude_sub = orb_subscribe(ORB_ID(gimbal_manager_set_attitude)); + _gimbal_manager_set_attitude_sub = orb_subscribe(ORB_ID(gimbal_manager_set_attitude)); if (_gimbal_manager_set_attitude_sub < 0) { return -errno; @@ -445,7 +444,7 @@ int InputMavlinkGimbalV2::initialize() return 0; } -void InputMavlinkGimbalV2::_stream_gimbal_manager_status() +void InputMavlinkGimbalV2::_stream_gimbal_manager_status(const ControlData &control_data) { gimbal_device_attitude_status_s gimbal_device_attitude_status{}; @@ -456,8 +455,8 @@ void InputMavlinkGimbalV2::_stream_gimbal_manager_status() gimbal_manager_status.timestamp = hrt_absolute_time(); gimbal_manager_status.flags = gimbal_device_attitude_status.device_flags; gimbal_manager_status.gimbal_device_id = 0; - gimbal_manager_status.primary_control_sysid = _sys_id_primary_control; - gimbal_manager_status.primary_control_compid = _comp_id_primary_control; + gimbal_manager_status.primary_control_sysid = control_data.sysid_primary_control; + gimbal_manager_status.primary_control_compid = control_data.compid_primary_control; gimbal_manager_status.secondary_control_sysid = 0; // TODO: support secondary control gimbal_manager_status.secondary_control_compid = 0; // TODO: support secondary control _gimbal_manager_status_pub.publish(gimbal_manager_status); @@ -488,424 +487,487 @@ void InputMavlinkGimbalV2::_stream_gimbal_manager_information() _gimbal_manager_info_pub.publish(gimbal_manager_info); } -int InputMavlinkGimbalV2::update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active) +InputMavlinkGimbalV2::UpdateResult +InputMavlinkGimbalV2::update(unsigned int timeout_ms, ControlData &control_data, bool already_active) { - _stream_gimbal_manager_status(); - - // Default to no change, set if we receive anything. - *control_data = nullptr; const int num_poll = 5; px4_pollfd_struct_t polls[num_poll]; - polls[0].fd = _gimbal_manager_set_attitude_sub; - polls[0].events = POLLIN; - polls[1].fd = _vehicle_roi_sub; - polls[1].events = POLLIN; - polls[2].fd = _position_setpoint_triplet_sub; - polls[2].events = POLLIN; - polls[3].fd = _vehicle_command_sub; - polls[3].events = POLLIN; - polls[4].fd = _gimbal_manager_set_manual_control_sub; - polls[4].events = POLLIN; + polls[0].fd = _gimbal_manager_set_attitude_sub; + polls[0].events = POLLIN; + polls[1].fd = _vehicle_roi_sub; + polls[1].events = POLLIN; + polls[2].fd = _position_setpoint_triplet_sub; + polls[2].events = POLLIN; + polls[3].fd = _vehicle_command_sub; + polls[3].events = POLLIN; + polls[4].fd = _gimbal_manager_set_manual_control_sub; + polls[4].events = POLLIN; - int poll_timeout = (int)timeout_ms; + int poll_timeout = (int) timeout_ms; + + hrt_abstime poll_start = hrt_absolute_time(); + + // If we get a command that we need to handle we exit the loop, otherwise we poll until we + // reach the timeout. + // We can't return early instead because we need to copy all topics that triggered poll. bool exit_loop = false; + UpdateResult update_result = already_active ? UpdateResult::UpdatedActive : UpdateResult::NoUpdate; while (!exit_loop && poll_timeout >= 0) { - hrt_abstime poll_start = hrt_absolute_time(); - int ret = px4_poll(polls, num_poll, poll_timeout); + const int ret = px4_poll(polls, num_poll, poll_timeout); - if (ret < 0) { - return -errno; + if (ret <= 0) { + // Error, or timeout, give up. + exit_loop = true; } - poll_timeout -= (hrt_absolute_time() - poll_start) / 1000; + if (polls[0].revents & POLLIN) { + gimbal_manager_set_attitude_s set_attitude; + orb_copy(ORB_ID(gimbal_manager_set_attitude), _gimbal_manager_set_attitude_sub, &set_attitude); - // if we get a command that we need to handle, we exit the loop, otherwise we poll until we reach the timeout - exit_loop = true; + update_result = _process_set_attitude(control_data, set_attitude); + } - if (ret == 0) { - // Timeout control_data already null. + if (polls[1].revents & POLLIN) { + vehicle_roi_s vehicle_roi; + orb_copy(ORB_ID(vehicle_roi), _vehicle_roi_sub, &vehicle_roi); + + UpdateResult new_result = _process_vehicle_roi(control_data, vehicle_roi); + + if (new_result != UpdateResult::NoUpdate) { + update_result = new_result; + } + } + + // check whether the position setpoint got updated + if (polls[2].revents & POLLIN) { + position_setpoint_triplet_s position_setpoint_triplet; + orb_copy(ORB_ID(position_setpoint_triplet), _position_setpoint_triplet_sub, + &position_setpoint_triplet); + + UpdateResult new_result = _process_position_setpoint_triplet(control_data, position_setpoint_triplet); + + if (new_result != UpdateResult::NoUpdate) { + update_result = new_result; + } + } + + if (polls[3].revents & POLLIN) { + vehicle_command_s vehicle_command; + orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &vehicle_command); + + UpdateResult new_result = _process_command(control_data, vehicle_command); + + if (new_result != UpdateResult::NoUpdate) { + update_result = new_result; + } + } + + if (polls[4].revents & POLLIN) { + gimbal_manager_set_manual_control_s set_manual_control; + orb_copy(ORB_ID(gimbal_manager_set_manual_control), _gimbal_manager_set_manual_control_sub, + &set_manual_control); + + update_result = _process_set_manual_control(control_data, set_manual_control); + } + + poll_timeout = timeout_ms - (hrt_absolute_time() - poll_start) / 1000; + } + + _stream_gimbal_manager_status(control_data); + + return update_result; +} + +InputMavlinkGimbalV2::UpdateResult InputMavlinkGimbalV2::_process_set_attitude(ControlData &control_data, + const gimbal_manager_set_attitude_s &set_attitude) +{ + + if (set_attitude.origin_sysid == control_data.sysid_primary_control && + set_attitude.origin_compid == control_data.compid_primary_control) { + const matrix::Quatf q(set_attitude.q); + const matrix::Vector3f angular_velocity(set_attitude.angular_velocity_x, + set_attitude.angular_velocity_y, + set_attitude.angular_velocity_z); + + _set_control_data_from_set_attitude(control_data, set_attitude.flags, q, angular_velocity); + return UpdateResult::UpdatedActive; + + } else { + PX4_DEBUG("Ignoring gimbal_manager_set_attitude from %d/%d", + set_attitude.origin_sysid, set_attitude.origin_compid); + return UpdateResult::UpdatedNotActive; + } +} + +InputMavlinkGimbalV2::UpdateResult InputMavlinkGimbalV2::_process_vehicle_roi(ControlData &control_data, + const vehicle_roi_s &vehicle_roi) +{ + control_data.gimbal_shutter_retract = false; + + if (vehicle_roi.mode == vehicle_roi_s::ROI_NONE) { + + control_data.type = ControlData::Type::Neutral; + _cur_roi_mode = vehicle_roi.mode; + return UpdateResult::UpdatedActiveOnce; + + } else if (vehicle_roi.mode == vehicle_roi_s::ROI_WPNEXT) { + control_data.type = ControlData::Type::LonLat; + _read_control_data_from_position_setpoint_sub(control_data); + control_data.type_data.lonlat.pitch_fixed_angle = -10.f; + + control_data.type_data.lonlat.roll_offset = vehicle_roi.roll_offset; + control_data.type_data.lonlat.pitch_offset = vehicle_roi.pitch_offset; + control_data.type_data.lonlat.yaw_offset = vehicle_roi.yaw_offset; + + _cur_roi_mode = vehicle_roi.mode; + + return UpdateResult::UpdatedActive; + + } else if (vehicle_roi.mode == vehicle_roi_s::ROI_LOCATION) { + control_data_set_lon_lat(control_data, vehicle_roi.lon, vehicle_roi.lat, vehicle_roi.alt); + + _cur_roi_mode = vehicle_roi.mode; + + return UpdateResult::UpdatedActive; + + } else if (vehicle_roi.mode == vehicle_roi_s::ROI_TARGET) { + //TODO is this even supported? + + return UpdateResult::NoUpdate; + + } + + return UpdateResult::NoUpdate; +} + +InputMavlinkGimbalV2::UpdateResult InputMavlinkGimbalV2::_process_position_setpoint_triplet(ControlData &control_data, + const position_setpoint_triplet_s &position_setpoint_triplet) +{ + if (_cur_roi_mode == vehicle_roi_s::ROI_WPNEXT) { + control_data.type_data.lonlat.lon = position_setpoint_triplet.current.lon; + control_data.type_data.lonlat.lat = position_setpoint_triplet.current.lat; + control_data.type_data.lonlat.altitude = position_setpoint_triplet.current.alt; + return UpdateResult::UpdatedActive; + + } else { + return UpdateResult::NoUpdate; + } +} + +InputMavlinkGimbalV2::UpdateResult +InputMavlinkGimbalV2::_process_command(ControlData &control_data, const vehicle_command_s &vehicle_command) +{ + // Process only if the command is for us or for anyone (component id 0). + const bool sysid_correct = + (vehicle_command.target_system == _parameters.mav_sysid) || (vehicle_command.target_system == 0); + const bool compid_correct = ((vehicle_command.target_component == _parameters.mav_compid) || + (vehicle_command.target_component == 0)); + + if (!sysid_correct || !compid_correct) { + return UpdateResult::NoUpdate; + } + + if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL) { + // FIXME: Remove me later. For now, we support this for legacy missions + // using gimbal v1 protocol. + + UpdateResult update_result = UpdateResult::NoUpdate; + + switch ((int) vehicle_command.param7) { + case vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT: + control_data.gimbal_shutter_retract = true; + + // fallthrough + + case vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL: + control_data.type = ControlData::Type::Neutral; + update_result = InputBase::UpdateResult::UpdatedActiveOnce; + break; + + case vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING: { + 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; + + // vmount spec has roll on channel 0, MAVLink spec has pitch on channel 0 + const float roll = math::radians(vehicle_command.param2); + // vmount spec has pitch on channel 1, MAVLink spec has roll on channel 1 + const float pitch = math::radians(vehicle_command.param1); + // both specs have yaw on channel 2 + float yaw = math::radians(vehicle_command.param3); + + // We expect angle of [-pi..+pi]. If the input range is [0..2pi] we can fix that. + if (yaw > M_PI_F) { + yaw -= 2 * M_PI_F; + } + + matrix::Eulerf euler(roll, pitch, yaw); + + matrix::Quatf q(euler); + q.copyTo(control_data.type_data.angle.q); + + 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; + update_result = InputBase::UpdateResult::UpdatedActive; + } + break; + + case vehicle_command_s::VEHICLE_MOUNT_MODE_RC_TARGETING: + update_result = UpdateResult::NoUpdate; + break; + + case vehicle_command_s::VEHICLE_MOUNT_MODE_GPS_POINT: + control_data_set_lon_lat(control_data, (double)vehicle_command.param6, (double)vehicle_command.param5, + vehicle_command.param4); + update_result = UpdateResult::UpdatedActive; + break; + } + + _ack_vehicle_command(vehicle_command, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + return update_result; + + } else if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE) { + + // Stabilization params are ignored. Use MNT_DO_STAB param instead. + + const int params[] = { + (int)((float) vehicle_command.param5 + 0.5f), + (int)((float) vehicle_command.param6 + 0.5f), + (int)(vehicle_command.param7 + 0.5f) + }; + + for (int i = 0; i < 3; ++i) { + + if (params[i] == 0) { + control_data.type_data.angle.frames[i] = + ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; + + } else if (params[i] == 1) { + control_data.type_data.angle.frames[i] = + ControlData::TypeData::TypeAngle::Frame::AngularRate; + + } else if (params[i] == 2) { + control_data.type_data.angle.frames[i] = + ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame; + + } else { + // Not supported, fallback to body angle. + control_data.type_data.angle.frames[i] = + ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; + } + } + + _ack_vehicle_command(vehicle_command, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + return UpdateResult::UpdatedActive; + + } else if (vehicle_command.command == + vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE) { + + const int param_sysid = roundf(vehicle_command.param1); + const int param_compid = roundf(vehicle_command.param2); + + uint8_t new_sysid_primary_control = [&]() { + if (param_sysid >= 0 && param_sysid < 256) { + // Valid new sysid. + return (uint8_t) param_sysid; + + } else if (param_sysid == -1) { + // leave unchanged + return control_data.sysid_primary_control; + + } else if (param_sysid == -2) { + // set itself + return (uint8_t) _parameters.mav_sysid; + + } else if (param_sysid == -3) { + // release control if in control + if (control_data.sysid_primary_control == vehicle_command.source_system) { + return (uint8_t) 0; + + } else { + return control_data.sysid_primary_control; + } + + } else { + PX4_WARN("Unknown param1 value for DO_GIMBAL_MANAGER_CONFIGURE"); + return control_data.sysid_primary_control; + } + }(); + + uint8_t new_compid_primary_control = [&]() { + if (param_compid >= 0 && param_compid < 256) { + // Valid new compid. + return (uint8_t) param_compid; + + } else if (param_compid == -1) { + // leave unchanged + return control_data.compid_primary_control; + + } else if (param_compid == -2) { + // set itself + return (uint8_t) _parameters.mav_compid; + + } else if (param_compid == -3) { + // release control if in control + if (control_data.compid_primary_control == vehicle_command.source_component) { + return (uint8_t) 0; + + } else { + return control_data.compid_primary_control; + } + + } else { + PX4_WARN("Unknown param2 value for DO_GIMBAL_MANAGER_CONFIGURE"); + return control_data.compid_primary_control; + } + }(); + + _ack_vehicle_command(vehicle_command, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + + if (new_sysid_primary_control != control_data.sysid_primary_control || + new_compid_primary_control != control_data.compid_primary_control) { + PX4_INFO("Configured primary gimbal control sysid/compid from %d/%d to %d/%d", + control_data.sysid_primary_control, control_data.compid_primary_control, + new_sysid_primary_control, new_compid_primary_control); + control_data.sysid_primary_control = new_sysid_primary_control; + control_data.compid_primary_control = new_compid_primary_control; + } + + return UpdateResult::UpdatedActive; + + // TODO: support secondary control + // TODO: support gimbal device id for multiple gimbals + + } else if (vehicle_command.command == + vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_PITCHYAW) { + + if (vehicle_command.source_system == control_data.sysid_primary_control && + vehicle_command.source_component == control_data.compid_primary_control) { + + const matrix::Eulerf euler(0.0f, math::radians(vehicle_command.param1), + math::radians(vehicle_command.param2)); + const matrix::Quatf q(euler); + const matrix::Vector3f angular_velocity(0.0f, vehicle_command.param3, + vehicle_command.param4); + const uint32_t flags = vehicle_command.param5; + + // TODO: support gimbal device id for multiple gimbals + + _set_control_data_from_set_attitude(control_data, flags, q, angular_velocity); + _ack_vehicle_command(vehicle_command, + vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + + return UpdateResult::UpdatedActive; } else { - if (polls[0].revents & POLLIN) { - gimbal_manager_set_attitude_s set_attitude; - orb_copy(ORB_ID(gimbal_manager_set_attitude), _gimbal_manager_set_attitude_sub, &set_attitude); - - if (set_attitude.origin_sysid == _sys_id_primary_control && - set_attitude.origin_compid == _comp_id_primary_control) { - const matrix::Quatf q(set_attitude.q); - const matrix::Vector3f angular_velocity(set_attitude.angular_velocity_x, - set_attitude.angular_velocity_y, - set_attitude.angular_velocity_z); - - _set_control_data_from_set_attitude(set_attitude.flags, q, angular_velocity); - *control_data = &_control_data; - - } else { - PX4_DEBUG("Ignoring gimbal_manager_set_attitude from %d/%d", - set_attitude.origin_sysid, set_attitude.origin_compid); - } - } - - if (polls[1].revents & POLLIN) { - vehicle_roi_s vehicle_roi; - orb_copy(ORB_ID(vehicle_roi), _vehicle_roi_sub, &vehicle_roi); - - _control_data.gimbal_shutter_retract = false; - - if (vehicle_roi.mode == vehicle_roi_s::ROI_NONE) { - - _control_data.type = ControlData::Type::Neutral; - *control_data = &_control_data; - _cur_roi_mode = vehicle_roi.mode; - - } else if (vehicle_roi.mode == vehicle_roi_s::ROI_WPNEXT) { - _control_data.type = ControlData::Type::LonLat; - _read_control_data_from_position_setpoint_sub(); - _control_data.type_data.lonlat.pitch_fixed_angle = -10.f; - - _control_data.type_data.lonlat.roll_angle = vehicle_roi.roll_offset; - _control_data.type_data.lonlat.pitch_angle_offset = vehicle_roi.pitch_offset; - _control_data.type_data.lonlat.yaw_angle_offset = vehicle_roi.yaw_offset; - - *control_data = &_control_data; - _cur_roi_mode = vehicle_roi.mode; - - } else if (vehicle_roi.mode == vehicle_roi_s::ROI_LOCATION) { - control_data_set_lon_lat(vehicle_roi.lon, vehicle_roi.lat, vehicle_roi.alt); - - *control_data = &_control_data; - _cur_roi_mode = vehicle_roi.mode; - - } else if (vehicle_roi.mode == vehicle_roi_s::ROI_TARGET) { - //TODO is this even suported? - exit_loop = false; - - } else { - exit_loop = false; - } - } - - // check whether the position setpoint got updated - if (polls[2].revents & POLLIN) { - if (_cur_roi_mode == vehicle_roi_s::ROI_WPNEXT) { - _read_control_data_from_position_setpoint_sub(); - *control_data = &_control_data; - - } else { // must do an orb_copy() in *every* case - position_setpoint_triplet_s position_setpoint_triplet; - orb_copy(ORB_ID(position_setpoint_triplet), _position_setpoint_triplet_sub, &position_setpoint_triplet); - exit_loop = false; - } - } - - if (polls[3].revents & POLLIN) { - vehicle_command_s vehicle_command; - orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &vehicle_command); - - // Process only if the command is for us or for anyone (component id 0). - const bool sysid_correct = (vehicle_command.target_system == _mav_sys_id) || (vehicle_command.target_system == 0); - const bool compid_correct = ((vehicle_command.target_component == _mav_comp_id) || - (vehicle_command.target_component == 0)); - - if (!sysid_correct || !compid_correct) { - exit_loop = false; - continue; - } - - if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL) { - // FIXME: Remove me later. For now, we support this for legacy missions - // using gimbal v1 protocol. - - switch ((int)vehicle_command.param7) { - case vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT: - _control_data.gimbal_shutter_retract = true; - - /* FALLTHROUGH */ - - case vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL: - _control_data.type = ControlData::Type::Neutral; - - *control_data = &_control_data; - break; - - case vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING: { - _control_data.type = ControlData::Type::Angle; - _control_data.type_data.angle.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; - - // vmount spec has roll on channel 0, MAVLink spec has pitch on channel 0 - const float roll = math::radians(vehicle_command.param2); - // vmount spec has pitch on channel 1, MAVLink spec has roll on channel 1 - const float pitch = math::radians(vehicle_command.param1); - // both specs have yaw on channel 2 - float yaw = math::radians(vehicle_command.param3); - - // We expect angle of [-pi..+pi]. If the input range is [0..2pi] we can fix that. - if (yaw > M_PI_F) { - yaw -= 2 * M_PI_F; - } - - matrix::Eulerf euler(roll, pitch, yaw); - - matrix::Quatf q(euler); - q.copyTo(_control_data.type_data.angle.q); - - _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; - - *control_data = &_control_data; - } - break; - - case vehicle_command_s::VEHICLE_MOUNT_MODE_RC_TARGETING: - break; - - case vehicle_command_s::VEHICLE_MOUNT_MODE_GPS_POINT: - control_data_set_lon_lat((double)vehicle_command.param6, (double)vehicle_command.param5, vehicle_command.param4); - - *control_data = &_control_data; - break; - } - - _ack_vehicle_command(&vehicle_command, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); - - } else if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE) { - - _control_data.stabilize_axis[0] = (int)(vehicle_command.param2 + 0.5f) == 1; - _control_data.stabilize_axis[1] = (int)(vehicle_command.param3 + 0.5f) == 1; - _control_data.stabilize_axis[2] = (int)(vehicle_command.param4 + 0.5f) == 1; - - - const int params[] = { - (int)((float)vehicle_command.param5 + 0.5f), - (int)((float)vehicle_command.param6 + 0.5f), - (int)(vehicle_command.param7 + 0.5f) - }; - - for (int i = 0; i < 3; ++i) { - - if (params[i] == 0) { - _control_data.type_data.angle.frames[i] = - ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; - - } else if (params[i] == 1) { - _control_data.type_data.angle.frames[i] = - ControlData::TypeData::TypeAngle::Frame::AngularRate; - - } else if (params[i] == 2) { - _control_data.type_data.angle.frames[i] = - ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame; - - } else { - // Not supported, fallback to body angle. - _control_data.type_data.angle.frames[i] = - ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; - } - } - - _control_data.type = ControlData::Type::Neutral; //always switch to neutral position - - *control_data = &_control_data; - _ack_vehicle_command(&vehicle_command, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); - - } else if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE) { - - const int param_sys_id = roundf(vehicle_command.param1); - const int param_comp_id = roundf(vehicle_command.param2); - - uint8_t new_sys_id_primary_control = [&]() { - if (param_sys_id >= 0 && param_sys_id < 256) { - // Valid new sysid. - return (uint8_t)param_sys_id; - - } else if (param_sys_id == -1) { - // leave unchanged - return _sys_id_primary_control; - - } else if (param_sys_id == -2) { - // set itself - return _mav_sys_id; - - } else if (param_sys_id == -3) { - // release control if in control - if (_sys_id_primary_control == vehicle_command.source_system) { - return (uint8_t)0; - - } else { - return _sys_id_primary_control; - } - - } else { - PX4_WARN("Unknown param1 value for DO_GIMBAL_MANAGER_CONFIGURE"); - return _sys_id_primary_control; - } - }(); - - uint8_t new_comp_id_primary_control = [&]() { - if (param_comp_id >= 0 && param_comp_id < 256) { - // Valid new compid. - return (uint8_t)param_comp_id; - - } else if (param_comp_id == -1) { - // leave unchanged - return _comp_id_primary_control; - - } else if (param_comp_id == -2) { - // set itself - return _mav_comp_id; - - } else if (param_comp_id == -3) { - // release control if in control - if (_comp_id_primary_control == vehicle_command.source_component) { - return (uint8_t)0; - - } else { - return _comp_id_primary_control; - } - - } else { - PX4_WARN("Unknown param2 value for DO_GIMBAL_MANAGER_CONFIGURE"); - return _comp_id_primary_control; - } - }(); - - - if (new_sys_id_primary_control != _sys_id_primary_control || - new_comp_id_primary_control != _comp_id_primary_control) { - PX4_INFO("Configured primary gimbal control sysid/compid from %d/%d to %d/%d", - _sys_id_primary_control, _comp_id_primary_control, - new_sys_id_primary_control, new_comp_id_primary_control); - _sys_id_primary_control = new_sys_id_primary_control; - _comp_id_primary_control = new_comp_id_primary_control; - } - - // TODO: support secondary control - // TODO: support gimbal device id for multiple gimbals - - _ack_vehicle_command(&vehicle_command, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); - - } else if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_PITCHYAW) { - - if (vehicle_command.source_system == _sys_id_primary_control && - vehicle_command.source_component == _comp_id_primary_control) { - - const matrix::Eulerf euler(0.0f, math::radians(vehicle_command.param1), math::radians(vehicle_command.param2)); - const matrix::Quatf q(euler); - const matrix::Vector3f angular_velocity(0.0f, vehicle_command.param3, vehicle_command.param4); - const uint32_t flags = vehicle_command.param5; - - // TODO: support gimbal device id for multiple gimbals - - _set_control_data_from_set_attitude(flags, q, angular_velocity); - *control_data = &_control_data; - _ack_vehicle_command(&vehicle_command, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); - - } else { - PX4_INFO("GIMBAL_MANAGER_PITCHYAW from %d/%d denied, in control: %d/%d", - vehicle_command.source_system, - vehicle_command.source_component, - _sys_id_primary_control, _comp_id_primary_control); - _ack_vehicle_command(&vehicle_command, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED); - } - - } else { - exit_loop = false; - } - - } - - if (polls[4].revents & POLLIN) { - gimbal_manager_set_manual_control_s set_manual_control; - orb_copy(ORB_ID(gimbal_manager_set_manual_control), _gimbal_manager_set_manual_control_sub, &set_manual_control); - - if (set_manual_control.origin_sysid == _sys_id_primary_control && - set_manual_control.origin_compid == _comp_id_primary_control) { - - const matrix::Quatf q = - (PX4_ISFINITE(set_manual_control.pitch) && PX4_ISFINITE(set_manual_control.yaw)) ? - matrix::Quatf(matrix::Eulerf(0.0f, set_manual_control.pitch, set_manual_control.yaw)) : - matrix::Quatf(NAN, NAN, NAN, NAN); - - const matrix::Vector3f angular_velocity = - (PX4_ISFINITE(set_manual_control.pitch_rate) && PX4_ISFINITE(set_manual_control.yaw_rate)) ? - matrix::Vector3f(0.0f, - math::radians(_mnt_rate_pitch) * set_manual_control.pitch_rate, - math::radians(_mnt_rate_yaw) * set_manual_control.yaw_rate) : - matrix::Vector3f(NAN, NAN, NAN); - - _set_control_data_from_set_attitude(set_manual_control.flags, q, angular_velocity); - *control_data = &_control_data; - - } else { - PX4_DEBUG("Ignoring gimbal_manager_set_manual_control from %d/%d", - set_manual_control.origin_sysid, set_manual_control.origin_compid); - } - } + PX4_INFO("GIMBAL_MANAGER_PITCHYAW from %d/%d denied, in control: %d/%d", + vehicle_command.source_system, + vehicle_command.source_component, + control_data.sysid_primary_control, control_data.compid_primary_control); + _ack_vehicle_command(vehicle_command, + vehicle_command_s::VEHICLE_CMD_RESULT_DENIED); + + return UpdateResult::UpdatedNotActive; } } - return 0; + return UpdateResult::NoUpdate; } -void InputMavlinkGimbalV2::_set_control_data_from_set_attitude(const uint32_t flags, const matrix::Quatf &q, +InputMavlinkGimbalV2::UpdateResult InputMavlinkGimbalV2::_process_set_manual_control(ControlData &control_data, + const gimbal_manager_set_manual_control_s &set_manual_control) +{ + if (set_manual_control.origin_sysid == control_data.sysid_primary_control && + set_manual_control.origin_compid == control_data.compid_primary_control) { + + const matrix::Quatf q = + (PX4_ISFINITE(set_manual_control.pitch) && PX4_ISFINITE(set_manual_control.yaw)) + ? + matrix::Quatf( + matrix::Eulerf(0.0f, set_manual_control.pitch, set_manual_control.yaw)) + : + matrix::Quatf(NAN, NAN, NAN, NAN); + + const matrix::Vector3f angular_velocity = + (PX4_ISFINITE(set_manual_control.pitch_rate) && + PX4_ISFINITE(set_manual_control.yaw_rate)) ? + matrix::Vector3f(0.0f, + math::radians(_parameters.mnt_rate_pitch) * set_manual_control.pitch_rate, + math::radians(_parameters.mnt_rate_yaw) * set_manual_control.yaw_rate) : + matrix::Vector3f(NAN, NAN, NAN); + + _set_control_data_from_set_attitude(control_data, set_manual_control.flags, q, + angular_velocity); + + return UpdateResult::UpdatedActive; + + } else { + PX4_DEBUG("Ignoring gimbal_manager_set_manual_control from %d/%d", + set_manual_control.origin_sysid, set_manual_control.origin_compid); + return UpdateResult::UpdatedNotActive; + } +} + +void InputMavlinkGimbalV2::_set_control_data_from_set_attitude(ControlData &control_data, const uint32_t flags, + const matrix::Quatf &q, const matrix::Vector3f &angular_velocity) { if ((flags & gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_RETRACT) != 0) { // not implemented in ControlData } else if ((flags & gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_NEUTRAL) != 0) { - _control_data.type = ControlData::Type::Neutral; + control_data.type = ControlData::Type::Neutral; } else { - _control_data.type = ControlData::Type::Angle; + control_data.type = ControlData::Type::Angle; - q.copyTo(_control_data.type_data.angle.q); + q.copyTo(control_data.type_data.angle.q); - _control_data.type_data.angle.angular_velocity[0] = angular_velocity(0); - _control_data.type_data.angle.angular_velocity[1] = angular_velocity(1); - _control_data.type_data.angle.angular_velocity[2] = angular_velocity(2); + control_data.type_data.angle.angular_velocity[0] = angular_velocity(0); + control_data.type_data.angle.angular_velocity[1] = angular_velocity(1); + control_data.type_data.angle.angular_velocity[2] = angular_velocity(2); - _control_data.type_data.angle.frames[0] = (flags & gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_ROLL_LOCK) + control_data.type_data.angle.frames[0] = (flags & gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_ROLL_LOCK) ? ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame : ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; - _control_data.type_data.angle.frames[1] = (flags & gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_PITCH_LOCK) + control_data.type_data.angle.frames[1] = (flags & gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_PITCH_LOCK) ? ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame : ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; - _control_data.type_data.angle.frames[2] = (flags & gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_YAW_LOCK) + control_data.type_data.angle.frames[2] = (flags & gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_YAW_LOCK) ? ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame : ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; } } //TODO move this one to input.cpp such that it can be shared across functions -void InputMavlinkGimbalV2::_ack_vehicle_command(vehicle_command_s *cmd, uint8_t result) +void InputMavlinkGimbalV2::_ack_vehicle_command(const vehicle_command_s &cmd, uint8_t result) { vehicle_command_ack_s vehicle_command_ack{}; vehicle_command_ack.timestamp = hrt_absolute_time(); - vehicle_command_ack.command = cmd->command; + vehicle_command_ack.command = cmd.command; vehicle_command_ack.result = result; - vehicle_command_ack.target_system = cmd->source_system; - vehicle_command_ack.target_component = cmd->source_component; + vehicle_command_ack.target_system = cmd.source_system; + vehicle_command_ack.target_component = cmd.source_component; uORB::Publication cmd_ack_pub{ORB_ID(vehicle_command_ack)}; cmd_ack_pub.publish(vehicle_command_ack); } -void InputMavlinkGimbalV2::_read_control_data_from_position_setpoint_sub() +void InputMavlinkGimbalV2::_read_control_data_from_position_setpoint_sub(ControlData &control_data) { position_setpoint_triplet_s position_setpoint_triplet; orb_copy(ORB_ID(position_setpoint_triplet), _position_setpoint_triplet_sub, &position_setpoint_triplet); - _control_data.type_data.lonlat.lon = position_setpoint_triplet.current.lon; - _control_data.type_data.lonlat.lat = position_setpoint_triplet.current.lat; - _control_data.type_data.lonlat.altitude = position_setpoint_triplet.current.alt; + control_data.type_data.lonlat.lon = position_setpoint_triplet.current.lon; + control_data.type_data.lonlat.lat = position_setpoint_triplet.current.lat; + control_data.type_data.lonlat.altitude = position_setpoint_triplet.current.alt; } } /* namespace vmount */ diff --git a/src/modules/vmount/input_mavlink.h b/src/modules/vmount/input_mavlink.h index 4c0a165ccc..023f71cae4 100644 --- a/src/modules/vmount/input_mavlink.h +++ b/src/modules/vmount/input_mavlink.h @@ -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 - * - */ #pragma once @@ -48,32 +43,30 @@ #include #include #include +#include +#include +#include #include #include -#include #include 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_info_pub{ORB_ID(gimbal_manager_information)}; uORB::Publication _gimbal_manager_status_pub{ORB_ID(gimbal_manager_status)}; uint8_t _cur_roi_mode = vehicle_roi_s::ROI_NONE; diff --git a/src/modules/vmount/input_rc.cpp b/src/modules/vmount/input_rc.cpp index 0731bb1150..e648e3ff3d 100644 --- a/src/modules/vmount/input_rc.cpp +++ b/src/modules/vmount/input_rc.cpp @@ -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 - * - */ #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 */ diff --git a/src/modules/vmount/input_rc.h b/src/modules/vmount/input_rc.h index 134a1406b6..0b74439c58 100644 --- a/src/modules/vmount/input_rc.h +++ b/src/modules/vmount/input_rc.h @@ -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 - * - */ #pragma once #include "input.h" +#include "vmount_params.h" #include 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 */ diff --git a/src/modules/vmount/input_test.cpp b/src/modules/vmount/input_test.cpp index 4670521ec3..4e556eb51b 100644 --- a/src/modules/vmount/input_test.cpp +++ b/src/modules/vmount/input_test.cpp @@ -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 - * - */ - #include "input_test.h" -#include - #include #include #include @@ -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 */ diff --git a/src/modules/vmount/input_test.h b/src/modules/vmount/input_test.h index 15e3addd4c..7965b42b74 100644 --- a/src/modules/vmount/input_test.h +++ b/src/modules/vmount/input_test.h @@ -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 - * - */ - #pragma once #include "input.h" +#include 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 _has_been_set {false}; }; diff --git a/src/modules/vmount/output.cpp b/src/modules/vmount/output.cpp index bd5c1b75fa..d9383ef5fe 100644 --- a/src/modules/vmount/output.cpp +++ b/src/modules/vmount/output.cpp @@ -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 - * - */ #include "output.h" -#include #include #include @@ -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 */ diff --git a/src/modules/vmount/output.h b/src/modules/vmount/output.h index 791c531999..cda74030b0 100644 --- a/src/modules/vmount/output.h +++ b/src/modules/vmount/output.h @@ -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 - * - */ #pragma once #include "common.h" +#include "vmount_params.h" #include #include #include @@ -47,71 +43,37 @@ #include #include #include -#include 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_pub{ORB_ID(mount_orientation)}; }; diff --git a/src/modules/vmount/output_mavlink.cpp b/src/modules/vmount/output_mavlink.cpp index f44278faef..73a303759f 100644 --- a/src/modules/vmount/output_mavlink.cpp +++ b/src/modules/vmount/output_mavlink.cpp @@ -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 - * - */ #include "output_mavlink.h" -#include #include -#include #include 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(control_data->type_data.angle.frames[0]); - vehicle_command.param6 = static_cast(control_data->type_data.angle.frames[1]); - vehicle_command.param7 = static_cast(control_data->type_data.angle.frames[2]); + vehicle_command.param5 = static_cast(control_data.type_data.angle.frames[0]); + vehicle_command.param6 = static_cast(control_data.type_data.angle.frames[1]); + vehicle_command.param7 = static_cast(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]; diff --git a/src/modules/vmount/output_mavlink.h b/src/modules/vmount/output_mavlink.h index 1d86e47a21..208ab7becb 100644 --- a/src/modules/vmount/output_mavlink.h +++ b/src/modules/vmount/output_mavlink.h @@ -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 - * - */ #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_pub{ORB_ID(vehicle_command)}; + uORB::Publication _gimbal_v1_command_pub{ORB_ID(gimbal_v1_command)}; uORB::Publication _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_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}; diff --git a/src/modules/vmount/output_rc.cpp b/src/modules/vmount/output_rc.cpp index e16a6c9b15..6c730b913b 100644 --- a/src/modules/vmount/output_rc.cpp +++ b/src/modules/vmount/output_rc.cpp @@ -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 - * - */ #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"); } diff --git a/src/modules/vmount/output_rc.h b/src/modules/vmount/output_rc.h index 52839cac69..ddc3610bef 100644 --- a/src/modules/vmount/output_rc.h +++ b/src/modules/vmount/output_rc.h @@ -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 - * - */ #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 */ diff --git a/src/modules/vmount/vmount.cpp b/src/modules/vmount/vmount.cpp index f54544237f..b51ef6de93 100644 --- a/src/modules/vmount/vmount.cpp +++ b/src/modules/vmount/vmount.cpp @@ -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 - * @author Julian Oes - * @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 #include -#include -#include #include -#include -#include -#include -#include -#include #include #include +#include "vmount_params.h" #include "input_mavlink.h" #include "input_rc.h" #include "input_test.h" @@ -66,105 +60,30 @@ #include #include -#include #include +#include 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 thread_should_exit {false}; +static px4::atomic 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 ", "Specify an axis and an angle in degrees", false); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } diff --git a/src/modules/vmount/vmount_params.c b/src/modules/vmount/vmount_params.c index 34a2f186bc..9baa25f03a 100644 --- a/src/modules/vmount/vmount_params.c +++ b/src/modules/vmount/vmount_params.c @@ -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); diff --git a/src/modules/vmount/vmount_params.h b/src/modules/vmount/vmount_params.h new file mode 100644 index 0000000000..52697a5113 --- /dev/null +++ b/src/modules/vmount/vmount_params.h @@ -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 +#include + +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 */