Video: Send VIDEO_STREAM_INFORMATION to GCS

Addresses feature request issue stephendade#169

Enables Rpanion to respond to send COMMAND_ACK and VIDEO_STREAM_INFORMATION messages via MavLink in response to a MAV_CMD_REQUEST_MESSAGE.

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)

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)

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,12 +81,28 @@ 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 {
     logManager.writetlog(packet, data)
     ntripClient.onMavPacket(packet, data)
+    vManager.onMavPacket(packet, data)
   } catch (err) {
     console.log(err)
   }

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