package org.sxkj.common.geo.utils;
|
|
import lombok.Data;
|
|
import java.util.HashMap;
|
import java.util.Map;
|
|
/**
|
* 云台角度计算
|
*/
|
public class GimbalAngles {
|
|
/**
|
* 根据云台角度计算当前 地图上的角度 0-360
|
*
|
* @param options
|
* @return
|
*/
|
public static double yawAngle(GimbalOptions options) {
|
double gimbalYaw = options != null && options.getGimbalYaw() != null ? options.getGimbalYaw() : 0;
|
double attitudeHead = options != null && options.getAttitudeHead() != null ? options.getAttitudeHead() : 0;
|
|
double yaw;
|
if (gimbalYaw > 180) {
|
yaw = gimbalYaw;
|
} else {
|
yaw = gimbalYaw - attitudeHead;
|
}
|
|
double result = 0;
|
if (yaw < 0) {
|
result = yaw + 360;
|
} else if (yaw > 0) {
|
result = yaw;
|
}
|
|
if ((yaw > -2 && yaw < 2) || (int) attitudeHead == (int) gimbalYaw) {
|
result = attitudeHead < 0 ? 180 + (180 + attitudeHead) : attitudeHead;
|
}
|
|
return result ;
|
}
|
|
|
|
|
// Pitch angle calculation
|
private static double pitchAngle(GimbalOptions options) {
|
double gimbalPitch = options != null && options.getGimbalPitch() != null ? options.getGimbalPitch() : 0;
|
return gimbalPitch;
|
}
|
|
// Example options class
|
@Data
|
public static class GimbalOptions {
|
private Double gimbalYaw;
|
private Double attitudeHead;
|
private Double gimbalPitch;
|
|
}
|
|
/**
|
* 无人机转向 需要加多少度减多少度
|
*/
|
public static final Map<String, Integer> DRONE_DIRECTION_ANGLES = new HashMap<>();
|
|
static {
|
DRONE_DIRECTION_ANGLES.put("左", -90);
|
DRONE_DIRECTION_ANGLES.put("右", 90);
|
DRONE_DIRECTION_ANGLES.put("前", 0);
|
DRONE_DIRECTION_ANGLES.put("后", 180);
|
|
}
|
|
}
|
