diff --git a/mavlink/mavManager.js b/mavlink/mavManager.js index 34565d94..cf631894 100644 --- a/mavlink/mavManager.js +++ b/mavlink/mavManager.js @@ -100,8 +100,16 @@ class mavManager { // send off initial messages this.sendVersionRequest() + + // Respond to MavLink commands that are targeted to the companion computer + } else if ( data.targetSystem === this.targetSystem && + data.targetComponent == minimal.MavComponent.ONBOARD_COMPUTER && + packet.header.msgid === common.CommandLong.MSG_ID + ){ + console.log("Received CommandLong addressed to onboard computer") + } else if (this.targetSystem !== packet.header.sysid) { - // don't use packets from other systems or components in Rpanion-server + //don't use packets from other systems or components in Rpanion-server return } @@ -266,6 +274,22 @@ class mavManager { this.sendData(heartbeatMessage, component) } + sendCommandAck (commandReceived, commandResult = 0, targetSystem = 255, targetComponent = minimal.MavComponent.MISSION_PLANNER) { + + // create a CommandAck packet + const commandAck = new common.CommandAck() + + commandAck.command = commandReceived + // result = 0 for "accepted and executed" + commandAck.result = commandResult + // resultParam2 is for optional additional result information. Not currently used by rpanion. + commandAck.resultParam2 = 0; + commandAck.targetSystem = targetSystem + commandAck.targetComponent = targetComponent + + this.sendData(commandAck) + } + sendReboot () { // create a reboot packet const command = new common.PreflightRebootShutdownCommand(this.targetSystem, this.targetComponent) diff --git a/mavlink/mavManager.test.js b/mavlink/mavManager.test.js index 25abc475..1c247958 100644 --- a/mavlink/mavManager.test.js +++ b/mavlink/mavManager.test.js @@ -137,6 +137,28 @@ describe('MAVLink Functions', function () { }) }) + it('#commandAckSend()', function (done) { + const m = new mavManager(2, '127.0.0.1', 15000) + const udpStream = udp.createSocket('udp4') + + m.eventEmitter.on('linkready', (info) => { + m.sendCommandAck() + }) + + udpStream.on('message', (msg, rinfo) => { + msg.should.eql(Buffer.from([253, 09, 00, 00, 00, 00, 191, 77, 00, 00, 00, 00, 00, 00, 00, 00, 00, 00, 255, 197, 27 ])) + m.close() + udpStream.close() + done() + }) + + udpStream.send(Buffer.from([0xfd, 0x06]), 15000, '127.0.0.1', (error) => { + if (error) { + console.error(error) + } + }) + }) + it('#perfTest()', function () { // how fast can we process packets and send out over udp? const m = new mavManager(2, '127.0.0.1', 15000) diff --git a/server/index.js b/server/index.js index e301d8ff..cdf3aa73 100644 --- a/server/index.js +++ b/server/index.js @@ -4,6 +4,7 @@ const compression = require('compression') const bodyParser = require('body-parser') const pino = require('express-pino-logger')() const process = require('process') +const { common } = require('node-mavlink') const networkManager = require('./networkManager') const aboutPage = require('./aboutInfo') @@ -80,11 +81,27 @@ ntripClient.eventEmitter.on('rtcmpacket', (msg, seq) => { } }) +// Got a VIDEO_STREAM_INFORMATION message, send to flight controller +// to do: get target system and component from vManager +vManager.eventEmitter.on('videostreaminfo', (msg, senderSysId, senderCompId) => { + // logManager.writetlog(msg.buf); + try { + if (fcManager.m) { + fcManager.m.sendCommandAck(common.VideoStreamInformation.MSG_ID, 0, senderSysId, senderCompId) + fcManager.m.sendData(msg) + //console.log("index.js Received videostreaminfo event") + } + } catch (err) { + console.log(err) + } +}) + // Connecting the flight controller datastream to the logger -// and ntrip +// and ntrip and video fcManager.eventEmitter.on('gotMessage', (packet, data) => { try { ntripClient.onMavPacket(packet, data) + vManager.onMavPacket(packet, data) } catch (err) { console.log(err) } diff --git a/server/videostream.js b/server/videostream.js index 8b365ddc..e7daee72 100644 --- a/server/videostream.js +++ b/server/videostream.js @@ -1,6 +1,8 @@ const { exec, spawn } = require('child_process') const os = require('os') const si = require('systeminformation') +const events = require('events') +const { common } = require('node-mavlink') class videoStream { constructor (settings, winston) { @@ -13,6 +15,9 @@ class videoStream { this.winston = winston + // For sending events outside of object + this.eventEmitter = new events.EventEmitter() + // load settings. savedDevice is a json object storing all settings this.active = this.settings.value('videostream.active', false) this.savedDevice = this.settings.value('videostream.savedDevice', null) @@ -282,6 +287,54 @@ class videoStream { } return ret } + + onMavPacket (packet, data) { + // FC is active + if (!this.active) { + return + } + + if (packet.header.msgid === common.CommandLong.MSG_ID & data._param1 === common.VideoStreamInformation.MSG_ID) { + console.log("Responding to MAVLink request for VideoStreamInformation") + this.winston.info('Responding to MAVLink request for VideoStreamInformation') + + const senderSysId = packet.header.sysid + const senderCompId = packet.header.compid + + // build a VIDEO_STREAM_INFORMATION packet + const msg = new common.VideoStreamInformation() + + // rpanion only supports a single stream, so streamId and count will always be 1 + msg.streamId = 1; + msg.count = 1; + + // 0 = VIDEO_STREAM_TYPE_RTSP + // 1 = VIDEO_STREAM_TYPE_RTPUDP + if(this.savedDevice.useUDP = "rtp") { + msg.type = 1 + } else { + msg.type = 0 + } + + // 1 = VIDEO_STREAM_STATUS_FLAGS_RUNNING + // 2 = VIDEO_STREAM_STATUS_FLAGS_THERMAL + msg.flags = 1; + msg.framerate = this.savedDevice.fps; + msg.resolutionH = this.savedDevice.width; + msg.resolutionV = this.savedDevice.height; + msg.bitrate = this.savedDevice.bitrate; + msg.rotation = this.savedDevice.rotation; + // Rpanion doesn't collect field of view values, so just set to zero + msg.hfov = 0; + msg.name = this.savedDevice.device; + // To do: add a UI option to select which interface's URI to send + msg.uri = this.deviceAddresses[1]; + + this.eventEmitter.emit('videostreaminfo', msg, senderSysId, senderCompId) + } + +} + } module.exports = videoStream