dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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.
This commit is contained in:
Julian Oes 2020-11-17 15:38:49 +01:00 committed by Daniel Agar
parent cb3ad39406
commit b0d7d19bab
6 changed files with 74 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
msg/gimbal_device_information.msg
View File

@ -2,9 +2,12 @@ uint64 timestamp # time since system start (microseconds)
uint8[32] vendor_name
uint8[32] model_name
uint8[32] custom_name
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_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_FOLLOW = 512
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_max # [rad]
float32 yaw_max # [rad]
float32 yaw_min # [rad]
float32 yaw_max # [rad]
uint8 gimbal_device_compid

30
msg/gimbal_manager_information.msg
View File

@ -1,13 +1,29 @@
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
float32 tilt_max # [rad]
float32 tilt_min # [rad]
float32 tilt_rate_max # [rad / s]
float32 roll_min # [rad]
float32 roll_max # [rad]
float32 pan_max # [rad]
float32 pan_min # [rad]
float32 pan_rate_max # [rad / s]
float32 pitch_min # [rad]
float32 pitch_max # [rad]
float32 yaw_min # [rad]
float32 yaw_max # [rad]

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

@ -70,8 +70,8 @@
#include <uORB/topics/estimator_status.h>
#include <uORB/topics/geofence_result.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_manager_information.h>
#include <uORB/topics/gimbal_manager_status.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/position_setpoint_triplet.h>
@ -2078,12 +2078,12 @@ public:
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;
}
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 */
MavlinkStreamGimbalManagerInformation(MavlinkStreamGimbalManagerInformation &) = delete;
@ -2095,22 +2095,21 @@ protected:
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
mavlink_gimbal_manager_information_t msg{};
msg.time_boot_ms = gimbal_device_information.timestamp / 1000;
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.tilt_min = gimbal_device_information.tilt_min;
msg.tilt_rate_max = gimbal_device_information.tilt_rate_max;
msg.pan_max = gimbal_device_information.pan_max;
msg.pan_min = gimbal_device_information.pan_min;
msg.pan_rate_max = gimbal_device_information.pan_rate_max;
msg.roll_min = gimbal_device_information.roll_min;
msg.roll_max = gimbal_device_information.roll_max;
msg.pitch_min = gimbal_device_information.pitch_min;
msg.pitch_max = gimbal_device_information.pitch_max;
msg.yaw_min = gimbal_device_information.yaw_min;
msg.yaw_max = gimbal_device_information.yaw_max;
mavlink_msg_gimbal_manager_information_send_struct(_mavlink->get_channel(), &msg);
@ -2172,7 +2171,6 @@ protected:
gimbal_manager_status_s 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{};
msg.time_boot_ms = gimbal_manager_status.timestamp / 1000;
msg.gimbal_device_id = gimbal_manager_status.gimbal_device_id;
@ -2240,7 +2238,6 @@ protected:
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)) {
// send out gimbal_manager_info with info from gimbal_device_information
mavlink_gimbal_device_set_attitude_t msg{};
msg.flags = gimbal_device_set_attitude.flags;
msg.target_system = gimbal_device_set_attitude.target_system;

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

@ -3012,13 +3012,20 @@ MavlinkReceiver::handle_message_gimbal_device_information(mavlink_message_t *msg
"vendor_name length doesn't match");
static_assert(sizeof(gimbal_information.model_name) == sizeof(gimbal_device_info_msg.model_name),
"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.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.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.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_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_device_information_pub.publish(gimbal_information);
}
/**

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

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

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

@ -46,7 +46,7 @@
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#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/vehicle_command.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 _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)};
map_projection_reference_s _projection_reference = {}; ///< reference to convert (lon, lat) to local [m]
uint8_t _cur_roi_mode = vehicle_roi_s::ROI_NONE;