From 6c766051e802f4a992297e1640e91202a605631c Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 9 Jan 2015 05:11:26 +0900 Subject: [PATCH] Mount_MAVLink: minimalist mavlink backend We assume MAVLink enabled mount will do all the hard work of support all modes --- libraries/AP_Mount/AP_Mount_MAVLink.cpp | 144 ++++++++++++++++++++++++ libraries/AP_Mount/AP_Mount_MAVLink.h | 70 ++++++++++++ 2 files changed, 214 insertions(+) create mode 100644 libraries/AP_Mount/AP_Mount_MAVLink.cpp create mode 100644 libraries/AP_Mount/AP_Mount_MAVLink.h diff --git a/libraries/AP_Mount/AP_Mount_MAVLink.cpp b/libraries/AP_Mount/AP_Mount_MAVLink.cpp new file mode 100644 index 0000000000..fe9b20f76d --- /dev/null +++ b/libraries/AP_Mount/AP_Mount_MAVLink.cpp @@ -0,0 +1,144 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#include + +// init - performs any required initialisation for this instance +void AP_Mount_MAVLink::init() +{ + // do nothing +} + +// update mount position - should be called periodically +void AP_Mount_MAVLink::update() +{ + // nothing to do for a MAVlink gimbal that supports all modes: + // MAV_MOUNT_MODE_RETRACT - nothing to do if the mount holds the retracted angles and + // we do not implement a separate servo based retract mechanism + // MAV_MOUNT_MODE_NEUTRAL - nothing to do if the mount holds the neutral angles + // MAV_MOUNT_MODE_MAVLINK_TARGETING - targets should already have been sent by a call to control_msg + // MAV_MOUNT_MODE_RC_TARGETING - mount should be able to see RC inputs published by flight controller + // MAV_MOUNT_MODE_GPS_POINT - mount should have received target sent from control_msg and + // should know vehicle's position which has been published by flight controller +} + +// has_pan_control - returns true if this mount can control it's pan (required for multicopters) +bool AP_Mount_MAVLink::has_pan_control() const +{ + // we do not have yaw control + return false; +} + +// set_mode - sets mount's mode +void AP_Mount_MAVLink::set_mode(enum MAV_MOUNT_MODE mode) +{ + // exit immediately if mount has not been found + if (!_enabled) { + return; + } + + // prepare and send command_long message with DO_SET_MODE command + mavlink_msg_command_long_send( + _chan, _sysid, _compid, // channel, system id, component id + MAV_CMD_DO_SET_MODE, // command number + 0, // confirmation: 0=first confirmation of this command + mode, // param1: mode + 0, // param2: custom mode + 0.0f, 0.0f, 0.0f,0.0f, 0.0f); // param3 ~ param 7: not used + + // record the mode change + _frontend.state[_instance]._mode = mode; +} + +// set_roi_target - sets target location that mount should attempt to point towards +void AP_Mount_MAVLink::set_roi_target(const struct Location &target_loc) +{ + // exit immediately if mount has not been found + if (!_enabled) { + return; + } + + // prepare and send command_long message + mavlink_msg_command_long_send( + _chan, _sysid, _compid, // channel, system id, component id + MAV_CMD_DO_MOUNT_CONTROL, // command number + 0, // confirmation: 0=first confirmation of this command + target_loc.lat, // param1: pitch (in degrees) or lat (as int32_t) + target_loc.lng, // param2: roll (in degrees) or lon (as int32_t) + target_loc.alt, // param3: yaw (in degrees) or alt (in meters). To-Do: clarify if this is absolute alt or relative to home + 0.0f,0.0f,0.0f, // param4 ~ param6 : not used + MAV_MOUNT_MODE_GPS_POINT); // param7: MAV_MOUNT_MODE enum value +} + +// configure_msg - process MOUNT_CONFIGURE messages received from GCS +void AP_Mount_MAVLink::configure_msg(mavlink_message_t* msg) +{ + // exit immediately if mount has not been found + if (!_enabled) { + return; + } + + // forward on message with updated channel, sysid, compid + mavlink_msg_mount_configure_send( + _chan, _sysid, _compid, + mavlink_msg_mount_configure_get_mount_mode(msg), + mavlink_msg_mount_configure_get_stab_roll(msg), + mavlink_msg_mount_configure_get_stab_pitch(msg), + mavlink_msg_mount_configure_get_stab_yaw(msg)); +} + +// control_msg - process MOUNT_CONTROL messages received from GCS +void AP_Mount_MAVLink::control_msg(mavlink_message_t* msg) +{ + // exit immediately if mount has not been found + if (!_enabled) { + return; + } + + // forward on message with updated channel, sysid, compid + mavlink_msg_mount_control_send( + _chan, _sysid, _compid, + mavlink_msg_mount_control_get_input_a(msg), + mavlink_msg_mount_control_get_input_b(msg), + mavlink_msg_mount_control_get_input_c(msg), + mavlink_msg_mount_control_get_save_position(msg)); +} + +// status_msg - called to allow mounts to send their status to GCS using the MOUNT_STATUS message +void AP_Mount_MAVLink::status_msg(mavlink_channel_t chan) +{ + // exit immediately if mount has not been found + if (!_enabled) { + return; + } +} + +// find_mount - search for MAVLink enabled mount +void AP_Mount_MAVLink::find_mount() +{ + // return immediately if target has already been found + if (_enabled) { + return; + } + + // To-Do: search for MAVLink enabled mount using MAVLink_routing table +} + +// send_angle_target - send earth-frame angle targets to mount +void AP_Mount_MAVLink::send_angle_target(const Vector3f& target_deg) +{ + // exit immediately if mount has not been found + if (!_enabled) { + return; + } + + // prepare and send command_long message with DO_MOUNT_CONTROL command + mavlink_msg_command_long_send( + _chan, _sysid, _compid, // channel, system id, component id + MAV_CMD_DO_MOUNT_CONTROL, // command number + 0, // confirmation: 0=first confirmation of this command + target_deg.y, // param1: pitch (in degrees) or lat (as int32_t) + target_deg.x, // param2: roll (in degrees) or lon (as int32_t) + target_deg.z, // param3: yaw (in degrees) or alt (in meters). + 0.0f,0.0f,0.0f, // param4 ~ param6 : not used + MAV_MOUNT_MODE_MAVLINK_TARGETING); // param7: MAV_MOUNT_MODE enum value +} diff --git a/libraries/AP_Mount/AP_Mount_MAVLink.h b/libraries/AP_Mount/AP_Mount_MAVLink.h new file mode 100644 index 0000000000..6342e90f8d --- /dev/null +++ b/libraries/AP_Mount/AP_Mount_MAVLink.h @@ -0,0 +1,70 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +/* + MAVLink enabled mount backend class + */ + +#ifndef __AP_MOUNT_MAVLINK_H__ +#define __AP_MOUNT_MAVLINK_H__ + +#include +#include +#include +#include +#include +#include +#include + +class AP_Mount_MAVLink : public AP_Mount_Backend +{ + +public: + // Constructor + AP_Mount_MAVLink(AP_Mount &frontend, uint8_t instance) : + AP_Mount_Backend(frontend, instance), + _enabled(false), + _chan(MAVLINK_COMM_0), + _sysid(0), + _compid(0) + {} + + // init - performs any required initialisation for this instance + virtual void init(); + + // update mount position - should be called periodically + virtual void update(); + + // has_pan_control - returns true if this mount can control it's pan (required for multicopters) + virtual bool has_pan_control() const; + + // set_mode - sets mount's mode + virtual void set_mode(enum MAV_MOUNT_MODE mode); + + // set_roi_target - sets target location that mount should attempt to point towards + virtual void set_roi_target(const struct Location &target_loc); + + // configure_msg - process MOUNT_CONFIGURE messages received from GCS + virtual void configure_msg(mavlink_message_t* msg); + + // control_msg - process MOUNT_CONTROL messages received from GCS + virtual void control_msg(mavlink_message_t* msg); + + // status_msg - called to allow mounts to send their status to GCS using the MOUNT_STATUS message + virtual void status_msg(mavlink_channel_t chan); + +private: + + // find_mount - search for MAVLink enabled mount + void find_mount(); + + // send_angle_target - send earth-frame angle targets (in degrees) to mount + void send_angle_target(const Vector3f& target_deg); + + // internal variables + bool _enabled; // gimbal becomes enabled once the mount has been discovered + mavlink_channel_t _chan; // telemetry channel used to communicate with mount + uint8_t _sysid; // system id of MAVLink enabled mount + uint8_t _compid; // component id of MAVLink enabled mount +}; + +#endif // __AP_MOUNT_MAVLINK_H__