From 812ca6dd10a2c8b11c05b8894b681241f887ca45 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Mon, 8 Feb 2016 16:37:00 -0800 Subject: [PATCH] Send home position in all cases --- src/comm/MockLink.cc | 39 +++++++++++++++++---------------------- 1 file changed, 17 insertions(+), 22 deletions(-) diff --git a/src/comm/MockLink.cc b/src/comm/MockLink.cc index e01fd9e1c..0564ea1e1 100644 --- a/src/comm/MockLink.cc +++ b/src/comm/MockLink.cc @@ -799,29 +799,24 @@ void MockLink::setMissionItemFailureMode(MockLinkMissionItemHandler::FailureMode void MockLink::_sendHomePosition(void) { - // APM stack does not yet support HOME_POSITION - - if (_firmwareType != MAV_AUTOPILOT_ARDUPILOTMEGA) { - - mavlink_message_t msg; + mavlink_message_t msg; - float bogus[4]; - bogus[0] = 0.0f; - bogus[1] = 0.0f; - bogus[2] = 0.0f; - bogus[3] = 0.0f; - - mavlink_msg_home_position_pack(_vehicleSystemId, - _vehicleComponentId, - &msg, - (int32_t)(_vehicleLatitude * 1E7), - (int32_t)(_vehicleLongitude * 1E7), - (int32_t)(_vehicleAltitude * 1000), - 0.0f, 0.0f, 0.0f, - &bogus[0], - 0.0f, 0.0f, 0.0f); - respondWithMavlinkMessage(msg); - } + float bogus[4]; + bogus[0] = 0.0f; + bogus[1] = 0.0f; + bogus[2] = 0.0f; + bogus[3] = 0.0f; + + mavlink_msg_home_position_pack(_vehicleSystemId, + _vehicleComponentId, + &msg, + (int32_t)(_vehicleLatitude * 1E7), + (int32_t)(_vehicleLongitude * 1E7), + (int32_t)(_vehicleAltitude * 1000), + 0.0f, 0.0f, 0.0f, + &bogus[0], + 0.0f, 0.0f, 0.0f); + respondWithMavlinkMessage(msg); } void MockLink::_sendGpsRawInt(void) -- 2.22.0