aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Mount_MAVLink: minimalist mavlink backend

Browse Source 
We assume MAVLink enabled mount will do all the hard work of support all modes
This commit is contained in:
Randy Mackay 2015-01-09 05:11:26 +09:00 committed by Andrew Tridgell
parent 8a9df1c894
commit 6c766051e8
2 changed files with 214 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

144
libraries/AP_Mount/AP_Mount_MAVLink.cpp Normal file
View File

@ -0,0 +1,144 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include <AP_Mount_MAVLink.h>
// 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
}

70
libraries/AP_Mount/AP_Mount_MAVLink.h Normal file
View File

@ -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 <AP_Math.h>
#include <AP_Common.h>
#include <AP_GPS.h>
#include <AP_AHRS.h>
#include <GCS_MAVLink.h>
#include <RC_Channel.h>
#include <AP_Mount_Backend.h>
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__