diff --git a/OCH/common/common/src/debug/java/com/mogo/och/common/module/debug/DebugDataDispatch.kt b/OCH/common/common/src/debug/java/com/mogo/och/common/module/debug/DebugDataDispatch.kt index e8fed9560d..c1189c762b 100644 --- a/OCH/common/common/src/debug/java/com/mogo/och/common/module/debug/DebugDataDispatch.kt +++ b/OCH/common/common/src/debug/java/com/mogo/och/common/module/debug/DebugDataDispatch.kt @@ -150,7 +150,7 @@ object DebugDataDispatch { stateAutopilot -> { val autopilotState = intent.getIntExtra("autopilotState", 0) val autopilotMode = intent.getIntExtra("autopilotMode", 0) - CallerAutoPilotStatusListenerManager.updateAutoPilotStatus(autopilotState,autopilotMode) + CallerAutoPilotStatusListenerManager.updateAutoPilotStatus(0, autopilotState, autopilotMode) } stateAutopilotFail -> { val newBuilder = MogoReportMsg.MogoReportMessage.newBuilder() 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 7bfb320121..b21ccbd6b8 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 @@ -40,6 +40,7 @@ import com.mogo.eagle.core.function.call.autopilot.CallerAutoPilotStatusListener 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.CallerAutoPilotStatusListenerManager.updateAutoPilotStatusFromCan import com.mogo.eagle.core.function.call.autopilot.CallerAutopilotActionsListenerManager import com.mogo.eagle.core.function.call.autopilot.CallerAutopilotActionsListenerManager.invokeAutopilotAbility import com.mogo.eagle.core.function.call.autopilot.CallerAutopilotCarConfigListenerManager.invokeAutopilotCarConfigData @@ -82,6 +83,7 @@ import com.mogo.eagle.core.utilcode.mogo.logger.CallerLogger import com.mogo.eagle.core.utilcode.mogo.logger.scene.SceneConstant.Companion.M_D_C import com.mogo.eagle.core.utilcode.util.DeviceUtils import com.mogo.support.obu.ObuScene +import com.zhidao.support.adas.high.AdasManager import com.zhidao.support.adas.high.OnAdasListener import com.zhidao.support.adas.high.chain.AdasChain import com.zhjt.mogo.adas.data.bean.AdasParam @@ -277,8 +279,12 @@ class MoGoAdasListenerImpl : OnAdasListener { autopilotState: MessagePad.AutopilotState? ) { if (autopilotState != null) { - //工控机模拟时间 - updateAutoPilotStatus(autopilotState.state, autopilotState.autopilotMode) + updateAutoPilotStatusFromCan(autopilotState.state) + if (AdasManager.getInstance() + .getNodeStateInfo(AdasConstants.NodeName.FSM2024).existState != NodeExistState.NODE_EXIST_NORMAL + ) { + updateAutoPilotStatus(1, autopilotState.state, autopilotState.autopilotMode) + } } } @@ -472,10 +478,27 @@ class MoGoAdasListenerImpl : OnAdasListener { linkChainLog = CHAIN_TYPE_SOCKET_AUTOPILOT, linkCode = CHAIN_SOURCE_ADAS, nodeAliasCode = CHAIN_CODE_ADAS_FSM_MSG, - paramIndexes = [0, 1] + paramIndexes = [0, 1, 2] ) - override fun onFSM2024State(header: MessagePad.Header, fsmState: Fsm2024.FSMStateMsg) { + override fun onFSM2024State( + header: MessagePad.Header, + fsmState: Fsm2024.FSMStateMsg, + autopilotState: Int, + autopilotMode: Int + ) { CallerFsm2024ListenerManager.invokeFSM2024State(fsmState) +// val mode: Int = if (fsmState.functionState != Fsm2024.State.ACTIVE) { +// 0//人工驾驶 +// } else { +// if (fsmState.activeMode == Fsm2024.ActiveMode.PILOT_ACTIVE) { +// 1//自动驾驶 +// } else if (fsmState.activeMode == Fsm2024.ActiveMode.PARALLEL_ACTIVE || fsmState.activeMode.number == 6) { +// 6//远程驾驶 +// } else { +// 0//人工驾驶 +// } +// } + updateAutoPilotStatus(2, autopilotState, autopilotMode) } /** 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 0e9aa30ad7..e8c5b59f6a 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 @@ -82,6 +82,7 @@ class MoGoAdasMsgConnectStatusListenerImpl : //与工控机断开连接，需要重置自动驾驶状态(包括上传至云平台缓存信息)，等待连接成功后同步状态信息 CallerAutoPilotStatusListenerManager.getAutoPilotStatusInfo().state = 0 CallerAutoPilotStatusListenerManager.getAutoPilotStatusInfo().pilotmode = 0 + CallerAutoPilotStatusListenerManager.getAutoPilotStatusInfo().autopilotStateSource = 0 isReceivedBasicInfoReq.set(false) isSentBasicInfoReq.set(false) } diff --git a/core/mogo-core-data/src/main/java/com/mogo/eagle/core/data/autopilot/AutopilotStatusInfo.kt b/core/mogo-core-data/src/main/java/com/mogo/eagle/core/data/autopilot/AutopilotStatusInfo.kt index a5bc942557..0452b59bd4 100644 --- a/core/mogo-core-data/src/main/java/com/mogo/eagle/core/data/autopilot/AutopilotStatusInfo.kt +++ b/core/mogo-core-data/src/main/java/com/mogo/eagle/core/data/autopilot/AutopilotStatusInfo.kt @@ -58,6 +58,12 @@ open class AutopilotStatusInfo : Serializable, Cloneable { @Volatile var state = 0 + /** + * 自动驾驶状态数据来源 0：默认/未知；1：底盘；2：FSM； + */ + @Volatile + var autopilotStateSource = 0 + /** * 底盘的自动驾驶状态 0非自动驾驶，1自动驾驶 */ 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 770ce62be2..ee52f9c5de 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 @@ -20,6 +20,13 @@ interface IMoGoAutopilotStatusListener { */ fun onAutopilotStatusResponse(state: Int) {} + /** + * 自动驾驶状态信息 数据源来自底盘 不管是否存在FSM 都将进行底盘自驾状态的转发 + * + * @param state 状态信息 + */ + fun onAutopilotStatusResponseFromCan(state: Int) {} + /** * 自动驾驶docker信息 * 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 4c0895ec04..c96fde4939 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 @@ -145,9 +145,23 @@ object CallerAutoPilotStatusListenerManager : CallerBase + if (oldValue != newValue) { + CallerTrace.write("AutoPilotStatusFromCan", mapOf("AutoPilotStatusFromCan" to newValue)) + M_LISTENERS.forEach { + val listener = it.value + listener.onAutopilotStatusResponseFromCan(newValue) + } + } + } - fun updateAutoPilotStatus(autopilotState: Int, autopilotMode: Int) { + fun updateAutoPilotStatusFromCan(autopilotState: Int) { + this.autopilotStateFromCan = autopilotState + } + + fun updateAutoPilotStatus(autopilotStateSource: Int,autopilotState: Int, autopilotMode: Int) { mAutopilotStatusInfo.state = autopilotState + mAutopilotStatusInfo.autopilotStateSource = autopilotStateSource this.autopilotState = autopilotState mAutopilotStatusInfo.pilotmode = autopilotMode invokeAutoPilotStatus() 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 250bc34d58..4f487ac081 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 @@ -243,10 +243,11 @@ public interface OnAdasListener { /** * FSM状态 * - * @param header 头 - * @param fsmState 数据 + * @param header 头 + * @param fsmState 数据 + * @param autopilotState 自动驾驶状态转换的与can adapter状态一致 */ - void onFSM2024State(@NonNull MessagePad.Header header, @NonNull Fsm2024.FSMStateMsg fsmState); + void onFSM2024State(@NonNull MessagePad.Header header, @NonNull Fsm2024.FSMStateMsg fsmState, int autopilotState, int autopilotMode); /** * 定位状态 diff --git a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/common/AutopilotReview.java b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/common/AutopilotReview.java index 3ad3ca761b..bc93b426f3 100644 --- a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/common/AutopilotReview.java +++ b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/common/AutopilotReview.java @@ -4,12 +4,8 @@ import android.os.SystemClock; import androidx.annotation.NonNull; - import com.zhjt.mogo.adas.data.bean.AutopilotStatistics; -import java.util.Timer; -import java.util.TimerTask; - import mogo.telematics.pad.MessagePad; import mogo_msg.MogoReportMsg; @@ -49,7 +45,6 @@ public class AutopilotReview { } - /** * 自动驾命令 * @@ -80,10 +75,10 @@ public class AutopilotReview { /** * 自动驾驶状态 目前只用于自动驾驶成功结果 * - * @param state + * @param autopilotState */ - public void onAutopilotResult(MessagePad.AutopilotState state) { - if (startReq != null && state != null && state.getState() == 2) { + public void onAutopilotResult(int autopilotState) { + if (startReq != null && autopilotState == 2) { onCallback(AutopilotStatistics.AUTOPILOT_START_STATUS.SUCCESSFUL); } } diff --git a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/msg/AutopilotStateMessage.java b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/msg/AutopilotStateMessage.java index 116832d07d..5dd98f67d2 100644 --- a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/msg/AutopilotStateMessage.java +++ b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/msg/AutopilotStateMessage.java @@ -10,6 +10,7 @@ import com.zhidao.support.adas.high.common.AutopilotReview; import com.zhidao.support.adas.high.common.CupidLogUtils; import com.zhidao.support.adas.high.common.ParallelDrivingManager; import com.zhidao.support.adas.high.protocol.RawData; +import com.zhjt.mogo.adas.data.AdasConstants; import mogo.telematics.pad.MessagePad; @@ -29,15 +30,17 @@ public class AutopilotStateMessage extends MyAbstractMessageHandler { if (AdasManager.getInstance().getCarConfig() == null || !(AdasManager.getInstance().getCarConfig().getMapVersion() >= 330 && AdasManager.getInstance().getCarConfig().getIsFutianSweeper())) { MessagePad.AutopilotState autopilotState = MessagePad.AutopilotState.parser().parseFrom(raw.originalData.toByteArray(), raw.getOffsetValue(), raw.getPackageLengthValue() - raw.getOffsetValue()); AdasChannel.calculateTimeConsumingOnDispatchRaw("自动驾驶状态", raw.receiveTime); - ParallelDrivingManager.getInstance().setAutopilotState(autopilotState.getState()); long nowTime = 0; if (CupidLogUtils.isEnableLog()) nowTime = SystemClock.elapsedRealtime(); if (adasListener != null) { adasListener.onAutopilotState(raw.getHeader(), autopilotState); } - if (autopilotReview != null) { - autopilotReview.onAutopilotResult(autopilotState); + if (AdasManager.getInstance().getNodeStateInfo(AdasConstants.NodeName.FSM2024).getExistState() != AdasConstants.NodeExistState.NODE_EXIST_NORMAL) { + ParallelDrivingManager.getInstance().setAutopilotState(autopilotState.getState()); + if (autopilotReview != null) { + autopilotReview.onAutopilotResult(autopilotState.getState()); + } } AdasChannel.calculateTimeConsumingBusiness("自动驾驶状态", nowTime); // CupidLogUtils.e("自动驾驶状态--->" + autopilotState.toString()); diff --git a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/msg/FSM2024StateMessage.java b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/msg/FSM2024StateMessage.java index 82821b0bfb..ad8e706374 100644 --- a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/msg/FSM2024StateMessage.java +++ b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/msg/FSM2024StateMessage.java @@ -5,7 +5,9 @@ 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.AutopilotReview; import com.zhidao.support.adas.high.common.CupidLogUtils; +import com.zhidao.support.adas.high.common.ParallelDrivingManager; import com.zhidao.support.adas.high.common.autopilot.ability.AutopilotAbilityManager; import com.zhidao.support.adas.high.protocol.RawData; @@ -15,19 +17,46 @@ import fsm.Fsm2024; * FSM状态 */ public class FSM2024StateMessage extends MyAbstractMessageHandler { + private final AutopilotReview autopilotReview; + public FSM2024StateMessage(AutopilotReview autopilotReview) { + this.autopilotReview = autopilotReview; + } @Override public void handlerMsg(RawData raw, OnAdasListener adasListener) throws InvalidProtocolBufferException { Fsm2024.FSMStateMsg fsmState = Fsm2024.FSMStateMsg.parser().parseFrom(raw.originalData.toByteArray(), raw.getOffsetValue(), raw.getPackageLengthValue() - raw.getOffsetValue()); AdasChannel.calculateTimeConsumingOnDispatchRaw("FSM状态", raw.receiveTime); AutopilotAbilityManager.getInstance().setFSM2024State(fsmState); + int mode = getAutopilotState(fsmState); + int autopilotState = mode + 1; long nowTime = 0; if (CupidLogUtils.isEnableLog()) nowTime = SystemClock.elapsedRealtime(); if (adasListener != null) { - adasListener.onFSM2024State(raw.getHeader(), fsmState); + adasListener.onFSM2024State(raw.getHeader(), fsmState, autopilotState, mode); } + ParallelDrivingManager.getInstance().setAutopilotState(autopilotState); + if (autopilotReview != null) { + autopilotReview.onAutopilotResult(autopilotState); + } + AdasChannel.calculateTimeConsumingBusiness("FSM状态", nowTime); } + private static int getAutopilotState(Fsm2024.FSMStateMsg fsmState) { + int mode; + if (fsmState.getFunctionState() != Fsm2024.State.ACTIVE) { + mode = 0;//人工驾驶 + } else { + if (fsmState.getActiveMode() == Fsm2024.ActiveMode.PILOT_ACTIVE) { + mode = 1;//自动驾驶 + } else if (fsmState.getActiveMode() == Fsm2024.ActiveMode.PARALLEL_ACTIVE || fsmState.getActiveMode().getNumber() == 6) { + mode = 6;//远程驾驶 + } else { + mode = 0;//人工驾驶 + } + } + return mode; + } + } diff --git a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/msg/FunctionStatesMessage.java b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/msg/FunctionStatesMessage.java index 435f84dff3..41485d8854 100644 --- a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/msg/FunctionStatesMessage.java +++ b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/msg/FunctionStatesMessage.java @@ -6,7 +6,9 @@ import com.google.protobuf.InvalidProtocolBufferException; import com.zhidao.support.adas.high.AdasChannel; import com.zhidao.support.adas.high.AdasManager; import com.zhidao.support.adas.high.OnAdasListener; +import com.zhidao.support.adas.high.common.AutopilotReview; import com.zhidao.support.adas.high.common.CupidLogUtils; +import com.zhidao.support.adas.high.common.ParallelDrivingManager; import com.zhidao.support.adas.high.common.autopilot.ability.AutopilotAbilityManager; import com.zhidao.support.adas.high.protocol.RawData; @@ -26,10 +28,11 @@ public class FunctionStatesMessage extends MyAbstractMessageHandler { private final Map modeMap = new HashMap<>(); private final AtomicLong timesCount = new AtomicLong(0);//几次，此接口1秒20次 private final AtomicInteger oldMode = new AtomicInteger(0); - + private final AutopilotReview autopilotReview; private FunctionStates.PilotDrivingFunctionState oldPilotDrivingFunctionState = null;//上次自动驾驶状态 - public FunctionStatesMessage() { + public FunctionStatesMessage(AutopilotReview autopilotReview) { + this.autopilotReview = autopilotReview; initModeMap(); } @@ -102,7 +105,10 @@ public class FunctionStatesMessage extends MyAbstractMessageHandler { private void sendAutopilotState(int mode, MessagePad.Header header, OnAdasListener adasListener) { MessagePad.AutopilotState autopilotState = MessagePad.AutopilotState.newBuilder().setAutopilotMode(mode).setState(mode + 1).build(); adasListener.onAutopilotState(header.toBuilder().setMsgType(MessagePad.MessageType.MsgTypeAutopilotState).build(), autopilotState); - + ParallelDrivingManager.getInstance().setAutopilotState(autopilotState.getState()); + if (autopilotReview != null) { + autopilotReview.onAutopilotResult(autopilotState.getState()); + } } private void initModeMap() { 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 6945df42d2..798042aada 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 @@ -185,7 +185,7 @@ public class MyMessageFactory implements IMyMessageFactory { } else if (messageType == MessageType.TYPE_RECEIVE_FSM2024_STATE.typeCode) { //FSM状态 if (fSM2024StateMessage == null) { - fSM2024StateMessage = new FSM2024StateMessage(); + fSM2024StateMessage = new FSM2024StateMessage(autopilotReview); } return fSM2024StateMessage; } else if (messageType == MessageType.TYPE_RECEIVE_LOC_STATE.typeCode) { @@ -221,7 +221,7 @@ public class MyMessageFactory implements IMyMessageFactory { } else if (messageType == MessageType.TYPE_RECEIVE_FUNCTION_STATES.typeCode) { //重构后的功能状态 if (functionStatesMessage == null) { - functionStatesMessage = new FunctionStatesMessage(); + functionStatesMessage = new FunctionStatesMessage(autopilotReview); } return functionStatesMessage; } else if (messageType == MessageType.TYPE_RECEIVE_BAG_MANAGER_CMD.typeCode) {