diff --git a/src/comm/MockLink.cc b/src/comm/MockLink.cc index ee371ff81572a8ab1341c84fe46055a0497761d4..a425891370a2a22393ff303a15a39e9c8ebd5b01 100644 --- a/src/comm/MockLink.cc +++ b/src/comm/MockLink.cc @@ -67,6 +67,10 @@ union px4_custom_mode { float data_float; }; +float MockLink::_vehicleLatitude = 47.633033f; +float MockLink::_vehicleLongitude = -122.08794f; +float MockLink::_vehicleAltitude = 2.5f; + MockLink::MockLink(MockConfiguration* config) : _missionItemHandler(this) , _name("MockLink") @@ -164,6 +168,7 @@ void MockLink::_run1HzTasks(void) void MockLink::_run10HzTasks(void) { if (_mavlinkStarted && _connected) { + _sendGpsRawInt(); } } @@ -696,7 +701,7 @@ void MockLink::setMissionItemFailureMode(MockLinkMissionItemHandler::FailureMode void MockLink::_sendHomePosition(void) { - mavlink_message_t msg; + mavlink_message_t msg; float bogus[4]; bogus[0] = 0.0f; @@ -707,11 +712,31 @@ void MockLink::_sendHomePosition(void) mavlink_msg_home_position_pack(_vehicleSystemId, _vehicleComponentId, &msg, - (int32_t)(47.633033f * 1E7), - (int32_t)(-122.08794f * 1E7), - (int32_t)(2.0f * 1000), + (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) +{ + static uint64_t timeTick = 0; + mavlink_message_t msg; + + mavlink_msg_gps_raw_int_pack(_vehicleSystemId, + _vehicleComponentId, + &msg, + timeTick++, // time since boot + 3, // 3D fix + (int32_t)(_vehicleLatitude * 1E7), + (int32_t)(_vehicleLongitude * 1E7), + (int32_t)(_vehicleAltitude * 1000), + UINT16_MAX, UINT16_MAX, // HDOP/VDOP not known + UINT16_MAX, // velocity not known + UINT16_MAX, // course over ground not known + 8); // satellite count + respondWithMavlinkMessage(msg); +} diff --git a/src/comm/MockLink.h b/src/comm/MockLink.h index d8692e06521ae9d67c6d64e9c75a1bb5a2f78f92..790291833c79625710c3a2ba7609f27e941af6c5 100644 --- a/src/comm/MockLink.h +++ b/src/comm/MockLink.h @@ -144,6 +144,7 @@ private: float _floatUnionForParam(int componentId, const QString& paramName); void _setParamFloatUnionIntoMap(int componentId, const QString& paramName, float paramFloat); void _sendHomePosition(void); + void _sendGpsRawInt(void); MockLinkMissionItemHandler _missionItemHandler; @@ -167,6 +168,10 @@ private: MAV_AUTOPILOT _autopilotType; MockLinkFileServer* _fileServer; + + static float _vehicleLatitude; + static float _vehicleLongitude; + static float _vehicleAltitude; }; #endif