Video: Send VIDEO_STREAM_INFORMATION to GCS

Addresses feature request #169.
Enables Rpanion to respond to send COMMAND_ACK and VIDEO_STREAM_INFORMATION messages via MAVLink in response to a MAV_CMD_REQUEST_MESSAGE.
Adds a React Select dropdown to the Video page to select which IP will be transmitted as part of the video stream URI that is sent in the MAVLink message.

mavlink/mavManager.js

@@ -100,6 +100,12 @@ 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
         return
@@ -245,14 +251,12 @@ class mavManager {
     })
   }
 
-  sendHeartbeat ( mavType = minimal.MavType.ONBOARD_CONTROLLER,
-                  autopilot = minimal.MavAutopilot.INVALID,
-                  baseMode = 0,
-                  customMode = 0,
-                  systemStatus = 0,
-                  component = minimal.MavComponent.ONBOARD_COMPUTER 
-                ) {
-
+  sendHeartbeat (mavType = minimal.MavType.ONBOARD_CONTROLLER,
+    autopilot = minimal.MavAutopilot.INVALID,
+    baseMode = 0,
+    customMode = 0,
+    systemStatus = 0,
+    component = minimal.MavComponent.ONBOARD_COMPUTER) {
   // create a heartbeat packet
     const heartbeatMessage = new minimal.Heartbeat()
 
@@ -265,7 +269,21 @@ 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)
@@ -402,4 +420,4 @@ class mavManager {
   }
 }
 
-module.exports = mavManager
+module.exports = mavManager

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,11 +81,25 @@ 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) => {
+  try {
+    if (fcManager.m) {
+      fcManager.m.sendCommandAck(common.VideoStreamInformation.MSG_ID, 0, senderSysId, senderCompId)
+      fcManager.m.sendData(msg)
+    }
+  } 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)
   }
