yangyakun921/MoGoEagleEye
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

add func

Browse Source
This commit is contained in:
zhongchao
2022-06-08 18:10:35 +08:00
committed by liujing
parent ffc352a605
commit f165359408
17 changed files with 188 additions and 145 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
core/function-impl/mogo-core-function-autopilot/src/main/java/com/mogo/eagle/core/function/autopilot/adapter/MoGoHandAdasMsgManager.java
View File

@@ -106,7 +106,6 @@ public class MoGoHandAdasMsgManager implements
@Override
public void onAutopilotCarConfig(@NotNull MessagePad.CarConfigResp carConfigResp) {
if (carConfigResp != null && !TextUtils.isEmpty(carConfigResp.getMacAddress())) {
Log.d("liyz", "司机端 onAutopilotCarConfig ---" + carConfigResp.getMacAddress());
CallerBindingcarManager.getBindingcarProvider().getBindingcarInfo(carConfigResp.getMacAddress(), MoGoAiCloudClientConfig.getInstance().getSn());
}
}

2
core/function-impl/mogo-core-function-bindingcar/src/main/java/com/mogo/eagle/core/function/bindingcar/BindingcarProvider.java
View File

@@ -96,7 +96,6 @@ public class BindingcarProvider implements IMoGoBindingcarProvider {
long oldHour = SharedPrefsMgr.getInstance(mContext).getLong("typeDriver", 0);
//如果2分钟内频繁调，需要拦截，业务导致的会多次请求工控机信息
if (HmiBuildConfig.isShowSnBindingView) {
Log.d("liyz", "driverScreen -----间隔时间 = " + (currentHour - oldHour) + "-- macAddress = " + macAddress + "--widevineIDWithMd5 = " + widevineIDWithMd5 + "--getScreenType() = " + getScreenType());
if (currentHour - oldHour > 1) {
SharedPrefsMgr.getInstance(mContext).putLong("typeDriver", System.currentTimeMillis() / (1000 * 60));
BindingcarNetWorkManager.getInstance().getBindingcarInfo(mContext, macAddress, widevineIDWithMd5, getScreenType());
@@ -109,7 +108,6 @@ public class BindingcarProvider implements IMoGoBindingcarProvider {
long oldHour = SharedPrefsMgr.getInstance(mContext).getLong("typePassenger", 0);
//如果2分钟内频繁调，需要拦截，业务导致的会多次请求工控机信息
if (HmiBuildConfig.isShowSnBindingView) {
Log.d("liyz", "passengerScreen --间隔时间 = " + (currentHour - oldHour) + "-- mAddress = " + macAddress + "--mWidevineIDWithMd5 = " + widevineIDWithMd5 + "--getScreenType() = " + getScreenType());
if (currentHour - oldHour > 1) {
SharedPrefsMgr.getInstance(mContext).putLong("typePassenger", System.currentTimeMillis() / (1000 * 60));
BindingcarNetWorkManager.getInstance().getBindingcarInfo(mContext, macAddress, widevineIDWithMd5, getScreenType());

5
core/function-impl/mogo-core-function-bindingcar/src/main/java/com/mogo/eagle/core/function/bindingcar/network/BindingcarNetWorkManager.java
View File

@@ -61,7 +61,6 @@ public class BindingcarNetWorkManager {
// String macAddress = "48:b0:2d:3a:bc:78";
// String sn = "X20202203105S688HZ";
Log.d("liyz", "getBindingcarInfo -- widevineIDWithMd5 = " + widevineIDWithMd5 + "--macAddress = " + macAddress + "--screenType = " + screenType);
BindingcarRequest request = new BindingcarRequest(macAddress, widevineIDWithMd5, screenType);
RequestBody requestBody = RequestBody.create(MediaType.get("application/json;charset=UTF-8"), GsonUtil.jsonFromObject(request));
mBindingcarApiService.getBindingcarInfo(token, requestBody)
@@ -76,7 +75,6 @@ public class BindingcarNetWorkManager {
public void onNext(@NonNull BindingcarInfo info) {
if (info != null && info.getData() != null) {
CallerLogger.INSTANCE.d(TAG, "getBindingcarInfo onNext info.getData() =" + info.getData().toString());
Log.d("liyz", "getBindingcarInfo onNext info.getData() =" + info.getData().toString() + "--compare = " + info.getData().getCompare());
if (info.getData().getCompare().equals("0")) {
CallerHmiManager.INSTANCE.showBindingcarDialog();
} else if (info.getData().getCompare().equals("3")) {
@@ -107,7 +105,6 @@ public class BindingcarNetWorkManager {
* mac: 48:b0:2d:3a:9c:19
*/
public void modifyBindingcar(String macAddress, String widevineIDWithMd5, BindingcarCallBack callBack, int screenType) {
Log.d("liyz", "modifyBindingcar --- widevineIDWithMd5 = " + widevineIDWithMd5 + "---macAddress = " + macAddress + "--screenType = " + screenType);
BindingcarRequest request = new BindingcarRequest(macAddress, widevineIDWithMd5, screenType);
RequestBody requestBody = RequestBody.create(MediaType.get("application/json;charset=UTF-8"), GsonUtil.jsonFromObject(request));
mBindingcarApiService.modifyBindingcarInfo(token, requestBody)
@@ -123,13 +120,11 @@ public class BindingcarNetWorkManager {
if (info != null) {
callBack.callBackResult(info);
CallerLogger.INSTANCE.d(TAG, "modifyBindingcar onNext code = " + info.code + "---msg = " + info.msg + "--info.toString() = " + info.toString());
Log.d("liyz", "modifyBindingcar onNext code = " + info.code + "---msg = " + info.msg + "--info.toString() = " + info.toString());
}
}
@Override
public void onError(@NonNull Throwable e) {
Log.e("liyz", "modifyBindingcar onError e = " + e.toString() + "---e.getMessage = " + e.getMessage());
CallerLogger.INSTANCE.e(TAG, "modifyBindingcar onError e = " + e.toString() + "---e.getMessage = " + e.getMessage());
}

2
core/function-impl/mogo-core-function-devatools/src/main/java/com/zhjt/mogo_core_function_devatools/status/flow/can/CanImpl.kt
View File

@@ -23,7 +23,6 @@ internal class CanImpl(ctx: Context) : IFlow<CanStatus>(ctx), IMoGoAutopilotVehi
private var job: Job? = null
override fun onCreate() {
Log.d(TAG, "-- onCreate --")
send(CanStatus(CallerAutoPilotManager.isConnected()))
CallerAutopilotVehicleStateListenerManager.addListener(TAG, this)
CallerAutoPilotStatusListenerManager.addListener(TAG, this)
@@ -86,7 +85,6 @@ internal class CanImpl(ctx: Context) : IFlow<CanStatus>(ctx), IMoGoAutopilotVehi
override fun onDestroy() {
super.onDestroy()
job?.safeCancel()
Log.d(TAG, "-- onDestroy --")
CallerAutopilotVehicleStateListenerManager.removeListener(TAG)
CallerAutoPilotStatusListenerManager.removeListener(TAG)
}

4
core/function-impl/mogo-core-function-devatools/src/main/java/com/zhjt/mogo_core_function_devatools/status/flow/ipc/IpcImpl.kt
View File

@@ -1,8 +1,6 @@
package com.zhjt.mogo_core_function_devatools.status.flow.ipc
import android.content.*
import android.util.*
import com.mogo.eagle.core.data.autopilot.*
import com.mogo.eagle.core.function.api.autopilot.*
import com.mogo.eagle.core.function.call.autopilot.*
import com.zhjt.mogo_core_function_devatools.status.flow.IFlow
@@ -17,7 +15,6 @@ internal class IpcImpl(ctx: Context): IFlow<IpcStatus>(ctx), IMoGoAutopilotStatu
private var state: Int = -1
override fun onCreate() {
Log.d(TAG, "-- onCreate --")
checkAndSend()
CallerAutoPilotStatusListenerManager.addListener(TAG, this)
}
@@ -36,7 +33,6 @@ internal class IpcImpl(ctx: Context): IFlow<IpcStatus>(ctx), IMoGoAutopilotStatu
override fun onDestroy() {
super.onDestroy()
Log.d(TAG, "-- onDestroy --")
CallerAutoPilotStatusListenerManager.removeListener(TAG)
}
}

10
core/function-impl/mogo-core-function-devatools/src/main/java/com/zhjt/mogo_core_function_devatools/status/flow/nets/NetsImpl.kt
View File

@@ -37,47 +37,39 @@ internal class NetsImpl(ctx: Context): IFlow<NetStatus>(ctx) {
override fun onAvailable(network: Network) {
super.onAvailable(network)
Log.d(TAG, "-- onAvailable --:: $network")
checkAndSend()
}
override fun onLosing(network: Network, maxMsToLive: Int) {
super.onLosing(network, maxMsToLive)
Log.d(TAG, "-- onLosing --:: $network::$maxMsToLive")
}
override fun onLost(network: Network) {
super.onLost(network)
Log.d(TAG, "-- onLost --:: $network")
checkAndSend()
}
override fun onUnavailable() {
super.onUnavailable()
Log.d(TAG, "-- onUnavailable --")
checkAndSend()
}
override fun onCapabilitiesChanged(network: Network, networkCapabilities: NetworkCapabilities) {
super.onCapabilitiesChanged(network, networkCapabilities)
Log.d(TAG, "-- onCapabilitiesChanged --:$network::$networkCapabilities")
checkAndSend()
}
override fun onLinkPropertiesChanged(network: Network, linkProperties: LinkProperties) {
super.onLinkPropertiesChanged(network, linkProperties)
Log.d(TAG, "-- onLinkPropertiesChanged --:$network::$linkProperties")
}
override fun onBlockedStatusChanged(network: Network, blocked: Boolean) {
super.onBlockedStatusChanged(network, blocked)
Log.d(TAG, "-- onBlockedStatusChanged --:$network::$blocked")
}
}
override fun onCreate() {
Log.d(TAG, "-- onCreate --")
checkAndSend()
val builder = NetworkRequest.Builder()
.addTransportType(NetworkCapabilities.TRANSPORT_WIFI)
@@ -106,7 +98,6 @@ internal class NetsImpl(ctx: Context): IFlow<NetStatus>(ctx) {
sr = connectionInfo.rxLinkSpeedMbps
}
val speed = Speed(tr, sr)
Log.d(TAG, "checkAndSend----:enable: $enabled :: name: $name")
send(enabled, name, speed)
}
@@ -129,7 +120,6 @@ internal class NetsImpl(ctx: Context): IFlow<NetStatus>(ctx) {
override fun onDestroy() {
super.onDestroy()
Log.d(TAG, "-- onDestroy --")
if (registered.compareAndSet(true, false)) {
connectMgr.unregisterNetworkCallback(cb)
}

4
core/function-impl/mogo-core-function-devatools/src/main/java/com/zhjt/mogo_core_function_devatools/status/flow/trace/TracingImpl.kt
View File

@@ -19,7 +19,6 @@ internal class TracingImpl(ctx: Context): IFlow<TracingStatus>(ctx), IMoGoAutopi
private var old: TracingStatus.Tracing = UNKNOWN
override fun onCreate() {
Log.d(TAG, "-- onCreate --")
val code = CallerAutoPilotStatusListenerManager.getAutoPilotReportMessageCode()
val state = code.toState() ?: UNKNOWN
old = state
@@ -31,7 +30,6 @@ internal class TracingImpl(ctx: Context): IFlow<TracingStatus>(ctx), IMoGoAutopi
override fun onAutopilotGuardian(guardianInfo: MogoReportMessage?) {
super.onAutopilotGuardian(guardianInfo)
val current = guardianInfo?.code
Log.d(TAG, "-- onAutopilotGuardian --: $current")
val newState = current?.toState()
if (newState != null && newState != old) {
send(TracingStatus(newState))
@@ -42,7 +40,6 @@ internal class TracingImpl(ctx: Context): IFlow<TracingStatus>(ctx), IMoGoAutopi
override fun onAutopilotStatusResponse(autoPilotStatusInfo: AutopilotStatusInfo) {
super.onAutopilotStatusResponse(autoPilotStatusInfo)
//Log.d(TAG, "-- onAutopilotStatusResponse -- autopilotMode: ${autoPilotStatusInfo.pilotmode} :: state: ${autoPilotStatusInfo.state}")
if (autoPilotStatusInfo.state == IMoGoAutopilotStatusListener.STATUS_AUTOPILOT_DISABLE) {
send(TracingStatus(UNKNOWN))
}
@@ -53,7 +50,6 @@ internal class TracingImpl(ctx: Context): IFlow<TracingStatus>(ctx), IMoGoAutopi
override fun onDestroy() {
super.onDestroy()
Log.d(TAG, "-- onDestroy --")
CallerAutoPilotStatusListenerManager.removeListener(TAG)
}
}

3
core/function-impl/mogo-core-function-devatools/src/main/java/com/zhjt/mogo_core_function_devatools/status/model/StatusModel.kt
View File

@@ -45,8 +45,6 @@ internal class StatusModel : ViewModel() {
val nv = ArrayList(v).also { it.updateOrInsert(s) }
val data = Pair(getExceptionStatus(nv), nv)
old.set(data)
Log.d(TAG, "status: $s")
Log.d(TAG, "data: ${data.second}")
status.postValue(data)
}
}
@@ -86,7 +84,6 @@ internal class StatusModel : ViewModel() {
is TracingStatus -> {
val c1 = CallerAutoPilotStatusListenerManager.getAutoPilotStatusInfo().state != IMoGoAutopilotStatusListener.STATUS_AUTOPILOT_DISABLE
val c2 = s.state.isException()
Log.d(TAG, "getExceptionStatus-::c1: $c1 -> c2: $c2")
if (c1 && c2) {
s
} else {

2
core/function-impl/mogo-core-function-hmi/src/main/java/com/mogo/eagle/core/function/hmi/notification/anim/DefaultAnimator.kt
View File

@@ -50,9 +50,7 @@ open class DefaultAnimator : OnFloatAnimator {
if (triple.third) params.x = value else params.y = value
// 动画执行过程中页面关闭，出现异常
windowManager.updateViewLayout(view, params)
Log.d("XXX", "update ---> ${it.animatedValue}, ${it.animatedFraction}, $value")
} catch (e: Exception) {
Log.d("XXX", "exception ---> $e")
cancel()
}
}

1
core/function-impl/mogo-core-function-hmi/src/main/java/com/mogo/eagle/core/function/hmi/ui/widget/V2XWarningView.kt
View File

@@ -53,7 +53,6 @@ class V2XWarningView @JvmOverloads constructor(
* @param closeTime 倒计时
*/
fun showWarning(direction: WarningDirectionEnum, closeTime: Long) {
// CallerLogger.d("$M_HMI$TAG", "预警红边：预警方向->$direction 预警倒计时->$closeTime")
Log.d(TAG, "预警红边：预警方向->$direction 预警倒计时->$closeTime")
UiThreadHandler.post {
// 如果传入的不是关闭显示，则设置倒计时，定时关闭红框警示

27
core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/CircleQueue.java
View File

@@ -1,7 +1,6 @@
package com.mogo.eagle.core.function.map;
import android.util.Log;
import java.util.List;
import java.util.Vector;
public class CircleQueue {
@@ -14,8 +13,12 @@ public class CircleQueue {
objQueue = new Vector<>(maxSize);
}
public int size() {
return objQueue.size();
}
public void addQueue(ObjQueue obj) {
if(objQueue.size() == maxSize){
if (objQueue.size() == maxSize) {
objQueue.remove(0);
}
objQueue.add(obj);
@@ -25,9 +28,19 @@ public class CircleQueue {
objQueue.remove(obj);
}
public void show() {
for (ObjQueue queue : objQueue) {
Log.i("CircleQueue","show : " + queue.getType());
}
public List<ObjQueue> getLastThreeFrame() {
return objQueue.subList(objQueue.size() - 3, objQueue.size());
}
public ObjQueue getLastFrame() {
return objQueue.lastElement();
}
@Override
public String toString() {
return "CircleQueue{" +
"objQueue=" + objQueue +
"size=" + objQueue.size() +
'}';
}
}

89
core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/IdentifyDataDrawer.java
View File

@@ -35,15 +35,6 @@ public class IdentifyDataDrawer {
*/
private static final ConcurrentHashMap<String, MessagePad.TrackedObject> mMarkersCaches = new ConcurrentHashMap<>();
/**
* kalman缓存数据
*/
private static final ConcurrentHashMap<String, KalmanFilter> algoCache = new ConcurrentHashMap<>();
/**
* 记录每次实际绘制的交通元素UUID
*/
private final ArrayList<String> trafficDataUuidList = new ArrayList<>();
/**
* 过滤后的数据集合
*/
@@ -75,31 +66,34 @@ public class IdentifyDataDrawer {
*/
@SuppressLint("NewApi")
public void renderAdasRecognizedResult(List<MessagePad.TrackedObject> resultList) {
if (resultList == null || resultList.isEmpty()) {
clearOldMarker();
CallerLogger.INSTANCE.w(TAG, "感知数据为空无需渲染……");
return;
}
// if (resultList == null || resultList.isEmpty()) {
// TrackManager.getInstance().clearAll(mContext);
// CallerLogger.INSTANCE.w(TAG, "感知数据为空无需渲染……");
// return;
// }
//
// if (!MogoApisHandler.getInstance().getApis().getStatusManagerApi().isVrMode()) {
// TrackManager.getInstance().clearAll(mContext);
// CallerLogger.INSTANCE.w(TAG, "渲染 adas 识别的数据 当前不是VR模式");
// return;
// }
if (!MogoApisHandler.getInstance().getApis().getStatusManagerApi().isVrMode()) {
clearOldMarker();
CallerLogger.INSTANCE.w(TAG, "渲染 adas 识别的数据 当前不是VR模式");
return;
}
// //清除缓存
// for (MessagePad.TrackedObject data : resultList) {
// if (trafficDataUuidList.size() > 0 && trafficDataUuidList.contains("" + data.getUuid())) {
// trafficDataUuidList.remove("" + data.getUuid());
// }
// }
// trafficDataUuidList.forEach(uuid -> {
// mMarkersCaches.remove(uuid);
// algoCache.remove(uuid);
// });
//清除缓存
for (MessagePad.TrackedObject data : resultList) {
if (trafficDataUuidList.size() > 0 && trafficDataUuidList.contains("" + data.getUuid())) {
trafficDataUuidList.remove("" + data.getUuid());
}
}
trafficDataUuidList.forEach(uuid -> {
mMarkersCaches.remove(uuid);
algoCache.remove(uuid);
});
ArrayList<MessagePad.TrackedObject> filterList = filterTrafficData(resultList);
TrackManager.getInstance().clearCache(mContext);
// ArrayList<MessagePad.TrackedObject> filterList = filterTrafficData(resultList);
ArrayList<MessagePad.TrackedObject> filterList = TrackManager.getInstance().filterTrafficData(resultList);
Log.i("0608arrow","size : " + filterList.size() + " , filterList : " + filterList.toString());
if (filterList.size() > 0) {
// 绘制新数据
MogoMarkerManager.getInstance(mContext)
@@ -114,7 +108,6 @@ public class IdentifyDataDrawer {
*/
private ArrayList<MessagePad.TrackedObject> filterTrafficData(List<MessagePad.TrackedObject> trafficData) {
mFilterTrafficData.clear();
trafficDataUuidList.clear();
for (MessagePad.TrackedObject data : trafficData) {
// 过滤掉未知感知数据
if (!FunctionBuildConfig.isDrawUnknownIdentifyData && data.getType() == TrafficTypeEnum.TYPE_TRAFFIC_ID_WEI_ZHI.getType()) {
@@ -133,45 +126,15 @@ public class IdentifyDataDrawer {
//更新已存在的感知物体数据
}
mMarkersCaches.put(uuid, data);
trafficDataUuidList.add(uuid);
}
return mFilterTrafficData;
}
//todo 相信滤波的定位点做验证，将原始data修改经纬度和航向角返回
private MessagePad.TrackedObject kalmanCorrectData(MessagePad.TrackedObject data) {
String uuid = "" + data.getUuid();
if (algoCache.containsKey(uuid)) {
Object o = algoCache.get(uuid);
KalmanFilter kf = (KalmanFilter) o;
assert kf != null;
algoCache.put(uuid, kf);
MessagePad.TrackedObject cacheTrackObj = mMarkersCaches.get(uuid);
assert cacheTrackObj != null;
if (data.getSpeed() < 0.5) {
return data.toBuilder().setHeading(cacheTrackObj.getHeading()).setLongitude(cacheTrackObj.getLongitude()).setLatitude(cacheTrackObj.getLatitude()).build();
} else {
return data;
}
} else {
double r = 0.000005;
if (AdasRecognizedType.valueFrom(data.getType()) == AdasRecognizedType.classIdTrafficBus || AdasRecognizedType.valueFrom(data.getType()) == AdasRecognizedType.classIdTrafficTruck) {
r = 0.00001;
}
algoCache.put(uuid, new KalmanFilter(data.getLongitude(), data.getLatitude(), r));
return data;
}
}
/**
* 清除旧的 marker 数据
*/
public void clearOldMarker() {
for (String uuid : trafficDataUuidList) {
MogoMarkerManager.getInstance(mContext)
.removeMarker(uuid);
}
trafficDataUuidList.clear();
TrackManager.getInstance().clearAll(mContext);
}
}

36
core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/TrackManager.java
View File

@@ -1,11 +1,17 @@
package com.mogo.eagle.core.function.map;
import android.content.Context;
import android.util.ArrayMap;
import android.util.Log;
import com.mogo.eagle.core.data.config.FunctionBuildConfig;
import com.mogo.eagle.core.data.enums.TrafficTypeEnum;
import com.mogo.eagle.core.function.call.autopilot.CallerAutoPilotStatusListenerManager;
import com.mogo.map.MogoMarkerManager;
import java.text.DecimalFormat;
import java.util.ArrayList;
import java.util.Iterator;
import java.util.List;
import mogo.telematics.pad.MessagePad;
@@ -20,6 +26,9 @@ public class TrackManager {
return TrackOwner.trackManager;
}
public static final DecimalFormat DF = new DecimalFormat("0.000000");
public static final int DISTANCE = 6371000;
/**
* marker缓存队列
*/
@@ -30,17 +39,18 @@ public class TrackManager {
*/
private final ArrayList<MessagePad.TrackedObject> mFilterTrafficData = new ArrayList<>();
public ArrayList<MessagePad.TrackedObject> filterTrafficData(List<MessagePad.TrackedObject> trafficData) {
//清空上次返回数据，做到缓存复用
mFilterTrafficData.clear();
//进入过滤机制的感知物体，首先从缓存队列中进行查找 uuid
for (MessagePad.TrackedObject data : trafficData) {
// todo 过滤掉未知感知数据，后面会依据危险等级显示
if (!FunctionBuildConfig.isDrawUnknownIdentifyData && data.getType() == TrafficTypeEnum.TYPE_TRAFFIC_ID_WEI_ZHI.getType()) {
//CallerLogger.INSTANCE.w(TAG, "未知感知类型数据，丢弃，不渲染");
continue;
}
String uuid = "" + data.getUuid();
TrackObj trackObj = mMarkersCaches.get(uuid);
if (trackObj != null) {
@@ -49,11 +59,33 @@ public class TrackManager {
} else {
trackObj = new TrackObj(data);
//todo 判断是否有重合元素 google s2
}
mMarkersCaches.put(uuid, trackObj);
}
return mFilterTrafficData;
}
public void clearCache(Context mContext) {
Iterator it = mMarkersCaches.keySet().iterator();
while (it.hasNext()) {
String key = (String) it.next();
TrackObj trackObj = mMarkersCaches.get(key);
if (CallerAutoPilotStatusListenerManager.INSTANCE.getCurWgs84SatelliteTime() - trackObj.getRecentlyTime() > 1000) {
Log.d("track","clearCache uuid : " + key);
mMarkersCaches.remove(key);
MogoMarkerManager.getInstance(mContext)
.removeMarker(key);
}
}
}
public void clearAll(Context mContext) {
Iterator it = mMarkersCaches.keySet().iterator();
while (it.hasNext()) {
String key = (String) it.next();
mMarkersCaches.remove(key);
MogoMarkerManager.getInstance(mContext)
.removeMarker(key);
}
}
}

146
core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/TrackObj.java
View File

@@ -1,8 +1,15 @@
package com.mogo.eagle.core.function.map;
import static com.mogo.eagle.core.function.map.TrackManager.DISTANCE;
import android.util.Log;
import com.mogo.eagle.core.utilcode.geometry.S2CellId;
import com.mogo.eagle.core.utilcode.geometry.S2LatLng;
import java.math.BigDecimal;
import java.util.List;
import mogo.telematics.pad.MessagePad;
public class TrackObj {
@@ -10,56 +17,119 @@ public class TrackObj {
private final CircleQueue circleQueue = new CircleQueue(10);
private final KalmanFilter kalmanFilter; //卡尔曼结果
private S2CellId s2CellId; //s2 id权重
private long recentlyTime;
private S2LatLng s2LatLng; //s2 经纬度
private long recentlyTime; //用于缓存帧数判断，暂定缓存1秒数据，中间如果有物体未出现，1秒后删除
private double headingDelta; //航向角德尔塔
private double speedDelta; //速度德尔塔
private double typeWeight; //类型权重
private final int[] observationType = new int[3]; //类型变化的观测数组
private String uuid;
public TrackObj(MessagePad.TrackedObject data) {
uuid = "" + data.getUuid();
kalmanFilter = new KalmanFilter(data.getLongitude(), data.getLatitude(), 0.0000005);
circleQueue.addQueue(new ObjQueue(data.getHeading(), data.getSpeed(), data.getType()));
recentlyTime = Double.valueOf(data.getSatelliteTime()).longValue();
recentlyTime = BigDecimal.valueOf(data.getSatelliteTime()).longValue();
S2LatLng s2LatLng = S2LatLng.fromDegrees(data.getLatitude(), data.getLongitude());
s2CellId = S2CellId.fromLatLng(s2LatLng).parent(21); //需要验证21前后
s2CellId = S2CellId.fromLatLng(s2LatLng).parent(22); //需要验证22前后
}
private MessagePad.TrackedObject cacheData;
//先处理kalman数据，将经纬度校准后，放入缓存队列，然后基于后序策略将各个项进行校准
public MessagePad.TrackedObject updateObj(MessagePad.TrackedObject data) {
cacheData = data.toBuilder().build();
recentlyTime = BigDecimal.valueOf(data.getSatelliteTime()).longValue();//todo 毫秒没有了
correct();
circleQueue.addQueue(new ObjQueue(cacheData.getHeading(), cacheData.getSpeed(), cacheData.getType()));
return cacheData.toBuilder().build();
}
private void correct() {
calLoc();
calHeading();
calType();
}
private void calLoc() {
//距离计算,位置修正
double[] lonLat = kalmanFilter.filter(cacheData.getLongitude(), cacheData.getLatitude());
if (s2LatLng != null) {
double distance = s2LatLng.getDistance(S2LatLng.fromDegrees(lonLat[0], lonLat[1])).distance(DISTANCE);
// DF.format(distance)
//速度小于0.5m/s,并且距离在15米范围内，则认为是相对静止状态(注意调整阈值)，不更新缓存点信息
if (cacheData.getSpeed() < 0.5 && distance < 15) {
Log.i("track", "uuid : " + uuid + " , 静止物体相对上一帧 distance : " + distance + " lon : " + s2LatLng.latDegrees() + " lat : " + s2LatLng.lngDegrees());
cacheData = cacheData.toBuilder().setLongitude(s2LatLng.latDegrees()).setLatitude(s2LatLng.lngDegrees()).build();
} else {
//不在阈值内则更新，代表物体移动，使用卡尔曼滤波经纬度数据
s2LatLng = S2LatLng.fromDegrees(lonLat[0], lonLat[1]);
s2CellId = S2CellId.fromLatLng(s2LatLng).parent(22);
cacheData = cacheData.toBuilder().setLongitude(lonLat[0]).setLatitude(lonLat[1]).build();
}
} else {
//首次更新
s2LatLng = S2LatLng.fromDegrees(lonLat[0], lonLat[1]);
s2CellId = S2CellId.fromLatLng(s2LatLng).parent(22);
}
}
private void calHeading() {
double speedAverage;
double newDelta;
ObjQueue lastObj;
if (circleQueue.size() >= 3) {
//计算差量
List<ObjQueue> objQueueList = circleQueue.getLastThreeFrame();
lastObj = objQueueList.get(2);
//计算平均速度
speedAverage = (objQueueList.get(0).getSpeed() + objQueueList.get(1).getSpeed() + objQueueList.get(2).getSpeed()) / 3;
double firstDelta = objQueueList.get(1).getHeading() - objQueueList.get(0).getHeading();
double secondDelta = objQueueList.get(2).getHeading() - objQueueList.get(1).getHeading();
newDelta = Math.abs(cacheData.getHeading() - lastObj.getHeading());
//按帧与帧之间的顺序变化
double abs = Math.abs(firstDelta - secondDelta);
//存在180度转向(有一帧出现错误)
if (Math.abs(abs - 180) < 5) {
headingDelta = firstDelta - secondDelta;
} else if (abs < 5) { //两帧之间差量比较均匀
headingDelta = firstDelta - secondDelta;
} else if (Math.abs(abs - 180) > 5 && newDelta < 5) { //前两帧数据中出现异常值，相信后序帧
headingDelta = newDelta;
}
} else {
lastObj = circleQueue.getLastFrame();
speedAverage = lastObj.getSpeed();
newDelta = Math.abs(cacheData.getHeading() - lastObj.getHeading());
headingDelta = newDelta;
}
//更正数据,速度小于0.5使用上一帧数据
if (speedAverage < 0.5) {
cacheData = cacheData.toBuilder().setHeading(circleQueue.getLastFrame().getHeading()).build();
}
//速度大于1.5并出现大幅度转向使用缓存帧和delta数据
if (cacheData.getSpeed() > 1.5 && newDelta > 10 && headingDelta != 0.0) {
cacheData = cacheData.toBuilder().setHeading(lastObj.getHeading() + headingDelta).build();
}
}
private void calType() {
}
public long getRecentlyTime() {
return recentlyTime;
}
public void setRecentlyTime(long recentlyTime) {
this.recentlyTime = recentlyTime;
}
public double getHeadingDelta() {
return headingDelta;
}
public void setHeadingDelta(double headingDelta) {
this.headingDelta = headingDelta;
}
public double getSpeedDelta() {
return speedDelta;
}
public void setSpeedDelta(double speedDelta) {
this.speedDelta = speedDelta;
}
public double getTypeWeight() {
return typeWeight;
}
public void setTypeWeight(double typeWeight) {
this.typeWeight = typeWeight;
}
//先处理kalman数据，将经纬度校准后，放入缓存队列，然后基于后序策略将各个项进行校准
public MessagePad.TrackedObject updateObj(MessagePad.TrackedObject data) {
// assert kf != null;
// double[] lonLat = kf.filter(data.getLongitude(), data.getLatitude());
return data;
@Override
public String toString() {
return "TrackObj{" +
"circleQueue=" + circleQueue +
", s2CellId=" + s2CellId +
", recentlyTime=" + recentlyTime +
", cacheData=" + cacheData +
'}';
}
}

0
core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/log Normal file
View File

0
core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/map/track Normal file
View File