From 7c82e9d58ea72413b457c78c76046f32d93b0423 Mon Sep 17 00:00:00 2001
From: Eric <eric.emj.johnson@gmail.com>
Date: Wed, 28 Feb 2024 14:16:58 -0800
Subject: [PATCH] libs: command ack: added logic to check command results on
 command longs

---
 src/libs/vehicle/ardupilot/ardupilot.ts | 109 +++++++++++++++++++-----
 src/libs/vehicle/types.ts               |  64 ++++++++++++++
 src/libs/vehicle/vehicle.ts             |   2 +
 3 files changed, 156 insertions(+), 19 deletions(-)

diff --git a/src/libs/vehicle/ardupilot/ardupilot.ts b/src/libs/vehicle/ardupilot/ardupilot.ts
index 231de6dff..182003f2e 100644
--- a/src/libs/vehicle/ardupilot/ardupilot.ts
+++ b/src/libs/vehicle/ardupilot/ardupilot.ts
@@ -1,3 +1,5 @@
+import Swal from 'sweetalert2'
+
 import { ConnectionManager } from '@/libs/connection/connection-manager'
 import type {
   MAVLinkMessageDictionary,
@@ -15,6 +17,7 @@ import {
   MavMissionType,
   MavModeFlag,
   MavParamType,
+  MavResult,
   MavState,
   MavType,
 } from '@/libs/connection/m2r/messages/mavlink2rest-enum'
@@ -34,6 +37,7 @@ import {
   Altitude,
   Attitude,
   Battery,
+  CommandAck,
   Coordinates,
   FixTypeGPS,
   Parameter,
@@ -116,6 +120,9 @@ export abstract class ArduPilotVehicle<Modes> extends Vehicle.AbstractVehicle<Mo
    * @param {number} param5
    * @param {number} param6
    * @param {number} param7
+   * @param {boolean} waitForAck If the command should wait for an acknowledgment
+   * @param {boolean} noAlert Controls if swal alerts should not be shown
+   * @returns {Promise<CommandAck>} A promise that resolves with the command acknowledgment.
    */
   sendCommandLong(
     mav_command: MavCmd,
@@ -125,26 +132,73 @@ export abstract class ArduPilotVehicle<Modes> extends Vehicle.AbstractVehicle<Mo
     param4?: number,
     param5?: number,
     param6?: number,
-    param7?: number
-  ): void {
-    const command: Message.CommandLong = {
-      type: MAVLinkType.COMMAND_LONG,
-      param1: param1 ?? 0,
-      param2: param2 ?? 0,
-      param3: param3 ?? 0,
-      param4: param4 ?? 0,
-      param5: param5 ?? 0,
-      param6: param6 ?? 0,
-      param7: param7 ?? 0,
-      command: {
-        type: mav_command,
-      },
-      target_system: this.currentSystemId,
-      target_component: 1,
-      confirmation: 0,
-    }
+    param7?: number,
+    waitForAck = true,
+    noAlert = false
+  ): Promise<CommandAck> {
+    return new Promise((resolve, reject) => {
+      // Construct the command
+      const command: Message.CommandLong = {
+        type: MAVLinkType.COMMAND_LONG,
+        param1: param1 ?? 0,
+        param2: param2 ?? 0,
+        param3: param3 ?? 0,
+        param4: param4 ?? 0,
+        param5: param5 ?? 0,
+        param6: param6 ?? 0,
+        param7: param7 ?? 0,
+        command: {
+          type: mav_command,
+        },
+        target_system: this.currentSystemId,
+        target_component: 1,
+        confirmation: 0,
+      }
+
+      this.write(command)
+      if (!waitForAck) {
+        resolve(new CommandAck())
+        return
+      }
+
+      let ackReceived = false
+      const ackHandler = (commandAck: CommandAck): void => {
+        console.log('Received command acknowledgment:', commandAck)
+        if (commandAck.command.type === mav_command) {
+          ackReceived = true
+          this.onCommandAck.remove(ackHandler)
 
-    this.write(command)
+          if (![MavResult.MAV_RESULT_ACCEPTED, MavResult.MAV_RESULT_IN_PROGRESS].includes(commandAck.result.type)) {
+            if (!noAlert) {
+              commandAck.alertMessage()
+            }
+            reject(new Error(`Command failed with result: ${commandAck.result.type}`))
+          } else {
+            this.onCommandAck.remove(ackHandler)
+            resolve(commandAck)
+          }
+        }
+      }
+
+      this.onCommandAck.add(ackHandler)
+
+      // Setup a timeout for the ACK
+      const timeout = 5000
+      setTimeout(() => {
+        if (!ackReceived) {
+          if (!noAlert) {
+            Swal.fire({
+              title: 'Command timeout',
+              text: 'No acknowledgment was received for the command: ' + mav_command + ' in ' + timeout / 1000 + 's',
+              icon: 'error',
+              confirmButtonText: 'OK',
+            })
+          }
+          this.onCommandAck.remove(ackHandler)
+          reject(new Error('Timeout waiting for command acknowledgment'))
+        }
+      }, timeout)
+    })
   }
 
   /**
@@ -245,6 +299,23 @@ export abstract class ArduPilotVehicle<Modes> extends Vehicle.AbstractVehicle<Mo
     this.onMAVLinkMessage.emit_value(mavlink_message.message.type, mavlink_message)
 
     switch (mavlink_message.message.type) {
+      // command_ack
+      case MAVLinkType.COMMAND_ACK: {
+        const commandAck = mavlink_message.message as Message.CommandAck
+
+        const ack = new CommandAck({
+          command: commandAck.command,
+          result: commandAck.result,
+          progress: commandAck.progress,
+          resultText: commandAck.resultText,
+          targetSystem: commandAck.targetSystem,
+          targetComponent: commandAck.targetComponent,
+        })
+
+        // emit on command_ack
+        this.onCommandAck.emit_value(ack)
+        break
+      }
       case MAVLinkType.AHRS2: {
         const ahrsMessage = mavlink_message.message as Message.Ahrs2
         this._altitude.msl = ahrsMessage.altitude
diff --git a/src/libs/vehicle/types.ts b/src/libs/vehicle/types.ts
index d34de2485..dff23d7d3 100644
--- a/src/libs/vehicle/types.ts
+++ b/src/libs/vehicle/types.ts
@@ -1,3 +1,7 @@
+import Swal from 'sweetalert2'
+
+import type { Type } from '@/libs/connection/m2r/messages/mavlink2rest'
+import { MavCmd, MavResult } from '@/libs/connection/m2r/messages/mavlink2rest-enum'
 import { AlertLevel } from '@/types/alert'
 
 /**
@@ -100,6 +104,66 @@ export class Coordinates {
   }
 }
 
+/**
+ * Command Acknowledgment
+ */
+export class CommandAck {
+  command: Type<MavCmd>
+  result: Type<MavResult>
+  progress: number
+  resultText: string
+  targetSystem: number
+  targetComponent: number
+
+  /**
+   * Creates an instance of CommandAck.
+   * @param {Partial<CommandAck>} [init]
+   */
+  constructor(init?: Partial<CommandAck>) {
+    Object.assign(this, init)
+  }
+
+  /**
+   * Alert message
+   */
+  public alertMessage(): void {
+    if (
+      this.result.type === MavResult.MAV_RESULT_TEMPORARILY_REJECTED ||
+      this.result.type === MavResult.MAV_RESULT_DENIED ||
+      this.result.type === MavResult.MAV_RESULT_UNSUPPORTED ||
+      this.result.type === MavResult.MAV_RESULT_FAILED
+    ) {
+      Swal.fire({
+        title: 'Command Failed',
+        text: 'Command: ' + this.command.type + ' failed with result: ' + this.result.type,
+        icon: 'error',
+        confirmButtonText: 'OK',
+      })
+    } else if (this.result.type === MavResult.MAV_RESULT_ACCEPTED) {
+      Swal.fire({
+        title: 'Command Accepted',
+        text: 'Command: ' + this.command.type + ' succeeded with: ' + this.result.type,
+        icon: 'success',
+        confirmButtonText: 'OK',
+      })
+    } else if (this.result.type === MavResult.MAV_RESULT_IN_PROGRESS) {
+      Swal.fire({
+        title: 'Command In Progress',
+        text: 'Command: ' + this.command.type + ' is in progress',
+        icon: 'info',
+        confirmButtonText: 'OK',
+      })
+    } else {
+      Swal.fire({
+        title: 'Result Unknown',
+        text: 'Command: ' + this.command.type + ' result is unknown',
+        icon: 'error',
+        confirmButtonText: 'OK',
+      })
+    }
+  }
+}
+
 /**
  * Body frame attitude
  */
diff --git a/src/libs/vehicle/vehicle.ts b/src/libs/vehicle/vehicle.ts
index 8fb6e0b5d..bc6322f5a 100644
--- a/src/libs/vehicle/vehicle.ts
+++ b/src/libs/vehicle/vehicle.ts
@@ -6,6 +6,7 @@ import type {
   Altitude,
   Attitude,
   Battery,
+  CommandAck,
   Coordinates,
   PageDescription,
   Parameter,
@@ -75,6 +76,7 @@ export abstract class AbstractVehicle<Modes> {
   onAltitude = new Signal<Altitude>()
   onBatteries = new Signal<Battery[]>()
   onCpuLoad = new Signal<number>()
+  onCommandAck = new Signal<CommandAck>()
   onGenericVariables = new Signal<Record<string, unknown>>()
   onMode = new Signal<Modes>()
   onPosition = new Signal<Coordinates>()