Commit 199c6f93 authored by Don Gagne's avatar Don Gagne

Delay HOME_POSITION to better simulate reality

parent af824e20
...@@ -88,6 +88,7 @@ MockLink::MockLink(MockConfiguration* config) ...@@ -88,6 +88,7 @@ MockLink::MockLink(MockConfiguration* config)
, _fileServer(NULL) , _fileServer(NULL)
, _sendStatusText(false) , _sendStatusText(false)
, _apmSendHomePositionOnEmptyList(false) , _apmSendHomePositionOnEmptyList(false)
, _sendHomePositionDelayCount(2)
{ {
_config = config; _config = config;
if (_config) { if (_config) {
...@@ -171,7 +172,12 @@ void MockLink::_run1HzTasks(void) ...@@ -171,7 +172,12 @@ void MockLink::_run1HzTasks(void)
{ {
if (_mavlinkStarted && _connected) { if (_mavlinkStarted && _connected) {
_sendHeartBeat(); _sendHeartBeat();
_sendHomePosition(); if (_sendHomePositionDelayCount > 0) {
// We delay home position a bit to be more realistic
_sendHomePositionDelayCount--;
} else {
_sendHomePosition();
}
if (_sendStatusText) { if (_sendStatusText) {
_sendStatusText = false; _sendStatusText = false;
_sendStatusTextMessages(); _sendStatusTextMessages();
...@@ -715,24 +721,29 @@ void MockLink::setMissionItemFailureMode(MockLinkMissionItemHandler::FailureMode ...@@ -715,24 +721,29 @@ void MockLink::setMissionItemFailureMode(MockLinkMissionItemHandler::FailureMode
void MockLink::_sendHomePosition(void) void MockLink::_sendHomePosition(void)
{ {
mavlink_message_t msg; // APM stack does not yet support HOME_POSITION
float bogus[4]; if (_firmwareType != MAV_AUTOPILOT_ARDUPILOTMEGA) {
bogus[0] = 0.0f;
bogus[1] = 0.0f; mavlink_message_t msg;
bogus[2] = 0.0f;
bogus[3] = 0.0f; float bogus[4];
bogus[0] = 0.0f;
mavlink_msg_home_position_pack(_vehicleSystemId, bogus[1] = 0.0f;
_vehicleComponentId, bogus[2] = 0.0f;
&msg, bogus[3] = 0.0f;
(int32_t)(_vehicleLatitude * 1E7),
(int32_t)(_vehicleLongitude * 1E7), mavlink_msg_home_position_pack(_vehicleSystemId,
(int32_t)(_vehicleAltitude * 1000), _vehicleComponentId,
0.0f, 0.0f, 0.0f, &msg,
&bogus[0], (int32_t)(_vehicleLatitude * 1E7),
0.0f, 0.0f, 0.0f); (int32_t)(_vehicleLongitude * 1E7),
respondWithMavlinkMessage(msg); (int32_t)(_vehicleAltitude * 1000),
0.0f, 0.0f, 0.0f,
&bogus[0],
0.0f, 0.0f, 0.0f);
respondWithMavlinkMessage(msg);
}
} }
void MockLink::_sendGpsRawInt(void) void MockLink::_sendGpsRawInt(void)
......
...@@ -191,6 +191,8 @@ private: ...@@ -191,6 +191,8 @@ private:
bool _sendStatusText; bool _sendStatusText;
bool _apmSendHomePositionOnEmptyList; bool _apmSendHomePositionOnEmptyList;
int _sendHomePositionDelayCount;
static float _vehicleLatitude; static float _vehicleLatitude;
static float _vehicleLongitude; static float _vehicleLongitude;
static float _vehicleAltitude; static float _vehicleAltitude;
......
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