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

vmount: publish gimbal v2 protocol for v1 gimbals 

This enables the use case where the gimbal v2 protocol is used
between the ground station and the drone, and the gimbal v1 protocol is
used between the drone and the gimbal.
This commit is contained in:
Julian Oes 2021-07-02 16:33:04 +02:00 committed by Daniel Agar
parent 6d74b390d9
commit 90fe4b7a54
2 changed files with 26 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
src/modules/vmount/output_mavlink.cpp
View File

@ -108,11 +108,34 @@ int OutputMavlinkV1::update(const ControlData *control_data)
_vehicle_command_pub.publish(vehicle_command);
_stream_device_attitude_status();
_last_update = t;
return 0;
}
void OutputMavlinkV1::_stream_device_attitude_status()
{
// This enables the use case where the gimbal v2 protocol is used
// between the ground station and the drone, and the gimbal v1 protocol is
// used between the drone and the gimbal.
gimbal_device_attitude_status_s attitude_status{};
attitude_status.timestamp = hrt_absolute_time();
attitude_status.target_system = 0;
attitude_status.target_component = 0;
attitude_status.device_flags = gimbal_device_attitude_status_s::DEVICE_FLAGS_NEUTRAL |
gimbal_device_attitude_status_s::DEVICE_FLAGS_ROLL_LOCK |
gimbal_device_attitude_status_s::DEVICE_FLAGS_PITCH_LOCK;
matrix::Eulerf euler(_angle_outputs[0], _angle_outputs[1], _angle_outputs[2]);
matrix::Quatf q(euler);
q.copyTo(attitude_status.q);
attitude_status.failure_flags = 0;
_attitude_status_pub.publish(attitude_status);
}
void OutputMavlinkV1::print_status()
{
PX4_INFO("Output: MAVLink gimbal protocol v1");

3
src/modules/vmount/output_mavlink.h
View File

@ -45,6 +45,7 @@
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/gimbal_device_set_attitude.h>
#include <uORB/topics/gimbal_device_information.h>
#include <uORB/topics/gimbal_device_attitude_status.h>
namespace vmount
@ -64,7 +65,9 @@ public:
virtual void print_status();
private:
void _stream_device_attitude_status();
uORB::Publication<vehicle_command_s> _vehicle_command_pub{ORB_ID(vehicle_command)};
uORB::Publication <gimbal_device_attitude_status_s> _attitude_status_pub{ORB_ID(gimbal_device_attitude_status)};
};
class OutputMavlinkV2 : public OutputBase