Add relative altitude on GoToAction

app/src/main/java/org/WenuLink/adapters/RequestCommand.kt

@@ -86,12 +86,19 @@ data class RequestTakeoff(val altitude: Float = 2f, val timeout: Long = 15_000L)
         val takeoffResult = ctx.dispatchAndAwait(WenuLinkCommand.Aircraft(TakeoffCommand(timeout)))
         if (takeoffResult.hasError) return takeoffResult
 
-        val coordinates = ctx.aircraft.currentCoordinates
+        // Capture current global coordinates
+        val coordinates = ctx.aircraft.globalCoordinates
+            ?: return CommandResult.error("No aircraft coordinates available")
+        val position = ctx.aircraft.localPosition
             ?: return CommandResult.error("No aircraft position available")
+        // update with relative altitude
+        val finalAltitude = altitude - position.alt
+
+        ctx.logger.i { "processTakeoff finalAltitude: $finalAltitude" }
 
         return ctx.dispatchAndAwait(
             WenuLinkCommand.Mission(
-                RepositionAction(coordinates.copy(alt = altitude), ctx.mission.flightSpeed)
+                RepositionAction(coordinates.copy(alt = finalAltitude), ctx.mission.flightSpeed)
             )
         )
     }

app/src/main/java/org/WenuLink/adapters/WenuLinkHandler.kt

@@ -44,7 +44,7 @@ class WenuLinkHandler : CommandHandler<WenuLinkHandler>() {
         }
     }
 
-    private val logger by taggedLogger(WenuLinkHandler::class.java.simpleName)
+    val logger by taggedLogger(WenuLinkHandler::class.java.simpleName)
     private var monitorJob: Job? = null
     private var controlAuthority = ControlAuthority(ControlAuthorityType.NONE)
     var startTimestamp: Long = -1

app/src/main/java/org/WenuLink/adapters/aircraft/AircraftHandler.kt

@@ -32,14 +32,24 @@ class AircraftHandler : CommandHandler<AircraftHandler>() {
         private set
     val telemetry = TelemetryHandler.getInstance()
     val currentTelemetry: TelemetryData? get() = telemetry.getData()
-    val currentCoordinates: Coordinates3D?
+    val globalCoordinates: Coordinates3D?
         get() {
-            val data = telemetry.getData() ?: return null
+            val data = currentTelemetry ?: return null
             val lat = data.latitude ?: return null
             val lon = data.longitude ?: return null
-            val rel = data.relativeAltitude ?: return null
+            val rel = data.altitude ?: return null
+            logger.i { "globalCoordinates($lat, $lon, $rel)" }
             return Coordinates3D(lat, lon, rel)
         }
+    val localPosition: Coordinates3D?
+        get() {
+            val data = currentTelemetry ?: return null
+            val posX = data.positionX.toDouble()
+            val posY = data.positionY.toDouble()
+            val posZ = data.positionZ
+            logger.i { "localCoordinates($posX, $posY, $posZ)" }
+            return Coordinates3D(posX, posY, posZ)
+        }
     var isPowerOff = true
     val parameters by lazy {
         ParameterRegistry(

app/src/main/java/org/WenuLink/sdk/FCManager.kt

@@ -49,26 +49,30 @@ object FCManager {
             "No FlightController"
         }
 
-    fun state2Telemetry(state: FlightControllerState): TelemetryData = TelemetryData(
-        roll = state.attitude.roll.finiteOr(0.0),
-        pitch = state.attitude.pitch.finiteOr(0.0),
-        yaw = state.attitude.yaw.finiteOr(0.0),
-        latitude = state.aircraftLocation.latitude.finiteOrNull(),
-        longitude = state.aircraftLocation.longitude.finiteOrNull(),
-        positionX = 0f,
-        positionY = 0f,
-        positionZ = 0f,
-        velocityX = state.velocityX.finiteOr(0f),
-        velocityY = state.velocityY.finiteOr(0f),
-        velocityZ = state.velocityZ.finiteOr(0f),
-        flightTime = state.flightTimeInSeconds,
-        takeOffAltitude = state.takeoffLocationAltitude.finiteOrNull(),
-        relativeAltitude = state.aircraftLocation.altitude.finiteOrNull(),
-        isFlying = state.isFlying,
-        motorsOn = state.areMotorsOn(),
-        satelliteCount = state.satelliteCount,
-        gpsFixType = GPSMapper.toMavlinkFixType(state.gpsSignalLevel)
-    )
+    fun state2Telemetry(state: FlightControllerState): TelemetryData {
+        val data = TelemetryData(
+            roll = state.attitude.roll.finiteOr(0.0),
+            pitch = state.attitude.pitch.finiteOr(0.0),
+            yaw = state.attitude.yaw.finiteOr(0.0),
+            latitude = state.aircraftLocation.latitude.finiteOrNull(),
+            longitude = state.aircraftLocation.longitude.finiteOrNull(),
+            positionX = 0f,
+            positionY = 0f,
+            positionZ = 0f,
+            velocityX = state.velocityX.finiteOr(0f),
+            velocityY = state.velocityY.finiteOr(0f),
+            velocityZ = state.velocityZ.finiteOr(0f),
+            flightTime = state.flightTimeInSeconds,
+            takeOffAltitude = state.takeoffLocationAltitude.finiteOrNull(),
+            relativeAltitude = state.aircraftLocation.altitude.finiteOrNull(),
+            isFlying = state.isFlying,
+            motorsOn = state.areMotorsOn(),
+            satelliteCount = state.satelliteCount,
+            gpsFixType = GPSMapper.toMavlinkFixType(state.gpsSignalLevel)
+        )
+        // TODO: update positionX and positionY
+        return data.copy(positionZ = data.relativeAltitude ?: data.positionZ)
+    }
 
     fun registerStateCallback(stateCallback: (FlightControllerState) -> Unit) =
         mInstance?.setStateCallback(stateCallback)