diff --git a/.github/workflows/release-package.yml b/.github/workflows/release-package.yml index a1cbdbe..473d31e 100644 --- a/.github/workflows/release-package.yml +++ b/.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: diff --git a/examples/dashboard/src/index.ts b/examples/dashboard/src/index.ts index 9eff9af..e740f99 100644 --- a/examples/dashboard/src/index.ts +++ b/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); diff --git a/examples/dashboard/src/system.ts b/examples/dashboard/src/system.ts index 47a4384..55c171e 100644 --- a/examples/dashboard/src/system.ts +++ b/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 += `
X ${innerStatus.x.toFixed(2)}
`; + statusStr += `
Y ${innerStatus.y.toFixed(2)}
`; + statusStr += `
Z ${innerStatus.z.toFixed(2)}ms
`; + + 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, diff --git a/package.json b/package.json index 19668be..1ed2847 100644 --- a/package.json +++ b/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 ", "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", diff --git a/src/RICCommsStats.ts b/src/RICCommsStats.ts index 5920436..d78914c 100755 --- a/src/RICCommsStats.ts +++ b/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++; diff --git a/src/RICConnector.ts b/src/RICConnector.ts index 17d8118..c788f13 100644 --- a/src/RICConnector.ts +++ b/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; diff --git a/src/RICMsgHandler.ts b/src/RICMsgHandler.ts index af58d8d..2089ac5 100644 --- a/src/RICMsgHandler.ts +++ b/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; diff --git a/src/RICROSSerial.ts b/src/RICROSSerial.ts index efc3941..b0223e1 100644 --- a/src/RICROSSerial.ts +++ b/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)}`); diff --git a/src/RICServoFaultDetector.ts b/src/RICServoFaultDetector.ts index b34f056..b0c321d 100644 --- a/src/RICServoFaultDetector.ts +++ b/src/RICServoFaultDetector.ts @@ -24,6 +24,45 @@ import RICLog from "./RICLog"; import RICMsgHandler from "./RICMsgHandler"; import { RICHWElemList_Min, RICReportMsg, RICServoFaultFlags, RICStateInfo } from "./RICTypes"; + +class HWStatusLastReported { + // a class that holds the last reported hw status for 30 seconds + // this is to prevent overloading marty with hwstatus requests + // when spurious reports are received + private static instance: HWStatusLastReported; + private lastReported: RICHWElemList_Min | null; + private lastReportedTime: number; + private static readonly MAX_TIME = 30000; // 30 seconds + + private constructor() { + this.lastReported = null; + this.lastReportedTime = 0; + } + + public static getInstance() { + if (!HWStatusLastReported.instance) { + HWStatusLastReported.instance = new HWStatusLastReported(); + } + return HWStatusLastReported.instance; + } + + public setLastReported(hwStatus: RICHWElemList_Min | string) { + // only set the last reported if it is not a string + if (typeof hwStatus === "string") { + return; + } + this.lastReported = hwStatus; + this.lastReportedTime = Date.now(); + } + + public getLastReported() { + if (Date.now() - this.lastReportedTime > HWStatusLastReported.MAX_TIME) { + return null; + } + return this.lastReported; + } +} + export default class RICServoFaultDetector { private _ricMsgHandler: RICMsgHandler; private static expirationDate: Date = new Date(); @@ -37,10 +76,17 @@ export default class RICServoFaultDetector { private async getAllServos(): Promise { RICServoFaultDetector._servoList = []; - const response = await this._ricMsgHandler.sendRICRESTURL("hwstatus/minstat?filterByType=SmartServo"); - if (!response || !response.hw) { - RICLog.warn("RICServoFaultDetector: Error getting servo list"); - return; + const cachedHwstatus = HWStatusLastReported.getInstance().getLastReported(); + let response; + if (cachedHwstatus) { + response = cachedHwstatus; + } else { + response = await this._ricMsgHandler.sendRICRESTURL("hwstatus/minstat?filterByType=SmartServo"); + if (!response || !response.hw) { + RICLog.warn("RICServoFaultDetector: Error getting servo list"); + return; + } + HWStatusLastReported.getInstance().setLastReported(response); } const servosWithIdAndName = response.hw.map((smartServo) => ({ id: smartServo.I, name: smartServo.n })); // filter only the servos that they have enabled the fault bit in their status byte @@ -138,6 +184,4 @@ export default class RICServoFaultDetector { // Check if the 6th bit (from the right) is enabled return Boolean(num & 64); } - - } \ No newline at end of file diff --git a/src/RICTypes.ts b/src/RICTypes.ts index 67e72f3..19c0e32 100644 --- a/src/RICTypes.ts +++ b/src/RICTypes.ts @@ -15,6 +15,7 @@ import { ROSSerialPowerStatus, ROSSerialAddOnStatusList, ROSSerialRobotStatus, + ROSSerialMagneto, } from './RICROSSerial'; import { RICUpdateEvent } from './RICUpdateEvents'; @@ -147,6 +148,8 @@ export class RICStateInfo { smartServosValidMs = 0; imuData: ROSSerialIMU = new ROSSerialIMU(); imuDataValidMs = 0; + magnetoData: ROSSerialMagneto = new ROSSerialMagneto(); + magnetoDataValidMs = 0; power: ROSSerialPowerStatus = new ROSSerialPowerStatus(); powerValidMs = 0; addOnInfo: ROSSerialAddOnStatusList = new ROSSerialAddOnStatusList();