diff --git a/modules/mogo-module-common/src/main/java/com/mogo/module/common/entity/CloudLocationInfo.java b/modules/mogo-module-common/src/main/java/com/mogo/module/common/entity/CloudLocationInfo.java index e6ea7dcfdd..02f0b43109 100644 --- a/modules/mogo-module-common/src/main/java/com/mogo/module/common/entity/CloudLocationInfo.java +++ b/modules/mogo-module-common/src/main/java/com/mogo/module/common/entity/CloudLocationInfo.java @@ -3,6 +3,8 @@ package com.mogo.module.common.entity; import android.os.Parcel; import android.os.Parcelable; +import java.util.Objects; + /** * 云端定位信息和自车定位信息 * @@ -17,7 +19,17 @@ public class CloudLocationInfo implements Parcelable { private double alt; private double speed; - public CloudLocationInfo(){ + public CloudLocationInfo() { + } + + public CloudLocationInfo(CloudLocationInfo info) { + this.lat = info.getLat(); + this.lon = info.getLon(); + this.heading = info.getHeading(); + this.systemTime = System.currentTimeMillis(); + this.satelliteTime = System.currentTimeMillis(); + this.alt = info.alt; + this.speed = info.speed; } protected CloudLocationInfo(Parcel in) { @@ -126,4 +138,27 @@ public class CloudLocationInfo implements Parcelable { ", speed=" + speed + '}'; } + + public String print() { + return "CloudLocation{ lon: " + lon + " lat: " + lat + " heading: " + heading + " speed: " + + speed+"}"; + } + + @Override + public boolean equals(Object o) { + if (this == o) { + return true; + } + if (o == null || getClass() != o.getClass()) { + return false; + } + CloudLocationInfo that = (CloudLocationInfo) o; + return Double.compare(that.lat, lat) == 0 && + Double.compare(that.lon, lon) == 0; + } + + @Override + public int hashCode() { + return Objects.hash(lat, lon); + } } diff --git a/modules/mogo-module-service/src/main/java/com/mogo/module/service/MogoServices.java b/modules/mogo-module-service/src/main/java/com/mogo/module/service/MogoServices.java index 3235cd0916..69a9d792ff 100644 --- a/modules/mogo-module-service/src/main/java/com/mogo/module/service/MogoServices.java +++ b/modules/mogo-module-service/src/main/java/com/mogo/module/service/MogoServices.java @@ -55,6 +55,8 @@ import com.mogo.module.service.refresh.AutoRefreshStrategy; import com.mogo.module.service.refresh.CustomRefreshStrategy; import com.mogo.module.service.refresh.RefreshObject; import com.mogo.module.service.strategy.CarIconDisplayStrategy; +import com.mogo.module.service.utils.LocationParseUtil; +import com.mogo.module.service.utils.SimpleLocationCorrectStrategy; import com.mogo.module.service.vrmode.VrModeController; import com.mogo.module.service.websocket.LocationResult; import com.mogo.module.service.websocket.OnePerSecondSendContent; @@ -465,22 +467,29 @@ public class MogoServices implements IMogoMapListener, } private void startSendCarLocationAndAdasRecognizedResult2Server(List cloudLocationInfo) { - Location lastCarLocation = mLastCarLocation; +// Location lastCarLocation = mLastCarLocation; + CloudLocationInfo lastInfo = LocationParseUtil.locationToCloudLocation(mLastCarLocation); + // 如果数组内容不为空，就用数组最后一个值 + if (cloudLocationInfo != null && !cloudLocationInfo.isEmpty()) { + lastInfo = cloudLocationInfo.get(cloudLocationInfo.size() - 1); + } LocationResult locationResult = null; - if ( lastCarLocation != null ) { + if ( lastInfo != null ) { + // 定位点预测纠偏 + lastInfo = SimpleLocationCorrectStrategy.getInstance().correct(lastInfo); + if (lastInfo == null) { + return; + } locationResult = new LocationResult(); - locationResult.lastCoordinate = new CloudLocationInfo(); - locationResult.lastCoordinate.setAlt( lastCarLocation.getAltitude() ); - locationResult.lastCoordinate.setHeading( lastCarLocation.getBearing() ); - locationResult.lastCoordinate.setLat( lastCarLocation.getLatitude() ); - locationResult.lastCoordinate.setLon( lastCarLocation.getLongitude() ); - locationResult.lastCoordinate.setSatelliteTime( lastCarLocation.getTime() ); - locationResult.lastCoordinate.setSystemTime( System.currentTimeMillis() ); - locationResult.lastCoordinate.setSpeed( lastCarLocation.getSpeed() ); + locationResult.lastCoordinate = lastInfo; locationResult.coordinates = new ArrayList<>(); locationResult.sn = com.mogo.commons.network.Utils.getSn(); - locationResult.mortonCode = MortonCode.wrapEncodeMorton( locationResult.lastCoordinate.getLon(), locationResult.lastCoordinate.getLat() ); - locationResult.coordinates.addAll( cloudLocationInfo ); + locationResult.mortonCode = MortonCode.wrapEncodeMorton( lastInfo.getLon(), lastInfo.getLat() ); + if(cloudLocationInfo == null){ + locationResult.coordinates.addAll(new ArrayList<>()); + }else { + locationResult.coordinates.addAll(cloudLocationInfo); + } } List< ADASRecognizedResult > recognizedResults = MarkerServiceHandler.getADASController().getLastADASRecognizedResult(); OnePerSecondSendContent content = new OnePerSecondSendContent(); diff --git a/modules/mogo-module-service/src/main/java/com/mogo/module/service/location/MogoRTKLocation.java b/modules/mogo-module-service/src/main/java/com/mogo/module/service/location/MogoRTKLocation.java index 703719a825..bf97d9629c 100644 --- a/modules/mogo-module-service/src/main/java/com/mogo/module/service/location/MogoRTKLocation.java +++ b/modules/mogo-module-service/src/main/java/com/mogo/module/service/location/MogoRTKLocation.java @@ -60,8 +60,7 @@ public class MogoRTKLocation { private void sendLocationData() { if (rtkLocationListener != null) { Logger.d(TAG, "sendLocationData size : " + cacheList.size()); - List list = new ArrayList(); - list.addAll(cacheList); + List list = new ArrayList<>(cacheList); rtkLocationListener.onLocationChanged(list); } if (cacheList != null && cacheList.size() > 0) { diff --git a/modules/mogo-module-service/src/main/java/com/mogo/module/service/utils/LocationParseUtil.java b/modules/mogo-module-service/src/main/java/com/mogo/module/service/utils/LocationParseUtil.java new file mode 100644 index 0000000000..17a20c8115 --- /dev/null +++ b/modules/mogo-module-service/src/main/java/com/mogo/module/service/utils/LocationParseUtil.java @@ -0,0 +1,40 @@ +package com.mogo.module.service.utils; + +import android.location.Location; + +import com.mogo.map.MogoLatLng; +import com.mogo.module.common.entity.CloudLocationInfo; + +/** + * 定位数据类型转换工具 + * + * @author tongchenfei + */ +public class LocationParseUtil { + /** + * 从Location 转 CloudLocationInfo + * @param info 待转数据 + * @return 转后数据 + */ + public static CloudLocationInfo locationToCloudLocation(Location info) { + if (info == null) { + return null; + } + CloudLocationInfo cloud = new CloudLocationInfo(); + cloud.setLat(info.getLatitude()); + cloud.setLon(info.getLongitude()); + cloud.setAlt(info.getAltitude()); + cloud.setHeading(info.getBearing()); + cloud.setSpeed(info.getSpeed()); + cloud.setSatelliteTime(info.getTime()); + cloud.setSystemTime(System.currentTimeMillis()); + return cloud; + } + + public static MogoLatLng cloudLocationToMogoLatLng(CloudLocationInfo info) { + if (info == null) { + return null; + } + return new MogoLatLng(info.getLat(), info.getLon()); + } +} diff --git a/modules/mogo-module-service/src/main/java/com/mogo/module/service/utils/SimpleLocationCorrectStrategy.java b/modules/mogo-module-service/src/main/java/com/mogo/module/service/utils/SimpleLocationCorrectStrategy.java new file mode 100644 index 0000000000..cb2cf47455 --- /dev/null +++ b/modules/mogo-module-service/src/main/java/com/mogo/module/service/utils/SimpleLocationCorrectStrategy.java @@ -0,0 +1,135 @@ +package com.mogo.module.service.utils; + +import android.location.Location; +import android.os.SystemClock; + +import com.mogo.module.common.MogoApisHandler; +import com.mogo.module.common.entity.CloudLocationInfo; +import com.mogo.utils.logger.Logger; + +import java.util.ArrayList; +import java.util.List; + +/** + * 定位预测纠错策略 + * + * @author tongchenfei + */ +public class SimpleLocationCorrectStrategy { + private static final String TAG = "SimpleLocationCorrectStrategy"; + private static final int ERR_COUNT_THRESHOLD = 3; + /** + * 目标距离误差是10米，就是在原目标距离基础上增加10米 + */ + private static final float TARGET_DISTANCE_DEVIATION = 10; + + private CloudLocationInfo lastLocation = null; + private long anchorTime; + private int errCount; + + private static SimpleLocationCorrectStrategy instance = new SimpleLocationCorrectStrategy(); + + public static SimpleLocationCorrectStrategy getInstance(){ + return instance; + } + + private List tmpList = new ArrayList<>(); + + public CloudLocationInfo correct(CloudLocationInfo info) { + Logger.d(TAG, "info: " + info.print()); + Logger.d("tmpList", "tmpList: " + tmpList); + if(isLocationValid(info)) { + tmpList.add(info); + if (lastLocation == null) { + lastLocation = info; + anchorTime = SystemClock.elapsedRealtime(); + Logger.d(TAG, "第一条数据"); + return info; + } + if (lastLocation.equals(info)) { + Logger.d(TAG, "相同坐标点=="); + return info; + } + try { + float targetDistance = + (float) (lastLocation.getSpeed() * (SystemClock.elapsedRealtime() - anchorTime) / 1000) + TARGET_DISTANCE_DEVIATION; + float distance = MogoApisHandler.getInstance().getApis().getMapServiceApi().getMapUIController().calculateLineDistance(LocationParseUtil.cloudLocationToMogoLatLng(lastLocation), LocationParseUtil.cloudLocationToMogoLatLng(info)); + Logger.d(TAG, + "准备计算{ lastInfo: " + lastLocation.print() + " info: " + info.print() + " targetDistance: " + targetDistance + " distance : " + distance + "}"); + if (distance <= targetDistance) { + // 新的定位点在目标距离范围内，认为此数据有效 + lastLocation = info; + anchorTime = SystemClock.elapsedRealtime(); + errCount = 0; + Logger.d(TAG, "在范围内，为有效点"); + return info; + } else { + // 出现异常点 + if (errCount >= ERR_COUNT_THRESHOLD) { + // 出错次数超过阈值，认为本次出错点为正确点 + lastLocation = info; + anchorTime = SystemClock.elapsedRealtime(); + errCount = 0; + Logger.d(TAG, "出错次数超限，异常点变有效点"); + return info; + } else { + // 按照上一个点的方向和速度，计算下一个点的位置，下一个点除坐标点外，其余数据与上一个点相同 +// CloudLocationInfo nextInfo = new CloudLocationInfo(lastLocation); +// nextInfo.setLon(targetDistance * Math.sin(lastLocation.getHeading() * Math.PI / 180)); +// nextInfo.setLat(targetDistance * Math.cos(lastLocation.getHeading() * Math.PI / 180)); +// lastLocation = nextInfo; + anchorTime = SystemClock.elapsedRealtime(); + errCount++; + Logger.d(TAG, "异常点纠偏 info: " + lastLocation); + return lastLocation; +// return nextInfo; + } + } + } catch (Exception e) { + Logger.e(TAG, e, "纠偏异常"); + e.printStackTrace(); + } + }else{ + Logger.d(TAG, "定位点异常"); + if (lastLocation == null) { + return null; + }else{ + try { + float targetDistance = + (float) (lastLocation.getSpeed() * (SystemClock.elapsedRealtime() - anchorTime) / 1000) + TARGET_DISTANCE_DEVIATION; + float distance = MogoApisHandler.getInstance().getApis().getMapServiceApi().getMapUIController().calculateLineDistance(LocationParseUtil.cloudLocationToMogoLatLng(lastLocation), LocationParseUtil.cloudLocationToMogoLatLng(info)); + Logger.d(TAG, + "异常定位点\n准备计算{ lastInfo: " + lastLocation.print() + " info: " + info.print() + " targetDistance: " + targetDistance + " distance : " + distance + "}"); + // 按照上一个点的方向和速度，计算下一个点的位置，下一个点除坐标点外，其余数据与上一个点相同 +// CloudLocationInfo nextInfo = new CloudLocationInfo(lastLocation); +// nextInfo.setLon(targetDistance * Math.sin(lastLocation.getHeading())); +// nextInfo.setLat(targetDistance * Math.cos(lastLocation.getHeading())); +// lastLocation = nextInfo; + anchorTime = SystemClock.elapsedRealtime(); + errCount++; + Logger.d(TAG, "异常点纠偏 info: " + lastLocation); +// return nextInfo; + return lastLocation; + }catch (Exception e){ + Logger.e(TAG, e, "纠偏异常"); + e.printStackTrace(); + } + } + } + return null; + } + + private boolean isLocationValid(CloudLocationInfo info) { + return info.getLat() != 0 && info.getLon() != 0; + } + + private float getDistance(CloudLocationInfo lastInfo, CloudLocationInfo info) { + float[] results=new float[1]; + try{ + Location.distanceBetween(lastInfo.getLat(), lastInfo.getLon(), info.getLat(), info.getLon(), results); + }catch(Exception e){ + e.printStackTrace(); + } + return results[0]*1000; + } +}