diff --git a/src/comm/MockLink.cc b/src/comm/MockLink.cc index 63f05728e3da6a72ded54e138db7abe7d86dd4ed..a91506a2e8302e7ca81f6f264a8e085efcbf78ae 100644 --- a/src/comm/MockLink.cc +++ b/src/comm/MockLink.cc @@ -158,6 +158,7 @@ void MockLink::_run1HzTasks(void) if (_mavlinkStarted && _connected) { _sendHeartBeat(); _sendVibration(); + _sendRCChannels(); if (_sendHomePositionDelayCount > 0) { // We delay home position a bit to be more realistic _sendHomePositionDelayCount--; @@ -1026,3 +1027,26 @@ MockLink* MockLink::startAPMArduPlaneMockLink(bool sendStatusText) return _startMockLink(mockConfig); } +void MockLink::_sendRCChannels(void) +{ + mavlink_message_t msg; + + mavlink_msg_rc_channels_pack(_vehicleSystemId, + _vehicleComponentId, + &msg, + 0, // time_boot_ms + 8, // chancount + 1500, // chan1_raw + 1500, // chan2_raw + 1500, // chan3_raw + 1500, // chan4_raw + 1500, // chan5_raw + 1500, // chan6_raw + 1500, // chan7_raw + 1500, // chan8_raw + UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, + 0); // rssi + + respondWithMavlinkMessage(msg); + +} diff --git a/src/comm/MockLink.h b/src/comm/MockLink.h index 499d7613ae7ac03d01d9ae1e32241dbd2d74b8a3..ef6ab0a7e8a10ca6cf0517afebbe7b21449ee0d5 100644 --- a/src/comm/MockLink.h +++ b/src/comm/MockLink.h @@ -173,6 +173,7 @@ private: void _sendVibration(void); void _sendStatusTextMessages(void); void _respondWithAutopilotVersion(void); + void _sendRCChannels(void); static MockLink* _startMockLink(MockConfiguration* mockConfig);