You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
705 lines
16 KiB
705 lines
16 KiB
package main
|
|
|
|
import (
|
|
"bytes"
|
|
"io"
|
|
"log"
|
|
"math"
|
|
"net/http"
|
|
"sort"
|
|
"strconv"
|
|
"time"
|
|
)
|
|
|
|
const (
|
|
missionTick = 200 * time.Millisecond
|
|
missionTimeScale = 10.0 // mock 加速倍数，便于在短时间内跑完航线
|
|
defaultCruiseSpeed = 8.0 // m/s
|
|
minLegDuration = 2 * time.Second
|
|
maxLegDuration = 12 * time.Second
|
|
maxHoldSec = 8
|
|
defaultTakeoffAlt = 50.0
|
|
)
|
|
|
|
// missionWaypoint 与后台 workflow.start_task params.waypoints 字段对齐
|
|
type missionWaypoint struct {
|
|
Seq int
|
|
Longitude float64
|
|
Latitude float64
|
|
Altitude float64
|
|
Speed float64
|
|
Yaw float64
|
|
HoldSec int
|
|
}
|
|
|
|
func (d *MockDock) beginMission(commandID string) bool {
|
|
d.mu.Lock()
|
|
defer d.mu.Unlock()
|
|
if d.inMission {
|
|
return false
|
|
}
|
|
d.inMission = true
|
|
d.missionPaused = false
|
|
d.missionCmdID = commandID
|
|
d.missionCancel = make(chan struct{})
|
|
return true
|
|
}
|
|
|
|
func (d *MockDock) endMission() {
|
|
d.mu.Lock()
|
|
defer d.mu.Unlock()
|
|
d.inMission = false
|
|
d.missionPaused = false
|
|
d.missionCmdID = ""
|
|
d.missionCancel = nil
|
|
}
|
|
|
|
func (d *MockDock) isRepeatCommand(commandID string) bool {
|
|
d.mu.Lock()
|
|
defer d.mu.Unlock()
|
|
return commandID != "" && d.inMission && d.missionCmdID == commandID
|
|
}
|
|
|
|
// cancelMission 通知当前任务中止，返回是否有正在执行的任务
|
|
func (d *MockDock) cancelMission() bool {
|
|
d.mu.Lock()
|
|
defer d.mu.Unlock()
|
|
if !d.inMission || d.missionCancel == nil {
|
|
return false
|
|
}
|
|
select {
|
|
case <-d.missionCancel:
|
|
default:
|
|
close(d.missionCancel)
|
|
}
|
|
d.missionPaused = false
|
|
return true
|
|
}
|
|
|
|
func (d *MockDock) setMissionPaused(paused bool) {
|
|
d.mu.Lock()
|
|
defer d.mu.Unlock()
|
|
if d.inMission {
|
|
d.missionPaused = paused
|
|
}
|
|
}
|
|
|
|
func (d *MockDock) currentCancel() <-chan struct{} {
|
|
d.mu.Lock()
|
|
defer d.mu.Unlock()
|
|
return d.missionCancel
|
|
}
|
|
|
|
func (d *MockDock) currentLLA() (lat, lon, alt float64) {
|
|
d.mu.Lock()
|
|
defer d.mu.Unlock()
|
|
return d.tele.latitude, d.tele.longitude, d.tele.altitude
|
|
}
|
|
|
|
func (d *MockDock) setPose(lat, lon, alt, speed, heading float64) {
|
|
d.mu.Lock()
|
|
d.tele.latitude = lat
|
|
d.tele.longitude = lon
|
|
d.tele.altitude = alt
|
|
d.tele.groundSpeed = speed
|
|
d.tele.heading = heading
|
|
d.tele.yaw = heading
|
|
if d.tele.batteryPct > 20 && speed > 0.5 {
|
|
d.tele.batteryPctDrain++
|
|
if d.tele.batteryPctDrain >= 20 {
|
|
d.tele.batteryPctDrain = 0
|
|
d.tele.batteryPct--
|
|
d.tele.batteryV = 22.0 + float64(d.tele.batteryPct)*0.037
|
|
}
|
|
}
|
|
d.mu.Unlock()
|
|
}
|
|
|
|
func (d *MockDock) setArmedFlying(alt float64) {
|
|
d.mu.Lock()
|
|
d.tele.armed = true
|
|
d.tele.flightMode = "AUTO"
|
|
d.tele.altitude = alt
|
|
d.doorState = "open"
|
|
d.mu.Unlock()
|
|
}
|
|
|
|
func (d *MockDock) snapHomeLanded() {
|
|
d.mu.Lock()
|
|
d.tele.armed = false
|
|
d.tele.flightMode = "STANDBY"
|
|
d.tele.groundSpeed = 0
|
|
d.tele.altitude = 0
|
|
d.tele.latitude = d.spec.DroneLat
|
|
d.tele.longitude = d.spec.DroneLon
|
|
d.tele.heading = 0
|
|
d.tele.yaw = 0
|
|
d.doorState = "closed"
|
|
d.mu.Unlock()
|
|
}
|
|
|
|
// runStartTask 执行 workflow.start_task：按航点飞行并持续改写遥测坐标
|
|
func (d *MockDock) runStartTask(cmd commandMsg) {
|
|
defer d.endMission()
|
|
|
|
taskID, missionID := extractTaskIDs(cmd)
|
|
waypoints := parseWaypoints(cmd.Params)
|
|
recordOriginalVideo := asBool(cmd.Params["recordOriginalVideo"])
|
|
originalVideo := parseOriginalVideo(cmd.Params["originalVideo"])
|
|
d.mu.Lock()
|
|
d.uploadedRoute = waypoints
|
|
d.mu.Unlock()
|
|
|
|
log.Printf("[%s] 开始执行任务 taskId=%s 航点数=%d", d.spec.DockID, taskID, len(waypoints))
|
|
for _, wp := range waypoints {
|
|
log.Printf("[%s] 航点 seq=%d lon=%.6f lat=%.6f alt=%.1f speed=%.1f hold=%ds",
|
|
d.spec.DockID, wp.Seq, wp.Longitude, wp.Latitude, wp.Altitude, wp.Speed, wp.HoldSec)
|
|
}
|
|
|
|
d.setDoor("open")
|
|
d.publishStateDock()
|
|
|
|
prep := []string{
|
|
"preparing_dock_takeoff",
|
|
"waiting_drone_online",
|
|
"checking_drone",
|
|
"route_uploading",
|
|
"route_ready",
|
|
}
|
|
for _, step := range prep {
|
|
d.publishWorkflow(cmd, taskID, missionID, "running", step)
|
|
if !d.sleepOrAbort(400 * time.Millisecond) {
|
|
d.finishWorkflow(cmd, taskID, missionID, "cancelled")
|
|
return
|
|
}
|
|
}
|
|
|
|
takeoffAlt := defaultTakeoffAlt
|
|
if len(waypoints) > 0 && waypoints[0].Altitude > 0 {
|
|
takeoffAlt = waypoints[0].Altitude
|
|
}
|
|
|
|
d.publishWorkflow(cmd, taskID, missionID, "running", "taking_off")
|
|
d.setArmedFlying(0)
|
|
d.publishStateDrone()
|
|
if !d.climbTo(takeoffAlt) {
|
|
d.returnHomeAndLand(true)
|
|
d.finishWorkflow(cmd, taskID, missionID, "cancelled")
|
|
return
|
|
}
|
|
d.publishStateDrone()
|
|
|
|
d.publishWorkflow(cmd, taskID, missionID, "running", "finishing_takeoff")
|
|
if !d.sleepOrAbort(400 * time.Millisecond) {
|
|
d.returnHomeAndLand(true)
|
|
d.finishWorkflow(cmd, taskID, missionID, "cancelled")
|
|
return
|
|
}
|
|
|
|
d.publishWorkflow(cmd, taskID, missionID, "running", "starting_mission")
|
|
if !d.sleepOrAbort(300 * time.Millisecond) {
|
|
d.returnHomeAndLand(true)
|
|
d.finishWorkflow(cmd, taskID, missionID, "cancelled")
|
|
return
|
|
}
|
|
|
|
d.publishWorkflow(cmd, taskID, missionID, "running", "mission_running")
|
|
if !d.flyWaypoints(waypoints) {
|
|
d.publishWorkflow(cmd, taskID, missionID, "running", "returning_to_home")
|
|
d.returnHomeAndLand(true)
|
|
d.finishWorkflow(cmd, taskID, missionID, "cancelled")
|
|
return
|
|
}
|
|
|
|
d.publishWorkflow(cmd, taskID, missionID, "running", "returning_to_home")
|
|
if !d.returnHomeAndLand(false) {
|
|
d.returnHomeAndLand(true)
|
|
d.finishWorkflow(cmd, taskID, missionID, "cancelled")
|
|
return
|
|
}
|
|
|
|
d.finishWorkflow(cmd, taskID, missionID, "succeeded")
|
|
if recordOriginalVideo {
|
|
d.uploadOriginalVideo(cmd, taskID, originalVideo)
|
|
}
|
|
log.Printf("[%s] 任务执行完成 taskId=%s", d.spec.DockID, taskID)
|
|
}
|
|
|
|
func (d *MockDock) runOneKeyTakeoff(cmd commandMsg) {
|
|
defer d.endMission()
|
|
taskID, missionID := extractTaskIDs(cmd)
|
|
d.setDoor("open")
|
|
d.publishStateDock()
|
|
d.publishWorkflow(cmd, taskID, missionID, "running", "taking_off")
|
|
d.setArmedFlying(0)
|
|
if !d.climbTo(defaultTakeoffAlt) {
|
|
d.finishWorkflow(cmd, taskID, missionID, "cancelled")
|
|
return
|
|
}
|
|
d.publishStateDrone()
|
|
d.publishWorkflow(cmd, taskID, missionID, "succeeded", "idle")
|
|
}
|
|
|
|
func (d *MockDock) runGenericWorkflow(cmd commandMsg) {
|
|
defer d.endMission()
|
|
taskID, missionID := extractTaskIDs(cmd)
|
|
|
|
d.setDoor("open")
|
|
d.publishStateDock()
|
|
|
|
steps := []string{
|
|
"preparing_dock_takeoff",
|
|
"waiting_drone_online",
|
|
"checking_drone",
|
|
"taking_off",
|
|
"finishing_takeoff",
|
|
"returning_to_home",
|
|
"preparing_dock_landing",
|
|
"returning",
|
|
"finishing_landing",
|
|
}
|
|
for _, step := range steps {
|
|
d.publishWorkflow(cmd, taskID, missionID, "running", step)
|
|
if step == "taking_off" {
|
|
d.setArmedFlying(0)
|
|
if !d.climbTo(defaultTakeoffAlt) {
|
|
d.returnHomeAndLand(true)
|
|
d.finishWorkflow(cmd, taskID, missionID, "cancelled")
|
|
return
|
|
}
|
|
}
|
|
if !d.sleepOrAbort(time.Second) {
|
|
d.returnHomeAndLand(true)
|
|
d.finishWorkflow(cmd, taskID, missionID, "cancelled")
|
|
return
|
|
}
|
|
}
|
|
|
|
d.returnHomeAndLand(true)
|
|
d.finishWorkflow(cmd, taskID, missionID, "succeeded")
|
|
}
|
|
|
|
func (d *MockDock) flyUploadedMission(waypoints []missionWaypoint) {
|
|
defer d.endMission()
|
|
log.Printf("[%s] 执行已上传航线，航点数=%d", d.spec.DockID, len(waypoints))
|
|
d.setArmedFlying(math.Max(d.currentAlt(), defaultTakeoffAlt))
|
|
if !d.flyWaypoints(waypoints) {
|
|
d.returnHomeAndLand(true)
|
|
return
|
|
}
|
|
d.returnHomeAndLand(true)
|
|
}
|
|
|
|
func (d *MockDock) flyWaypoints(waypoints []missionWaypoint) bool {
|
|
for i, wp := range waypoints {
|
|
alt := wp.Altitude
|
|
if alt <= 0 {
|
|
_, _, alt = d.currentLLA()
|
|
if alt <= 0 {
|
|
alt = defaultTakeoffAlt
|
|
}
|
|
}
|
|
speed := wp.Speed
|
|
if speed <= 0 {
|
|
speed = defaultCruiseSpeed
|
|
}
|
|
log.Printf("[%s] 飞向航点 %d/%d (seq=%d)", d.spec.DockID, i+1, len(waypoints), wp.Seq)
|
|
if !d.flyTo(wp.Latitude, wp.Longitude, alt, speed, false) {
|
|
return false
|
|
}
|
|
if wp.HoldSec > 0 {
|
|
if !d.holdAt(wp.HoldSec, wp.Yaw) {
|
|
return false
|
|
}
|
|
} else if wp.Yaw != 0 {
|
|
d.mu.Lock()
|
|
d.tele.heading = wp.Yaw
|
|
d.tele.yaw = wp.Yaw
|
|
d.mu.Unlock()
|
|
}
|
|
d.publishStateDrone()
|
|
}
|
|
return true
|
|
}
|
|
|
|
func (d *MockDock) climbTo(alt float64) bool {
|
|
lat, lon, cur := d.currentLLA()
|
|
if cur < 1 {
|
|
d.setPose(lat, lon, 1, 0, d.currentHeading())
|
|
}
|
|
return d.flyTo(lat, lon, alt, 5, false)
|
|
}
|
|
|
|
func (d *MockDock) currentAlt() float64 {
|
|
_, _, alt := d.currentLLA()
|
|
return alt
|
|
}
|
|
|
|
func (d *MockDock) currentHeading() float64 {
|
|
d.mu.Lock()
|
|
defer d.mu.Unlock()
|
|
return d.tele.heading
|
|
}
|
|
|
|
func (d *MockDock) returnHomeAndLand(forced bool) bool {
|
|
homeAlt := math.Max(d.currentAlt(), 20)
|
|
if !d.flyTo(d.spec.DroneLat, d.spec.DroneLon, homeAlt, defaultCruiseSpeed, forced) && !forced {
|
|
return false
|
|
}
|
|
if !d.flyTo(d.spec.DroneLat, d.spec.DroneLon, 0, 3, forced) && !forced {
|
|
return false
|
|
}
|
|
d.snapHomeLanded()
|
|
d.publishStateDrone()
|
|
d.publishStateDock()
|
|
return true
|
|
}
|
|
|
|
func (d *MockDock) rtlAndLand() {
|
|
if !d.beginMission("rtl") {
|
|
return
|
|
}
|
|
defer d.endMission()
|
|
d.returnHomeAndLand(true)
|
|
}
|
|
|
|
func (d *MockDock) finishWorkflow(cmd commandMsg, taskID, missionID, state string) {
|
|
if state == "cancelled" {
|
|
d.setDoor("closed")
|
|
d.publishStateDock()
|
|
}
|
|
d.publishWorkflow(cmd, taskID, missionID, state, "idle")
|
|
}
|
|
|
|
// flyTo 按球面插值飞向目标点；forced 时忽略暂停/取消（返航落地用）
|
|
func (d *MockDock) uploadOriginalVideo(cmd commandMsg, taskID string, video originalVideoUpload) {
|
|
if video.VideoID == 0 || video.ExecutionID == 0 || video.UploadURL == "" {
|
|
d.publishOriginalVideo(cmd, taskID, video, "failed", "UPLOAD_CONFIG_INVALID", 0)
|
|
return
|
|
}
|
|
if video.UploadExpireAt > 0 && time.Now().UnixMilli() >= video.UploadExpireAt {
|
|
d.publishOriginalVideo(cmd, taskID, video, "failed", "UPLOAD_URL_EXPIRED", 0)
|
|
return
|
|
}
|
|
|
|
body := []byte("mock original video fixture\n")
|
|
req, err := http.NewRequest(http.MethodPut, video.UploadURL, bytes.NewReader(body))
|
|
if err != nil {
|
|
d.publishOriginalVideo(cmd, taskID, video, "failed", "OSS_PUT_FAILED", 0)
|
|
return
|
|
}
|
|
req.Header.Set("Content-Type", "video/mp4")
|
|
resp, err := (&http.Client{Timeout: 20 * time.Second}).Do(req)
|
|
if err != nil {
|
|
d.publishOriginalVideo(cmd, taskID, video, "failed", "OSS_PUT_FAILED", 0)
|
|
return
|
|
}
|
|
_, _ = io.Copy(io.Discard, resp.Body)
|
|
resp.Body.Close()
|
|
if resp.StatusCode < http.StatusOK || resp.StatusCode >= http.StatusMultipleChoices {
|
|
d.publishOriginalVideo(cmd, taskID, video, "failed", "OSS_PUT_FAILED", 0)
|
|
return
|
|
}
|
|
d.publishOriginalVideo(cmd, taskID, video, "completed", "", int64(len(body)))
|
|
}
|
|
|
|
func (d *MockDock) publishOriginalVideo(cmd commandMsg, taskID string, video originalVideoUpload, eventType, errorCode string, fileSize int64) {
|
|
d.publishDrone(d.topic("internal/original-video"), 1, false, d.spec.DroneSN, map[string]any{
|
|
"extension": "laic.mock.original-video.v1",
|
|
"eventId": "mock-original-video-" + strconv.FormatInt(video.VideoID, 10),
|
|
"eventType": eventType,
|
|
"videoId": video.VideoID,
|
|
"executionId": video.ExecutionID,
|
|
"taskId": taskID,
|
|
"commandId": cmd.CommandID,
|
|
"fileSize": fileSize,
|
|
"duration": 1,
|
|
"errorCode": nilIfEmpty(errorCode),
|
|
"uploadedAt": time.Now().UnixMilli(),
|
|
})
|
|
}
|
|
|
|
type originalVideoUpload struct {
|
|
VideoID int64
|
|
ExecutionID int64
|
|
UploadURL string
|
|
UploadExpireAt int64
|
|
}
|
|
|
|
func parseOriginalVideo(value any) originalVideoUpload {
|
|
data, ok := value.(map[string]any)
|
|
if !ok {
|
|
return originalVideoUpload{}
|
|
}
|
|
return originalVideoUpload{
|
|
VideoID: asInt64(data["videoId"]),
|
|
ExecutionID: asInt64(data["executionId"]),
|
|
UploadURL: asString(data["uploadUrl"]),
|
|
UploadExpireAt: asInt64(data["uploadExpireAt"]),
|
|
}
|
|
}
|
|
|
|
func asString(value any) string {
|
|
text, _ := value.(string)
|
|
return text
|
|
}
|
|
|
|
func asBool(value any) bool {
|
|
valueBool, _ := value.(bool)
|
|
return valueBool
|
|
}
|
|
|
|
func asInt64(value any) int64 {
|
|
switch v := value.(type) {
|
|
case float64:
|
|
return int64(v)
|
|
case string:
|
|
parsed, _ := strconv.ParseInt(v, 10, 64)
|
|
return parsed
|
|
default:
|
|
return 0
|
|
}
|
|
}
|
|
|
|
func (d *MockDock) flyTo(lat, lon, alt, speed float64, forced bool) bool {
|
|
if speed <= 0 {
|
|
speed = defaultCruiseSpeed
|
|
}
|
|
fromLat, fromLon, fromAlt := d.currentLLA()
|
|
dist := haversineM(fromLat, fromLon, lat, lon)
|
|
dAlt := math.Abs(alt - fromAlt)
|
|
heading := bearingDeg(fromLat, fromLon, lat, lon)
|
|
if dist < 0.5 {
|
|
heading = d.currentHeading()
|
|
}
|
|
|
|
horizSec := dist / speed / missionTimeScale
|
|
vertSec := dAlt / 5.0 / missionTimeScale
|
|
sec := math.Max(horizSec, vertSec)
|
|
dur := time.Duration(sec * float64(time.Second))
|
|
if (dist > 1 || dAlt > 1) && dur < minLegDuration {
|
|
dur = minLegDuration
|
|
}
|
|
if dur > maxLegDuration {
|
|
dur = maxLegDuration
|
|
}
|
|
if dur < missionTick {
|
|
dur = missionTick
|
|
}
|
|
|
|
steps := int(dur / missionTick)
|
|
if steps < 1 {
|
|
steps = 1
|
|
}
|
|
for i := 1; i <= steps; i++ {
|
|
if !forced && !d.waitMissionTick() {
|
|
return false
|
|
}
|
|
if forced {
|
|
time.Sleep(missionTick)
|
|
}
|
|
t := float64(i) / float64(steps)
|
|
d.setPose(
|
|
lerp(fromLat, lat, t),
|
|
lerp(fromLon, lon, t),
|
|
lerp(fromAlt, alt, t),
|
|
speed,
|
|
heading,
|
|
)
|
|
}
|
|
d.setPose(lat, lon, alt, 0, heading)
|
|
return true
|
|
}
|
|
|
|
func (d *MockDock) holdAt(seconds int, yaw float64) bool {
|
|
if seconds > maxHoldSec {
|
|
seconds = maxHoldSec
|
|
}
|
|
if yaw != 0 {
|
|
d.mu.Lock()
|
|
d.tele.heading = yaw
|
|
d.tele.yaw = yaw
|
|
d.tele.groundSpeed = 0
|
|
d.mu.Unlock()
|
|
} else {
|
|
d.setPoseKeepLLA(0)
|
|
}
|
|
ticks := seconds * int(time.Second/missionTick)
|
|
for i := 0; i < ticks; i++ {
|
|
if !d.waitMissionTick() {
|
|
return false
|
|
}
|
|
}
|
|
return true
|
|
}
|
|
|
|
func (d *MockDock) setPoseKeepLLA(speed float64) {
|
|
d.mu.Lock()
|
|
d.tele.groundSpeed = speed
|
|
d.mu.Unlock()
|
|
}
|
|
|
|
func (d *MockDock) waitMissionTick() bool {
|
|
cancel := d.currentCancel()
|
|
for {
|
|
if isClosed(cancel) {
|
|
return false
|
|
}
|
|
d.mu.Lock()
|
|
paused := d.missionPaused
|
|
d.mu.Unlock()
|
|
if !paused {
|
|
if cancel == nil {
|
|
time.Sleep(missionTick)
|
|
return true
|
|
}
|
|
select {
|
|
case <-cancel:
|
|
return false
|
|
case <-time.After(missionTick):
|
|
return true
|
|
}
|
|
}
|
|
if cancel == nil {
|
|
time.Sleep(100 * time.Millisecond)
|
|
continue
|
|
}
|
|
select {
|
|
case <-cancel:
|
|
return false
|
|
case <-time.After(100 * time.Millisecond):
|
|
}
|
|
}
|
|
}
|
|
|
|
func (d *MockDock) sleepOrAbort(wait time.Duration) bool {
|
|
cancel := d.currentCancel()
|
|
if cancel == nil {
|
|
time.Sleep(wait)
|
|
return true
|
|
}
|
|
t := time.NewTimer(wait)
|
|
defer t.Stop()
|
|
select {
|
|
case <-cancel:
|
|
return false
|
|
case <-t.C:
|
|
return true
|
|
}
|
|
}
|
|
|
|
func extractTaskIDs(cmd commandMsg) (taskID, missionID string) {
|
|
if cmd.Params == nil {
|
|
return "", ""
|
|
}
|
|
if v, ok := cmd.Params["taskId"].(string); ok {
|
|
taskID = v
|
|
}
|
|
if v, ok := cmd.Params["missionId"].(string); ok {
|
|
missionID = v
|
|
}
|
|
return taskID, missionID
|
|
}
|
|
|
|
func parseWaypoints(params map[string]any) []missionWaypoint {
|
|
if params == nil {
|
|
return nil
|
|
}
|
|
raw, ok := params["waypoints"]
|
|
if !ok || raw == nil {
|
|
return nil
|
|
}
|
|
arr, ok := raw.([]any)
|
|
if !ok {
|
|
return nil
|
|
}
|
|
out := make([]missionWaypoint, 0, len(arr))
|
|
for i, item := range arr {
|
|
m, ok := item.(map[string]any)
|
|
if !ok {
|
|
continue
|
|
}
|
|
wp := missionWaypoint{
|
|
Seq: asInt(m["seq"]),
|
|
Longitude: asFloat(m["longitude"]),
|
|
Latitude: asFloat(m["latitude"]),
|
|
Altitude: asFloat(m["altitude"]),
|
|
Speed: asFloat(m["speed"]),
|
|
Yaw: asFloat(m["yaw"]),
|
|
HoldSec: asInt(m["holdSec"]),
|
|
}
|
|
if wp.Seq == 0 {
|
|
wp.Seq = i
|
|
}
|
|
if wp.Longitude == 0 && wp.Latitude == 0 {
|
|
continue
|
|
}
|
|
out = append(out, wp)
|
|
}
|
|
sort.SliceStable(out, func(i, j int) bool { return out[i].Seq < out[j].Seq })
|
|
return out
|
|
}
|
|
|
|
func asFloat(v any) float64 {
|
|
switch n := v.(type) {
|
|
case float64:
|
|
return n
|
|
case float32:
|
|
return float64(n)
|
|
case int:
|
|
return float64(n)
|
|
case int32:
|
|
return float64(n)
|
|
case int64:
|
|
return float64(n)
|
|
case uint64:
|
|
return float64(n)
|
|
default:
|
|
return 0
|
|
}
|
|
}
|
|
|
|
func asInt(v any) int {
|
|
return int(asFloat(v))
|
|
}
|
|
|
|
func isClosed(ch <-chan struct{}) bool {
|
|
if ch == nil {
|
|
return false
|
|
}
|
|
select {
|
|
case <-ch:
|
|
return true
|
|
default:
|
|
return false
|
|
}
|
|
}
|
|
|
|
func lerp(a, b, t float64) float64 {
|
|
return a + (b-a)*t
|
|
}
|
|
|
|
func haversineM(lat1, lon1, lat2, lon2 float64) float64 {
|
|
const earthRadiusM = 6371000.0
|
|
phi1 := lat1 * math.Pi / 180
|
|
phi2 := lat2 * math.Pi / 180
|
|
dPhi := (lat2 - lat1) * math.Pi / 180
|
|
dLambda := (lon2 - lon1) * math.Pi / 180
|
|
a := math.Sin(dPhi/2)*math.Sin(dPhi/2) +
|
|
math.Cos(phi1)*math.Cos(phi2)*math.Sin(dLambda/2)*math.Sin(dLambda/2)
|
|
c := 2 * math.Atan2(math.Sqrt(a), math.Sqrt(1-a))
|
|
return earthRadiusM * c
|
|
}
|
|
|
|
func bearingDeg(lat1, lon1, lat2, lon2 float64) float64 {
|
|
phi1 := lat1 * math.Pi / 180
|
|
phi2 := lat2 * math.Pi / 180
|
|
dLambda := (lon2 - lon1) * math.Pi / 180
|
|
y := math.Sin(dLambda) * math.Cos(phi2)
|
|
x := math.Cos(phi1)*math.Sin(phi2) - math.Sin(phi1)*math.Cos(phi2)*math.Cos(dLambda)
|
|
deg := math.Atan2(y, x) * 180 / math.Pi
|
|
if deg < 0 {
|
|
deg += 360
|
|
}
|
|
return deg
|
|
}
|
|
|