@@ -391,10 +406,11 @@ app.get('/api/softwareinfo', (req, res) => {
 
 app.get('/api/videodevices', (req, res) => {
   vManager.populateAddresses()
-  vManager.getVideoDevices((err, devices, active, seldevice, selRes, selRot, selbitrate, selfps, SeluseUDP, SeluseUDPIP, SeluseUDPPort, timestamp, fps, FPSMax, vidres) => {
+  vManager.getVideoDevices((err, devices, active, seldevice, selRes, selRot, selbitrate, selfps, SeluseUDP, SeluseUDPIP, SeluseUDPPort, timestamp, fps, FPSMax, vidres, selMavURI) => {
     if (!err) {
       res.setHeader('Content-Type', 'application/json')
       res.send(JSON.stringify({
+        ifaces: vManager.ifaces,
         dev: devices,
         vidDeviceSelected: seldevice,
         vidres: vidres,
@@ -410,7 +426,8 @@ app.get('/api/videodevices', (req, res) => {
         timestamp,
         error: null,
         fps: fps,
-        FPSMax: FPSMax
+        FPSMax: FPSMax,
+        mavStreamSelected: vManager.selMavURI
       }))
     } else {
       res.setHeader('Content-Type', 'application/json')
@@ -671,7 +688,7 @@ app.post('/api/startstopvideo', [check('active').isBoolean(),
     return res.status(422).json(ret)
   }
   // user wants to start/stop video streaming
-  vManager.startStopStreaming(req.body.active, req.body.device, req.body.height, req.body.width, req.body.format, req.body.rotation, req.body.bitrate, req.body.fps, req.body.useUDP, req.body.useUDPIP, req.body.useUDPPort, req.body.useTimestamp, (err, status, addresses) => {
+  vManager.startStopStreaming(req.body.active, req.body.device, req.body.height, req.body.width, req.body.format, req.body.rotation, req.body.bitrate, req.body.fps, req.body.useUDP, req.body.useUDPIP, req.body.useUDPPort, req.body.useTimestamp, req.body.mavStreamSelected, (err, status, addresses) => {
     if (!err) {
       res.setHeader('Content-Type', 'application/json')
       const ret = { streamingStatus: status, streamAddresses: addresses }

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)
@@ -21,12 +26,12 @@ class videoStream {
     // need to scan for video devices first though
     if (this.active) {
       this.active = false
-      this.getVideoDevices((error, devices, active, seldevice, selRes, selRot, selbitrate, thisps, selUDP, selUDPIP, selUDPPort, useTimestamp) => {
+      this.getVideoDevices((error, devices, active, seldevice, selRes, selRot, selbitrate, selfps, selUDP, selUDPIP, selUDPPort, useTimestamp, selMavURI) => {
         if (!error) {
           this.startStopStreaming(true, this.savedDevice.device, this.savedDevice.height,
             this.savedDevice.width, this.savedDevice.format,
             this.savedDevice.rotation, this.savedDevice.bitrate, this.savedDevice.fps, this.savedDevice.useUDP,
-            this.savedDevice.useUDPIP, this.savedDevice.useUDPPort, this.savedDevice.useTimestamp,
+            this.savedDevice.useUDPIP, this.savedDevice.useUDPPort, this.savedDevice.useTimestamp, this.savedDevice.mavStreamSelected,
             (err, status, addresses) => {
               if (err) {
                 // failed setup, reset settings
@@ -57,7 +62,7 @@ class videoStream {
   // video streaming
   getVideoDevices (callback) {
     // get all video device details
-    // callback is: err, devices, active, seldevice, selRes, selRot, selbitrate, selfps, SeluseUDP, SeluseUDPIP, SeluseUDPPort, timestamp, fps, FPSMax, vidres
+    // callback is: err, devices, active, seldevice, selRes, selRot, selbitrate, selfps, SeluseUDP, SeluseUDPIP, SeluseUDPPort, timestamp, fps, FPSMax, vidres, selMavURI
     exec('python3 ./python/gstcaps.py', (error, stdout, stderr) => {
       const warnstrings = ['DeprecationWarning', 'gst_element_message_full_with_details', 'camera_manager.cpp', 'Unsupported V4L2 pixel format']
       if (stderr && !warnstrings.some(wrn => stderr.includes(wrn))) {
@@ -76,7 +81,7 @@ class videoStream {
           return callback(null, this.devices, this.active, this.devices[0], this.devices[0].caps[0],
             { label: '0°', value: 0 }, 1100, fpsSelected, false, '127.0.0.1', 5400, false,
             (this.devices[0].caps[0].fps !== undefined) ? this.devices[0].caps[0].fps : [],
-            this.devices[0].caps[0].fpsmax, this.devices[0].caps)
+            this.devices[0].caps[0].fpsmax, this.devices[0].caps, { label: '127.0.0.1', value: 0 })
         } else {
           // format saved settings
           const seldevice = this.devices.filter(it => it.value === this.savedDevice.device)
@@ -88,7 +93,7 @@ class videoStream {
             return callback(null, this.devices, this.active, this.devices[0], this.devices[0].caps[0],
               { label: '0°', value: 0 }, 1100, fpsSelected, false, '127.0.0.1', 5400, false,
               (this.devices[0].caps[0].fps !== undefined) ? this.devices[0].caps[0].fps : [],
-              this.devices[0].caps[0].fpsmax, this.devices[0].caps)
+              this.devices[0].caps[0].fpsmax, this.devices[0].caps, { label: '127.0.0.1', value: 0 })
           }
           const selRes = seldevice[0].caps.filter(it => it.value === this.savedDevice.width.toString() + 'x' + this.savedDevice.height.toString() + 'x' + this.savedDevice.format.toString().split('/')[1])
           let selFPS = this.savedDevice.fps
@@ -102,7 +107,7 @@ class videoStream {
               { label: this.savedDevice.rotation.toString() + '°', value: this.savedDevice.rotation },
               this.savedDevice.bitrate, selFPS, this.savedDevice.useUDP, this.savedDevice.useUDPIP,
               this.savedDevice.useUDPPort, this.savedDevice.useTimestamp, (selRes[0].fps !== undefined) ? selRes[0].fps : [],
-              selRes[0].fpsmax, seldevice[0].caps)
+              selRes[0].fpsmax, seldevice[0].caps, this.savedDevice.mavStreamSelected)
           } else {
             // bad settings
             console.error('Bad video settings. Resetting' + seldevice + ', ' + selRes)
@@ -111,7 +116,7 @@ class videoStream {
             return callback(null, this.devices, this.active, this.devices[0], this.devices[0].caps[0],
               { label: '0°', value: 0 }, 1100, fpsSelected, false, '127.0.0.1', 5400, false,
               (this.devices[0].caps[0].fps !== undefined) ? this.devices[0].caps[0].fps : [],
-              this.devices[0].caps[0].fpsmax, this.devices[0].caps)
+              this.devices[0].caps[0].fpsmax, this.devices[0].caps, { label: '127.0.0.1', value: 0 })
           }
         }
       }
@@ -156,7 +161,7 @@ class videoStream {
     return iface
   }
 
-  async startStopStreaming (active, device, height, width, format, rotation, bitrate, fps, useUDP, useUDPIP, useUDPPort, useTimestamp, callback) {
+  async startStopStreaming (active, device, height, width, format, rotation, bitrate, fps, useUDP, useUDPIP, useUDPPort, useTimestamp, mavStreamSelected, callback) {
     // if current state same, don't do anything
     if (this.active === active) {
       console.log('Video current same')
@@ -195,7 +200,8 @@ class videoStream {
         useUDP,
         useUDPIP,
         useUDPPort,
-        useTimestamp
+        useTimestamp,
+        mavStreamSelected
       }
 
       // note that video device URL's are the alphanumeric characters only. So /dev/video0 -> devvideo0
@@ -282,6 +288,53 @@ 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[this.savedDevice.mavStreamSelected]
+
+      // console.log("mavStreamSelected: " + this.savedDevice.mavStreamSelected)
+      this.eventEmitter.emit('videostreaminfo', msg, senderSysId, senderCompId)
+    }
+  }
 }
 
 module.exports = videoStream

server/videostream.test.js

@@ -31,7 +31,7 @@ describe('Video Functions', function () {
     const vManager = new VideoStream(settings, winston)
 
     vManager.populateAddresses()
-    vManager.getVideoDevices(function (err, devices, active, seldevice, selRes, selRot, selbitrate, selfps, SeluseUDP, SeluseUDPIP, SeluseUDPPort, timestamp, fps, FPSMax, vidres) {
+    vManager.getVideoDevices(function (err, devices, active, seldevice, selRes, selRot, selbitrate, selfps, SeluseUDP, SeluseUDPIP, SeluseUDPPort, timestamp, fps, FPSMax, vidres, selMavURI) {
       assert.equal(err, null)
       assert.equal(active, false)
       assert.notEqual(seldevice, null)
@@ -46,6 +46,7 @@ describe('Video Functions', function () {
       assert.notEqual(fps, null)
       assert.notEqual(FPSMax, null)
       assert.notEqual(vidres, null)
+      assert.notEqual(selMavURI, null)
       done()
     })
   }).timeout(5000)
@@ -62,11 +63,11 @@ describe('Video Functions', function () {
     settings.clear()
     const vManager = new VideoStream(settings, winston)
 
-    vManager.startStopStreaming(true, 'testsrc', '1080', '1920', 'video/x-h264', '0', '1000', '5', false, false, false, true, function (err, status, addresses) {
+    vManager.startStopStreaming(true, 'testsrc', '1080', '1920', 'video/x-h264', '0', '1000', '5', false, false, false, true, '0', function (err, status, addresses) {
       assert.equal(err, null)
       assert.equal(status, true)
       assert.notEqual(vManager.deviceStream.pid, null)
-      vManager.startStopStreaming(false, 'testsrc', '1080', '1920', 'video/x-h264', '0', '1000', '5', false, false, false, true, function (err, status, addresses) {
+      vManager.startStopStreaming(false, 'testsrc', '1080', '1920', 'video/x-h264', '0', '1000', '5', false, false, false, true, '0', function (err, status, addresses) {
         assert.equal(err, null)
         assert.equal(status, false)
         done()

src/video.js

@@ -12,6 +12,7 @@ class VideoPage extends basePage {
   constructor(props) {
     super(props);
     this.state = {
+      ifaces: [],
       dev: [],
       vidDeviceSelected: this.props.vidDeviceSelected,
       vidres: [],
@@ -30,7 +31,8 @@ class VideoPage extends basePage {
       loading: true,
       error: null,
       infoMessage: null,
-      timestamp: false
+      timestamp: false,
+      mavStreamSelected: { label: "127.0.0.1", value: 0 }
     }
   }
 
@@ -94,6 +96,11 @@ class VideoPage extends basePage {
     this.setState({ timestamp: !this.state.timestamp });
   }
 
+  handleMavStreamChange = (value) => {
+    //new value for selected stream IP
+    this.setState({ mavStreamSelected: value });
+  }
+
   handleStreaming = (event) => {
     //user clicked start/stop streaming
     this.setState({ waiting: true }, () => {
@@ -117,6 +124,7 @@ class VideoPage extends basePage {
           useUDPIP: this.state.useUDPIP,
           useUDPPort: this.state.useUDPPort,
           useTimestamp: this.state.timestamp,
+          mavStreamSelected: this.state.mavStreamSelected.value,
         })
       }).then(response => response.json()).then(state => { this.setState(state); this.setState({ waiting: false }) });
     });
@@ -186,6 +194,15 @@ class VideoPage extends basePage {
             <input disabled={this.state.streamingStatus} type="number" name="fps" min="1" max={this.state.FPSMax} step="1" onChange={this.handleFPSChange} value={this.state.fpsSelected} />fps (max: {this.state.FPSMax})
           </div>
         </div>
+        <br />
+    <h3>MAVLink Video Streaming Service</h3>
+    <p><i>Configuration for advertising the video stream via MAVLink. See <a href='https://mavlink.io/en/services/camera.html#video_streaming'>here</a> for details.</i></p>
+    <div className="form-group row" style={{ marginBottom: '5px' }}>
+      <label className="col-sm-4 col-form-label">Video source IP Address</label>
+      <div className="col-sm-8">
+        <Select isDisabled={this.state.streamingStatus} onChange={this.handleMavStreamChange} options={this.state.ifaces.map((item, index) => ({ value: index, label: item}))} value={this.state.mavStreamSelected} />
+      </div>
+    </div>
         <div style={{ display: (this.state.UDPChecked) ? "block" : "none" }}>
           <div className="form-group row" style={{ marginBottom: '5px' }}>
             <label className="col-sm-4 col-form-label ">Destination IP</label>