Commit 812ca6dd authored by Don Gagne's avatar Don Gagne

Send home position in all cases

parent 647b56e4
......@@ -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)
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment