Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

Mount_SToRM32: find gimbal channel on startup

Browse Source
This commit is contained in:
Randy Mackay 2015-07-24 20:54:50 +09:00
parent e7df54ca75
commit b13921a7aa
2 changed files with 36 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
libraries/AP_Mount/AP_Mount_SToRM32.cpp
View File

@ -9,25 +9,18 @@ extern const AP_HAL::HAL& hal;
AP_Mount_SToRM32::AP_Mount_SToRM32(AP_Mount &frontend, AP_Mount::mount_state &state, uint8_t instance) :
AP_Mount_Backend(frontend, state, instance),
_initialised(false),
_sysid(0),
_compid(0),
_chan(MAVLINK_COMM_0),
_last_send(0)
{}
// init - performs any required initialisation for this instance
void AP_Mount_SToRM32::init(const AP_SerialManager& serial_manager)
{
// get_mavlink_channel for MAVLink2
if (serial_manager.get_mavlink_channel(AP_SerialManager::SerialProtocol_MAVLink, 1, _chan)) {
_initialised = true;
set_mode((enum MAV_MOUNT_MODE)_state._default_mode.get());
}
}
// update mount position - should be called periodically
void AP_Mount_SToRM32::update()
{
// exit immediately if not initialised
if (!_initialised) {
find_gimbal();
return;
}
@ -113,6 +106,24 @@ void AP_Mount_SToRM32::status_msg(mavlink_channel_t chan)
mavlink_msg_mount_status_send(chan, 0, 0, ToDeg(_angle_ef_target_rad.y)*100, ToDeg(_angle_ef_target_rad.x)*100, ToDeg(_angle_ef_target_rad.z)*100);
}
// search for gimbal in GCS_MAVLink routing table
void AP_Mount_SToRM32::find_gimbal()
{
// return immediately if initialised
if (_initialised) {
return;
}
// return if search time has has passed
if (hal.scheduler->millis() > AP_MOUNT_STORM32_SEARCH_MS) {
return;
}
if (GCS_MAVLINK::find_by_mavtype(MAV_TYPE_GIMBAL, _sysid, _compid, _chan)) {
_initialised = true;
}
}
// send_do_mount_control - send a COMMAND_LONG containing a do_mount_control message
void AP_Mount_SToRM32::send_do_mount_control(float pitch_deg, float roll_deg, float yaw_deg, enum MAV_MOUNT_MODE mount_mode)
{
@ -121,14 +132,19 @@ void AP_Mount_SToRM32::send_do_mount_control(float pitch_deg, float roll_deg, fl
return;
}
// check we have space for the message
if (comm_get_txspace(_chan) < MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_MSG_ID_COMMAND_LONG_LEN) {
return;
}
// reverse pitch and yaw control
pitch_deg = -pitch_deg;
yaw_deg = -yaw_deg;
// send command_long command containing a do_mount_control command
mavlink_msg_command_long_send(_chan,
AP_MOUNT_STORM32_SYSID,
AP_MOUNT_STORM32_COMPID,
_sysid,
_compid,
MAV_CMD_DO_MOUNT_CONTROL,
0, // confirmation of zero means this is the first time this message has been sent
pitch_deg,

12
libraries/AP_Mount/AP_Mount_SToRM32.h
View File

@ -13,14 +13,13 @@
#include <AP_Math.h>
#include <AP_Common.h>
#include <AP_GPS.h>
#include <GCS.h>
#include <GCS_MAVLink.h>
#include <RC_Channel.h>
#include <AP_Mount_Backend.h>
#define AP_MOUNT_STORM32_SYSID 71 // hardcoded system id
#define AP_MOUNT_STORM32_COMPID 67 // hard coded component id for communicating with the gimbal
#define AP_MOUNT_STORM32_RESEND_MS 1000 // resend angle targets to gimbal once per second
#define AP_MOUNT_STORM32_SEARCH_MS 60000 // search for gimbal for 1 minute after startup
class AP_Mount_SToRM32 : public AP_Mount_Backend
{
@ -30,7 +29,7 @@ public:
AP_Mount_SToRM32(AP_Mount &frontend, AP_Mount::mount_state &state, uint8_t instance);
// init - performs any required initialisation for this instance
virtual void init(const AP_SerialManager& serial_manager);
virtual void init(const AP_SerialManager& serial_manager) {}
// update mount position - should be called periodically
virtual void update();
@ -46,11 +45,16 @@ public:
private:
// search for gimbal in GCS_MAVLink routing table
void find_gimbal();
// send_do_mount_control - send a COMMAND_LONG containing a do_mount_control message
void send_do_mount_control(float pitch_deg, float roll_deg, float yaw_deg, enum MAV_MOUNT_MODE mount_mode);
// internal variables
bool _initialised; // true once the driver has been initialised
uint8_t _sysid; // sysid of gimbal
uint8_t _compid; // component id of gimbal
mavlink_channel_t _chan; // mavlink channel used to communicate with gimbal. Currently hard-coded to Telem2
uint32_t _last_send; // system time of last do_mount_control sent to gimbal
};