diff --git a/libraries/AP_Mount/AP_Mount_Gremsy.cpp b/libraries/AP_Mount/AP_Mount_Gremsy.cpp index ef5e0bd526..5ed787755e 100644 --- a/libraries/AP_Mount/AP_Mount_Gremsy.cpp +++ b/libraries/AP_Mount/AP_Mount_Gremsy.cpp @@ -97,7 +97,7 @@ void AP_Mount_Gremsy::update() bool AP_Mount_Gremsy::healthy() const { // unhealthy until gimbal has been found and replied with device info - if (!_found_gimbal || !_got_device_info) { + if (_link == nullptr || !_got_device_info) { return false; } @@ -149,17 +149,16 @@ void AP_Mount_Gremsy::find_gimbal() } // search for a mavlink enabled gimbal - if (!_found_gimbal) { + if (_link == nullptr) { // we expect that instance 0 has compid = MAV_COMP_ID_GIMBAL, instance 1 has compid = MAV_COMP_ID_GIMBAL2, etc uint8_t compid = (_instance == 0) ? MAV_COMP_ID_GIMBAL : MAV_COMP_ID_GIMBAL2 + (_instance - 1); - if (GCS_MAVLINK::find_by_mavtype_and_compid(MAV_TYPE_GIMBAL, compid, _sysid, _chan)) { - _compid = compid; - _found_gimbal = true; - return; - } else { + _link = GCS_MAVLINK::find_by_mavtype_and_compid(MAV_TYPE_GIMBAL, compid, _sysid); + if (_link == nullptr) { // have not yet found a gimbal so return return; } + + _compid = compid; } // request GIMBAL_DEVICE_INFORMATION @@ -229,6 +228,11 @@ void AP_Mount_Gremsy::handle_gimbal_device_attitude_status(const mavlink_message // request GIMBAL_DEVICE_INFORMATION message void AP_Mount_Gremsy::request_gimbal_device_information() const { + if (_link == nullptr) { + return; + } + const mavlink_channel_t _chan = _link->get_chan(); + // check we have space for the message if (!HAVE_PAYLOAD_SPACE(_chan, COMMAND_LONG)) { return; @@ -245,8 +249,12 @@ void AP_Mount_Gremsy::request_gimbal_device_information() const // start sending ATTITUDE and AUTOPILOT_STATE_FOR_GIMBAL_DEVICE to gimbal bool AP_Mount_Gremsy::start_sending_attitude_to_gimbal() { + // better safe than sorry: + if (_link == nullptr) { + return false; + } // send AUTOPILOT_STATE_FOR_GIMBAL_DEVICE - const MAV_RESULT res = gcs().set_message_interval(_chan, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE, AP_MOUNT_GREMSY_ATTITUDE_INTERVAL_US); + const MAV_RESULT res = _link->set_message_interval(MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE, AP_MOUNT_GREMSY_ATTITUDE_INTERVAL_US); // return true on success return (res == MAV_RESULT_ACCEPTED); @@ -255,6 +263,11 @@ bool AP_Mount_Gremsy::start_sending_attitude_to_gimbal() // send GIMBAL_DEVICE_SET_ATTITUDE to gimbal to command gimbal to retract (aka relax) void AP_Mount_Gremsy::send_gimbal_device_retract() const { + if (_link == nullptr) { + return; + } + const mavlink_channel_t _chan = _link->get_chan(); + // check we have space for the message if (!HAVE_PAYLOAD_SPACE(_chan, GIMBAL_DEVICE_SET_ATTITUDE)) { return; @@ -274,6 +287,11 @@ void AP_Mount_Gremsy::send_gimbal_device_retract() const // earth_frame should be true if yaw_rads target is an earth frame rate, false if body_frame void AP_Mount_Gremsy::send_gimbal_device_set_rate(float roll_rads, float pitch_rads, float yaw_rads, bool earth_frame) const { + if (_link == nullptr) { + return; + } + const mavlink_channel_t _chan = _link->get_chan(); + // check we have space for the message if (!HAVE_PAYLOAD_SPACE(_chan, GIMBAL_DEVICE_SET_ATTITUDE)) { return; @@ -301,6 +319,11 @@ void AP_Mount_Gremsy::send_gimbal_device_set_attitude(float roll_rad, float pitc return; } + if (_link == nullptr) { + return; + } + const mavlink_channel_t _chan = _link->get_chan(); + // check we have space for the message if (!HAVE_PAYLOAD_SPACE(_chan, GIMBAL_DEVICE_SET_ATTITUDE)) { return; diff --git a/libraries/AP_Mount/AP_Mount_Gremsy.h b/libraries/AP_Mount/AP_Mount_Gremsy.h index 40f1acd591..4895fcd500 100644 --- a/libraries/AP_Mount/AP_Mount_Gremsy.h +++ b/libraries/AP_Mount/AP_Mount_Gremsy.h @@ -71,11 +71,10 @@ private: void send_gimbal_device_set_attitude(float roll_rad, float pitch_rad, float yaw_rad, bool earth_frame) const; // internal variables - bool _found_gimbal; // true once a MAVLink enabled gimbal has been found bool _got_device_info; // true once gimbal has provided device info bool _initialised; // true once the gimbal has provided a GIMBAL_DEVICE_INFORMATION uint32_t _last_devinfo_req_ms; // system time that GIMBAL_DEVICE_INFORMATION was last requested (used to throttle requests) - mavlink_channel_t _chan; // mavlink channel used to communicate with gimbal + class GCS_MAVLINK *_link; // link we have found gimbal on; nullptr if not seen yet uint8_t _sysid; // sysid of gimbal uint8_t _compid; // component id of gimbal mavlink_gimbal_device_attitude_status_t _gimbal_device_attitude_status; // copy of most recently received gimbal status