Handling Magnetometer Sensor Data for Batch 6+ Robots

.github/workflows/release-package.yml

@@ -2,11 +2,20 @@ name: Publish Package to npmjs
 on:
   release:
     types: [created]
+  workflow_dispatch:
+    inputs:
+      branch:
+        description: 'The branch to checkout'
+        required: false
+        default: 'master'
 jobs:
   build:
     runs-on: ubuntu-latest
     steps:
       - uses: actions/checkout@v2
+        with:
+          ref: ${{ github.event.inputs.branch || 'master' }}
+
       # Setup .npmrc file to publish to npm
       - uses: actions/setup-node@v2
         with:

examples/dashboard/src/index.ts

@@ -1,6 +1,6 @@
 import { acceptCheckCorrectRIC, connectBLE, connectWiFi, connectWebSerial, disconnect, rejectCheckCorrectRIC, startCheckCorrectRIC } from './connect';
 import { sendREST, streamSoundFile } from './stream';
-import { imuStatusFormat, robotStatusFormat, servoStatusFormat, addonListFormat, tableFormat, sysInfoGet, connPerfTest, setReconnect, pixGetColourStr, commsStatusFormat, powerStatusFormat, addonValListFormat } from './system';
+import { imuStatusFormat, robotStatusFormat, servoStatusFormat, addonListFormat, tableFormat, sysInfoGet, connPerfTest, setReconnect, pixGetColourStr, commsStatusFormat, powerStatusFormat, addonValListFormat, magnetoStatusFormat } from './system';
 import { RICConnEvent } from '../../../src/RICConnEvents';
 import { RICUpdateEvent } from '../../../src/RICUpdateEvents';
 import RICConnector from '../../../src/RICConnector';
@@ -50,6 +50,7 @@ globalThis.ricConnector = new RICConnector();
 if (globalThis.ricConnector) {
   globalThis.ricConnector.setupUpdateManager("2.0.0", 
               `https://updates.robotical.io/live/martyv2/rev{HWRevNo}/current_version.json`, 
+              "",
               fileDownloader);
   globalThis.ricConnector.setEventListener(eventListener);
 }
