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

vmount/navigator/mavlink: gimbal v2 changes 

This is a collection of commits all having to do with changes in the
Mavlink gimbal v2 protocol as described in:
https://mavlink.io/en/services/gimbal_v2.html
This commit is contained in:
Julian Oes 2020-10-09 18:12:53 +02:00 committed by Daniel Agar
parent 422bac4bfd
commit e6b1775bb6
31 changed files with 790 additions and 285 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ROMFS/px4fmu_common/init.d-posix/rcS
View File

@ -243,7 +243,7 @@ then
sh etc/init.d-posix/rc.mavlink_override
else
# GCS link
mavlink start -x -u $udp_gcs_port_local -r 4000000
mavlink start -x -u $udp_gcs_port_local -r 4000000 -f
mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u $udp_gcs_port_local
mavlink stream -r 50 -s LOCAL_POSITION_NED -u $udp_gcs_port_local
mavlink stream -r 50 -s GLOBAL_POSITION_INT -u $udp_gcs_port_local
@ -255,7 +255,7 @@ else
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u $udp_gcs_port_local
# API/Offboard link
mavlink start -x -u $udp_offboard_port_local -r 4000000 -m onboard -o $udp_offboard_port_remote
mavlink start -x -u $udp_offboard_port_local -r 4000000 -f -m onboard -o $udp_offboard_port_remote
# Onboard link to camera
mavlink start -x -u $udp_onboard_payload_port_local -r 4000 -f -m onboard -o $udp_onboard_payload_port_remote

1
msg/CMakeLists.txt
View File

