Set mode

services/common/messages/telemetry.proto

@@ -24,6 +24,7 @@ message Overview {
     Velocity vel = 5;
     Speed speed = 6;
     Battery battery = 7;
+    Mode mode = 8;
 }
 
 // WGS84 in degrees
@@ -115,3 +116,7 @@ message MissionCurrent {
     double time = 1;
     uint32 item_number = 2;
 }
+
+message Mode {
+    string name = 1;
+}

services/telemetry/src/mode.js

@@ -0,0 +1,110 @@
+import logger from './common/logger';
+import _ from 'lodash';
+import { cleanupAfter } from './util';
+
+const MODE_TIMEOUT = 15000;
+
+const mode_mapping_apm = {
+  0: 'MANUAL',
+  1: 'CIRCLE',
+  2: 'STABILIZE',
+  3: 'TRAINING',
+  4: 'ACRO',
+  5: 'FBWA',
+  6: 'FBWB',
+  7: 'CRUISE',
+  8: 'AUTOTUNE',
+  10: 'AUTO',
+  11: 'RTL',
+  12: 'LOITER',
+  14: 'LAND',
+  15: 'GUIDED',
+  16: 'INITIALISING',
+  17: 'QSTABILIZE',
+  18: 'QHOVER',
+  19: 'QLOITER',
+  20: 'QLAND',
+  21: 'QRTL',
+  22: 'QAUTOTUNE'
+};
+
+const nameMappingAPM = _.invert(mode_mapping_apm);
+
+let unknownWarn = true;
+
+const MAV_TYPE_FIXED_WING = 1;
+const MAV_AUTOPILOT_ARDUPILOTMEGA = 3;
+const MAV_CMD_DO_SET_MODE = 176;
+const MAV_RESULT_ACCEPTED = 0;
+const MAV_RESULT_TEMPORARILY_REJECTED = 1;
+const MAV_RESULT_IN_PROGRESS = 5;
+
+export function onHeartbeat(fields) {
+
+  let ov = this._overview;
+  ov.time = Date.now() / 1000;
+
+  if (fields.type == MAV_TYPE_FIXED_WING &&
+    fields.autopilot == MAV_AUTOPILOT_ARDUPILOTMEGA) {
+    ov.mode.name = mode_mapping_apm[fields.custom_mode];
+  } else if (unknownWarn) {
+    unknownWarn = false;
+    if (fields.type !== MAV_TYPE_FIXED_WING)
+      logger.warn(`Unknown autopilot ${fields.autopilot}`);
+    if (fields.autopilot !== MAV_AUTOPILOT_ARDUPILOTMEGA)
+      logger.warn(`Unknown vehicle type ${fields.type}`);
+  }
+}
+
+export async function sendMode(mav, mode) {
+  let modeNumber = nameMappingAPM[mode.name];
+
+  if (modeNumber === undefined) {
+    throw new Error(`Invalid mode name ${mode.name}`);
+  }
+
+  let setCurrentInt;
+  let onAck;
+
+  let cleanupObj = cleanupAfter(() => {
+    clearInterval(setCurrentInt);
+    mav.removeListener('COMMAND_ACK', onAck);
+  }, MODE_TIMEOUT, 'setting mode took too long');
+
+  // Send a mode set current message repetitively.
+  _sendCommandLong(mav, modeNumber, 0);
+  let confirmation = 1;
+
+  setCurrentInt = setInterval(() => {
+    return _sendCommandLong(mav, modeNumber, confirmation++);
+  }, 1000);
+
+  onAck = (fields) => {
+    if (fields.type !== MAV_CMD_DO_SET_MODE) {
+      return;
+    } else if (fields.result === MAV_RESULT_ACCEPTED) {
+      cleanupObj.finish();
+    } else if (fields.result === MAV_RESULT_TEMPORARILY_REJECTED
+     || fields.result === MAV_RESULT_IN_PROGRESS) {
+      return;
+    } else {
+      let err = Error('COMMAND_ACK returned CMD ID ' + fields.result);
+      cleanupObj.finish(err);
+    }
+  };
+
+  mav.on('COMMAND_ACK', onAck);
+
+  await cleanupObj.wait();
+}
+
+async function _sendCommandLong(mav, modeNumber, confirmation) {
+  await mav.send('COMMAND_LONG', {
+    target_system: 1,
+    target_component: 0,
+    command: 'MAV_CMD_DO_SET_MODE',
+    confirmation,
+    param1: 0,
+    param2: modeNumber
+  });
+}

services/telemetry/src/plane.js

@@ -5,6 +5,7 @@ import logger from './common/logger';
 import MavlinkSocket from './mavlink-socket';
 import { receiveMission, sendMission, sendMissionCurrent } from './mission';
 import { degrees, modDegrees360, modDegrees180 } from './util';
+import { onHeartbeat, sendMode } from './mode';
 
 const ConnectionState = Object.freeze({
   NOT_CONNECTED: Symbol('not_connected'),
@@ -39,7 +40,8 @@ export default class Plane {
       alt: {},
       vel: {},
       speed: {},
-      battery: {}
+      battery: {},
+      mode: {}
     });
 
     // Will be assigned on each GLOBAL_POSITION_INT message.
@@ -190,6 +192,18 @@ export default class Plane {
     });
   }
 
+  async setMode(mode) {
+    await this._execTransaction(async () => {
+      this._cxnState = ConnectionState.WRITING;
+
+      try {
+        await sendMode(this._mav, mode);
+      } finally {
+        this._cxnState = ConnectionState.IDLE;
+      }
+    });
+  }
+
   // Add an async task to the queue.
   async _execTransaction(asyncTask) {
     return await new Promise((resolve, reject) => {
@@ -245,7 +259,8 @@ export default class Plane {
       'MISSION_CURRENT': this._onMissionCurrent.bind(this),
       'MISSION_ITEM': this._onMissionItem.bind(this),
       'VFR_HUD': this._onVfrHud.bind(this),
-      'SYS_STATUS': this._onSysStatus.bind(this)
+      'SYS_STATUS': this._onSysStatus.bind(this),
+      'HEARTBEAT': onHeartbeat.bind(this)
     };
 
     // Make every message definition .on

services/telemetry/src/router.js

@@ -59,4 +59,9 @@ router.post('/api/mission-current', missionCurrent, timeout, async (ctx) => {
   ctx.status = 200;
 });
 
+router.post('/api/mode', timeout, async (ctx) => {
+  await ctx.plane.setMode(ctx.request.proto);
+  ctx.status = 200;
+});
+
 export default router;

services/telemetry/test/service.test.js

@@ -27,6 +27,7 @@ beforeAll(async () => {
   await planeSitl.start();
 
   planeIp = (await planeSitl.inspect()).NetworkSettings.IPAddress;
+
 }, 10000);
 
 // Stop the plane-sitl container.
@@ -193,6 +194,7 @@ test('get overview telemetry', async () => {
 
   expect(res.status).toEqual(200);
   expect(res.body.pos.lat).toBeTruthy();
+  expect(res.body.mode.name).toEqual('MANUAL');
 });
 
 // Take down the service once the other service tests are done.