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

vmount/mavlink: update gimbal information message 

We should be using gimbal_manager_information and not
gimbal_device_information. Plus, this updates the fields and flags
according to the MAVLink changes.
release/1.12
Julian Oes 4 years ago committed by Daniel Agar
parent
cb3ad39406
commit
b0d7d19bab
6 changed files with 78 additions and 53 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 15
      msg/gimbal_device_information.msg
  2. 30
      msg/gimbal_manager_information.msg
  3. 27
      src/modules/mavlink/mavlink_messages.cpp
  4. 10
      src/modules/mavlink/mavlink_receiver.cpp
  5. 45
      src/modules/vmount/input_mavlink.cpp
  6. 4
      src/modules/vmount/input_mavlink.h

15
msg/gimbal_device_information.msg
Unescape View File

@ -2,9 +2,12 @@ uint64 timestamp # time since system start (microseconds)
uint8[32] vendor_name uint8[32] vendor_name
uint8[32] model_name uint8[32] model_name
uint8[32] custom_name
uint32 firmware_version uint32 firmware_version
uint32 hardware_version
uint64 uid
uint16 capability_flags uint16 cap_flags
uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_RETRACT = 1 uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_RETRACT = 1
uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_NEUTRAL = 2 uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_NEUTRAL = 2
@ -17,11 +20,17 @@ uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_PITCH_LOCK = 128
uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_YAW_AXIS = 256 uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_YAW_AXIS = 256
uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_YAW_FOLLOW = 512 uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_YAW_FOLLOW = 512
uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_YAW_LOCK = 1024 uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_YAW_LOCK = 1024
uint32 GIMBAL_DEVICE_CAP_FLAGS_SUPPORTS_INFINITE_YAW = 2048
uint16 custom_cap_flags
float32 roll_min # [rad]
float32 roll_max # [rad]
float32 pitch_max # [rad]
float32 pitch_min # [rad] float32 pitch_min # [rad]
float32 pitch_max # [rad]
float32 yaw_max # [rad]
float32 yaw_min # [rad] float32 yaw_min # [rad]
float32 yaw_max # [rad]
uint8 gimbal_device_compid uint8 gimbal_device_compid

30
msg/gimbal_manager_information.msg
Unescape View File

@ -1,13 +1,29 @@
uint64 timestamp # time since system start (microseconds) uint64 timestamp # time since system start (microseconds)
uint32 capability_flags uint32 cap_flags
uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_RETRACT = 1
uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_NEUTRAL = 2
uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_ROLL_AXIS = 4
uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_ROLL_FOLLOW = 8
uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_ROLL_LOCK = 16
uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_PITCH_AXIS = 32
uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_PITCH_FOLLOW = 64
uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_PITCH_LOCK = 128
uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_YAW_AXIS = 256
uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_YAW_FOLLOW = 512
uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_YAW_LOCK = 1024
uint32 GIMBAL_MANAGER_CAP_FLAGS_SUPPORTS_INFINITE_YAW = 2048
uint32 GIMBAL_MANAGER_CAP_FLAGS_CAN_POINT_LOCATION_LOCAL = 65536
uint32 GIMBAL_MANAGER_CAP_FLAGS_CAN_POINT_LOCATION_GLOBAL = 131072
uint8 gimbal_device_id uint8 gimbal_device_id
float32 tilt_max # [rad] float32 roll_min # [rad]
float32 tilt_min # [rad] float32 roll_max # [rad]
float32 tilt_rate_max # [rad / s]
float32 pitch_min # [rad]
float32 pitch_max # [rad]
float32 pan_max # [rad] float32 yaw_min # [rad]
float32 pan_min # [rad] float32 yaw_max # [rad]
float32 pan_rate_max # [rad / s]

27
src/modules/mavlink/mavlink_messages.cpp
Unescape View File