@ -68,6 +68,7 @@ set(msg_files
geofence_result.msg
gimbal_device_set_attitude.msg
gimbal_manager_set_attitude.msg
gimbal_manager_set_manual_control.msg
gimbal_device_attitude_status.msg
gimbal_device_information.msg
gimbal_manager_information.msg

12
msg/gimbal_device_information.msg
View File

@ -18,10 +18,10 @@ 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
float32 tilt_max # [rad]
float32 tilt_min # [rad]
float32 tilt_rate_max # [rad / s]
float32 pitch_max # [rad]
float32 pitch_min # [rad]
float32 pan_max # [rad]
float32 pan_min # [rad]
float32 pan_rate_max # [rad / s]
float32 yaw_max # [rad]
float32 yaw_min # [rad]
uint8 gimbal_device_compid

7
msg/gimbal_manager_set_attitude.msg
View File

@ -1,5 +1,8 @@
uint64 timestamp # time since system start (microseconds)
uint8 origin_sysid
uint8 origin_compid
uint8 target_system
uint8 target_component
@ -8,10 +11,6 @@ uint32 GIMBAL_MANAGER_FLAGS_NEUTRAL = 2
uint32 GIMBAL_MANAGER_FLAGS_ROLL_LOCK = 4
uint32 GIMBAL_MANAGER_FLAGS_PITCH_LOCK = 8
uint32 GIMBAL_MANAGER_FLAGS_YAW_LOCK = 16
uint32 GIMBAL_MANAGER_FLAGS_ANGULAR_VELOCITY_RELATIVE_TO_FOCAL_LENGTH = 1048576
uint32 GIMBAL_MANAGER_FLAGS_NUDGE = 2097152
uint32 GIMBAL_MANAGER_FLAGS_OVERRIDE = 4194304
uint32 GIMBAL_MANAGER_FLAGS_NONE = 8388608
uint32 flags
uint8 gimbal_device_id

21
msg/gimbal_manager_set_manual_control.msg Normal file
View File

@ -0,0 +1,21 @@
uint64 timestamp # time since system start (microseconds)
uint8 origin_sysid
uint8 origin_compid
uint8 target_system
uint8 target_component
uint32 GIMBAL_MANAGER_FLAGS_RETRACT = 1
uint32 GIMBAL_MANAGER_FLAGS_NEUTRAL = 2
uint32 GIMBAL_MANAGER_FLAGS_ROLL_LOCK = 4
uint32 GIMBAL_MANAGER_FLAGS_PITCH_LOCK = 8
uint32 GIMBAL_MANAGER_FLAGS_YAW_LOCK = 16
uint32 flags
uint8 gimbal_device_id
float32 pitch # unitless -1..1, can be NAN
float32 yaw # unitless -1..1, can be NAN
float32 pitch_rate # unitless -1..1, can be NAN
float32 yaw_rate # unitless -1..1, can be NAN

4
msg/gimbal_manager_status.msg
View File

@ -2,3 +2,7 @@ uint64 timestamp # time since system start (microseconds)
uint32 flags
uint8 gimbal_device_id
uint8 primary_control_sysid
uint8 primary_control_compid
uint8 secondary_control_sysid
uint8 secondary_control_compid

4
msg/vehicle_command.msg
View File

@ -75,7 +75,9 @@ uint16 VEHICLE_CMD_START_RX_PAIR = 500 # Starts receiver pairing |0:Spektrum|
uint16 VEHICLE_CMD_REQUEST_MESSAGE = 512 # Request to send a single instance of the specified message
uint16 VEHICLE_CMD_SET_CAMERA_MODE = 530 # Set camera capture mode (photo, video, etc.)
uint16 VEHICLE_CMD_SET_CAMERA_ZOOM = 531 # Set camera zoom
uint16 VEHICLE_CMD_DO_GIMBAL_MANAGER_ATTITUDE = 1000 # Setpoint to be sent to a gimbal manager to set a gimbal attitude
uint16 VEHICLE_CMD_SET_CAMERA_FOCUS = 532
uint16 VEHICLE_CMD_DO_GIMBAL_MANAGER_PITCHYAW = 1000 # Setpoint to be sent to a gimbal manager to set a gimbal pitch and yaw
uint16 VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE = 1001 # Gimbal configuration to set which sysid/compid is in primary and secondary control
uint16 VEHICLE_CMD_DO_TRIGGER_CONTROL = 2003 # Enable or disable on-board camera triggering system
uint16 VEHICLE_CMD_LOGGING_START = 2510 # start streaming ULog data
uint16 VEHICLE_CMD_LOGGING_STOP = 2511 # stop streaming ULog data

1
src/modules/commander/Commander.cpp
View File

@ -1258,6 +1258,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
case vehicle_command_s::VEHICLE_CMD_DO_SET_CAM_TRIGG_INTERVAL:
case vehicle_command_s::VEHICLE_CMD_SET_CAMERA_MODE:
case vehicle_command_s::VEHICLE_CMD_SET_CAMERA_ZOOM:
case vehicle_command_s::VEHICLE_CMD_SET_CAMERA_FOCUS:
case vehicle_command_s::VEHICLE_CMD_DO_CHANGE_SPEED:
case vehicle_command_s::VEHICLE_CMD_DO_LAND_START:
case vehicle_command_s::VEHICLE_CMD_DO_GO_AROUND:

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

@ -1482,6 +1482,111 @@ protected:
}
};
class MavlinkStreamAutopilotStateForGimbalDevice : public MavlinkStream
{
public:
const char *get_name() const override
{
return MavlinkStreamAutopilotStateForGimbalDevice::get_name_static();
}
static constexpr const char *get_name_static()
{
return "AUTOPILOT_STATE_FOR_GIMBAL_DEVICE";
}
static constexpr uint16_t get_id_static()
{
return MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE;
}
uint16_t get_id() override
{
return get_id_static();
}
static MavlinkStream *new_instance(Mavlink *mavlink)
{
return new MavlinkStreamAutopilotStateForGimbalDevice(mavlink);
}
unsigned get_size() override
{
return _att_sub.advertised() ? MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
}
private:
uORB::Subscription _att_sub{ORB_ID(vehicle_attitude)};
uORB::Subscription _lpos_sub{ORB_ID(vehicle_local_position)};
uORB::Subscription _att_sp_sub{ORB_ID(vehicle_attitude_setpoint)};
uORB::Subscription _est_sub{ORB_ID(estimator_status)};
uORB::Subscription _landed_sub{ORB_ID(vehicle_land_detected)};
/* do not allow top copying this class */
MavlinkStreamAutopilotStateForGimbalDevice(MavlinkStreamAutopilotStateForGimbalDevice &) = delete;
MavlinkStreamAutopilotStateForGimbalDevice &operator = (const MavlinkStreamAutopilotStateForGimbalDevice &) = delete;
protected:
explicit MavlinkStreamAutopilotStateForGimbalDevice(Mavlink *mavlink) : MavlinkStream(mavlink)
{}
bool send(const hrt_abstime t) override
{
if (_att_sub.advertised()) {
mavlink_autopilot_state_for_gimbal_device_t msg{};
{
vehicle_attitude_s att{};
_att_sub.copy(&att);
msg.time_boot_us = att.timestamp;
msg.q[0] = att.q[0];
msg.q[1] = att.q[1];
msg.q[2] = att.q[2];
msg.q[3] = att.q[3];
msg.q_estimated_delay_us = 0; // I don't know.
}
{
vehicle_local_position_s lpos{};
_lpos_sub.copy(&lpos);
msg.vx = lpos.vx;
msg.vy = lpos.vy;
msg.vz = lpos.vz;
msg.v_estimated_delay_us = 0; // I don't know.
}
{
vehicle_attitude_setpoint_s att_sp{};
_att_sp_sub.copy(&att_sp);
msg.feed_forward_angular_velocity_z = att_sp.yaw_sp_move_rate;
}
{
estimator_status_s est{};
_est_sub.copy(&est);
msg.estimator_status = est.solution_status_flags;
}
{
vehicle_land_detected_s land_detected{};
_landed_sub.copy(&land_detected);
// Ignore take-off and landing states for now.
msg.landed_state = land_detected.landed ? MAV_LANDED_STATE_ON_GROUND : MAV_LANDED_STATE_IN_AIR;
}
mavlink_msg_autopilot_state_for_gimbal_device_send_struct(_mavlink->get_channel(), &msg);
return true;
}
return false;
}
};
class MavlinkStreamSystemTime : public MavlinkStream
{
public:
@ -3342,6 +3447,7 @@ static const StreamListItem streams_list[] = {
create_stream_list_item<MavlinkStreamGimbalDeviceAttitudeStatus>(),
create_stream_list_item<MavlinkStreamGimbalManagerInformation>(),
create_stream_list_item<MavlinkStreamGimbalManagerStatus>(),
create_stream_list_item<MavlinkStreamAutopilotStateForGimbalDevice>(),
create_stream_list_item<MavlinkStreamGimbalDeviceSetAttitude>(),
create_stream_list_item<MavlinkStreamHomePosition>(),
create_stream_list_item<MavlinkStreamServoOutputRaw<0> >(),

6
src/modules/mavlink/mavlink_mission.cpp
View File

@ -1475,6 +1475,9 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
case MAV_CMD_DO_DIGICAM_CONTROL:
case MAV_CMD_DO_MOUNT_CONFIGURE:
case MAV_CMD_DO_MOUNT_CONTROL:
case NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW:
case NAV_CMD_SET_CAMERA_FOCUS:
case NAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE:
case MAV_CMD_IMAGE_START_CAPTURE:
case MAV_CMD_IMAGE_STOP_CAPTURE:
case MAV_CMD_VIDEO_START_CAPTURE:
@ -1574,12 +1577,15 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *
case NAV_CMD_DO_CONTROL_VIDEO:
case NAV_CMD_DO_MOUNT_CONFIGURE:
case NAV_CMD_DO_MOUNT_CONTROL:
case NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW:
case NAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE:
case NAV_CMD_DO_SET_ROI:
case NAV_CMD_DO_SET_CAM_TRIGG_DIST:
case NAV_CMD_OBLIQUE_SURVEY:
case NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL:
case NAV_CMD_SET_CAMERA_MODE:
case NAV_CMD_SET_CAMERA_ZOOM:
case NAV_CMD_SET_CAMERA_FOCUS:
case NAV_CMD_DO_VTOL_TRANSITION:
break;

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

@ -283,6 +283,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
handle_message_gimbal_manager_set_attitude(msg);
break;
case MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL:
handle_message_gimbal_manager_set_manual_control(msg);
break;
case MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION:
handle_message_gimbal_device_information(msg);
break;
@ -2946,16 +2950,39 @@ void MavlinkReceiver::CheckHeartbeats(const hrt_abstime &t, bool force)
}
}
void
MavlinkReceiver::handle_message_gimbal_manager_set_manual_control(mavlink_message_t *msg)
{
mavlink_gimbal_manager_set_manual_control_t set_manual_control_msg;
mavlink_msg_gimbal_manager_set_manual_control_decode(msg, &set_manual_control_msg);
gimbal_manager_set_manual_control_s set_manual_control{};
set_manual_control.timestamp = hrt_absolute_time();
set_manual_control.origin_sysid = msg->sysid;
set_manual_control.origin_compid = msg->compid;
set_manual_control.target_system = set_manual_control_msg.target_system;
set_manual_control.target_component = set_manual_control_msg.target_component;
set_manual_control.flags = set_manual_control_msg.flags;
set_manual_control.gimbal_device_id = set_manual_control_msg.gimbal_device_id;
set_manual_control.pitch = set_manual_control_msg.pitch;
set_manual_control.yaw = set_manual_control_msg.yaw;
set_manual_control.pitch_rate = set_manual_control_msg.pitch_rate;
set_manual_control.yaw_rate = set_manual_control_msg.yaw_rate;
_gimbal_manager_set_manual_control_pub.publish(set_manual_control);
}
void
MavlinkReceiver::handle_message_gimbal_manager_set_attitude(mavlink_message_t *msg)
{
mavlink_gimbal_manager_set_attitude_t set_attitude_msg;
mavlink_msg_gimbal_manager_set_attitude_decode(msg, &set_attitude_msg);
gimbal_manager_set_attitude_s gimbal_attitude{};
gimbal_attitude.timestamp = hrt_absolute_time();
gimbal_attitude.origin_sysid = msg->sysid;
gimbal_attitude.origin_compid = msg->compid;
gimbal_attitude.target_system = set_attitude_msg.target_system;
gimbal_attitude.target_component = set_attitude_msg.target_component;
gimbal_attitude.flags = set_attitude_msg.flags;
@ -2969,7 +2996,6 @@ MavlinkReceiver::handle_message_gimbal_manager_set_attitude(mavlink_message_t *m
gimbal_attitude.angular_velocity_z = set_attitude_msg.angular_velocity_z;
_gimbal_manager_set_attitude_pub.publish(gimbal_attitude);
}
void
@ -2994,13 +3020,13 @@ MavlinkReceiver::handle_message_gimbal_device_information(mavlink_message_t *msg
gimbal_information.firmware_version = gimbal_device_info_msg.firmware_version;
gimbal_information.capability_flags = gimbal_device_info_msg.cap_flags;
gimbal_information.tilt_max = gimbal_device_info_msg.tilt_max;
gimbal_information.tilt_min = gimbal_device_info_msg.tilt_min;
gimbal_information.tilt_rate_max = gimbal_device_info_msg.tilt_rate_max;
gimbal_information.pitch_max = gimbal_device_info_msg.pitch_max;
gimbal_information.pitch_min = gimbal_device_info_msg.pitch_min;
gimbal_information.pan_max = gimbal_device_info_msg.pan_max;
gimbal_information.pan_min = gimbal_device_info_msg.pan_min;
gimbal_information.pan_rate_max = gimbal_device_info_msg.pan_rate_max;
gimbal_information.yaw_max = gimbal_device_info_msg.yaw_max;
gimbal_information.yaw_min = gimbal_device_info_msg.yaw_min;
gimbal_information.gimbal_device_compid = msg->compid;
_gimbal_device_information_pub.publish(gimbal_information);

5
src/modules/mavlink/mavlink_receiver.h
View File

@ -68,6 +68,7 @@
#include <uORB/topics/follow_target.h>
#include <uORB/topics/generator_status.h>
#include <uORB/topics/gimbal_manager_set_attitude.h>
#include <uORB/topics/gimbal_manager_set_manual_control.h>
#include <uORB/topics/gimbal_device_information.h>
#include <uORB/topics/gps_inject_data.h>
#include <uORB/topics/home_position.h>
@ -205,6 +206,7 @@ private:
void handle_message_vision_position_estimate(mavlink_message_t *msg);
void handle_message_onboard_computer_status(mavlink_message_t *msg);
void handle_message_gimbal_manager_set_attitude(mavlink_message_t *msg);
void handle_message_gimbal_manager_set_manual_control(mavlink_message_t *msg);
void handle_message_gimbal_device_information(mavlink_message_t *msg);
#if !defined(CONSTRAINED_FLASH)
@ -357,6 +359,9 @@ private:
uORB::Publication<collision_report_s> _collision_report_pub{ORB_ID(collision_report)};
uORB::Publication<differential_pressure_s> _differential_pressure_pub{ORB_ID(differential_pressure)};
uORB::Publication<follow_target_s> _follow_target_pub{ORB_ID(follow_target)};
uORB::Publication<gimbal_manager_set_attitude_s> _gimbal_manager_set_attitude_pub{ORB_ID(gimbal_manager_set_attitude)};
uORB::Publication<gimbal_manager_set_manual_control_s> _gimbal_manager_set_manual_control_pub{ORB_ID(gimbal_manager_set_manual_control)};
uORB::Publication<gimbal_device_information_s> _gimbal_device_information_pub{ORB_ID(gimbal_device_information)};
uORB::Publication<irlock_report_s> _irlock_report_pub{ORB_ID(irlock_report)};
uORB::Publication<gimbal_manager_set_attitude_s> _gimbal_manager_set_attitude_pub{ORB_ID(gimbal_manager_set_attitude)};
uORB::Publication<gimbal_device_information_s> _gimbal_device_information_pub{ORB_ID(gimbal_device_information)};

4
src/modules/navigator/mission.cpp
View File

@ -146,13 +146,15 @@ void
Mission::on_inactivation()
{
// Disable camera trigger
vehicle_command_s cmd = {};
vehicle_command_s cmd {};
cmd.command = vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL;
// Pause trigger
cmd.param1 = -1.0f;
cmd.param3 = 1.0f;
_navigator->publish_vehicle_cmd(&cmd);
_navigator->release_gimbal_control();
if (_navigator->get_precland()->is_activated()) {
_navigator->get_precland()->on_inactivation();
}

8
src/modules/navigator/mission_block.cpp
View File

@ -96,6 +96,8 @@ MissionBlock::is_mission_item_reached()
case NAV_CMD_DO_CONTROL_VIDEO:
case NAV_CMD_DO_MOUNT_CONFIGURE:
case NAV_CMD_DO_MOUNT_CONTROL:
case NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW:
case NAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE:
case NAV_CMD_DO_SET_ROI:
case NAV_CMD_DO_SET_ROI_LOCATION:
case NAV_CMD_DO_SET_ROI_WPNEXT_OFFSET:
@ -105,6 +107,7 @@ MissionBlock::is_mission_item_reached()
case NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL:
case NAV_CMD_SET_CAMERA_MODE:
case NAV_CMD_SET_CAMERA_ZOOM:
case NAV_CMD_SET_CAMERA_FOCUS:
return true;
case NAV_CMD_DO_VTOL_TRANSITION:
@ -472,7 +475,10 @@ MissionBlock::issue_command(const mission_item_s &item)
} else {
_action_start = hrt_absolute_time();
// mission_item -> vehicle_command
// This is to support legacy DO_MOUNT_CONTROL as part of a mission.
if (item.nav_cmd == NAV_CMD_DO_MOUNT_CONTROL) {
_navigator->acquire_gimbal_control();
}
// we're expecting a mission command item here so assign the "raw" inputs to the command
// (MAV_FRAME_MISSION mission item)

6
src/modules/navigator/mission_feasibility_checker.cpp
View File

@ -262,6 +262,8 @@ MissionFeasibilityChecker::checkMissionItemValidity(const mission_s &mission)
missionitem.nav_cmd != NAV_CMD_DO_CONTROL_VIDEO &&
missionitem.nav_cmd != NAV_CMD_DO_MOUNT_CONFIGURE &&
missionitem.nav_cmd != NAV_CMD_DO_MOUNT_CONTROL &&
missionitem.nav_cmd != NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW &&
missionitem.nav_cmd != NAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE &&
missionitem.nav_cmd != NAV_CMD_DO_SET_ROI &&
missionitem.nav_cmd != NAV_CMD_DO_SET_ROI_LOCATION &&
missionitem.nav_cmd != NAV_CMD_DO_SET_ROI_WPNEXT_OFFSET &&
@ -271,6 +273,7 @@ MissionFeasibilityChecker::checkMissionItemValidity(const mission_s &mission)
missionitem.nav_cmd != NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL &&
missionitem.nav_cmd != NAV_CMD_SET_CAMERA_MODE &&
missionitem.nav_cmd != NAV_CMD_SET_CAMERA_ZOOM &&
missionitem.nav_cmd != NAV_CMD_SET_CAMERA_FOCUS &&
missionitem.nav_cmd != NAV_CMD_DO_VTOL_TRANSITION) {
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: item %i: unsupported cmd: %d", (int)(i + 1),
@ -389,6 +392,8 @@ MissionFeasibilityChecker::checkTakeoff(const mission_s &mission, float home_alt
missionitem.nav_cmd != NAV_CMD_DO_CONTROL_VIDEO &&
missionitem.nav_cmd != NAV_CMD_DO_MOUNT_CONFIGURE &&
missionitem.nav_cmd != NAV_CMD_DO_MOUNT_CONTROL &&
missionitem.nav_cmd != NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW &&
missionitem.nav_cmd != NAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE &&
missionitem.nav_cmd != NAV_CMD_DO_SET_ROI &&
missionitem.nav_cmd != NAV_CMD_DO_SET_ROI_LOCATION &&
missionitem.nav_cmd != NAV_CMD_DO_SET_ROI_WPNEXT_OFFSET &&
@ -398,6 +403,7 @@ MissionFeasibilityChecker::checkTakeoff(const mission_s &mission, float home_alt
missionitem.nav_cmd != NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL &&
missionitem.nav_cmd != NAV_CMD_SET_CAMERA_MODE &&
missionitem.nav_cmd != NAV_CMD_SET_CAMERA_ZOOM &&
missionitem.nav_cmd != NAV_CMD_SET_CAMERA_FOCUS &&
missionitem.nav_cmd != NAV_CMD_DO_VTOL_TRANSITION);
}
}

3
src/modules/navigator/navigation.h
View File

@ -86,6 +86,9 @@ enum NAV_CMD {
NAV_CMD_OBLIQUE_SURVEY = 260,
NAV_CMD_SET_CAMERA_MODE = 530,
NAV_CMD_SET_CAMERA_ZOOM = 531,
NAV_CMD_SET_CAMERA_FOCUS = 532,
NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW = 1000,
NAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE = 1001,
NAV_CMD_IMAGE_START_CAPTURE = 2000,
NAV_CMD_IMAGE_STOP_CAPTURE = 2001,
NAV_CMD_DO_TRIGGER_CONTROL = 2003,

4
src/modules/navigator/navigator.h
View File

@ -302,6 +302,9 @@ public:
bool force_vtol();
void acquire_gimbal_control();
void release_gimbal_control();
private:
DEFINE_PARAMETERS(
(ParamFloat<px4::params::NAV_LOITER_RAD>) _param_nav_loiter_rad, /**< loiter radius for fixedwing */
@ -403,7 +406,6 @@ private:
float _mission_cruising_speed_fw{-1.0f};
float _mission_throttle{NAN};
bool _mission_landing_in_progress{false}; // this flag gets set if the mission is currently executing on a landing pattern
// if mission mode is inactive, this flag will be cleared after 2 seconds

24
src/modules/navigator/navigator_main.cpp
View File

@ -1374,6 +1374,30 @@ Navigator::publish_vehicle_command_ack(const vehicle_command_s &cmd, uint8_t res
_vehicle_cmd_ack_pub.publish(command_ack);
}
void
Navigator::acquire_gimbal_control()
{
vehicle_command_s vcmd = {};
vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE;
vcmd.param1 = _vstatus.system_id;
vcmd.param2 = _vstatus.component_id;
vcmd.param3 = -1.0f; // Leave unchanged.
vcmd.param4 = -1.0f; // Leave unchanged.
publish_vehicle_cmd(&vcmd);
}
void
Navigator::release_gimbal_control()
{
vehicle_command_s vcmd = {};
vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE;
vcmd.param1 = -3.0f; // Remove control if it had it.
vcmd.param2 = -3.0f; // Remove control if it had it.
vcmd.param3 = -1.0f; // Leave unchanged.
vcmd.param4 = -1.0f; // Leave unchanged.
publish_vehicle_cmd(&vcmd);
}
int Navigator::print_usage(const char *reason)
{
if (reason) {

17
src/modules/vmount/common.h
View File

@ -41,11 +41,9 @@
#include <stdint.h>
namespace vmount
{
/**
* @struct ControlData
* This defines the common API between an input and an output of the vmount driver.
@ -58,7 +56,6 @@ struct ControlData {
Neutral = 0, /**< move to neutral position */
Angle, /**< control the roll, pitch & yaw angle directly */
LonLat /**< control via lon, lat */
//TODO: add more, like smooth curve, ... ?
};
@ -66,13 +63,15 @@ struct ControlData {
union TypeData {
struct TypeAngle {
float angles[3]; /**< attitude angles (roll, pitch, yaw) in rad, [-pi, +pi] or rad/s (if is angular rate) */
float q[4]; /**< attitude quaternion */
float angular_velocity[3]; // angular velocity
// according to DO_MOUNT_CONFIGURE
enum class Frame : uint8_t {
AngleBodyFrame = 0, /**< Angle in body frame. */
AngularRate, /**< Angular rate in rad/s. */
AngleAbsoluteFrame /**< Angle in absolute frame. */
} frames[3]; /**< Frame of given angle. */
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. */
} angle;
struct TypeLonLat {
@ -89,10 +88,8 @@ struct ControlData {
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) */
};
} /* namespace vmount */

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

@ -52,7 +52,8 @@ namespace vmount
class InputBase
{
public:
virtual ~InputBase() {}
InputBase() = default;
virtual ~InputBase() = default;
/**
* Wait for an input update, with a timeout.

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

@ -44,6 +44,8 @@
#include <uORB/topics/vehicle_roi.h>
#include <uORB/topics/vehicle_command_ack.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/gimbal_manager_set_attitude.h>
#include <uORB/topics/gimbal_manager_set_manual_control.h>
#include <drivers/drv_hrt.h>
#include <lib/parameters/param.h>
#include <px4_platform_common/defines.h>
@ -176,17 +178,6 @@ void InputMavlinkROI::print_status()
InputMavlinkCmdMount::InputMavlinkCmdMount()
{
param_t handle = param_find("MAV_SYS_ID");
if (handle != PARAM_INVALID) {
param_get(handle, &_mav_sys_id);
}
handle = param_find("MAV_COMP_ID");
if (handle != PARAM_INVALID) {
param_get(handle, &_mav_comp_id);
}
}
InputMavlinkCmdMount::~InputMavlinkCmdMount()
@ -273,24 +264,30 @@ int InputMavlinkCmdMount::update_impl(unsigned int timeout_ms, ControlData **con
*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::AngleBodyFrame;
_control_data.type_data.angle.frames[1] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame;
_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
_control_data.type_data.angle.angles[0] = vehicle_command.param2 * M_DEG_TO_RAD_F;
// vmount spec has pitch on channel 1, MAVLink spec has roll on channel 1
_control_data.type_data.angle.angles[1] = vehicle_command.param1 * M_DEG_TO_RAD_F;
// both specs have yaw on channel 2
_control_data.type_data.angle.angles[2] = vehicle_command.param3 * M_DEG_TO_RAD_F;
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::AngleBodyFrame;
_control_data.type_data.angle.frames[1] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame;
_control_data.type_data.angle.frames[2] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame;
// We expect angle of [-pi..+pi]. If the input range is [0..2pi] we can fix that.
if (_control_data.type_data.angle.angles[2] > M_PI_F) {
_control_data.type_data.angle.angles[2] -= 2 * M_PI_F;
// vmount spec has roll on channel 0, MAVLink spec has pitch on channel 0
const float roll = vehicle_command.param2 * M_DEG_TO_RAD_F;
// vmount spec has pitch on channel 1, MAVLink spec has roll on channel 1
const float pitch = vehicle_command.param1 * M_DEG_TO_RAD_F;
// both specs have yaw on channel 2
float yaw = vehicle_command.param3 * M_DEG_TO_RAD_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;
}
*control_data = &_control_data;
break;
case vehicle_command_s::VEHICLE_MOUNT_MODE_RC_TARGETING:
@ -374,25 +371,14 @@ void InputMavlinkCmdMount::print_status()
PX4_INFO("Input: Mavlink (CMD_MOUNT)");
}
InputMavlinkGimbalV2::InputMavlinkGimbalV2(bool has_v2_gimbal_device)
InputMavlinkGimbalV2::InputMavlinkGimbalV2(bool has_v2_gimbal_device, 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)
{
param_t handle = param_find("MAV_SYS_ID");
if (handle != PARAM_INVALID) {
param_get(handle, &_mav_sys_id);
}
handle = param_find("MAV_COMP_ID");
if (handle != PARAM_INVALID) {
param_get(handle, &_mav_comp_id);
}
if (has_v2_gimbal_device) {
/* smart gimbal: ask GIMBAL_DEVICE_INFORMATION to it */
_request_gimbal_device_information();
} else {
if (!has_v2_gimbal_device) {
/* dumb gimbal or MAVLink v1 protocol gimbal: fake GIMBAL_DEVICE_INFORMATION */
_stream_gimbal_manager_information();
}
@ -415,6 +401,10 @@ InputMavlinkGimbalV2::~InputMavlinkGimbalV2()
if (_vehicle_command_sub >= 0) {
orb_unsubscribe(_vehicle_command_sub);
}
if (_gimbal_manager_set_manual_control_sub >= 0) {
orb_unsubscribe(_gimbal_manager_set_manual_control_sub);
}
}
@ -447,6 +437,10 @@ int InputMavlinkGimbalV2::initialize()
return -errno;
}
if ((_gimbal_manager_set_manual_control_sub = orb_subscribe(ORB_ID(gimbal_manager_set_manual_control))) < 0) {
return -errno;
}
// rate-limit inputs to 100Hz. If we don't do this and the output is configured to mavlink mode,
// it will publish vehicle_command's as well, causing the input poll() in here to return
// immediately, which in turn will cause an output update and thus a busy loop.
@ -457,17 +451,21 @@ int InputMavlinkGimbalV2::initialize()
void InputMavlinkGimbalV2::_stream_gimbal_manager_status()
{
gimbal_device_attitude_status_s gimbal_device_attitude_status{};
if (_gimbal_device_attitude_status_sub.updated()) {
_gimbal_device_attitude_status_sub.copy(&_gimbal_device_attitude_status);
_gimbal_device_attitude_status_sub.copy(&gimbal_device_attitude_status);
}
gimbal_manager_status_s gimbal_manager_status{};
gimbal_manager_status.timestamp = hrt_absolute_time();
gimbal_manager_status.flags = _gimbal_device_attitude_status.device_flags;
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.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);
}
void InputMavlinkGimbalV2::_stream_gimbal_manager_information()
@ -489,33 +487,14 @@ void InputMavlinkGimbalV2::_stream_gimbal_manager_information()
gimbal_device_information_s::GIMBAL_DEVICE_CAP_FLAGS_HAS_YAW_AXIS |
gimbal_device_information_s::GIMBAL_DEVICE_CAP_FLAGS_HAS_YAW_LOCK;
gimbal_device_info.tilt_max = M_PI_F / 2;
gimbal_device_info.tilt_min = -M_PI_F / 2;
gimbal_device_info.tilt_rate_max = 1;
gimbal_device_info.pan_max = M_PI_F;
gimbal_device_info.pan_min = -M_PI_F;
gimbal_device_info.pan_rate_max = 1;
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_device_info_pub.publish(gimbal_device_info);
}
void InputMavlinkGimbalV2::_request_gimbal_device_information()
{
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.confirmation = 0;
vehicle_cmd.from_external = false;
uORB::PublicationQueued<vehicle_command_s> vehicle_command_pub{ORB_ID(vehicle_command)};
vehicle_command_pub.publish(vehicle_cmd);
}
int InputMavlinkGimbalV2::update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active)
{
_stream_gimbal_manager_status();
@ -523,7 +502,7 @@ int InputMavlinkGimbalV2::update_impl(unsigned int timeout_ms, ControlData **con
// Default to no change, set if we receive anything.
*control_data = nullptr;
const int num_poll = 4;
const int num_poll = 5;
px4_pollfd_struct_t polls[num_poll];
polls[0].fd = _gimbal_manager_set_attitude_sub;
polls[0].events = POLLIN;
@ -533,6 +512,8 @@ int InputMavlinkGimbalV2::update_impl(unsigned int timeout_ms, ControlData **con
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;
@ -560,13 +541,20 @@ int InputMavlinkGimbalV2::update_impl(unsigned int timeout_ms, ControlData **con
gimbal_manager_set_attitude_s set_attitude;
orb_copy(ORB_ID(gimbal_manager_set_attitude), _gimbal_manager_set_attitude_sub, &set_attitude);
const float pitch = matrix::Eulerf(matrix::Quatf(set_attitude.q)).phi(); // rad
const float roll = matrix::Eulerf(matrix::Quatf(set_attitude.q)).theta();
const float yaw = matrix::Eulerf(matrix::Quatf(set_attitude.q)).psi();
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, pitch, set_attitude.angular_velocity_y, yaw,
set_attitude.angular_velocity_z, roll, set_attitude.angular_velocity_x);
*control_data = &_control_data;
_set_control_data_from_set_attitude(set_attitude.flags, q, angular_velocity);
*control_data = &_control_data;
} else {
PX4_WARN("Ignoring gimbal_manager_set_attitude from %d/%d",
set_attitude.origin_sysid, set_attitude.origin_compid);
}
}
if (polls[1].revents & POLLIN) {
@ -642,16 +630,233 @@ int InputMavlinkGimbalV2::update_impl(unsigned int timeout_ms, ControlData **con
continue;
}
if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_ATTITUDE) {
_set_control_data_from_set_attitude((uint32_t)vehicle_command.param5, vehicle_command.param3, vehicle_command.param1,
vehicle_command.param3, vehicle_command.param2);
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::AngleBodyFrame;
_control_data.type_data.angle.frames[1] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame;
_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 = vehicle_command.param2 * M_DEG_TO_RAD_F;
// vmount spec has pitch on channel 1, MAVLink spec has roll on channel 1
const float pitch = vehicle_command.param1 * M_DEG_TO_RAD_F;
// both specs have yaw on channel 2
float yaw = vehicle_command.param3 * M_DEG_TO_RAD_F;
// 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);
_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, vehicle_command.param1 * M_DEG_TO_RAD_F, vehicle_command.param2 * M_DEG_TO_RAD_F);
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_ERR("GIMBAL_MANAGER_PITCHYAW denied, not in 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,
_mnt_rate_pitch * M_DEG_TO_RAD_F * set_manual_control.pitch_rate,
_mnt_rate_yaw * M_DEG_TO_RAD_F * 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_WARN("Ignoring gimbal_manager_set_manual_control from %d/%d",
set_manual_control.origin_sysid, set_manual_control.origin_compid);
}
}
}
}
@ -671,25 +876,30 @@ void InputMavlinkGimbalV2::_transform_lon_lat_to_angle(const double roi_lon, con
_control_data.type_data.angle.frames[1] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame;
_control_data.type_data.angle.frames[2] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame;
_control_data.type_data.angle.angles[0] = 0.f;
const float roll = 0.0f;
float pitch = 0.0f;
float yaw = 0.0f;
// interface: use fixed pitch value > -pi otherwise consider ROI altitude
if (_control_data.type_data.lonlat.pitch_fixed_angle >= -M_PI_F) {
_control_data.type_data.angle.angles[1] = _control_data.type_data.lonlat.pitch_fixed_angle;
pitch = _control_data.type_data.lonlat.pitch_fixed_angle;
} else {
_control_data.type_data.angle.angles[1] = _calculate_pitch(roi_lon, roi_lat, roi_alt, vehicle_global_position);
pitch = _calculate_pitch(roi_lon, roi_lat, roi_alt, vehicle_global_position);
}
_control_data.type_data.angle.angles[2] = get_bearing_to_next_waypoint(vlat, vlon, roi_lat,
roi_lon) - vehicle_global_position.yaw;
yaw = get_bearing_to_next_waypoint(vlat, vlon, roi_lat, roi_lon) - vehicle_local_position.yaw;
// add offsets from VEHICLE_CMD_DO_SET_ROI_WPNEXT_OFFSET
_control_data.type_data.angle.angles[1] += _control_data.type_data.lonlat.pitch_angle_offset;
_control_data.type_data.angle.angles[2] += _control_data.type_data.lonlat.yaw_angle_offset;
pitch += _control_data.type_data.lonlat.pitch_angle_offset;
yaw += _control_data.type_data.lonlat.yaw_angle_offset;
// make sure yaw is wrapped correctly for the output
_control_data.type_data.angle.angles[2] = wrap_pi(_control_data.type_data.angle.angles[2]);
yaw = wrap_pi(yaw);
matrix::Eulerf euler(roll, pitch, yaw);
matrix::Quatf q(euler);
q.copyTo(_control_data.type_data.angle.q);
}
float InputMavlinkGimbalV2::_calculate_pitch(double lon, double lat, float altitude,
@ -718,81 +928,48 @@ void InputMavlinkGimbalV2::_read_lat_lon_alt_from_position_setpoint_sub(double &
alt_sp = (double)position_setpoint_triplet.current.alt;
}
void InputMavlinkGimbalV2::_set_control_data_from_set_attitude(const uint32_t flags, const float pitch_angle,
const float pitch_rate, const float yaw_angle, const float yaw_rate, float roll_angle, float roll_rate)
void InputMavlinkGimbalV2::_set_control_data_from_set_attitude(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;
} else if ((flags & gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_NONE) != 0) {
// don't do anything
} else {
_control_data.type = ControlData::Type::Angle;
_control_data.type_data.angle.frames[0] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame;
_control_data.type_data.angle.frames[1] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame;
_control_data.type_data.angle.frames[2] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame;
if (_is_roi_set && (flags & gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_NUDGE) != 0) {
// add set_attitude.q to existing tracking angle or ROI
// track message not yet implemented
_control_data.type_data.angle.angles[0] += pitch_angle;
_control_data.type_data.angle.angles[1] += roll_angle;
_control_data.type_data.angle.angles[2] += yaw_angle;
_control_data.type_data.angle.q[0] = q(0);
_control_data.type_data.angle.q[1] = q(1);
_control_data.type_data.angle.q[2] = q(2);
_control_data.type_data.angle.q[3] = q(3);
} else {
_control_data.type_data.angle.angles[0] = pitch_angle;
_control_data.type_data.angle.angles[1] = roll_angle;
_control_data.type_data.angle.angles[2] = yaw_angle;
}
_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);
if (_is_roi_set && (flags & gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_OVERRIDE) != 0) {
// overides tracking or ROI angle with set_attitude.q, respects flag GIMBAL_MANAGER_FLAGS_YAW_LOCK
_control_data.type_data.angle.angles[0] = pitch_angle;
_control_data.type_data.angle.angles[1] = roll_angle;
_control_data.type_data.angle.angles[2] = yaw_angle;
}
_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;
if (PX4_ISFINITE(roll_rate)) { //roll
_control_data.type_data.angle.frames[0] = ControlData::TypeData::TypeAngle::Frame::AngularRate;
_control_data.type_data.angle.angles[0] = roll_rate; //rad/s
}
_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;
if (PX4_ISFINITE(pitch_rate)) { //pitch
_control_data.type_data.angle.frames[1] = ControlData::TypeData::TypeAngle::Frame::AngularRate;
_control_data.type_data.angle.angles[1] = pitch_rate; //rad/s
}
if (PX4_ISFINITE(yaw_rate)) { //yaw
_control_data.type_data.angle.frames[2] = ControlData::TypeData::TypeAngle::Frame::AngularRate;
_control_data.type_data.angle.angles[2] = yaw_rate; //rad/s
}
if (flags & gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_ROLL_LOCK) {
// stay horizontal with the horizon
_control_data.type_data.angle.frames[0] = ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame;
}
if (flags & gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_PITCH_LOCK) {
_control_data.type_data.angle.frames[1] = ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame;
}
if (flags & gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_YAW_LOCK) {
_control_data.type_data.angle.frames[2] = ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame;
}
_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)
void InputMavlinkGimbalV2::_ack_vehicle_command(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.result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
vehicle_command_ack.result = result;
vehicle_command_ack.target_system = cmd->source_system;
vehicle_command_ack.target_component = cmd->source_component;

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

@ -45,7 +45,6 @@
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/gimbal_manager_set_attitude.h>
#include <uORB/topics/gimbal_device_attitude_status.h>
#include <uORB/topics/gimbal_device_information.h>
#include <uORB/topics/gimbal_manager_status.h>
@ -110,7 +109,8 @@ private:
class InputMavlinkGimbalV2 : public InputBase
{
public:
InputMavlinkGimbalV2(bool has_v2_gimbal_device);
InputMavlinkGimbalV2(bool has_v2_gimbal_device, uint8_t mav_sys_id, uint8_t mav_comp_id, float &mnt_rate_pitch,
float &mnt_rate_yaw);
virtual ~InputMavlinkGimbalV2();
virtual void print_status();
@ -125,20 +125,26 @@ private:
float _calculate_pitch(double lon, double lat, float altitude,
const vehicle_global_position_s &global_position);
void _read_lat_lon_alt_from_position_setpoint_sub(double &lon_sp, double &lat_sp, double &alt_sp);
void _set_control_data_from_set_attitude(const uint32_t flags, const float pitch_angle, const float pitch_rate,
const float yaw_angle, const float yaw_rate, float roll_angle = 0, float roll_rate = 0);
void _ack_vehicle_command(vehicle_command_s *cmd);
void _set_control_data_from_set_attitude(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 _stream_gimbal_manager_information();
void _request_gimbal_device_information();
void _stream_gimbal_manager_status();
int _vehicle_roi_sub = -1;
int _gimbal_manager_set_attitude_sub = -1;
int _gimbal_manager_set_manual_control_sub = -1;
int _position_setpoint_triplet_sub = -1;
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
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;
bool _is_roi_set{false};
@ -148,9 +154,6 @@ private:
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;
gimbal_device_attitude_status_s _gimbal_device_attitude_status{};
};
} /* namespace vmount */

11
src/modules/vmount/input_rc.cpp
View File

@ -41,6 +41,7 @@
#include <math.h>
#include <errno.h>
#include <matrix/matrix/math.hpp>
#include <px4_platform_common/posix.h>
#include <px4_platform_common/defines.h>
@ -130,10 +131,14 @@ bool InputRC::_read_control_data_from_subscription(ControlData &control_data, bo
_first_time = false;
for (int i = 0; i < 3; ++i) {
control_data.type_data.angle.frames[i] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame;
control_data.type_data.angle.angles[i] = new_aux_values[i] * M_PI_F;
matrix::Eulerf euler(new_aux_values[0] * M_PI_F, new_aux_values[1] * M_PI_F,
new_aux_values[2] * M_PI_F);
matrix::Quatf q(euler);
q.copyTo(control_data.type_data.angle.q);
for (int i = 0; i < 3; ++i) {
// We always use follow mode with RC input for now.
control_data.type_data.angle.frames[i] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame;
_last_set_aux_values[i] = new_aux_values[i];
}

14
src/modules/vmount/input_test.cpp
View File

@ -42,6 +42,7 @@
#include <math.h>
#include <px4_platform_common/posix.h>
#include <lib/matrix/matrix/math.hpp>
namespace vmount
@ -69,9 +70,16 @@ int InputTest::update(unsigned int timeout_ms, ControlData **control_data, bool
_control_data.type_data.angle.frames[1] = ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame;
_control_data.type_data.angle.frames[2] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame;
for (int i = 0; i < 3; ++i) {
_control_data.type_data.angle.angles[i] = _angles[i] * M_DEG_TO_RAD_F;
}
matrix::Eulerf euler(
_angles[0] * M_DEG_TO_RAD_F,
_angles[1] * M_DEG_TO_RAD_F,
_angles[2] * M_DEG_TO_RAD_F);
matrix::Quatf q(euler);
_control_data.type_data.angle.q[0] = q(0);
_control_data.type_data.angle.q[1] = q(1);
_control_data.type_data.angle.q[2] = q(2);
_control_data.type_data.angle.q[3] = q(3);
_control_data.gimbal_shutter_retract = false;
*control_data = &_control_data;

32
src/modules/vmount/output.cpp
View File

@ -108,21 +108,27 @@ void OutputBase::_set_angle_setpoints(const ControlData *control_data)
switch (control_data->type) {
case ControlData::Type::Angle:
for (int i = 0; i < 3; ++i) {
switch (control_data->type_data.angle.frames[i]) {
case ControlData::TypeData::TypeAngle::Frame::AngularRate:
_angle_speeds[i] = control_data->type_data.angle.angles[i];
break;
case ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame:
_absolute_angle[i] = false;
_angle_setpoints[i] = control_data->type_data.angle.angles[i];
break;
{
matrix::Quatf q(control_data->type_data.angle.q);
matrix::Eulerf euler(q);
case ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame:
_absolute_angle[i] = true;
_angle_setpoints[i] = control_data->type_data.angle.angles[i];
break;
for (int i = 0; i < 3; ++i) {
switch (control_data->type_data.angle.frames[i]) {
case ControlData::TypeData::TypeAngle::Frame::AngularRate:
break;
case ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame:
_absolute_angle[i] = false;
break;
case ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame:
_absolute_angle[i] = true;
break;
}
_angle_speeds[i] = control_data->type_data.angle.angular_velocity[i];
_angle_setpoints[i] = euler(i);
}
}

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

@ -67,8 +67,8 @@ struct OutputConfig {
float roll_offset; /**< Offset for roll channel in radians */
float yaw_offset; /**< Offset for yaw channel in radians */
uint32_t mavlink_sys_id; /**< Mavlink target system id for mavlink output */
uint32_t mavlink_comp_id;
uint32_t mavlink_sys_id_v1; /**< Mavlink target system id for mavlink output only for v1 */
uint32_t mavlink_comp_id_v1;
};

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

@ -59,8 +59,8 @@ int OutputMavlinkV1::update(const ControlData *control_data)
{
vehicle_command_s vehicle_command{};
vehicle_command.timestamp = hrt_absolute_time();
vehicle_command.target_system = (uint8_t)_config.mavlink_sys_id;
vehicle_command.target_component = (uint8_t)_config.mavlink_comp_id;
vehicle_command.target_system = (uint8_t)_config.mavlink_sys_id_v1;
vehicle_command.target_component = (uint8_t)_config.mavlink_comp_id_v1;
if (control_data) {
//got new command
@ -118,29 +118,68 @@ void OutputMavlinkV1::print_status()
PX4_INFO("Output: MAVLink gimbal protocol v1");
}
OutputMavlinkV2::OutputMavlinkV2(const OutputConfig &output_config)
: OutputBase(output_config)
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)
{
}
int OutputMavlinkV2::update(const ControlData *control_data)
{
if (control_data) {
//got new command
_set_angle_setpoints(control_data);
_publish_gimbal_device_set_attitude(control_data);
}
_handle_position_update();
_check_for_gimbal_device_information();
hrt_abstime t = hrt_absolute_time();
_calculate_output_angles(t);
_last_update = t;
if (!_gimbal_device_found && t - _last_gimbal_device_checked > 1000000) {
_request_gimbal_device_information();
_last_gimbal_device_checked = t;
} else {
// Only start sending attitude setpoints once a device has been discovered.
if (control_data) {
//got new command
_set_angle_setpoints(control_data);
_publish_gimbal_device_set_attitude(control_data);
}
_handle_position_update();
_calculate_output_angles(t);
_last_update = t;
}
return 0;
}
void OutputMavlinkV2::_request_gimbal_device_information()
{
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.confirmation = 0;
vehicle_cmd.from_external = false;
uORB::PublicationQueued<vehicle_command_s> vehicle_command_pub{ORB_ID(vehicle_command)};
vehicle_command_pub.publish(vehicle_cmd);
}
void OutputMavlinkV2::_check_for_gimbal_device_information()
{
gimbal_device_information_s gimbal_device_information;
if (_gimbal_device_information_sub.update(&gimbal_device_information)) {
_gimbal_device_found = true;
_gimbal_device_compid = gimbal_device_information.gimbal_device_compid;
}
}
void OutputMavlinkV2::print_status()
{
PX4_INFO("Output: MAVLink gimbal protocol v2");
@ -150,50 +189,50 @@ void OutputMavlinkV2::_publish_gimbal_device_set_attitude(const ControlData *con
{
gimbal_device_set_attitude_s set_attitude{};
set_attitude.timestamp = hrt_absolute_time();
set_attitude.target_system = (uint8_t)_config.mavlink_sys_id;
set_attitude.target_component = (uint8_t)_config.mavlink_comp_id;
set_attitude.target_system = (uint8_t)_mav_sys_id;
set_attitude.target_component = _gimbal_device_compid;
matrix::Eulerf euler(control_data->type_data.angle.angles[0], control_data->type_data.angle.angles[1],
control_data->type_data.angle.angles[2]);
matrix::Quatf q(euler);
set_attitude.q[0] = q(0);
set_attitude.q[1] = q(1);
set_attitude.q[2] = q(2);
set_attitude.q[3] = q(3);
if (control_data->type_data.angle.frames[0] == ControlData::TypeData::TypeAngle::Frame::AngularRate) {
set_attitude.angular_velocity_x = control_data->type_data.angle.angles[0]; //roll
}
if (control_data->type_data.angle.frames[1] == ControlData::TypeData::TypeAngle::Frame::AngularRate) {
set_attitude.angular_velocity_y = control_data->type_data.angle.angles[1]; //pitch
}
if (control_data->type_data.angle.frames[2] == ControlData::TypeData::TypeAngle::Frame::AngularRate) {
set_attitude.angular_velocity_z = control_data->type_data.angle.angles[2];
}
if (control_data->type == ControlData::Type::Neutral) {
switch (control_data->type) {
case ControlData::Type::Neutral:
set_attitude.flags |= gimbal_device_set_attitude_s::GIMBAL_DEVICE_FLAGS_NEUTRAL;
}
set_attitude.angular_velocity_x = NAN;
set_attitude.angular_velocity_y = NAN;
set_attitude.angular_velocity_z = NAN;
set_attitude.q[0] = NAN;
set_attitude.q[1] = NAN;
set_attitude.q[2] = NAN;
set_attitude.q[3] = NAN;
break;
if (control_data->type_data.angle.frames[0] == ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame) {
set_attitude.flags |= gimbal_device_set_attitude_s::GIMBAL_DEVICE_FLAGS_ROLL_LOCK;
}
case ControlData::Type::Angle:
set_attitude.angular_velocity_x = control_data->type_data.angle.angular_velocity[0];
set_attitude.angular_velocity_y = control_data->type_data.angle.angular_velocity[1];
set_attitude.angular_velocity_z = control_data->type_data.angle.angular_velocity[2];
set_attitude.q[0] = control_data->type_data.angle.q[0];
set_attitude.q[1] = control_data->type_data.angle.q[1];
set_attitude.q[2] = control_data->type_data.angle.q[2];
set_attitude.q[3] = control_data->type_data.angle.q[3];
if (control_data->type_data.angle.frames[1] == ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame) {
set_attitude.flags |= gimbal_device_set_attitude_s::GIMBAL_DEVICE_FLAGS_PITCH_LOCK;
}
if (control_data->type_data.angle.frames[0] == ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame) {
set_attitude.flags |= gimbal_device_set_attitude_s::GIMBAL_DEVICE_FLAGS_ROLL_LOCK;
}
if (control_data->type_data.angle.frames[2] == ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame) {
set_attitude.flags |= gimbal_device_set_attitude_s::GIMBAL_DEVICE_FLAGS_YAW_LOCK;
if (control_data->type_data.angle.frames[1] == ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame) {
set_attitude.flags |= gimbal_device_set_attitude_s::GIMBAL_DEVICE_FLAGS_PITCH_LOCK;
}
if (control_data->type_data.angle.frames[2] == ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame) {
set_attitude.flags |= gimbal_device_set_attitude_s::GIMBAL_DEVICE_FLAGS_YAW_LOCK;
}
break;
case ControlData::Type::LonLat:
// FIXME: needs conversion to angle.
break;
}
_gimbal_device_set_attitude_pub.publish(set_attitude);
}
} /* namespace vmount */

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

@ -44,6 +44,7 @@
#include <uORB/Publication.hpp>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/gimbal_device_set_attitude.h>
#include <uORB/topics/gimbal_device_information.h>
namespace vmount
@ -69,7 +70,7 @@ private:
class OutputMavlinkV2 : public OutputBase
{
public:
OutputMavlinkV2(const OutputConfig &output_config);
OutputMavlinkV2(int32_t mav_sys_id, int32_t mav_comp_id, const OutputConfig &output_config);
virtual ~OutputMavlinkV2() = default;
virtual int update(const ControlData *control_data);
@ -78,7 +79,17 @@ public:
private:
void _publish_gimbal_device_set_attitude(const ControlData *control_data);
void _request_gimbal_device_information();
void _check_for_gimbal_device_information();
uORB::Publication<gimbal_device_set_attitude_s> _gimbal_device_set_attitude_pub{ORB_ID(gimbal_device_set_attitude)};
uORB::Subscription _gimbal_device_information_sub{ORB_ID(gimbal_device_information)};
int32_t _mav_sys_id{1}; ///< our mavlink system id
int32_t _mav_comp_id{1}; ///< our mavlink component id
uint8_t _gimbal_device_compid{0};
hrt_abstime _last_gimbal_device_checked{0};
bool _gimbal_device_found {false};
};
} /* namespace vmount */

5
src/modules/vmount/output_rc.cpp
View File

@ -100,10 +100,7 @@ void OutputRC::_stream_device_attitude_status()
matrix::Eulerf euler(_angle_outputs[0], _angle_outputs[1], _angle_outputs[2]);
matrix::Quatf q(euler);
attitude_status.q[0] = q(0);
attitude_status.q[1] = q(1);
attitude_status.q[2] = q(2);
attitude_status.q[3] = q(3);
q.copyTo(attitude_status.q);
attitude_status.failure_flags = 0;
_attitude_status_pub.publish(attitude_status);

69
src/modules/vmount/vmount.cpp
View File

@ -88,8 +88,8 @@ static volatile ThreadData *g_thread_data = nullptr;
struct Parameters {
int32_t mnt_mode_in;
int32_t mnt_mode_out;
int32_t mnt_mav_sysid;
int32_t mnt_mav_compid;
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;
@ -102,6 +102,10 @@ struct Parameters {
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;
bool operator!=(const Parameters &p)
{
@ -109,8 +113,8 @@ struct Parameters {
#pragma GCC diagnostic ignored "-Wfloat-equal"
return mnt_mode_in != p.mnt_mode_in ||
mnt_mode_out != p.mnt_mode_out ||
mnt_mav_sysid != p.mnt_mav_sysid ||
mnt_mav_compid != p.mnt_mav_compid ||
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 ||
@ -122,7 +126,9 @@ struct Parameters {
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;
mnt_off_yaw != p.mnt_off_yaw ||
mav_sys_id != p.mav_sys_id ||
mav_comp_id != p.mav_comp_id;
#pragma GCC diagnostic pop
}
@ -131,8 +137,8 @@ struct Parameters {
struct ParameterHandles {
param_t mnt_mode_in;
param_t mnt_mode_out;
param_t mnt_mav_sysid;
param_t mnt_mav_compid;
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;
@ -145,6 +151,10 @@ struct ParameterHandles {
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;
};
@ -229,8 +239,8 @@ static int vmount_thread_main(int argc, char *argv[])
output_config.pitch_offset = params.mnt_off_pitch * M_DEG_TO_RAD_F;
output_config.roll_offset = params.mnt_off_roll * M_DEG_TO_RAD_F;
output_config.yaw_offset = params.mnt_off_yaw * M_DEG_TO_RAD_F;
output_config.mavlink_sys_id = params.mnt_mav_sysid;
output_config.mavlink_comp_id = params.mnt_mav_compid;
output_config.mavlink_sys_id_v1 = params.mnt_mav_sys_id_v1;
output_config.mavlink_comp_id_v1 = params.mnt_mav_comp_id_v1;
bool alloc_failed = false;
thread_data.input_objs_len = 1;
@ -271,7 +281,11 @@ static int vmount_thread_main(int argc, char *argv[])
break;
case 4: //MAVLINK_V2
thread_data.input_objs[0] = new InputMavlinkGimbalV2(params.mnt_mode_out == 2);
thread_data.input_objs[0] = new InputMavlinkGimbalV2(params.mnt_mode_out == 2,
params.mav_sys_id,
params.mav_comp_id,
params.mnt_rate_pitch,
params.mnt_rate_yaw);
break;
default:
@ -302,7 +316,7 @@ static int vmount_thread_main(int argc, char *argv[])
break;
case 2: //MAVLink v2 gimbal protocol
thread_data.output_obj = new OutputMavlinkV2(output_config);
thread_data.output_obj = new OutputMavlinkV2(params.mav_sys_id, params.mav_comp_id, output_config);
if (!thread_data.output_obj) { alloc_failed = true; }
@ -375,9 +389,9 @@ static int vmount_thread_main(int argc, char *argv[])
break;
}
//only publish the mount orientation if the mode is not mavlink
//if the gimbal speaks mavlink it publishes its own orientation
if (params.mnt_mode_out != 1) { // 1 = MAVLINK
// 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();
}
@ -550,8 +564,8 @@ void update_params(ParameterHandles &param_handles, Parameters &params, bool &go
Parameters prev_params = params;
param_get(param_handles.mnt_mode_in, &params.mnt_mode_in);
param_get(param_handles.mnt_mode_out, &params.mnt_mode_out);
param_get(param_handles.mnt_mav_sysid, &params.mnt_mav_sysid);
param_get(param_handles.mnt_mav_compid, &params.mnt_mav_compid);
param_get(param_handles.mnt_mav_sys_id_v1, &params.mnt_mav_sys_id_v1);
param_get(param_handles.mnt_mav_comp_id_v1, &params.mnt_mav_comp_id_v1);
param_get(param_handles.mnt_ob_lock_mode, &params.mnt_ob_lock_mode);
param_get(param_handles.mnt_ob_norm_mode, &params.mnt_ob_norm_mode);
param_get(param_handles.mnt_man_pitch, &params.mnt_man_pitch);
@ -564,6 +578,10 @@ void update_params(ParameterHandles &param_handles, Parameters &params, bool &go
param_get(param_handles.mnt_off_pitch, &params.mnt_off_pitch);
param_get(param_handles.mnt_off_roll, &params.mnt_off_roll);
param_get(param_handles.mnt_off_yaw, &params.mnt_off_yaw);
param_get(param_handles.mav_sys_id, &params.mav_sys_id);
param_get(param_handles.mav_comp_id, &params.mav_comp_id);
param_get(param_handles.mnt_rate_pitch, &params.mnt_rate_pitch);
param_get(param_handles.mnt_rate_yaw, &params.mnt_rate_yaw);
got_changes = prev_params != params;
}
@ -572,8 +590,8 @@ bool get_params(ParameterHandles &param_handles, Parameters &params)
{
param_handles.mnt_mode_in = param_find("MNT_MODE_IN");
param_handles.mnt_mode_out = param_find("MNT_MODE_OUT");
param_handles.mnt_mav_sysid = param_find("MNT_MAV_SYSID");
param_handles.mnt_mav_compid = param_find("MNT_MAV_COMPID");
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_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");
@ -586,11 +604,15 @@ bool get_params(ParameterHandles &param_handles, Parameters &params)
param_handles.mnt_off_pitch = param_find("MNT_OFF_PITCH");
param_handles.mnt_off_roll = param_find("MNT_OFF_ROLL");
param_handles.mnt_off_yaw = param_find("MNT_OFF_YAW");
param_handles.mav_sys_id = param_find("MAV_SYS_ID");
param_handles.mav_comp_id = param_find("MAV_COMP_ID");
param_handles.mnt_rate_pitch = param_find("MNT_RATE_PITCH");
param_handles.mnt_rate_yaw = param_find("MNT_RATE_YAW");
if (param_handles.mnt_mode_in == PARAM_INVALID ||
param_handles.mnt_mode_out == PARAM_INVALID ||
param_handles.mnt_mav_sysid == PARAM_INVALID ||
param_handles.mnt_mav_compid == PARAM_INVALID ||
param_handles.mnt_mav_sys_id_v1 == PARAM_INVALID ||
param_handles.mnt_mav_comp_id_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 ||
@ -602,7 +624,12 @@ bool get_params(ParameterHandles &param_handles, Parameters &params)
param_handles.mnt_range_yaw == PARAM_INVALID ||
param_handles.mnt_off_pitch == PARAM_INVALID ||
param_handles.mnt_off_roll == PARAM_INVALID ||
param_handles.mnt_off_yaw == 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.mnt_rate_pitch == PARAM_INVALID ||
param_handles.mnt_rate_yaw == PARAM_INVALID
) {
return false;
}

20
src/modules/vmount/vmount_params.c
View File

@ -235,3 +235,23 @@ PARAM_DEFINE_FLOAT(MNT_OFF_ROLL, 0.0f);
* @group Mount
*/
PARAM_DEFINE_FLOAT(MNT_OFF_YAW, 0.0f);
/**
* Angular pitch rate for manual input in degrees/second.
* Full stick input [-1..1] translats to [-pitch rate..pitch rate].
*
* @min 1.0
* @max 90.0
* @group Mount
*/
PARAM_DEFINE_FLOAT(MNT_RATE_PITCH, 30.0f);
/**
* Angular yaw rate for manual input in degrees/second.
* Full stick input [-1..1] translats to [-yaw rate..yaw rate].
*
* @min 1.0
* @max 90.0
* @group Mount
*/
PARAM_DEFINE_FLOAT(MNT_RATE_YAW, 30.0f);