From edcc2754f74f918bf0dcb35c7b7326f2b1b6bb89 Mon Sep 17 00:00:00 2001 From: xinfengkun Date: Thu, 17 Aug 2023 17:50:20 +0800 Subject: [PATCH] =?UTF-8?q?[600][adas][data-center]SSM=E5=AE=9A=E9=A2=91?= =?UTF-8?q?=E6=8E=A5=E5=85=A5=EF=BC=9B=E8=83=BD=E5=90=A6=E5=90=AF=E5=8A=A8?= =?UTF-8?q?=E8=87=AA=E9=A9=BE=E5=85=BC=E5=AE=B9=E8=80=81=E7=89=88=E6=9C=AC?= =?UTF-8?q?=EF=BC=9B=E4=BB=A5=E5=8F=8A=E6=96=B0=E7=89=88=E6=9C=AC=E3=80=81?= =?UTF-8?q?FSM=E3=80=81=E4=B8=8D=E5=90=8C=E8=BD=A6=E5=9E=8B=E7=9A=84?= =?UTF-8?q?=E5=85=BC=E5=AE=B9=E3=80=81=E4=B8=8D=E5=90=8C=E5=9F=9F=E6=8E=A7?= =?UTF-8?q?=E7=89=88=E6=9C=AC=E5=85=BC=E5=AE=B9=EF=BC=9B=E5=A4=84=E7=90=86?= =?UTF-8?q?=E6=95=B0=E6=8D=AE=E4=B8=AD=E5=BF=83=E8=83=BD=E5=90=A6=E5=90=AF?= =?UTF-8?q?=E5=8A=A8=E8=87=AA=E9=A9=BE=E7=AE=A1=E7=90=86=E7=B1=BB=20?= =?UTF-8?q?=E5=88=9D=E5=A7=8B=E5=8C=96=E8=B0=83=E7=94=A8=E8=8E=B7=E5=8F=96?= =?UTF-8?q?=E5=9F=9F=E6=8E=A7=E8=BF=9E=E6=8E=A5=E7=8A=B6=E6=80=81=E5=AF=BC?= =?UTF-8?q?=E8=87=B4=E7=9A=84Netty=E9=87=8D=E5=A4=8D=E5=88=9D=E5=A7=8B?= =?UTF-8?q?=E5=8C=96=E9=97=AE=E9=A2=98?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../bus/passenger/model/BusPassengerModel.kt | 3 + .../mogo/och/bus/presenter/BusPresenter.java | 6 + .../mogo/och/bus/presenter/BusPresenter.java | 6 + .../sweepercloud/model/SweeperTaskModel.java | 6 + .../sweeper/presenter/SweeperPresenter.java | 5 + .../java/com/mogo/och/taxi/model/TaxiModel.kt | 3 + .../com/mogo/och/taxi/model/TaxiModel.java | 4 + .../autopilot/adapter/MoGoAdasListenerImpl.kt | 6 + .../MoGoAdasMsgConnectStatusListenerImpl.kt | 1 + .../status/flow/can/CanImpl.kt | 9 ++ .../status/flow/rtk/RTKImpl.kt | 14 +- .../routeoverlay/MogoRouteOverlayManager.java | 5 + .../autopilot/IMoGoAutopilotStatusListener.kt | 6 + .../CallerAutoPilotStatusListenerManager.kt | 14 ++ .../CallerAutopilotActionsListenerManager.kt | 37 ++--- .../src/main/proto/message_pad.proto | 4 + .../src/main/proto/personal/ssm_base.proto | 18 +++ .../src/main/proto/ssm_info.proto | 54 +++++++ .../src/main/proto/system_status_info.proto | 22 +-- libraries/mogo-adas/build.gradle | 1 + .../zhidao/support/adas/high/AdasChannel.java | 8 + .../zhidao/support/adas/high/AdasManager.java | 1 + .../support/adas/high/IAdasNetCommApi.java | 1 + .../support/adas/high/OnAdasListener.java | 13 +- .../support/adas/high/common/MessageType.java | 1 + .../ability/AutopilotAbility230.java | 2 +- .../ability/AutopilotAbility250.java | 11 +- .../ability/AutopilotAbility330.java | 11 +- .../ability/AutopilotAbility350And360.java | 147 ++++++++++++++++++ .../ability/AutopilotAbility360.java | 134 ++++++++++++++++ .../ability/AutopilotAbilityManager.java | 85 +++++++++- .../adas/high/msg/MyMessageFactory.java | 7 + .../adas/high/msg/StatusQueryRespMessage.java | 1 + .../adas/high/msg/SystemStatusMessage.java | 33 ++++ 34 files changed, 619 insertions(+), 60 deletions(-) create mode 100644 libraries/mogo-adas-data/src/main/proto/personal/ssm_base.proto create mode 100644 libraries/mogo-adas-data/src/main/proto/ssm_info.proto create mode 100644 libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/common/autopilot/ability/AutopilotAbility350And360.java create mode 100644 libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/common/autopilot/ability/AutopilotAbility360.java create mode 100644 libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/msg/SystemStatusMessage.java diff --git a/OCH/mogo-och-bus-passenger/src/jinlvvan/java/com/mogo/och/bus/passenger/model/BusPassengerModel.kt b/OCH/mogo-och-bus-passenger/src/jinlvvan/java/com/mogo/och/bus/passenger/model/BusPassengerModel.kt index f8138e6112..52325b172e 100644 --- a/OCH/mogo-och-bus-passenger/src/jinlvvan/java/com/mogo/och/bus/passenger/model/BusPassengerModel.kt +++ b/OCH/mogo-och-bus-passenger/src/jinlvvan/java/com/mogo/och/bus/passenger/model/BusPassengerModel.kt @@ -74,6 +74,7 @@ import com.mogo.och.common.module.utils.PinYinUtil import mogo.telematics.pad.MessagePad import mogo.telematics.pad.MessagePad.ArrivalNotification import mogo.telematics.pad.MessagePad.GlobalPathResp +import system_master.SsmInfo import system_master.SystemStatusInfo import java.util.concurrent.ConcurrentHashMap @@ -514,6 +515,8 @@ object BusPassengerModel{ } override fun onAutopilotStatusRespByQuery(status: SystemStatusInfo.StatusInfo) {} + + override fun onSystemStatus(statusInf: SsmInfo.SsmStatusInf) {} } private val moGoAutopilotPlanningListener: IMoGoPlanningRottingListener = object : IMoGoPlanningRottingListener { diff --git a/OCH/mogo-och-bus/src/main/java/com/mogo/och/bus/presenter/BusPresenter.java b/OCH/mogo-och-bus/src/main/java/com/mogo/och/bus/presenter/BusPresenter.java index e30ceb2684..b61a1ef1ce 100644 --- a/OCH/mogo-och-bus/src/main/java/com/mogo/och/bus/presenter/BusPresenter.java +++ b/OCH/mogo-och-bus/src/main/java/com/mogo/och/bus/presenter/BusPresenter.java @@ -39,6 +39,7 @@ import java.util.List; import mogo.telematics.pad.MessagePad; import mogo_msg.MogoReportMsg; +import system_master.SsmInfo; import system_master.SystemStatusInfo; /** @@ -248,6 +249,11 @@ public class BusPresenter extends Presenter public void onAutopilotStatusRespByQuery(@NonNull SystemStatusInfo.StatusInfo status) { } + + @Override + public void onSystemStatus(@NonNull SsmInfo.SsmStatusInf statusInf) { + } + @Override public void loginSuccess(DriverStatusQueryRespBean data) { CallerLogger.INSTANCE.d(M_BUS + TAG, " loginStatus =" + LoginStatusManager.isLogin()); diff --git a/OCH/mogo-och-shuttle/src/main/java/com/mogo/och/bus/presenter/BusPresenter.java b/OCH/mogo-och-shuttle/src/main/java/com/mogo/och/bus/presenter/BusPresenter.java index 1d0c948311..de2a58d256 100644 --- a/OCH/mogo-och-shuttle/src/main/java/com/mogo/och/bus/presenter/BusPresenter.java +++ b/OCH/mogo-och-shuttle/src/main/java/com/mogo/och/bus/presenter/BusPresenter.java @@ -42,6 +42,7 @@ import java.util.List; import mogo.telematics.pad.MessagePad; import mogo_msg.MogoReportMsg; +import system_master.SsmInfo; import system_master.SystemStatusInfo; /** @@ -260,6 +261,11 @@ public class BusPresenter extends Presenter public void onAutopilotStatusRespByQuery(@NonNull SystemStatusInfo.StatusInfo status) { } + + @Override + public void onSystemStatus(@NonNull SsmInfo.SsmStatusInf statusInf) { + } + @Override public void loginSuccess(DriverStatusQueryRespBean data) { CallerLogger.INSTANCE.d(M_BUS + TAG, " loginStatus =" + LoginStatusManager.isLogin()); diff --git a/OCH/sweeper/sweeper-cloud/src/main/java/com/mogo/och/sweepercloud/model/SweeperTaskModel.java b/OCH/sweeper/sweeper-cloud/src/main/java/com/mogo/och/sweepercloud/model/SweeperTaskModel.java index 349be74b75..49e6b7b34e 100644 --- a/OCH/sweeper/sweeper-cloud/src/main/java/com/mogo/och/sweepercloud/model/SweeperTaskModel.java +++ b/OCH/sweeper/sweeper-cloud/src/main/java/com/mogo/och/sweepercloud/model/SweeperTaskModel.java @@ -63,6 +63,7 @@ import io.reactivex.plugins.RxJavaPlugins; import mogo.telematics.pad.MessagePad; import mogo_msg.MogoReportMsg; import planning.RoboSweeperTaskIndexOuterClass; +import system_master.SsmInfo; import system_master.SystemStatusInfo; /** @@ -642,6 +643,11 @@ public class SweeperTaskModel implements IMoGoSweeperFutianCloudTaskListener, IM // CallerLogger.INSTANCE.d(M_SWEEPER + TAG, "onAutopilotStatusRespByQuery status:" + status.getSysState().getNumber()); } + @Override + public void onSystemStatus(@NonNull SsmInfo.SsmStatusInf statusInf) { + } + + @Override public void onAutopilotRouteLineId(long lineId) { diff --git a/OCH/sweeper/sweeper/src/main/java/com/mogo/och/sweeper/presenter/SweeperPresenter.java b/OCH/sweeper/sweeper/src/main/java/com/mogo/och/sweeper/presenter/SweeperPresenter.java index 7e18efb729..288a79755b 100644 --- a/OCH/sweeper/sweeper/src/main/java/com/mogo/och/sweeper/presenter/SweeperPresenter.java +++ b/OCH/sweeper/sweeper/src/main/java/com/mogo/och/sweeper/presenter/SweeperPresenter.java @@ -52,6 +52,7 @@ import chassis.ChassisStatesOuterClass; import mogo.telematics.pad.MessagePad; import mogo_msg.MogoReportMsg; import planning.RoboSweeperTaskIndexOuterClass; +import system_master.SsmInfo; import system_master.SystemStatusInfo; /** @@ -229,6 +230,10 @@ public class SweeperPresenter extends Presenter } + @Override + public void onSystemStatus(@NonNull SsmInfo.SsmStatusInf statusInf) { + } + @Override public void onAutopilotArriveAtStation(@Nullable MessagePad.ArrivalNotification arrivalNotification) { SweeperTaskModel.getInstance().onArriveTaskEnd(arrivalNotification); diff --git a/OCH/taxi/mogo-och-taxi-unmanned/src/main/java/com/mogo/och/taxi/model/TaxiModel.kt b/OCH/taxi/mogo-och-taxi-unmanned/src/main/java/com/mogo/och/taxi/model/TaxiModel.kt index a9b7c16918..825443b084 100644 --- a/OCH/taxi/mogo-och-taxi-unmanned/src/main/java/com/mogo/och/taxi/model/TaxiModel.kt +++ b/OCH/taxi/mogo-och-taxi-unmanned/src/main/java/com/mogo/och/taxi/model/TaxiModel.kt @@ -91,6 +91,7 @@ import mogo.telematics.pad.MessagePad import mogo.telematics.pad.MessagePad.ArrivalNotification import mogo.telematics.pad.MessagePad.GlobalPathResp import mogo_msg.MogoReportMsg.MogoReportMessage +import system_master.SsmInfo import system_master.SystemStatusInfo import java.io.IOException @@ -864,6 +865,8 @@ object TaxiModel { } override fun onAutopilotStatusRespByQuery(status: SystemStatusInfo.StatusInfo) {} + + override fun onSystemStatus(statusInf: SsmInfo.SsmStatusInf) {} } private fun arriveTerminal() { diff --git a/OCH/taxi/mogo-och-taxi/src/main/java/com/mogo/och/taxi/model/TaxiModel.java b/OCH/taxi/mogo-och-taxi/src/main/java/com/mogo/och/taxi/model/TaxiModel.java index a8f1bb625a..17673e2d3b 100644 --- a/OCH/taxi/mogo-och-taxi/src/main/java/com/mogo/och/taxi/model/TaxiModel.java +++ b/OCH/taxi/mogo-och-taxi/src/main/java/com/mogo/och/taxi/model/TaxiModel.java @@ -98,6 +98,7 @@ import io.reactivex.functions.Consumer; import io.reactivex.plugins.RxJavaPlugins; import mogo.telematics.pad.MessagePad; import mogo_msg.MogoReportMsg; +import system_master.SsmInfo; import system_master.SystemStatusInfo; /** @@ -1297,6 +1298,9 @@ public class TaxiModel { @Override public void onAutopilotStatusRespByQuery(@NonNull SystemStatusInfo.StatusInfo status) { } + @Override + public void onSystemStatus(@NonNull SsmInfo.SsmStatusInf statusInf) { + } }; private final IMoGoPlanningRottingListener moGoAutopilotPlanningListener = new IMoGoPlanningRottingListener() { diff --git a/core/function-impl/mogo-core-function-datacenter/src/main/java/com/mogo/eagle/core/function/datacenter/autopilot/adapter/MoGoAdasListenerImpl.kt b/core/function-impl/mogo-core-function-datacenter/src/main/java/com/mogo/eagle/core/function/datacenter/autopilot/adapter/MoGoAdasListenerImpl.kt index 30d22b1487..4f7ff6105a 100644 --- a/core/function-impl/mogo-core-function-datacenter/src/main/java/com/mogo/eagle/core/function/datacenter/autopilot/adapter/MoGoAdasListenerImpl.kt +++ b/core/function-impl/mogo-core-function-datacenter/src/main/java/com/mogo/eagle/core/function/datacenter/autopilot/adapter/MoGoAdasListenerImpl.kt @@ -30,6 +30,7 @@ import com.mogo.eagle.core.function.call.autopilot.CallerAutoPilotStatusListener import com.mogo.eagle.core.function.call.autopilot.CallerAutoPilotStatusListenerManager.invokeAutopilotGuardian import com.mogo.eagle.core.function.call.autopilot.CallerAutoPilotStatusListenerManager.invokeAutopilotSNRequest import com.mogo.eagle.core.function.call.autopilot.CallerAutoPilotStatusListenerManager.invokeAutopilotStatusRespByQuery +import com.mogo.eagle.core.function.call.autopilot.CallerAutoPilotStatusListenerManager.invokeSystemStatus import com.mogo.eagle.core.function.call.autopilot.CallerAutoPilotStatusListenerManager.updateAutoPilotDockerInfo import com.mogo.eagle.core.function.call.autopilot.CallerAutoPilotStatusListenerManager.updateAutoPilotStatus import com.mogo.eagle.core.function.call.autopilot.CallerAutopilotActionsListenerManager.invokeAutopilotAbility @@ -75,6 +76,7 @@ import perception.TrafficLightOuterClass import planning.RoboSweeperTaskIndexOuterClass import prediction.Prediction import record_cache.RecordPanelOuterClass +import system_master.SsmInfo import system_master.SystemStatusInfo /** @@ -438,6 +440,10 @@ class MoGoAdasListenerImpl : OnAdasListener { invokeAutopilotStatusRespByQuery(statusInfo) } + override fun onSystemStatus(header: MessagePad.Header?, statusInf: SsmInfo.SsmStatusInf?) { + invokeSystemStatus(statusInf) + } + /** * 数据采集配置应答 */ diff --git a/core/function-impl/mogo-core-function-datacenter/src/main/java/com/mogo/eagle/core/function/datacenter/autopilot/adapter/MoGoAdasMsgConnectStatusListenerImpl.kt b/core/function-impl/mogo-core-function-datacenter/src/main/java/com/mogo/eagle/core/function/datacenter/autopilot/adapter/MoGoAdasMsgConnectStatusListenerImpl.kt index d1e903d5a3..ceff1aad76 100644 --- a/core/function-impl/mogo-core-function-datacenter/src/main/java/com/mogo/eagle/core/function/datacenter/autopilot/adapter/MoGoAdasMsgConnectStatusListenerImpl.kt +++ b/core/function-impl/mogo-core-function-datacenter/src/main/java/com/mogo/eagle/core/function/datacenter/autopilot/adapter/MoGoAdasMsgConnectStatusListenerImpl.kt @@ -38,6 +38,7 @@ class MoGoAdasMsgConnectStatusListenerImpl : init { CallerCloudListenerManager.addListener(TAG, this) + CallerAutopilotActionsListenerManager.setConnected(AdasManager.getInstance().ipcConnectionStatus == Constants.IPC_CONNECTION_STATUS.CONNECTED) } override fun onConnectionIPCStatus(ipcConnectionStatus: Int, reason: String?) { diff --git a/core/function-impl/mogo-core-function-devatools/src/main/java/com/zhjt/mogo_core_function_devatools/status/flow/can/CanImpl.kt b/core/function-impl/mogo-core-function-devatools/src/main/java/com/zhjt/mogo_core_function_devatools/status/flow/can/CanImpl.kt index 01044b7daa..e6b92611d7 100644 --- a/core/function-impl/mogo-core-function-devatools/src/main/java/com/zhjt/mogo_core_function_devatools/status/flow/can/CanImpl.kt +++ b/core/function-impl/mogo-core-function-devatools/src/main/java/com/zhjt/mogo_core_function_devatools/status/flow/can/CanImpl.kt @@ -13,6 +13,7 @@ import com.zhjt.mogo_core_function_devatools.status.flow.IFlow import kotlinx.coroutines.Job import mogo_msg.MogoReportMsg.MogoReportMessage import planning.RoboSweeperTaskIndexOuterClass +import system_master.SsmInfo import system_master.SystemStatusInfo.StatusInfo import java.util.concurrent.atomic.AtomicInteger @@ -115,6 +116,14 @@ internal class CanImpl(ctx: Context) : } } + override fun onSystemStatus(statusInf: SsmInfo.SsmStatusInf) { + val state = statusInf.healthInfoList?.find { "can_adapter".equals(it.name, true) }?.state?.ordinal + Log.d(TAG, "state: $state") + if (state != null) { + this.state.set(state) + } + } + override fun onDestroy() { super.onDestroy() job?.safeCancel() diff --git a/core/function-impl/mogo-core-function-devatools/src/main/java/com/zhjt/mogo_core_function_devatools/status/flow/rtk/RTKImpl.kt b/core/function-impl/mogo-core-function-devatools/src/main/java/com/zhjt/mogo_core_function_devatools/status/flow/rtk/RTKImpl.kt index 20c8b349f9..a25e636ae8 100644 --- a/core/function-impl/mogo-core-function-devatools/src/main/java/com/zhjt/mogo_core_function_devatools/status/flow/rtk/RTKImpl.kt +++ b/core/function-impl/mogo-core-function-devatools/src/main/java/com/zhjt/mogo_core_function_devatools/status/flow/rtk/RTKImpl.kt @@ -16,7 +16,8 @@ import kotlinx.coroutines.Dispatchers import kotlinx.coroutines.Job import kotlinx.coroutines.delay import kotlinx.coroutines.launch -import system_master.SystemStatusInfo.HealthInfo +import system_master.SsmInfo +import system_master.SsmBase.HealthInfo import system_master.SystemStatusInfo.StatusInfo import java.util.concurrent.atomic.AtomicBoolean import java.util.concurrent.atomic.AtomicReference @@ -72,6 +73,17 @@ internal class RTKImpl(ctx: Context): IFlow(ctx), IMoGoAutopilotStatu } } + override fun onSystemStatus(statusInf: SsmInfo.SsmStatusInf) { + check?.takeIf { it.isActive }?.cancel() + isOldVersion.set(false) + val info = statusInf.healthInfoList?.find { "localization".equals(it.name, true) } + Log.d(TAG, "info: $info") + if (info != null) { + healthInfo.set(info) + send(RTKStatus(getDesc(), getCode())) + } + } + override fun onAutopilotIpcConnectStatusChanged(status: Int, reason: String?) { super.onAutopilotIpcConnectStatusChanged(status, reason) if (!CallerAutoPilotControlManager.isConnected()) { diff --git a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/business/routeoverlay/MogoRouteOverlayManager.java b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/business/routeoverlay/MogoRouteOverlayManager.java index fdb750665e..f4f3a42934 100644 --- a/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/business/routeoverlay/MogoRouteOverlayManager.java +++ b/core/function-impl/mogo-core-function-map/src/main/java/com/mogo/eagle/core/function/business/routeoverlay/MogoRouteOverlayManager.java @@ -23,6 +23,7 @@ import java.util.concurrent.atomic.AtomicInteger; import mogo.telematics.pad.MessagePad; import mogo_msg.MogoReportMsg; +import system_master.SsmInfo; import system_master.SystemStatusInfo; public class MogoRouteOverlayManager implements @@ -134,6 +135,10 @@ public class MogoRouteOverlayManager implements public void onAutopilotStatusRespByQuery(@NonNull SystemStatusInfo.StatusInfo status) { } + @Override + public void onSystemStatus(@NonNull SsmInfo.SsmStatusInf statusInf) { + } + @Override public void onAutopilotRouteLineId(long lineId) { diff --git a/core/mogo-core-function-api/src/main/java/com/mogo/eagle/core/function/api/autopilot/IMoGoAutopilotStatusListener.kt b/core/mogo-core-function-api/src/main/java/com/mogo/eagle/core/function/api/autopilot/IMoGoAutopilotStatusListener.kt index abf75fd255..d70810e024 100644 --- a/core/mogo-core-function-api/src/main/java/com/mogo/eagle/core/function/api/autopilot/IMoGoAutopilotStatusListener.kt +++ b/core/mogo-core-function-api/src/main/java/com/mogo/eagle/core/function/api/autopilot/IMoGoAutopilotStatusListener.kt @@ -61,6 +61,12 @@ interface IMoGoAutopilotStatusListener { */ fun onAutopilotStatusRespByQuery(status: SystemStatusInfo.StatusInfo) {} + /** + * 定频SSM + * 老版本 SSM（SystemStatusInfo.StatusInfo） HQ、M1 MAP350开始弃用，其他车型MAP360开始弃用 + */ + fun onSystemStatus(statusInf: SsmInfo.SsmStatusInf) {} + /** * 自动驾驶路线ID回调 */ diff --git a/core/mogo-core-function-call/src/main/java/com/mogo/eagle/core/function/call/autopilot/CallerAutoPilotStatusListenerManager.kt b/core/mogo-core-function-call/src/main/java/com/mogo/eagle/core/function/call/autopilot/CallerAutoPilotStatusListenerManager.kt index 2e2df750e4..c6eae4c451 100644 --- a/core/mogo-core-function-call/src/main/java/com/mogo/eagle/core/function/call/autopilot/CallerAutoPilotStatusListenerManager.kt +++ b/core/mogo-core-function-call/src/main/java/com/mogo/eagle/core/function/call/autopilot/CallerAutoPilotStatusListenerManager.kt @@ -7,6 +7,7 @@ import com.mogo.eagle.core.function.call.base.CallerBase import com.mogo.eagle.core.utilcode.util.GsonUtils import mogo.telematics.pad.MessagePad import mogo_msg.MogoReportMsg +import system_master.SsmInfo import system_master.SystemStatusInfo import kotlin.properties.Delegates @@ -207,6 +208,19 @@ object CallerAutoPilotStatusListenerManager : CallerBase + val listener = itx.value + listener.onSystemStatus(it) + } + } + } + /** * 更新自动驾驶控制参数，结束自动驾驶时候需要更新为null，且更新时候同时触发onAutopilotStatusResponse回调 */ diff --git a/core/mogo-core-function-call/src/main/java/com/mogo/eagle/core/function/call/autopilot/CallerAutopilotActionsListenerManager.kt b/core/mogo-core-function-call/src/main/java/com/mogo/eagle/core/function/call/autopilot/CallerAutopilotActionsListenerManager.kt index bb494b549e..d2cd6305fe 100644 --- a/core/mogo-core-function-call/src/main/java/com/mogo/eagle/core/function/call/autopilot/CallerAutopilotActionsListenerManager.kt +++ b/core/mogo-core-function-call/src/main/java/com/mogo/eagle/core/function/call/autopilot/CallerAutopilotActionsListenerManager.kt @@ -1,8 +1,6 @@ package com.mogo.eagle.core.function.call.autopilot -import android.util.Log import com.mogo.eagle.core.function.api.autopilot.IMoGoAutopilotActionsListener -import com.mogo.eagle.core.function.call.autopilot.CallerAutoPilotControlManager.isConnected import com.mogo.eagle.core.function.call.base.CallerBase import com.zhjt.mogo.adas.data.bean.UnableAutopilotReason @@ -17,10 +15,9 @@ object CallerAutopilotActionsListenerManager : CallerBase? = null init { -// isConnected = isConnected() //TODO 重复初始 isAutopilotAbility = false unableAutopilotReasons = disconnectedReason() - printLog("初始化") +// printLog("初始化") } private fun disconnectedReason(): ArrayList { @@ -50,7 +47,7 @@ object CallerAutopilotActionsListenerManager : CallerBase old.sortWith(compareBy(UnableAutopilotReason::hashCode)); isEquals = onw.toTypedArray() contentEquals old.toTypedArray() - Log.i( - "ddd", - "都不为null时=${isEquals}=${this.isAutopilotAbility} ${(if (this.unableAutopilotReasons == null) null else this.unableAutopilotReasons.toString())}" - ) +// Log.i( +// "ddd", +// "都不为null时=${isEquals}=${this.isAutopilotAbility} ${(if (this.unableAutopilotReasons == null) null else this.unableAutopilotReasons.toString())}" +// ) } } } else isEquals = !(unableAutopilotReasons != null || this.unableAutopilotReasons != null) - Log.i( - "ddd", - " 两个List是否相同=${isEquals} 新是否能启动自驾=${isAutopilotAbility} 老是否能启动自驾=${this.isAutopilotAbility} 新List是否为空=${unableAutopilotReasons == null} 老List是否为空=${this.unableAutopilotReasons == null}" - ) +// Log.i( +// "ddd", +// " 两个List是否相同=${isEquals} 新是否能启动自驾=${isAutopilotAbility} 老是否能启动自驾=${this.isAutopilotAbility} 新List是否为空=${unableAutopilotReasons == null} 老List是否为空=${this.unableAutopilotReasons == null}" +// ) if (this.isAutopilotAbility != isAutopilotAbility || !isEquals) { this.isAutopilotAbility = isAutopilotAbility this.unableAutopilotReasons = unableAutopilotReasons notification() - printLog("更新数据") +// printLog("更新数据") } } } @@ -94,10 +91,10 @@ object CallerAutopilotActionsListenerManager : CallerBase=350开始走定频SSM，其他车型360开始 + if ((config.getMapVersion() >= 30500 && (config.getIsJinlvM1() || config.getIsHQ())) || config.getMapVersion() >= 30600) { + return false; + } + } MessagePad.StatusQueryReq req = MessagePad.StatusQueryReq .newBuilder() .build(); diff --git a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/AdasManager.java b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/AdasManager.java index c156d93f18..3bf97a5cce 100644 --- a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/AdasManager.java +++ b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/AdasManager.java @@ -578,6 +578,7 @@ public class AdasManager implements IAdasNetCommApi { * * @return boolean */ + @Deprecated//HQ、M1 MAP350开始弃用，其他车型MAP360开始弃用 @Override public boolean sendStatusQueryReq() { return mChannel != null && mChannel.sendStatusQueryReq(); diff --git a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/IAdasNetCommApi.java b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/IAdasNetCommApi.java index 625d02081a..588a32918c 100644 --- a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/IAdasNetCommApi.java +++ b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/IAdasNetCommApi.java @@ -266,6 +266,7 @@ public interface IAdasNetCommApi { * * @return 加入WS发送消息队列是否成功 */ + @Deprecated//HQ、M1 MAP350开始弃用，其他车型MAP360开始弃用 boolean sendStatusQueryReq(); /** 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 bbdcaeb515..67f070b4c2 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 @@ -20,7 +20,6 @@ import com.zhjt.mogo.adas.data.sweeper.task.status.SweeperTaskStatus; import com.zhjt.mogo.adas.data.sweeper.task.stop.SweeperTaskStop; import java.util.ArrayList; -import java.util.List; import bag_manager.BagManagerOuterClass; import chassis.ChassisStatesOuterClass; @@ -34,6 +33,7 @@ import perception.TrafficLightOuterClass; import planning.RoboSweeperTaskIndexOuterClass; import prediction.Prediction; import record_cache.RecordPanelOuterClass; +import system_master.SsmInfo; import system_master.SystemStatusInfo; /** @@ -197,8 +197,19 @@ public interface OnAdasListener { * @param header 头 * @param statusInfo 数据 */ + @Deprecated//HQ、M1 MAP350开始弃用，其他车型MAP360开始弃用 void onStatusQueryResp(MessagePad.Header header, SystemStatusInfo.StatusInfo statusInfo); + /** + * 定频SSM接口 + * 1hz hq m1 MAP350开始支持，其他车型MAP360开始支持 + * 定频SSM接入后 onStatusQueryResp 状态查询应答接口将弃用 + * + * @param header 头 + * @param statusInf 数据 + */ + void onSystemStatus(MessagePad.Header header, SsmInfo.SsmStatusInf statusInf); + /** * 数据采集配置应答 * diff --git a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/common/MessageType.java b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/common/MessageType.java index 8b3d310d69..08428bb6f8 100644 --- a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/common/MessageType.java +++ b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/common/MessageType.java @@ -30,6 +30,7 @@ public enum MessageType { TYPE_RECEIVE_FUNCTION_STATES(MessagePad.MessageType.MsgTypeFunctionStates, "重构后功能状态"), TYPE_RECEIVE_BACK_CAMERA_VIDEO(MessagePad.MessageType.MsgTypeBackCameraVideo, "后摄像头"), TYPE_RECEIVE_M1_STITCHED_VIDEO(MessagePad.MessageType.MsgTypeM1StitchedVideo, "M1拼接视频"), + TYPE_RECEIVE_SSM(MessagePad.MessageType.MsgTypeSSMState, "SSM系统状态"), TYPE_RECEIVE_BASIC_INFO_REQ(MessagePad.MessageType.MsgTypeBasicInfoReq, "自动驾驶设备基础信息请求"), TYPE_SEND_BASIC_INFO_RESP(MessagePad.MessageType.MsgTypeBasicInfoResp, "自动驾驶设备基础信息应答"), diff --git a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/common/autopilot/ability/AutopilotAbility230.java b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/common/autopilot/ability/AutopilotAbility230.java index e7c6801119..36202fca78 100644 --- a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/common/autopilot/ability/AutopilotAbility230.java +++ b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/common/autopilot/ability/AutopilotAbility230.java @@ -53,7 +53,7 @@ public class AutopilotAbility230 { Chassis.GearPosition gear = chassisStates.getGearSystemStates().getGearPosition(); if (!AutopilotAbilityManager.getInstance().isLaunchAutopilot(gear)) { isAutopilotAbility = false; - unableAutopilotReasons = AutopilotAbilityManager.getInstance().addUnableAutopilotReason(unableAutopilotReasons, UnableAutopilotReason.SourceType.CHASSIS, "当前档位无法启动自驾"); + unableAutopilotReasons = AutopilotAbilityManager.getInstance().addUnableAutopilotReason(unableAutopilotReasons, UnableAutopilotReason.SourceType.CHASSIS, "档位异常"); } } //TODO 关于手刹：不同车型的实现不同所以目前没法使用此字段 diff --git a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/common/autopilot/ability/AutopilotAbility250.java b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/common/autopilot/ability/AutopilotAbility250.java index 7dc31cb07f..42ac6a90d9 100644 --- a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/common/autopilot/ability/AutopilotAbility250.java +++ b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/common/autopilot/ability/AutopilotAbility250.java @@ -79,17 +79,14 @@ public class AutopilotAbility250 { SystemStatusInfo.NodeFaultList nodeFaultList = statusInfo.getAutoPilotUnreadyList(); if (nodeFaultList.getSum() > 0) { List list = nodeFaultList.getNodeList(); - StringBuilder builder = new StringBuilder(); for (SystemStatusInfo.NodeInfo info : list) { - builder.append(info.getNodeName()); + String nodeName = info.getNodeName(); int state = info.getState(); if (state < AutopilotAbilityManager.NODE_INFO_STATE.length) { - builder.append(AutopilotAbilityManager.NODE_INFO_STATE[state]); + unableAutopilotReasons = AutopilotAbilityManager.getInstance().addUnableAutopilotReason(unableAutopilotReasons, UnableAutopilotReason.SourceType.SSM, nodeName + AutopilotAbilityManager.NODE_INFO_STATE[state]); } else { - builder.append("未知异常 "); + unableAutopilotReasons = AutopilotAbilityManager.getInstance().addUnableAutopilotReason(unableAutopilotReasons, UnableAutopilotReason.SourceType.SSM, nodeName + "未知异常"); } - unableAutopilotReasons = AutopilotAbilityManager.getInstance().addUnableAutopilotReason(unableAutopilotReasons, UnableAutopilotReason.SourceType.SSM, builder.toString()); - builder.setLength(0); } } else { unableAutopilotReasons = AutopilotAbilityManager.getInstance().addUnableAutopilotReason(unableAutopilotReasons, UnableAutopilotReason.SourceType.SSM, "未知异常节点"); @@ -144,7 +141,7 @@ public class AutopilotAbility250 { Chassis.GearPosition gear = chassisStates.getGearSystemStates().getGearPosition(); if (!AutopilotAbilityManager.getInstance().isLaunchAutopilot(gear)) { isAutopilotAbility = false; - unableAutopilotReasons = AutopilotAbilityManager.getInstance().addUnableAutopilotReason(unableAutopilotReasons, UnableAutopilotReason.SourceType.CHASSIS, "当前档位无法启动自驾"); + unableAutopilotReasons = AutopilotAbilityManager.getInstance().addUnableAutopilotReason(unableAutopilotReasons, UnableAutopilotReason.SourceType.CHASSIS, "档位异常"); } } //TODO 关于手刹：不同车型的实现不同所以目前没法使用此字段 diff --git a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/common/autopilot/ability/AutopilotAbility330.java b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/common/autopilot/ability/AutopilotAbility330.java index 1dbcdeb4b9..a11254d0da 100644 --- a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/common/autopilot/ability/AutopilotAbility330.java +++ b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/common/autopilot/ability/AutopilotAbility330.java @@ -82,17 +82,14 @@ public class AutopilotAbility330 { SystemStatusInfo.NodeFaultList nodeFaultList = statusInfo.getAutoPilotUnreadyList(); if (nodeFaultList.getSum() > 0) { List list = nodeFaultList.getNodeList(); - StringBuilder builder = new StringBuilder(); for (SystemStatusInfo.NodeInfo info : list) { - builder.append(info.getNodeName()); + String nodeName = info.getNodeName(); int state = info.getState(); if (state < AutopilotAbilityManager.NODE_INFO_STATE.length) { - builder.append(AutopilotAbilityManager.NODE_INFO_STATE[state]); + unableAutopilotReasons = AutopilotAbilityManager.getInstance().addUnableAutopilotReason(unableAutopilotReasons, UnableAutopilotReason.SourceType.SSM, nodeName + AutopilotAbilityManager.NODE_INFO_STATE[state]); } else { - builder.append("未知异常 "); + unableAutopilotReasons = AutopilotAbilityManager.getInstance().addUnableAutopilotReason(unableAutopilotReasons, UnableAutopilotReason.SourceType.SSM, nodeName + "未知异常"); } - unableAutopilotReasons = AutopilotAbilityManager.getInstance().addUnableAutopilotReason(unableAutopilotReasons, UnableAutopilotReason.SourceType.SSM, builder.toString()); - builder.setLength(0); } } else { unableAutopilotReasons = AutopilotAbilityManager.getInstance().addUnableAutopilotReason(unableAutopilotReasons, UnableAutopilotReason.SourceType.SSM, "未知异常节点"); @@ -145,7 +142,7 @@ public class AutopilotAbility330 { } } if (unableAutopilotReasons == null || unableAutopilotReasons.isEmpty()) { - unableAutopilotReasons = AutopilotAbilityManager.getInstance().addUnableAutopilotReason(unableAutopilotReasons, UnableAutopilotReason.SourceType.LIB, "FSM未给出原因"); + unableAutopilotReasons = AutopilotAbilityManager.getInstance().addUnableAutopilotReason(unableAutopilotReasons, UnableAutopilotReason.SourceType.LIB, "FSM数据异常"); } } } diff --git a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/common/autopilot/ability/AutopilotAbility350And360.java b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/common/autopilot/ability/AutopilotAbility350And360.java new file mode 100644 index 0000000000..0b94ccc7b7 --- /dev/null +++ b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/common/autopilot/ability/AutopilotAbility350And360.java @@ -0,0 +1,147 @@ +package com.zhidao.support.adas.high.common.autopilot.ability; + +import androidx.annotation.Nullable; + +import com.zhidao.support.adas.high.AdasManager; +import com.zhidao.support.adas.high.common.CupidLogUtils; +import com.zhjt.mogo.adas.data.bean.UnableAutopilotReason; + +import java.util.ArrayList; + +import chassis.Chassis; +import chassis.ChassisStatesOuterClass; +import system_master.SsmInfo; + +/** + * 是否可以启动自动驾驶能力检测 工控机版本>=350&&(isHQ||isJinlvM1) || 工控机版本>=360&&!isFutianSweeper 使用此类 + * 目前监控了定频SSM的数据和FSM状态原因查询 + * 没有使用监控事件报告的原因是因为，部分异常没进行正常恢复通知，例如收到了异常监控数据，但是异常恢复之后没有恢复的通知 + *

+ */ +public class AutopilotAbility350And360 { + private static final String TAG = AutopilotAbility250.class.getSimpleName(); + private ChassisStatesOuterClass.ChassisStates chassisStates; + private int masterVersion = -1;//Master版本 + private OnAutopilotAbilityListener listener; + + protected interface OnAutopilotAbilityListener { + void onAutopilotAbility(boolean isAutopilotAbility, @Nullable ArrayList unableAutopilotReasons); + + } + + protected AutopilotAbility350And360() { + this.masterVersion = -1; + } + + + protected void setStatusInfo(SsmInfo.SsmStatusInf statusInfo) { + + onCallback(statusInfo); + } + + protected void setChassisStates(ChassisStatesOuterClass.ChassisStates chassisStates) { + this.chassisStates = chassisStates; + } + + private void onCallback(SsmInfo.SsmStatusInf statusInfo) { + boolean isAutopilotAbility = true;//是否能启动自动驾驶 + ArrayList unableAutopilotReasons = null;//不能启动自动驾驶原因 + //检测节点状态相关 + if (statusInfo != null) { + if (masterVersion == -1 && statusInfo.hasMasterVersion()) { + //截取Master Version + masterVersion = AdasManager.getInstance().parseVersion(false, statusInfo.getMasterVersion()); + } + CupidLogUtils.i(TAG, "MasterVersion=" + masterVersion); + //SSM 3版本兼容 + if (masterVersion > 2 && statusInfo.hasAutoPilotReady() && statusInfo.hasAutoPilotUnreadyReason()) { + isAutopilotAbility = statusInfo.getAutoPilotReady(); + if (!isAutopilotAbility) { + unableAutopilotReasons = AutopilotAbilityManager.getInstance().addUnableAutopilotReason(unableAutopilotReasons, UnableAutopilotReason.SourceType.SSM, statusInfo.getAutoPilotUnreadyReason()); + } + } else if (masterVersion > 1 && statusInfo.hasAutoPilotReady()) {//如果 maser version 大于1，还需要判断AutoPilotReady字段是否存在，以确保MAP版本和SSM Maser版本不陪配情况逻辑能正常执行 + isAutopilotAbility = statusInfo.getAutoPilotReady(); + if (!isAutopilotAbility) { + int count = statusInfo.getAutoPilotUnreadyListCount(); + if (count > 0) { + for (int i = 0; i < count; i++) { + SsmInfo.NodeInf info = statusInfo.getAutoPilotUnreadyList(i); + String nodeName = info.getNodeName(); + int state = info.getState().getNumber(); + if (state < AutopilotAbilityManager.NODE_INFO_STATE_FIXED_FREQUENCY.length) { + unableAutopilotReasons = AutopilotAbilityManager.getInstance().addUnableAutopilotReason(unableAutopilotReasons, UnableAutopilotReason.SourceType.SSM, nodeName + AutopilotAbilityManager.NODE_INFO_STATE_FIXED_FREQUENCY[state]); + } else { + unableAutopilotReasons = AutopilotAbilityManager.getInstance().addUnableAutopilotReason(unableAutopilotReasons, UnableAutopilotReason.SourceType.SSM, nodeName + "未知异常"); + } + } + } else { + unableAutopilotReasons = AutopilotAbilityManager.getInstance().addUnableAutopilotReason(unableAutopilotReasons, UnableAutopilotReason.SourceType.SSM, "未知异常节点"); + } + } + } else { + SsmInfo.ModeState modeState = statusInfo.getModeState(); + if (modeState != SsmInfo.ModeState.MODE_RUN_UNREADY && modeState != SsmInfo.ModeState.MODE_RUN_READY) { + isAutopilotAbility = false; + String unableAutopilotReason = null; + if (modeState == SsmInfo.ModeState.MODE_STOP_UNREADY) { + unableAutopilotReason = "系统处于停止模式（未就绪）"; + } else if (modeState == SsmInfo.ModeState.MODE_STOP_READY) { + unableAutopilotReason = "系统处于停止模式"; + } else if (modeState == SsmInfo.ModeState.MODE_IDLE_UNREADY) { + unableAutopilotReason = "系统处于空闲模式（未就绪）"; + } else if (modeState == SsmInfo.ModeState.MODE_IDLE_READY) { + unableAutopilotReason = "系统处于空闲模式"; + } else { + unableAutopilotReason = "未知系统模式"; + } + unableAutopilotReasons = AutopilotAbilityManager.getInstance().addUnableAutopilotReason(unableAutopilotReasons, UnableAutopilotReason.SourceType.SSM, unableAutopilotReason); + } + } + } else { + isAutopilotAbility = false;//是否能启动自动驾驶 + unableAutopilotReasons = AutopilotAbilityManager.getInstance().addUnableAutopilotReason(unableAutopilotReasons, UnableAutopilotReason.SourceType.LIB, "SSM数据异常"); + } + //检测底盘相关 + if (chassisStates != null) { + if (chassisStates.hasBrakeSystemStates()) { + float brake = chassisStates.getBrakeSystemStates().getBrakePedalResponsePosition(); + if (brake > 0) { + isAutopilotAbility = false; + unableAutopilotReasons = AutopilotAbilityManager.getInstance().addUnableAutopilotReason(unableAutopilotReasons, UnableAutopilotReason.SourceType.CHASSIS, "制动踏板被踩下"); + } + } + if (chassisStates.hasGearSystemStates()) { + Chassis.GearPosition gear = chassisStates.getGearSystemStates().getGearPosition(); + if (!AutopilotAbilityManager.getInstance().isLaunchAutopilot(gear)) { + isAutopilotAbility = false; + unableAutopilotReasons = AutopilotAbilityManager.getInstance().addUnableAutopilotReason(unableAutopilotReasons, UnableAutopilotReason.SourceType.CHASSIS, "档位异常"); + } + } + //TODO 关于手刹：不同车型的实现不同所以目前没法使用此字段 + +// //电子驻车制动系统 +// if (chassisStates.hasEpbSystemStates()) { +// ChassisStatesOuterClass.EPBSystemStates epb = chassisStates.getEpbSystemStates(); +// if (epb.hasEpbEnableState()){ +// epb.getEpbWorkState(); +// } +// } + + } + if (listener != null) { + listener.onAutopilotAbility(isAutopilotAbility, unableAutopilotReasons); + } + } + + protected void start(OnAutopilotAbilityListener listener) { + this.listener = listener; + } + + protected void stop() { + this.chassisStates = null; + this.masterVersion = -1; + this.listener = null; + } + + +} diff --git a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/common/autopilot/ability/AutopilotAbility360.java b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/common/autopilot/ability/AutopilotAbility360.java new file mode 100644 index 0000000000..37ea146528 --- /dev/null +++ b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/common/autopilot/ability/AutopilotAbility360.java @@ -0,0 +1,134 @@ +package com.zhidao.support.adas.high.common.autopilot.ability; + +import android.text.TextUtils; + +import androidx.annotation.Nullable; + +import com.zhidao.support.adas.high.AdasManager; +import com.zhidao.support.adas.high.common.CupidLogUtils; +import com.zhjt.mogo.adas.data.bean.UnableAutopilotReason; + +import java.util.ArrayList; + +import function_state_management.FSMStatusReasonQueryOuterClass; +import system_master.SsmInfo; + +/** + * 是否可以启动自动驾驶能力检测 工控机版本>=360&&isFutianSweeper 使用此类 + * TODO 目前只用于清扫车，其他车型需要MAP支持 FSM的相关功能 + * 目前监控了定频SSM的数据和FSM状态原因查询 + * 没有使用监控事件报告的原因是因为，部分异常没进行正常恢复通知，例如收到了异常监控数据，但是异常恢复之后没有恢复的通知 + */ +public class AutopilotAbility360 { + private static final String TAG = AutopilotAbility360.class.getSimpleName(); + + private volatile FSMStatusReasonQueryOuterClass.FSMStatusReasonRespond fsmStatusReasonRespond;//自动驾驶状态为OFF的原因 + private int masterVersion = -1;//Master版本 + private OnAutopilotAbilityListener listener; + + protected interface OnAutopilotAbilityListener { + void onAutopilotAbility(boolean isAutopilotAbility, @Nullable ArrayList unableAutopilotReasons); + + + } + + protected AutopilotAbility360() { + this.masterVersion = -1; + } + + protected synchronized void setFsmStatusReasonRespond(FSMStatusReasonQueryOuterClass.FSMStatusReasonRespond fsmStatusReasonRespond) { + this.fsmStatusReasonRespond = fsmStatusReasonRespond; + } + + protected void setStatusInfo(SsmInfo.SsmStatusInf statusInfo) { + onCallback(statusInfo); + } + + + private void onCallback(SsmInfo.SsmStatusInf statusInfo) { + boolean isAutopilotAbility = true;//是否能启动自动驾驶 + ArrayList unableAutopilotReasons = null;//不能启动自动驾驶原因 + //检测节点状态相关 + if (statusInfo != null) { + if (masterVersion == -1 && statusInfo.hasMasterVersion()) { + //截取Master Version + masterVersion = AdasManager.getInstance().parseVersion(false, statusInfo.getMasterVersion()); + } + CupidLogUtils.i(TAG, "MasterVersion=" + masterVersion); + //SSM 3版本兼容 + if (masterVersion > 2 && statusInfo.hasAutoPilotReady() && statusInfo.hasAutoPilotUnreadyReason()) { + isAutopilotAbility = statusInfo.getAutoPilotReady(); + if (!isAutopilotAbility) { + unableAutopilotReasons = AutopilotAbilityManager.getInstance().addUnableAutopilotReason(unableAutopilotReasons, UnableAutopilotReason.SourceType.SSM, statusInfo.getAutoPilotUnreadyReason()); + } + } else if (masterVersion > 1 && statusInfo.hasAutoPilotReady()) {//如果 maser version 大于1，还需要判断AutoPilotReady字段是否存在，以确保MAP版本和SSM Maser版本不陪配情况逻辑能正常执行 + isAutopilotAbility = statusInfo.getAutoPilotReady(); + if (!isAutopilotAbility) { + int count = statusInfo.getAutoPilotUnreadyListCount(); + if (count > 0) { + for (int i = 0; i < count; i++) { + SsmInfo.NodeInf info = statusInfo.getAutoPilotUnreadyList(i); + String nodeName = info.getNodeName(); + int state = info.getState().getNumber(); + if (state < AutopilotAbilityManager.NODE_INFO_STATE_FIXED_FREQUENCY.length) { + unableAutopilotReasons = AutopilotAbilityManager.getInstance().addUnableAutopilotReason(unableAutopilotReasons, UnableAutopilotReason.SourceType.SSM, nodeName + AutopilotAbilityManager.NODE_INFO_STATE_FIXED_FREQUENCY[state]); + } else { + unableAutopilotReasons = AutopilotAbilityManager.getInstance().addUnableAutopilotReason(unableAutopilotReasons, UnableAutopilotReason.SourceType.SSM, nodeName + "未知异常"); + } + } + } else { + unableAutopilotReasons = AutopilotAbilityManager.getInstance().addUnableAutopilotReason(unableAutopilotReasons, UnableAutopilotReason.SourceType.SSM, "未知异常节点"); + } + } + } else { + SsmInfo.ModeState modeState = statusInfo.getModeState(); + if (modeState != SsmInfo.ModeState.MODE_RUN_UNREADY && modeState != SsmInfo.ModeState.MODE_RUN_READY) { + isAutopilotAbility = false; + String unableAutopilotReason = null; + if (modeState == SsmInfo.ModeState.MODE_STOP_UNREADY) { + unableAutopilotReason = "系统处于停止模式（未就绪）"; + } else if (modeState == SsmInfo.ModeState.MODE_STOP_READY) { + unableAutopilotReason = "系统处于停止模式"; + } else if (modeState == SsmInfo.ModeState.MODE_IDLE_UNREADY) { + unableAutopilotReason = "系统处于空闲模式（未就绪）"; + } else if (modeState == SsmInfo.ModeState.MODE_IDLE_READY) { + unableAutopilotReason = "系统处于空闲模式"; + } else { + unableAutopilotReason = "未知系统模式"; + } + unableAutopilotReasons = AutopilotAbilityManager.getInstance().addUnableAutopilotReason(unableAutopilotReasons, UnableAutopilotReason.SourceType.SSM, unableAutopilotReason); + } + } + } else { + isAutopilotAbility = false;//是否能启动自动驾驶 + unableAutopilotReasons = AutopilotAbilityManager.getInstance().addUnableAutopilotReason(unableAutopilotReasons, UnableAutopilotReason.SourceType.LIB, "SSM数据异常"); + } + if (fsmStatusReasonRespond != null) { + int count = fsmStatusReasonRespond.getFsmStatusReasonRespondCount(); + if (count > 0) { + isAutopilotAbility = false; + for (int i = 0; i < count; i++) { + String respond = fsmStatusReasonRespond.getFsmStatusReasonRespond(i); + if (!TextUtils.isEmpty(respond)) { + unableAutopilotReasons = AutopilotAbilityManager.getInstance().addUnableAutopilotReason(unableAutopilotReasons, UnableAutopilotReason.SourceType.FSM, respond); + } + } + if (unableAutopilotReasons == null || unableAutopilotReasons.isEmpty()) { + unableAutopilotReasons = AutopilotAbilityManager.getInstance().addUnableAutopilotReason(unableAutopilotReasons, UnableAutopilotReason.SourceType.LIB, "FSM数据异常"); + } + } + } + if (listener != null) { + listener.onAutopilotAbility(isAutopilotAbility, unableAutopilotReasons); + } + } + + protected void start(OnAutopilotAbilityListener listener) { + this.listener = listener; + } + + protected void stop() { + this.masterVersion = -1; + this.listener = null; + } +} diff --git a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/common/autopilot/ability/AutopilotAbilityManager.java b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/common/autopilot/ability/AutopilotAbilityManager.java index 14e567760e..08b2f45622 100644 --- a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/common/autopilot/ability/AutopilotAbilityManager.java +++ b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/common/autopilot/ability/AutopilotAbilityManager.java @@ -2,6 +2,7 @@ package com.zhidao.support.adas.high.common.autopilot.ability; import android.os.Handler; import android.os.Message; +import android.util.Log; import androidx.annotation.Nullable; @@ -20,6 +21,7 @@ import chassis.Chassis; import chassis.ChassisStatesOuterClass; import function_state_management.FSMStatusReasonQueryOuterClass; import mogo.telematics.pad.MessagePad; +import system_master.SsmInfo; import system_master.SystemStatusInfo; /** @@ -29,21 +31,26 @@ import system_master.SystemStatusInfo; *

* 此定时器不能停止 鹰眼中存在UI更新依赖循环查询系统状态 */ -public class AutopilotAbilityManager implements AutopilotAbility230.OnAutopilotAbilityListener, AutopilotAbility250.OnAutopilotAbilityListener, AutopilotAbility330.OnAutopilotAbilityListener { +public class AutopilotAbilityManager implements AutopilotAbility230.OnAutopilotAbilityListener, AutopilotAbility250.OnAutopilotAbilityListener, AutopilotAbility330.OnAutopilotAbilityListener, AutopilotAbility350And360.OnAutopilotAbilityListener, AutopilotAbility360.OnAutopilotAbilityListener { private static final String TAG = AutopilotAbilityManager.class.getSimpleName(); protected static final int WHAT_TIMEOUT = 0; protected static final int DEFAULT_TIMEOUT = 2500; protected static final long DEFAULT_DETECTION_TIME = 3 * 1000L;//默认检测周期 - protected static final String[] NODE_INFO_STATE = {"未知状态 ", "依赖未就绪 ", "启动中 ", "运行 ", "停止 ", "无法启动状态 ", "人为启动状态 ", "人为关闭状态 "}; + protected static final String[] NODE_INFO_STATE = {"未知状态", "依赖未就绪", "启动中", "运行", "停止", "无法启动状态", "人为启动状态", "人为关闭状态"}; + protected static final String[] NODE_INFO_STATE_FIXED_FREQUENCY = {"未知状态", "依赖未就绪", "启动中", "运行", "停止", "无法启动状态", "非自动启动状态", "非自动关闭状态"}; private static volatile AutopilotAbilityManager INSTANCE; private OnAdasListener listener; private Handler handler; private OnAutopilotAbilityListener onAutopilotAbilityListener; private int mapVersion = -1;//工控机版本 private boolean isFutianSweeper = false;//是否是福田清扫车 + private boolean isJinlvM1 = false;//是否是M1 + private boolean isHQ = false;//是否是HQ private AutopilotAbility230 autopilotAbility230; private AutopilotAbility250 autopilotAbility250; private AutopilotAbility330 autopilotAbility330; + private AutopilotAbility350And360 autopilotAbility350And360; + private AutopilotAbility360 autopilotAbility360; private Timer startTimer; /** * 不能启动自动驾驶的档位 @@ -106,6 +113,8 @@ public class AutopilotAbilityManager implements AutopilotAbility230.OnAutopilotA public void setCarConfig(MessagePad.CarConfigResp carConfig) { mapVersion = carConfig.getMapVersion(); isFutianSweeper = carConfig.getIsFutianSweeper(); + isJinlvM1 = carConfig.getIsJinlvM1(); + isHQ = carConfig.getIsHQ(); if (mapVersion != -1) { stopTimer(); CupidLogUtils.i(TAG, "工控机版本=" + mapVersion); @@ -171,6 +180,15 @@ public class AutopilotAbilityManager implements AutopilotAbility230.OnAutopilotA } } + public void setStatusInfo(SsmInfo.SsmStatusInf statusInfo) { + if (autopilotAbility350And360 != null) { + autopilotAbility350And360.setStatusInfo(statusInfo); + } + if (autopilotAbility360 != null) { + autopilotAbility360.setStatusInfo(statusInfo); + } + } + /** * 底盘状态更新 * @@ -183,6 +201,9 @@ public class AutopilotAbilityManager implements AutopilotAbility230.OnAutopilotA if (autopilotAbility250 != null) { autopilotAbility250.setChassisStates(chassisStates); } + if (autopilotAbility350And360 != null) { + autopilotAbility350And360.setChassisStates(chassisStates); + } } /** @@ -194,6 +215,9 @@ public class AutopilotAbilityManager implements AutopilotAbility230.OnAutopilotA if (autopilotAbility330 != null) { autopilotAbility330.setFsmStatusReasonRespond(fsmStatusReasonRespond); } + if (autopilotAbility360 != null) { + autopilotAbility360.setFsmStatusReasonRespond(fsmStatusReasonRespond); + } } public void onHandleMessage(Message msg) { @@ -208,27 +232,54 @@ public class AutopilotAbilityManager implements AutopilotAbility230.OnAutopilotA } private void initAutopilotAbility() { - //目前只有MAP330的清扫车用的新的FSM状态原因查询 - if (mapVersion >= 30300 && isFutianSweeper) { - CupidLogUtils.i(TAG, "是否可以启动自动驾驶能力检测使用330版本"); + if (mapVersion >= 30600 && isFutianSweeper) { + Log.i(TAG, "能否启动自驾能力检测使用版本：360清扫车专用"); stop230(); stop250(); + stop330(); + stop350And360(); + if (autopilotAbility360 == null) { + autopilotAbility360 = new AutopilotAbility360(); + autopilotAbility360.start(this); + } + + } else if ((mapVersion >= 30500 && (isJinlvM1 || isHQ)) || mapVersion >= 30600) { + Log.i(TAG, "能否启动自驾能力检测使用版本：350和360共用"); + stop230(); + stop250(); + stop330(); + stop360(); + if (autopilotAbility350And360 == null) { + autopilotAbility350And360 = new AutopilotAbility350And360(); + autopilotAbility350And360.start(this); + } + + } else if (mapVersion >= 30300 && isFutianSweeper) {//目前只有MAP330的清扫车用的新的FSM状态原因查询 + Log.i(TAG, "能否启动自驾能力检测使用版本：330清扫车专用"); + stop230(); + stop250(); + stop350And360(); + stop360(); if (autopilotAbility330 == null) { autopilotAbility330 = new AutopilotAbility330(mapVersion); autopilotAbility330.start(this); } } else if (mapVersion >= 20500) { - CupidLogUtils.i(TAG, "是否可以启动自动驾驶能力检测使用250版本"); + Log.i(TAG, "能否启动自驾能力检测使用版本：250"); stop230(); stop330(); + stop350And360(); + stop360(); if (autopilotAbility250 == null) { autopilotAbility250 = new AutopilotAbility250(mapVersion); autopilotAbility250.start(this); } } else { - CupidLogUtils.i(TAG, "是否可以启动自动驾驶能力检测使用230版本"); + Log.i(TAG, "能否启动自驾能力检测使用版本：230"); stop250(); stop330(); + stop350And360(); + stop360(); if (autopilotAbility230 == null) { autopilotAbility230 = new AutopilotAbility230(); autopilotAbility230.start(this); @@ -258,6 +309,20 @@ public class AutopilotAbilityManager implements AutopilotAbility230.OnAutopilotA } } + private void stop350And360() { + if (autopilotAbility350And360 != null) { + autopilotAbility350And360.stop(); + autopilotAbility350And360 = null; + } + } + + private void stop360() { + if (autopilotAbility360 != null) { + autopilotAbility360.stop(); + autopilotAbility360 = null; + } + } + private void stopTimer() { if (startTimer != null) { startTimer.cancel(); @@ -280,7 +345,7 @@ public class AutopilotAbilityManager implements AutopilotAbility230.OnAutopilotA initAutopilotAbility(); } } - }, 8000L);//延迟执行，避免刚连接成功后底盘信息无法及时同步 + }, 8000L);//8秒原因：需要后去CarConfig 对象，两个地方调用initAutopilotAbility(); 初始化 一个在这，另一个在setCarConfig(),如果setCarConfig() 证明获取版本还未成功，获取版本会重试3次每次间隔两秒 } } @@ -290,9 +355,13 @@ public class AutopilotAbilityManager implements AutopilotAbility230.OnAutopilotA stop230(); stop250(); stop330(); + stop350And360(); + stop360(); handler = null; mapVersion = -1; isFutianSweeper = false; + isHQ = false; + isJinlvM1 = false; } } diff --git a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/msg/MyMessageFactory.java b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/msg/MyMessageFactory.java index 796296c390..c3195b3de7 100644 --- a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/msg/MyMessageFactory.java +++ b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/msg/MyMessageFactory.java @@ -30,6 +30,7 @@ public class MyMessageFactory implements IMyMessageFactory { private IMsg warnMessage;//预警数据 private IMsg arrivalNotificationMessage;//到站提醒 private IMsg statusQueryRespMessage;//状态查询应答 + private IMsg systemStatusMessage;//定频SSM private IMsg recordDataConfigRespMessage;//数据采集配置应答 private IMsg planningDecisionStateMessage;//planning决策状态 private IMsg obuWarningDataMessage;//工控机透传OBU V2I数据 @@ -161,6 +162,12 @@ public class MyMessageFactory implements IMyMessageFactory { statusQueryRespMessage = new StatusQueryRespMessage(); } return statusQueryRespMessage; + } else if (messageType == MessageType.TYPE_RECEIVE_SSM.typeCode) { + //定频SSM + if (systemStatusMessage == null) { + systemStatusMessage = new SystemStatusMessage(); + } + return systemStatusMessage; } else if (messageType == MessageType.TYPE_RECEIVE_RECORD_DATA_CONFIG_RESP.typeCode) { //数据采集配置应答 if (recordDataConfigRespMessage == null) { 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 2d37f3ced7..987f41b13e 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 @@ -14,6 +14,7 @@ import system_master.SystemStatusInfo; /** * 状态查询应答 */ +@Deprecated//HQ、M1 MAP350开始弃用，其他车型MAP360开始弃用 public class StatusQueryRespMessage extends MyAbstractMessageHandler { @Override diff --git a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/msg/SystemStatusMessage.java b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/msg/SystemStatusMessage.java new file mode 100644 index 0000000000..863ad996d5 --- /dev/null +++ b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/msg/SystemStatusMessage.java @@ -0,0 +1,33 @@ +package com.zhidao.support.adas.high.msg; + +import android.os.SystemClock; + +import com.google.protobuf.InvalidProtocolBufferException; +import com.zhidao.support.adas.high.AdasChannel; +import com.zhidao.support.adas.high.OnAdasListener; +import com.zhidao.support.adas.high.common.CupidLogUtils; +import com.zhidao.support.adas.high.common.autopilot.ability.AutopilotAbilityManager; +import com.zhidao.support.adas.high.protocol.RawData; + +import system_master.SsmInfo; + +/** + * 定频SSM 接口 + */ +public class SystemStatusMessage extends MyAbstractMessageHandler { + + @Override + public void handlerMsg(RawData raw, OnAdasListener adasListener) throws InvalidProtocolBufferException { + SsmInfo.SsmStatusInf statusInfo = SsmInfo.SsmStatusInf.parser().parseFrom(raw.originalData.toByteArray(), raw.getOffsetValue(), raw.getPackageLengthValue() - raw.getOffsetValue()); + AdasChannel.calculateTimeConsumingOnDispatchRaw("定频SSM", raw.receiveTime); + AutopilotAbilityManager.getInstance().setStatusInfo(statusInfo); + long nowTime = 0; + if (CupidLogUtils.isEnableLog()) + nowTime = SystemClock.elapsedRealtime(); + if (adasListener != null) { + adasListener.onSystemStatus(raw.getHeader(), statusInfo); + } + AdasChannel.calculateTimeConsumingBusiness("定频SSM", nowTime); + } + +}