@ -70,8 +70,8 @@
#include <uORB/topics/estimator_status.h> #include <uORB/topics/estimator_status.h>
#include <uORB/topics/geofence_result.h> #include <uORB/topics/geofence_result.h>
#include <uORB/topics/gimbal_device_attitude_status.h> #include <uORB/topics/gimbal_device_attitude_status.h>
#include <uORB/topics/gimbal_device_information.h>
#include <uORB/topics/gimbal_device_set_attitude.h> #include <uORB/topics/gimbal_device_set_attitude.h>
#include <uORB/topics/gimbal_manager_information.h>
#include <uORB/topics/gimbal_manager_status.h> #include <uORB/topics/gimbal_manager_status.h>
#include <uORB/topics/home_position.h> #include <uORB/topics/home_position.h>
#include <uORB/topics/position_setpoint_triplet.h> #include <uORB/topics/position_setpoint_triplet.h>
@ -2078,12 +2078,12 @@ public:
unsigned get_size() override unsigned get_size() override
{ {
return _gimbal_device_information_sub.advertised() ? (MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_LEN + return _gimbal_manager_information_sub.advertised() ? (MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_LEN +
MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
} }
private: private:
uORB::Subscription _gimbal_device_information_sub{ORB_ID(gimbal_device_information)}; uORB::Subscription _gimbal_manager_information_sub{ORB_ID(gimbal_manager_information)};
/* do not allow top copying this class */ /* do not allow top copying this class */
MavlinkStreamGimbalManagerInformation(MavlinkStreamGimbalManagerInformation &) = delete; MavlinkStreamGimbalManagerInformation(MavlinkStreamGimbalManagerInformation &) = delete;
@ -2095,22 +2095,21 @@ protected:
bool send(const hrt_abstime t) override bool send(const hrt_abstime t) override
{ {
gimbal_device_information_s gimbal_device_information; gimbal_manager_information_s gimbal_manager_information;
if (_gimbal_device_information_sub.advertised() && _gimbal_device_information_sub.copy(&gimbal_device_information)) { if (_gimbal_manager_information_sub.advertised() && _gimbal_manager_information_sub.copy(&gimbal_device_information)) {
// send out gimbal_manager_info with info from gimbal_device_information // send out gimbal_manager_info with info from gimbal_device_information
mavlink_gimbal_manager_information_t msg{}; mavlink_gimbal_manager_information_t msg{};
msg.time_boot_ms = gimbal_device_information.timestamp / 1000; msg.time_boot_ms = gimbal_device_information.timestamp / 1000;
msg.gimbal_device_id = 0; msg.gimbal_device_id = 0;
msg.cap_flags = gimbal_device_information.capability_flags; msg.cap_flags = gimbal_device_information.cap_flags;
msg.tilt_max = gimbal_device_information.tilt_max; msg.roll_min = gimbal_device_information.roll_min;
msg.tilt_min = gimbal_device_information.tilt_min; msg.roll_max = gimbal_device_information.roll_max;
msg.tilt_rate_max = gimbal_device_information.tilt_rate_max; msg.pitch_min = gimbal_device_information.pitch_min;
msg.pitch_max = gimbal_device_information.pitch_max;
msg.pan_max = gimbal_device_information.pan_max; msg.yaw_min = gimbal_device_information.yaw_min;
msg.pan_min = gimbal_device_information.pan_min; msg.yaw_max = gimbal_device_information.yaw_max;
msg.pan_rate_max = gimbal_device_information.pan_rate_max;
mavlink_msg_gimbal_manager_information_send_struct(_mavlink->get_channel(), &msg); mavlink_msg_gimbal_manager_information_send_struct(_mavlink->get_channel(), &msg);
@ -2172,7 +2171,6 @@ protected:
gimbal_manager_status_s gimbal_manager_status; gimbal_manager_status_s gimbal_manager_status;
if (_gimbal_manager_status_sub.advertised() && _gimbal_manager_status_sub.copy(&gimbal_manager_status)) { if (_gimbal_manager_status_sub.advertised() && _gimbal_manager_status_sub.copy(&gimbal_manager_status)) {
// send out gimbal_manager_info with info from gimbal_device_information
mavlink_gimbal_manager_status_t msg{}; mavlink_gimbal_manager_status_t msg{};
msg.time_boot_ms = gimbal_manager_status.timestamp / 1000; msg.time_boot_ms = gimbal_manager_status.timestamp / 1000;
msg.gimbal_device_id = gimbal_manager_status.gimbal_device_id; msg.gimbal_device_id = gimbal_manager_status.gimbal_device_id;
@ -2240,7 +2238,6 @@ protected:
gimbal_device_set_attitude_s gimbal_device_set_attitude; gimbal_device_set_attitude_s gimbal_device_set_attitude;
if (_gimbal_device_set_attitude_sub.advertised() && _gimbal_device_set_attitude_sub.copy(&gimbal_device_set_attitude)) { if (_gimbal_device_set_attitude_sub.advertised() && _gimbal_device_set_attitude_sub.copy(&gimbal_device_set_attitude)) {
// send out gimbal_manager_info with info from gimbal_device_information
mavlink_gimbal_device_set_attitude_t msg{}; mavlink_gimbal_device_set_attitude_t msg{};
msg.flags = gimbal_device_set_attitude.flags; msg.flags = gimbal_device_set_attitude.flags;
msg.target_system = gimbal_device_set_attitude.target_system; msg.target_system = gimbal_device_set_attitude.target_system;

10
src/modules/mavlink/mavlink_receiver.cpp
Unescape View File

@ -3012,13 +3012,20 @@ MavlinkReceiver::handle_message_gimbal_device_information(mavlink_message_t *msg
"vendor_name length doesn't match"); "vendor_name length doesn't match");
static_assert(sizeof(gimbal_information.model_name) == sizeof(gimbal_device_info_msg.model_name), static_assert(sizeof(gimbal_information.model_name) == sizeof(gimbal_device_info_msg.model_name),
"model_name length doesn't match"); "model_name length doesn't match");
static_assert(sizeof(gimbal_information.custom_name) == sizeof(gimbal_device_info_msg.custom_name),
"custom_name length doesn't match");
memcpy(gimbal_information.vendor_name, gimbal_device_info_msg.vendor_name, sizeof(gimbal_information.vendor_name)); memcpy(gimbal_information.vendor_name, gimbal_device_info_msg.vendor_name, sizeof(gimbal_information.vendor_name));
memcpy(gimbal_information.model_name, gimbal_device_info_msg.model_name, sizeof(gimbal_information.model_name)); memcpy(gimbal_information.model_name, gimbal_device_info_msg.model_name, sizeof(gimbal_information.model_name));
memcpy(gimbal_information.custom_name, gimbal_device_info_msg.custom_name, sizeof(gimbal_information.custom_name));
gimbal_device_info_msg.vendor_name[sizeof(gimbal_device_info_msg.vendor_name) - 1] = '\0'; gimbal_device_info_msg.vendor_name[sizeof(gimbal_device_info_msg.vendor_name) - 1] = '\0';
gimbal_device_info_msg.model_name[sizeof(gimbal_device_info_msg.model_name) - 1] = '\0'; gimbal_device_info_msg.model_name[sizeof(gimbal_device_info_msg.model_name) - 1] = '\0';
gimbal_device_info_msg.custom_name[sizeof(gimbal_device_info_msg.custom_name) - 1] = '\0';
gimbal_information.firmware_version = gimbal_device_info_msg.firmware_version; gimbal_information.firmware_version = gimbal_device_info_msg.firmware_version;
gimbal_information.capability_flags = gimbal_device_info_msg.cap_flags; gimbal_information.hardware_version = gimbal_device_info_msg.hardware_version;
gimbal_information.cap_flags = gimbal_device_info_msg.cap_flags;
gimbal_information.custom_cap_flags = gimbal_device_info_msg.custom_cap_flags;
gimbal_information.uid = gimbal_device_info_msg.uid;
gimbal_information.pitch_max = gimbal_device_info_msg.pitch_max; gimbal_information.pitch_max = gimbal_device_info_msg.pitch_max;
gimbal_information.pitch_min = gimbal_device_info_msg.pitch_min; gimbal_information.pitch_min = gimbal_device_info_msg.pitch_min;
@ -3029,7 +3036,6 @@ MavlinkReceiver::handle_message_gimbal_device_information(mavlink_message_t *msg
gimbal_information.gimbal_device_compid = msg->compid; gimbal_information.gimbal_device_compid = msg->compid;
_gimbal_device_information_pub.publish(gimbal_information); _gimbal_device_information_pub.publish(gimbal_information);
} }
/** /**

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

@ -40,7 +40,7 @@
#include "input_mavlink.h" #include "input_mavlink.h"
#include <uORB/Publication.hpp> #include <uORB/Publication.hpp>
#include <uORB/topics/gimbal_device_information.h> #include <uORB/topics/gimbal_manager_information.h>
#include <uORB/topics/vehicle_roi.h> #include <uORB/topics/vehicle_roi.h>
#include <uORB/topics/vehicle_command_ack.h> #include <uORB/topics/vehicle_command_ack.h>
#include <uORB/topics/position_setpoint_triplet.h> #include <uORB/topics/position_setpoint_triplet.h>
@ -470,29 +470,26 @@ void InputMavlinkGimbalV2::_stream_gimbal_manager_status()
void InputMavlinkGimbalV2::_stream_gimbal_manager_information() void InputMavlinkGimbalV2::_stream_gimbal_manager_information()
{ {
gimbal_device_information_s gimbal_device_info; // TODO: Take gimbal_device_information into account.
gimbal_device_info.timestamp = hrt_absolute_time();
const char vendor_name[] = "PX4"; gimbal_manager_information_s gimbal_manager_info;
const char model_name[] = "AUX gimbal"; gimbal_manager_info.timestamp = hrt_absolute_time();
strncpy((char *)gimbal_device_info.vendor_name, vendor_name, sizeof(gimbal_device_info.vendor_name)); gimbal_manager_info.cap_flags =
strncpy((char *)gimbal_device_info.model_name, model_name, sizeof(gimbal_device_info.model_name)); gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_NEUTRAL |
gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_ROLL_LOCK |
gimbal_device_info.firmware_version = 0; gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_PITCH_AXIS |
gimbal_device_info.capability_flags = 0; gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_PITCH_LOCK |
gimbal_device_info.capability_flags = gimbal_device_information_s::GIMBAL_DEVICE_CAP_FLAGS_HAS_NEUTRAL | gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_YAW_AXIS |
gimbal_device_information_s::GIMBAL_DEVICE_CAP_FLAGS_HAS_ROLL_LOCK | gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_YAW_LOCK |
gimbal_device_information_s::GIMBAL_DEVICE_CAP_FLAGS_HAS_PITCH_AXIS | gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_CAN_POINT_LOCATION_GLOBAL;
gimbal_device_information_s::GIMBAL_DEVICE_CAP_FLAGS_HAS_PITCH_LOCK |
gimbal_device_information_s::GIMBAL_DEVICE_CAP_FLAGS_HAS_YAW_AXIS | gimbal_manager_info.pitch_max = M_PI_F / 2;
gimbal_device_information_s::GIMBAL_DEVICE_CAP_FLAGS_HAS_YAW_LOCK; gimbal_manager_info.pitch_min = -M_PI_F / 2;
gimbal_manager_info.yaw_max = M_PI_F;
gimbal_device_info.pitch_max = M_PI_F / 2; gimbal_manager_info.yaw_min = -M_PI_F;
gimbal_device_info.pitch_min = -M_PI_F / 2;
gimbal_device_info.yaw_max = M_PI_F; _gimbal_manager_info_pub.publish(gimbal_manager_info);
gimbal_device_info.yaw_min = -M_PI_F;
_gimbal_device_info_pub.publish(gimbal_device_info);
} }
int InputMavlinkGimbalV2::update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active) int InputMavlinkGimbalV2::update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active)

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

@ -46,7 +46,7 @@
#include <uORB/Publication.hpp> #include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp> #include <uORB/Subscription.hpp>
#include <uORB/topics/gimbal_device_attitude_status.h> #include <uORB/topics/gimbal_device_attitude_status.h>
#include <uORB/topics/gimbal_device_information.h> #include <uORB/topics/gimbal_manager_information.h>
#include <uORB/topics/gimbal_manager_status.h> #include <uORB/topics/gimbal_manager_status.h>
#include <uORB/topics/vehicle_command.h> #include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_roi.h> #include <uORB/topics/vehicle_roi.h>
@ -150,7 +150,7 @@ private:
uORB::Subscription _gimbal_device_attitude_status_sub{ORB_ID(gimbal_device_attitude_status)}; 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::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)};
uORB::Publication<gimbal_device_information_s> _gimbal_device_info_pub{ORB_ID(gimbal_device_information)}; uORB::Publication<gimbal_manager_information_s> _gimbal_manager_info_pub{ORB_ID(gimbal_manager_information)};
uORB::Publication<gimbal_manager_status_s> _gimbal_manager_status_pub{ORB_ID(gimbal_manager_status)}; uORB::Publication<gimbal_manager_status_s> _gimbal_manager_status_pub{ORB_ID(gimbal_manager_status)};
map_projection_reference_s _projection_reference = {}; ///< reference to convert (lon, lat) to local [m] map_projection_reference_s _projection_reference = {}; ///< reference to convert (lon, lat) to local [m]
uint8_t _cur_roi_mode = vehicle_roi_s::ROI_NONE; uint8_t _cur_roi_mode = vehicle_roi_s::ROI_NONE;

Write Preview
Loading…
Cancel
Save