From d6433266d21e71737e01f709633d005d58085d1f Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 19 Jan 2015 22:39:13 +0900 Subject: [PATCH] Mount_MAVLink: use SerialManager for init use serial_manager's get_mavlink_channel --- libraries/AP_Mount/AP_Mount_MAVLink.cpp | 22 ++++++++++++++++++++-- libraries/AP_Mount/AP_Mount_MAVLink.h | 6 ++++-- 2 files changed, 24 insertions(+), 4 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount_MAVLink.cpp b/libraries/AP_Mount/AP_Mount_MAVLink.cpp index 3caa8cd6b1..89be19ad36 100644 --- a/libraries/AP_Mount/AP_Mount_MAVLink.cpp +++ b/libraries/AP_Mount/AP_Mount_MAVLink.cpp @@ -1,16 +1,24 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- #include +#include // init - performs any required initialisation for this instance -void AP_Mount_MAVLink::init() +void AP_Mount_MAVLink::init(const AP_SerialManager& serial_manager) { - // do nothing + // use mavlink channel associated with MAVLink2 protocol + if (serial_manager.get_mavlink_channel(AP_SerialManager::SerialProtocol_MAVLink2, _chan)) { + _initialised = true; + } } // update mount position - should be called periodically void AP_Mount_MAVLink::update() { + // exit immediately if not initialised + if (!_initialised) { + return; + } // update based on mount mode switch(_frontend.get_mode(_instance)) { @@ -60,6 +68,11 @@ bool AP_Mount_MAVLink::has_pan_control() const // set_mode - sets mount's mode void AP_Mount_MAVLink::set_mode(enum MAV_MOUNT_MODE mode) { + // exit immediately if not initialised + if (!_initialised) { + return; + } + // map requested mode to mode that mount can actually support enum MAV_MOUNT_MODE mode_to_send = mode; switch (mode) { @@ -98,6 +111,11 @@ void AP_Mount_MAVLink::status_msg(mavlink_channel_t chan) // send_angle_target - send earth-frame angle targets to mount void AP_Mount_MAVLink::send_angle_target(const Vector3f& target, bool target_in_degrees) { + // exit immediately if not initialised + if (!_initialised) { + return; + } + // convert to degrees if necessary Vector3f target_deg = target; if (!target_in_degrees) { diff --git a/libraries/AP_Mount/AP_Mount_MAVLink.h b/libraries/AP_Mount/AP_Mount_MAVLink.h index 3bc428de50..7c07f99608 100644 --- a/libraries/AP_Mount/AP_Mount_MAVLink.h +++ b/libraries/AP_Mount/AP_Mount_MAVLink.h @@ -24,12 +24,13 @@ public: // Constructor AP_Mount_MAVLink(AP_Mount &frontend, AP_Mount::mount_state state, uint8_t instance) : AP_Mount_Backend(frontend, state, instance), + _initialised(false), _chan(MAVLINK_COMM_0), _last_mode(MAV_MOUNT_MODE_RETRACT) {} // init - performs any required initialisation for this instance - virtual void init(); + virtual void init(const AP_SerialManager& serial_manager); // update mount position - should be called periodically virtual void update(); @@ -49,7 +50,8 @@ private: void send_angle_target(const Vector3f& target, bool target_in_degrees); // internal variables - mavlink_channel_t _chan; // telemetry channel used to communicate with mount + bool _initialised; // true once the driver has been initialised + mavlink_channel_t _chan; // telemetry channel used to communicate with mount enum MAV_MOUNT_MODE _last_mode; // last mode sent to mount Vector3f _last_angle_target; // last angle target sent to mount };