diff --git a/app_ipc_monitoring/src/main/java/com/zhidao/adas/client/ui/MainActivity.java b/app_ipc_monitoring/src/main/java/com/zhidao/adas/client/ui/MainActivity.java index b62e568141..8f52510cf2 100644 --- a/app_ipc_monitoring/src/main/java/com/zhidao/adas/client/ui/MainActivity.java +++ b/app_ipc_monitoring/src/main/java/com/zhidao/adas/client/ui/MainActivity.java @@ -84,6 +84,7 @@ import com.zhidao.support.adas.high.AdasOptions; import com.zhidao.support.adas.high.OnAdasConnectStatusListener; import com.zhidao.support.adas.high.OnAdasListener; import com.zhidao.support.adas.high.OnMultiDeviceListener; +import com.zhidao.support.adas.high.bean.AutopilotAbility; import com.zhidao.support.adas.high.bean.VersionCompatibility; import com.zhidao.support.adas.high.common.ByteUtil; import com.zhidao.support.adas.high.common.Constants.IPC_CONNECTION_STATUS; @@ -110,6 +111,7 @@ import java.util.Locale; import java.util.Set; import java.util.concurrent.ScheduledExecutorService; +import chassis.Chassis; import chassis.VehicleStateOuterClass; import io.netty.channel.Channel; import mogo.telematics.pad.MessagePad; @@ -164,6 +166,8 @@ public class MainActivity extends BaseActivity implements OnAdasListener, OnAdas private ListPopupWindow listPopupWindow; private FloatWindow floatWindow; private View include_title; + private boolean isAutopilotAbility = true; + private String autopilotAbilityReason; // @Override // protected void onStart() { // super.onStart(); @@ -796,6 +800,50 @@ public class MainActivity extends BaseActivity implements OnAdasListener, OnAdas DataDistribution.getInstance().addData(base); } + @Override + public void onAutopilotAbility(AutopilotAbility ability) { + isAutopilotAbility = true; + autopilotAbilityReason = null; + if (ability.gear == null || ability.gear == Chassis.GearPosition.GEAR_P || ability.gear == Chassis.GearPosition.GEAR_R) { + isAutopilotAbility = false; + autopilotAbilityReason = "挡位不正常"; + } + if (isAutopilotAbility) + if (ability.brake > 0) { + isAutopilotAbility = false; + autopilotAbilityReason = "制动踏板被踩下"; + } + if (isAutopilotAbility) + if (ability.statusInfo == null) { + isAutopilotAbility = false; + autopilotAbilityReason = "系统Topic不正常"; + } + if (isAutopilotAbility) + if (ability.statusInfo.getSysState() == SystemStatusInfo.SystemState.SYS_STARTING) { + isAutopilotAbility = false; + autopilotAbilityReason = "系统正在启动"; + } + if (isAutopilotAbility) + if (ability.statusInfo.getSysState() == SystemStatusInfo.SystemState.SYS_EXITING) { + isAutopilotAbility = false; + autopilotAbilityReason = "系统正在关闭"; + } + if (isAutopilotAbility) + if (ability.statusInfo.getSysState() == SystemStatusInfo.SystemState.SYS_FAULT) { + isAutopilotAbility = false; + autopilotAbilityReason = "系统异常"; + } + if (isAutopilotAbility) + for (SystemStatusInfo.HealthInfo healthInfo : ability.statusInfo.getHealthInfoList()) { + if (healthInfo.getState() == SystemStatusInfo.HealthState.FAULT) { + isAutopilotAbility = false; + autopilotAbilityReason = healthInfo.getName() + " Topic异常"; + break; + } + } + Log.i(TAG, "是否可以启动自动驾驶=" + isAutopilotAbility + " 原因=" + autopilotAbilityReason); + } + @Override public void onTrajectory(MessagePad.Header header, MessagePad.Trajectory trajectory) { Trajectory base = new Trajectory(header, trajectory, sdf); diff --git a/core/function-impl/mogo-core-function-autopilot/src/main/java/com/mogo/eagle/core/function/autopilot/adapter/MoGoAdasListenerImpl.kt b/core/function-impl/mogo-core-function-autopilot/src/main/java/com/mogo/eagle/core/function/autopilot/adapter/MoGoAdasListenerImpl.kt index 1a1d52ec09..27bcb9b9e5 100644 --- a/core/function-impl/mogo-core-function-autopilot/src/main/java/com/mogo/eagle/core/function/autopilot/adapter/MoGoAdasListenerImpl.kt +++ b/core/function-impl/mogo-core-function-autopilot/src/main/java/com/mogo/eagle/core/function/autopilot/adapter/MoGoAdasListenerImpl.kt @@ -50,6 +50,7 @@ import com.mogo.eagle.core.function.call.autopilot.CallerAutopilotVehicleStateLi import com.mogo.eagle.core.function.call.map.CallerMapUIServiceManager import com.zhidao.support.adas.high.AdasManager import com.zhidao.support.adas.high.OnAdasListener +import com.zhidao.support.adas.high.bean.AutopilotAbility import com.zhidao.support.adas.high.common.ProtocolStatus import com.zhjt.service.chain.ChainLog import com.zhjt.service.chain.TracingConstants.Endpoint.Companion.PAD @@ -427,4 +428,11 @@ class MoGoAdasListenerImpl : OnAdasListener { override fun onError(status: ProtocolStatus, bytes: ByteArray) { } + + /** + * 是否可以启动自动驾驶 + * 使用方法查看：app_ipc_monitoring/uiMainActivity/onAutopilotAbility + */ + override fun onAutopilotAbility(ability: AutopilotAbility?) { + } } \ No newline at end of file diff --git a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/AdasChannel.java b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/AdasChannel.java index 606496ab5e..aa1125bae1 100644 --- a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/AdasChannel.java +++ b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/AdasChannel.java @@ -19,6 +19,7 @@ import androidx.annotation.Nullable; import com.google.protobuf.InvalidProtocolBufferException; import com.zhidao.support.adas.high.bean.VersionCompatibility; +import com.zhidao.support.adas.high.common.AutopilotAbilityManager; import com.zhidao.support.adas.high.common.ByteUtil; import com.zhidao.support.adas.high.common.Constants; import com.zhidao.support.adas.high.common.CupidLogUtils; @@ -506,9 +507,10 @@ public class AdasChannel implements IAdasNetCommApi, FpgaSocket.IWebSocketConnec private void updateConnectStatus(@Define.IPCConnectionStatus int status, @Nullable String reason) { ipcConnectionStatus.set(status); if (status == Constants.IPC_CONNECTION_STATUS.CONNECTED) { - + AutopilotAbilityManager.getInstance().start(); startCheckCompatibility(); } else { + AutopilotAbilityManager.getInstance().stop(); stopCheckCompatibility(); } if (status == Constants.IPC_CONNECTION_STATUS.DISCONNECTED) { @@ -596,6 +598,7 @@ public class AdasChannel implements IAdasNetCommApi, FpgaSocket.IWebSocketConnec void setOnAdasListener(OnAdasListener adasListener) { mAdasListener = adasListener; + AutopilotAbilityManager.getInstance().setOnAdasListener(adasListener); } @Override diff --git a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/OnAdasListener.java b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/OnAdasListener.java index b40ce186dc..6f9d7f09ba 100644 --- a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/OnAdasListener.java +++ b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/OnAdasListener.java @@ -1,5 +1,6 @@ package com.zhidao.support.adas.high; +import com.zhidao.support.adas.high.bean.AutopilotAbility; import com.zhidao.support.adas.high.common.ProtocolStatus; import chassis.VehicleStateOuterClass; @@ -186,4 +187,10 @@ public interface OnAdasListener { */ void onError(ProtocolStatus status, byte[] bytes); + /** + * 是否有能力启动自动驾驶 + * + * @param ability 是否可以启动自动驾驶 + */ + void onAutopilotAbility(AutopilotAbility ability); } diff --git a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/bean/AutopilotAbility.java b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/bean/AutopilotAbility.java new file mode 100644 index 0000000000..7ecbfe6bb5 --- /dev/null +++ b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/bean/AutopilotAbility.java @@ -0,0 +1,39 @@ +package com.zhidao.support.adas.high.bean; + +import chassis.Chassis; +import system_master.SystemStatusInfo; + +/** + * 自动驾驶能力 能否启动自动驾驶 + * 未赋值表示接口通讯异常或者工控机相关节点异常 如果回调持续都是未赋值的状态表示此问题是持久性的 + */ +public class AutopilotAbility { + /** + * 刹车踏板开度 0~100 + * -1表示未赋值 + */ + public final float brake; + /** + * 挡位 + * null表示未赋值 + */ + public final Chassis.GearPosition gear; + /** + * 状态查询结果 + * null表示未赋值 + * 不能启动自动驾驶的判断 + * statusInfo.getHealthInfoList().get().getState()== SystemStatusInfo.HealthState.FAULT + * statusInfo.getSysState()== SystemStatusInfo.SystemState.SYS_STARTING + * statusInfo.getSysState()== SystemStatusInfo.SystemState.SYS_EXITING + * statusInfo.getSysState()== SystemStatusInfo.SystemState.SYS_FAULT + */ + public final SystemStatusInfo.StatusInfo statusInfo; + + public AutopilotAbility(float brake, Chassis.GearPosition gear, SystemStatusInfo.StatusInfo statusInfo) { + this.brake = brake; + this.gear = gear; + this.statusInfo = statusInfo; + + + } +} diff --git a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/common/AutopilotAbilityManager.java b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/common/AutopilotAbilityManager.java new file mode 100644 index 0000000000..889616b91b --- /dev/null +++ b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/common/AutopilotAbilityManager.java @@ -0,0 +1,82 @@ +package com.zhidao.support.adas.high.common; + +import com.zhidao.support.adas.high.AdasManager; +import com.zhidao.support.adas.high.OnAdasListener; +import com.zhidao.support.adas.high.bean.AutopilotAbility; + +import java.util.Timer; +import java.util.TimerTask; + +import chassis.Chassis; +import chassis.VehicleStateOuterClass; +import system_master.SystemStatusInfo; + +/** + * 是否可以启动自动驾驶能力检测 + */ +public class AutopilotAbilityManager { + private static final String TAG = AutopilotAbilityManager.class.getSimpleName(); + private static final long DEFAULT_DETECTION_TIME = 3 * 1000L;//默认检测时间 + private static volatile AutopilotAbilityManager INSTANCE; + + private OnAdasListener listener; + private volatile Timer timer; + + private VehicleStateOuterClass.VehicleState vehicleState; + + public void setOnAdasListener(OnAdasListener listener) { + this.listener = listener; + } + + private AutopilotAbilityManager() { + } + + public static AutopilotAbilityManager getInstance() { + if (INSTANCE == null) { + synchronized (AutopilotAbilityManager.class) { + if (INSTANCE == null) { + INSTANCE = new AutopilotAbilityManager(); + } + } + } + return INSTANCE; + } + + public void setStatusInfo(SystemStatusInfo.StatusInfo statusInfo) { + if (listener != null) { + float brake = -1; + Chassis.GearPosition gear = null; + if (vehicleState != null) { + brake = vehicleState.getBrake(); + gear = vehicleState.getGear(); + } + listener.onAutopilotAbility(new AutopilotAbility(brake, gear, statusInfo)); + } + } + + public void setVehicleState(VehicleStateOuterClass.VehicleState vehicleState) { + this.vehicleState = vehicleState; + } + + public synchronized void start() { + if (timer == null) { + timer = new Timer(); + timer.schedule(new TimerTask() { + @Override + public void run() { + AdasManager.getInstance().sendStatusQueryReq(); + } + }, 2000L, DEFAULT_DETECTION_TIME);//延迟执行，避免刚连接成功后底盘信息无法及时同步 + } + } + + public synchronized void stop() { + if (timer != null) { + timer.cancel(); + timer = null; + } + this.vehicleState = null; + } + + +} diff --git a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/common/ReceiveTimeoutManager.java b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/common/ReceiveTimeoutManager.java index 8238010917..6bfdc5931e 100644 --- a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/common/ReceiveTimeoutManager.java +++ b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/common/ReceiveTimeoutManager.java @@ -5,7 +5,7 @@ import java.util.TimerTask; import java.util.concurrent.atomic.AtomicBoolean; /** - * 写成单例比较好控制 比如refreshLast()方法可以放到任何接收的回调中,目前放在AutopilotStateMessage 数据接口中，此接口数据刷新不是太频繁 + * 写成单例比较好控制 比如refreshLast()方法可以放到任何接收的回调中 * 与工控机链接成功后，工控机断开连接超时监测管理器 * 由于没有心跳机制，如果服务器端断网或其他非正常断开，客户端是无感知的，所以需要根据最后一次接收数据的时间进行超时检测 */ diff --git a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/msg/StatusQueryRespMessage.java b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/msg/StatusQueryRespMessage.java index ddf101b865..48b22ceae7 100644 --- a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/msg/StatusQueryRespMessage.java +++ b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/msg/StatusQueryRespMessage.java @@ -2,6 +2,7 @@ package com.zhidao.support.adas.high.msg; import com.google.protobuf.InvalidProtocolBufferException; import com.zhidao.support.adas.high.OnAdasListener; +import com.zhidao.support.adas.high.common.AutopilotAbilityManager; import com.zhidao.support.adas.high.protocol.RawData; import mogo.telematics.pad.MessagePad; @@ -15,6 +16,7 @@ public class StatusQueryRespMessage extends MyAbstractMessageHandler { @Override public void handlerMsg(RawData raw, OnAdasListener adasListener) throws InvalidProtocolBufferException { SystemStatusInfo.StatusInfo statusInfo= SystemStatusInfo.StatusInfo.parser().parseFrom(raw.originalData.toByteArray(), raw.getOffsetValue(), raw.getPackageLengthValue() - raw.getOffsetValue()); + AutopilotAbilityManager.getInstance().setStatusInfo(statusInfo); if (adasListener != null) { adasListener.onStatusQueryResp(raw.getHeader(), statusInfo); } diff --git a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/msg/VehicleStateMessage.java b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/msg/VehicleStateMessage.java index ba8d1acdc8..90255446a6 100644 --- a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/msg/VehicleStateMessage.java +++ b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/msg/VehicleStateMessage.java @@ -2,6 +2,7 @@ package com.zhidao.support.adas.high.msg; import com.google.protobuf.InvalidProtocolBufferException; import com.zhidao.support.adas.high.OnAdasListener; +import com.zhidao.support.adas.high.common.AutopilotAbilityManager; import com.zhidao.support.adas.high.protocol.RawData; import chassis.VehicleStateOuterClass; @@ -14,6 +15,7 @@ public class VehicleStateMessage extends MyAbstractMessageHandler { @Override public void handlerMsg(RawData raw, OnAdasListener adasListener) throws InvalidProtocolBufferException { VehicleStateOuterClass.VehicleState vehicleState = VehicleStateOuterClass.VehicleState.parser().parseFrom(raw.originalData.toByteArray(), raw.getOffsetValue(), raw.getPackageLengthValue() - raw.getOffsetValue()); + AutopilotAbilityManager.getInstance().setVehicleState(vehicleState); if (adasListener != null) { adasListener.onVehicleState(raw.getHeader(), vehicleState); }