@@ -100,6 +101,7 @@ function updateStatus() {
   formatStatus("robotStatus", ricState.robotStatus, ricState.robotStatusValidMs, robotStatusFormat, "robot-status-container");
   formatStatus("powerStatus", ricState.power, ricState.powerValidMs, powerStatusFormat, "power-status-container");
   formatStatus("imuStatus", ricState.imuData, ricState.imuDataValidMs, imuStatusFormat, "imu-status-container");
+  formatStatus("magnetoStatus", ricState.magnetoData, ricState.magnetoDataValidMs, magnetoStatusFormat, "magneto-status-container");
   formatStatus("servoStatus", ricState.smartServos, ricState.smartServosValidMs, servoStatusFormat, "servo-status-container");
   formatStatus("sysInfoStatus", ricSystem.getCachedSystemInfo(), ricSystem.getCachedSystemInfo()?.validMs, tableFormat, "sysinfo-list-container");
   formatStatus("addonsStatus", ricSystem.getCachedAddOnList(), null, addonListFormat, "addon-list-container");
@@ -174,6 +176,7 @@ function component() {
   genStatusBlock('robot-status-container', 'info-status-container', statusContainer);
   genStatusBlock('power-status-container', 'info-status-container', statusContainer);
   genStatusBlock('imu-status-container', 'info-status-container', statusContainer);
+  genStatusBlock('magneto-status-container', 'info-status-container', statusContainer);
   genStatusBlock('servo-status-container', 'info-status-container', statusContainer);
   genStatusBlock('sysinfo-list-container', 'info-status-container', statusContainer);
   genStatusBlock('addon-list-container', 'info-status-container', statusContainer);

examples/dashboard/src/system.ts

@@ -1,7 +1,7 @@
 import RICCommsStats from "../../../src/RICCommsStats";
 import RICConnector from "../../../src/RICConnector";
 import RICLog from "../../../src/RICLog";
-import { ROSSerialAddOnStatus, ROSSerialIMU, ROSSerialPowerStatus, ROSSerialRGBT, ROSSerialRobotStatus, ROSSerialSmartServos } from "../../../src/RICROSSerial";
+import { ROSSerialAddOnStatus, ROSSerialIMU, ROSSerialMagneto, ROSSerialPowerStatus, ROSSerialRGBT, ROSSerialRobotStatus, ROSSerialSmartServos } from "../../../src/RICROSSerial";
 import { Dictionary, RICHWElem } from "../../../src/RICTypes";
 import { RICRoboticalAddOns } from "@robotical/ricjs-robotical-addons";
 
@@ -133,6 +133,17 @@ export function imuStatusFormat(name:string, imuStatus:ROSSerialIMU): string {
   return statusStr;
 }
 
+export function magnetoStatusFormat(name:string, magnetoStatus:ROSSerialMagneto): string {
+
+    const innerStatus = magnetoStatus.magneto;
+    let statusStr = "";
+    statusStr += `<div class="flag-info">X ${innerStatus.x.toFixed(2)}</div>`;
+    statusStr += `<div class="flag-info">Y ${innerStatus.y.toFixed(2)}</div>`;
+    statusStr += `<div class="flag-info">Z ${innerStatus.z.toFixed(2)}ms</div>`;
+
+    return statusStr;
+  }
+
 export function servoStatusFormat(name:string, servoStatus:ROSSerialSmartServos): string {
   if (!checkNewData(name, servoStatus)) {
     return "";
@@ -246,6 +257,7 @@ export function commsStatusFormat(name:string, commsStats:RICCommsStats): string
   commsStats.getSmartServosRate();
   commsStats.getAddOnPubRate();
   commsStats.getIMURate();
+  commsStats.getMagnetoRate();
   commsStats.getPowerStatusRate();
   commsStats.getRobotStatusRate();
 
@@ -269,6 +281,8 @@ export function commsStatusFormat(name:string, commsStats:RICCommsStats): string
     "SmartServosRate": commsStats._msgSmartServosPS.toFixed(2),
     "IMU": commsStats._msgIMU,
     "IMURate": commsStats._msgIMUPS.toFixed(2),
+    "Magneto": commsStats._msgMagneto,
+    "MagnetoRate": commsStats._msgMagnetoPS.toFixed(2),
     "PowerStatus": commsStats._msgPowerStatus,
     "PowerStatusRate": commsStats._msgPowerStatusPS.toFixed(2), 
     "AddOnPub": commsStats._msgAddOnPub,

package.json

@@ -1,6 +1,6 @@
 {
     "name": "@robotical/ricjs",
-    "version": "1.16.4",
+    "version": "1.1.0-magnetometer",
     "description": "Javascript/TS library for Robotical RIC",
     "author": "Rob Dobson <rob@dobson.com>",
     "repository": {
@@ -26,7 +26,8 @@
         "ts-node": "ts-node",
         "docs": "typedoc --entryPoints src/main.ts",
         "build": "tsc -p tsconfig.json",
-        "build-all": "npm run clean && npm run build"
+        "build-all": "npm run clean && npm run build",
+        "watch": "tsc -p tsconfig.json --watch"
     },
     "devDependencies": {
         "@types/jest": "^27.4.0",

src/RICCommsStats.ts

@@ -28,24 +28,28 @@ export default class RICCommsStats {
 
   _msgSmartServos = 0;
   _msgIMU = 0;
+  _msgMagneto = 0;
   _msgPowerStatus = 0;
   _msgAddOnPub = 0;
   _msgRobotStatus = 0;
 
   _msgSmartServosPS = 0;
   _msgIMUPS = 0;
+  _msgMagnetoPS = 0;
   _msgPowerStatusPS = 0;
   _msgAddOnPubPS = 0;
   _msgRobotStatusPS = 0;
 
   _msgSmartServosCountInWindow = 0;
   _msgIMUCountInWindow = 0;
+  _msgMagnetoCountInWindow = 0;
   _msgPowerStatusCountInWindow = 0;
   _msgAddOnPubCountInWindow = 0;
   _msgRobotStatusCountInWindow = 0;
 
   _msgSmartServosLastCalcMs = 0;
   _msgIMULastCalcMs = 0;
+  _msgMagnetoLastCalcMs = 0;
   _msgPowerStatusLastCalcMs = 0;
   _msgAddOnPubLastCalcMs = 0;
   _msgRobotStatusLastCalcMs = 0;
@@ -74,21 +78,25 @@ export default class RICCommsStats {
     this._msgRetry = 0;
     this._msgSmartServos = 0;
     this._msgIMU = 0;
+    this._msgMagneto = 0;
     this._msgPowerStatus = 0;
     this._msgAddOnPub = 0;
     this._msgRobotStatus = 0;
     this._msgSmartServosPS = 0;
     this._msgIMUPS = 0;
+    this._msgMagnetoPS = 0;
     this._msgPowerStatusPS = 0;
     this._msgAddOnPubPS = 0;
     this._msgRobotStatusPS = 0;
     this._msgSmartServosCountInWindow = 0;
     this._msgIMUCountInWindow = 0;
+    this._msgMagnetoCountInWindow = 0;
     this._msgPowerStatusCountInWindow = 0;
     this._msgAddOnPubCountInWindow = 0;
     this._msgRobotStatusCountInWindow = 0;
     this._msgSmartServosLastCalcMs = Date.now();
     this._msgIMULastCalcMs = Date.now();
+    this._msgMagnetoLastCalcMs = Date.now();
     this._msgPowerStatusLastCalcMs = Date.now();
     this._msgAddOnPubLastCalcMs = Date.now();
     this._msgRobotStatusLastCalcMs = Date.now();
@@ -156,6 +164,17 @@ export default class RICCommsStats {
     return this._msgIMUPS;
   }
 
+  getMagnetoRate(): number {
+    if (this._msgMagnetoLastCalcMs + 1000 < Date.now()) {
+      this._msgMagnetoPS =
+        (1000.0 * this._msgMagnetoCountInWindow) /
+        (Date.now() - this._msgMagnetoLastCalcMs);
+      this._msgMagnetoLastCalcMs = Date.now();
+      this._msgMagnetoCountInWindow = 0;
+    }
+    return this._msgMagnetoPS;
+  }
+
   getPowerStatusRate(): number {
     if (this._msgPowerStatusLastCalcMs + 1000 < Date.now()) {
       this._msgPowerStatusPS =
@@ -245,6 +264,12 @@ export default class RICCommsStats {
     // Don't call msgRx() as double counting msgs with smartServos
   }
 
+  recordMagneto(): void {
+    this._msgMagneto++;
+    this._msgMagnetoCountInWindow++;
+    // NT: Not sure if we should call msgRx() here or not (like with IMU above)
+  }
+
   recordPowerStatus(): void {
     this._msgPowerStatus++;
     this._msgPowerStatusCountInWindow++;

src/RICConnector.ts

@@ -20,7 +20,7 @@ import RICAddOnManager from "./RICAddOnManager";
 import RICSystem from "./RICSystem";
 import RICFileHandler from "./RICFileHandler";
 import RICStreamHandler from "./RICStreamHandler";
-import { ROSSerialAddOnStatusList, ROSSerialIMU, ROSSerialPowerStatus, ROSSerialRobotStatus, ROSSerialSmartServos } from "./RICROSSerial";
+import { ROSSerialAddOnStatusList, ROSSerialIMU, ROSSerialMagneto, ROSSerialPowerStatus, ROSSerialRobotStatus, ROSSerialSmartServos } from "./RICROSSerial";
 import RICUtils from "./RICUtils";
 import RICLog from "./RICLog";
 import { RICConnEvent, RICConnEventNames } from "./RICConnEvents";
@@ -358,6 +358,12 @@ export default class RICConnector {
     this._ricStateInfo.imuDataValidMs = Date.now();
   }
 
+  onRxMagneto(magnetoData: ROSSerialMagneto): void {
+    // RICLog.verbose(`onRxMagneto ${JSON.stringify(magnetoData)}`);
+    this._ricStateInfo.magnetoData = magnetoData;
+    this._ricStateInfo.magnetoDataValidMs = Date.now();
+  }
+
   onRxPowerStatus(powerStatus: ROSSerialPowerStatus): void {
     // RICLog.verbose(`onRxPowerStatus ${JSON.stringify(powerStatus)}`);
     this._ricStateInfo.power = powerStatus;

src/RICMsgHandler.ts

@@ -19,6 +19,7 @@ import {
   ROSSerialPowerStatus,
   ROSSerialAddOnStatusList,
   ROSSerialRobotStatus,
+  ROSSerialMagneto,
 } from './RICROSSerial';
 import {
   PROTOCOL_RICREST,
@@ -71,6 +72,7 @@ export interface RICMessageResult {
   onRxUnnumberedMsg(msgRsltJsonObj: object): void;
   onRxSmartServo(smartServos: ROSSerialSmartServos): void;
   onRxIMU(imuData: ROSSerialIMU): void;
+  onRxMagneto(magnetoData: ROSSerialMagneto): void;
   onRxPowerStatus(powerStatus: ROSSerialPowerStatus): void;
   onRxAddOnPub(addOnInfo: ROSSerialAddOnStatusList): void;
   onRobotStatus(robotStatus: ROSSerialRobotStatus): void;

src/RICROSSerial.ts

@@ -30,6 +30,14 @@ export class ROSSerialIMU {
   } = { x: 0, y: 0, z: 0 };
 }
 
+export class ROSSerialMagneto {
+  magneto: {
+    x: number;
+    y: number;
+    z: number;
+  } = { x: 0, y: 0, z: 0 };
+}
+
 export class ROSSerialPowerStatus {
   powerStatus: {
     battRemainCapacityPercent: number;
@@ -44,18 +52,18 @@ export class ROSSerialPowerStatus {
     powerUSBIsValid: boolean;
     powerFlags: number;
   } = {
-    battRemainCapacityPercent: 0,
-    battTempDegC: 0,
-    battRemainCapacityMAH: 0,
-    battFullCapacityMAH: 0,
-    battCurrentMA: 0,
-    power5VOnTimeSecs: 0,
-    power5VIsOn: false,
-    powerUSBIsConnected: false,
-    battInfoValid: false,
-    powerUSBIsValid: false,
-    powerFlags: 0,
-  };
+      battRemainCapacityPercent: 0,
+      battTempDegC: 0,
+      battRemainCapacityMAH: 0,
+      battFullCapacityMAH: 0,
+      battCurrentMA: 0,
+      power5VOnTimeSecs: 0,
+      power5VIsOn: false,
+      powerUSBIsConnected: false,
+      battInfoValid: false,
+      powerUSBIsValid: false,
+      powerFlags: 0,
+    };
 }
 
 export class ROSSerialAddOnStatus {
@@ -64,7 +72,7 @@ export class ROSSerialAddOnStatus {
   whoAmI = "";
   name = "";
   status = 0;
-  vals: { [key: string]: number | boolean | string} = {};
+  vals: { [key: string]: number | boolean | string } = {};
 }
 
 export class ROSSerialAddOnStatusList {
@@ -102,24 +110,25 @@ export class ROSSerialRobotStatus {
     wifiRSSI: number;
     bleRSSI: number;
   } = {
-    flags: 0,
-    isMoving: false,
-    isPaused: false,
-    isFwUpdating: false,
-    workQCount: 0,
-    heapFree: 0,
-    heapMin: 0,
-    pixRGBT: [],
-    loopMsAvg: 0,
-    loopMsMax: 0,
-    wifiRSSI: 0,
-    bleRSSI: 0,
-  };
+      flags: 0,
+      isMoving: false,
+      isPaused: false,
+      isFwUpdating: false,
+      workQCount: 0,
+      heapFree: 0,
+      heapMin: 0,
+      pixRGBT: [],
+      loopMsAvg: 0,
+      loopMsMax: 0,
+      wifiRSSI: 0,
+      bleRSSI: 0,
+    };
 }
 
 export type ROSSerialMsg =
   | ROSSerialSmartServos
   | ROSSerialIMU
+  | ROSSerialMagneto
   | ROSSerialPowerStatus
   | ROSSerialAddOnStatusList
   | ROSSerialRobotStatus;
@@ -134,7 +143,7 @@ export class RICROSSerial {
   ): void {
     // Payload may contain multiple ROSSerial messages
     let msgPos = startPos;
-    for (;;) {
+    for (; ;) {
       const remainingMsgLen = rosSerialMsg.length - msgPos;
 
       // ROSSerial ROSTopics
@@ -143,6 +152,7 @@ export class RICROSSerial {
       const ROSTOPIC_V2_POWER_STATUS = 122;
       const ROSTOPIC_V2_ADDONS = 123;
       const ROSTOPIC_V2_ROBOT_STATUS = 124;
+      const ROSTOPIC_V2_MAGNETOMETER = 125;
 
       // ROSSerial message format
       const RS_MSG_MIN_LENGTH = 8;
@@ -187,7 +197,7 @@ export class RICROSSerial {
         // we need to register the static addons here in case
         // marty only has static addons (and so the rostopic_v2_addons case
         // never runs)
-        let allAdons: ROSSerialAddOnStatusList = {addons: []};
+        let allAdons: ROSSerialAddOnStatusList = { addons: [] };
         const staticAddons = addOnManager.getProcessedStaticAddons();
         for (const staticAddon of staticAddons) {
           allAdons.addons.push(staticAddon);
@@ -229,6 +239,11 @@ export class RICROSSerial {
             RICMessageResult.onRobotStatus(this.extractRobotStatus(payload));
             commsStats.recordRobotStatus();
             break;
+          case ROSTOPIC_V2_MAGNETOMETER:
+            // Magnetometer
+            RICMessageResult.onRxMagneto(this.extractMagneto(payload));
+            commsStats.recordMagneto();
+            break;
           default:
             // Unknown topic
             RICMessageResult.onRxOtherROSSerialMsg(topicID, payload);
@@ -276,6 +291,20 @@ export class RICROSSerial {
     return { accel: { x: x / 1024, y: y / 1024, z: z / 1024 } };
   }
 
+  static extractMagneto(buf: Uint8Array): ROSSerialMagneto {
+    // V2 ROSTOPIC MAGNETOMETER message layout
+    // const ROS_MAGNETOMETER_BYTES = 13
+    // const ROS_MAGNETOMETER_POS_X = 0
+    // const ROS_MAGNETOMETER_POS_Y = 4
+    // const ROS_MAGNETOMETER_POS_Z = 8
+    // const ROS_MAGNETOMETER_POS_IDNO = 12
+    // Three magnetometer floats
+    const x = RICUtils.getBEFloatFromBuf(buf);
+    const y = RICUtils.getBEFloatFromBuf(buf.slice(4));
+    const z = RICUtils.getBEFloatFromBuf(buf.slice(8));
+    return { magneto: { x: x, y: y, z: z } };
+  }
+
   static extractPowerStatus(buf: Uint8Array): ROSSerialPowerStatus {
     // Power indicator values
     // RICLog.debug(`PowerStatus ${RICUtils.bufferToHex(buf)}`);