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 033f7abf8b..c04b1ac032 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 @@ -74,6 +74,7 @@ import com.zhjt.mogo.adas.data.sweeper.task.status.SweeperTaskStatus import com.zhjt.mogo.adas.data.sweeper.task.stop.SweeperTaskStop import com.zhjt.service.chain.ChainLog import fault_management.FmInfo +import fsm.Fsm2024 import function_state_management.FunctionStates import localization.LocState import mogo.telematics.pad.MessagePad @@ -492,6 +493,17 @@ class MoGoAdasListenerImpl : OnAdasListener { CallerFaultManagementStateListenerManager.invokeFaultManagementState(fmInfo) } + + /** + * FSM状态 + * 目前在启动自驾前置条件检测440版本中使用 + * @param header 头 + * @param fsmState 数据 + */ + override fun onFSM2024State(header: MessagePad.Header, fsmState: Fsm2024.FSMStateMsg) { + CallerFsm2024ListenerManager.invokeFSM2024State(fsmState) + } + /** * 定位状态 * 定位呈现状态透传 用于pad图标显示 1hz 所有车型MAP440开始支持 @@ -997,10 +1009,26 @@ class MoGoAdasListenerImpl : OnAdasListener { CallerV2nNioEventListenerManager.invokeV2nNioCongestionEvent(congestion) } + /** + * 域控SSM接口接收超时 + * 状态变动时才会回调，默认SSM状态正常 + * + * @param isTimeout true：SSM接口接收超时 false：SSM接口恢复正常 + */ override fun onSsmReceiveTimeout(isTimeout: Boolean) { CallerAutoPilotStatusListenerManager.invokeSsmReceiveTimeout(isTimeout) } + /** + * 域控FSM接口接收超时 + * 状态变动时才会回调，默认FSM状态正常 前提是存在FSM接口 + * + * @param isTimeout true：FSM接口接收超时 false：FSM接口恢复正常 + */ + override fun onFsm2024ReceiveTimeout(isTimeout: Boolean) { + CallerAutoPilotStatusListenerManager.invokeFsmReceiveTimeout(isTimeout) + } + /** * 是否可以启动自动驾驶 * 使用方法查看：app_ipc_monitoring/uiMainActivity/onAutopilotAbility 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 e8cba74ab1..b51a0ad17e 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 @@ -56,6 +56,14 @@ interface IMoGoAutopilotStatusListener { */ fun onSsmReceiveTimeout(isTimeout: Boolean) {} + /** + * 域控FSM接口接收超时 + * 状态变动时才会回调，默认FSM状态正常 前提是存在FSM接口 + * + * @param isTimeout true：FSM接口接收超时 false：FSM接口恢复正常 + */ + fun onFsmReceiveTimeout(isTimeout: Boolean) {} + /** * 工控机主动查询 AdasManager#sendStatusQueryReq()，后会收到如下回调 */ diff --git a/core/mogo-core-function-api/src/main/java/com/mogo/eagle/core/function/api/autopilot/IMoGoFsm2024Listener.kt b/core/mogo-core-function-api/src/main/java/com/mogo/eagle/core/function/api/autopilot/IMoGoFsm2024Listener.kt new file mode 100644 index 0000000000..3da37890b7 --- /dev/null +++ b/core/mogo-core-function-api/src/main/java/com/mogo/eagle/core/function/api/autopilot/IMoGoFsm2024Listener.kt @@ -0,0 +1,18 @@ +package com.mogo.eagle.core.function.api.autopilot + +import fsm.Fsm2024 + +/** + * 新版FSM状态 + */ +interface IMoGoFsm2024Listener { + + /** + * FSM状态 + * 目前在启动自驾前置条件检测440版本中使用 + * @param header 头 + * @param fsmState 数据 + */ + fun onFSM2024State(fsmState: Fsm2024.FSMStateMsg) + +} \ No newline at end of file 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 95df93ec34..d2bf8ec4f0 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 @@ -236,6 +236,19 @@ object CallerAutoPilotStatusListenerManager : CallerBase() { + + /** + * 绿波通行（单路口）事件推送, 透传 + */ + fun invokeFSM2024State(fsmState: Fsm2024.FSMStateMsg) { + M_LISTENERS.forEach { + val listener = it.value + listener.onFSM2024State(fsmState) + } + } + + +} \ No newline at end of file diff --git a/libraries/mogo-adas-data/src/main/java/com/zhjt/mogo/adas/common/MessageType.java b/libraries/mogo-adas-data/src/main/java/com/zhjt/mogo/adas/common/MessageType.java index 5001453be1..623bb4d14d 100644 --- a/libraries/mogo-adas-data/src/main/java/com/zhjt/mogo/adas/common/MessageType.java +++ b/libraries/mogo-adas-data/src/main/java/com/zhjt/mogo/adas/common/MessageType.java @@ -30,6 +30,7 @@ public enum MessageType { TYPE_RECEIVE_M1_STITCHED_VIDEO(MessagePad.MessageType.MsgTypeM1StitchedVideo, "M1拼接视频"), TYPE_RECEIVE_SSM(MessagePad.MessageType.MsgTypeSSMState, "SSM系统状态"), TYPE_RECEIVE_FM_STATE(MessagePad.MessageType.MsgTypeFMState, "FM状态"), + TYPE_RECEIVE_FSM2024_STATE(MessagePad.MessageType.MsgTypeFSM2024State, "FSM状态"), TYPE_RECEIVE_LOC_STATE(MessagePad.MessageType.MsgTypeLocState, "定位状态"), TYPE_RECEIVE_BASIC_INFO_REQ(MessagePad.MessageType.MsgTypeBasicInfoReq, "自动驾驶设备基础信息请求"), diff --git a/libraries/mogo-adas-data/src/main/java/com/zhjt/mogo/adas/data/bean/LaunchConditionData.java b/libraries/mogo-adas-data/src/main/java/com/zhjt/mogo/adas/data/bean/LaunchConditionData.java index e906c4f36b..5cb6b0df08 100644 --- a/libraries/mogo-adas-data/src/main/java/com/zhjt/mogo/adas/data/bean/LaunchConditionData.java +++ b/libraries/mogo-adas-data/src/main/java/com/zhjt/mogo/adas/data/bean/LaunchConditionData.java @@ -11,6 +11,7 @@ import org.json.JSONObject; import chassis.Chassis; import chassis.ChassisStatesOuterClass; +import fsm.Fsm2024; import function_state_management.FSMStatusReasonQueryOuterClass; import system_master.SsmInfo; import system_master.SystemStatusInfo; @@ -45,14 +46,19 @@ public class LaunchConditionData { */ public final SsmInfo.SsmStatusInf ssmInfo; public final FSMStatusReasonQueryOuterClass.FSMStatusReasonRespond fsmStatusReasonRespond;//FSM数据 + public final Fsm2024.FSMStateMsg fsmState;//FSM数据 public final long createTime; + public LaunchConditionData(String abilityVersion, Fsm2024.FSMStateMsg fsmState) { + this(abilityVersion, null, Float.MAX_VALUE, null, null, null, null, fsmState); + } + public LaunchConditionData(String abilityVersion, SystemStatusInfo.StatusInfo statusInfo, SsmInfo.SsmStatusInf ssmInfo, FSMStatusReasonQueryOuterClass.FSMStatusReasonRespond fsmStatusReasonRespond) { - this(abilityVersion, null, Float.MAX_VALUE, null, statusInfo, ssmInfo, fsmStatusReasonRespond); + this(abilityVersion, null, Float.MAX_VALUE, null, statusInfo, ssmInfo, fsmStatusReasonRespond, null); } public LaunchConditionData(String abilityVersion, @@ -62,6 +68,17 @@ public class LaunchConditionData { SystemStatusInfo.StatusInfo statusInfo, SsmInfo.SsmStatusInf ssmInfo, FSMStatusReasonQueryOuterClass.FSMStatusReasonRespond fsmStatusReasonRespond) { + this(abilityVersion, chassisStates, oldSteering, light, statusInfo, ssmInfo, fsmStatusReasonRespond, null); + } + + public LaunchConditionData(String abilityVersion, + ChassisStatesOuterClass.ChassisStates chassisStates, + float oldSteering, + Chassis.LightSwitch light, + SystemStatusInfo.StatusInfo statusInfo, + SsmInfo.SsmStatusInf ssmInfo, + FSMStatusReasonQueryOuterClass.FSMStatusReasonRespond fsmStatusReasonRespond, + Fsm2024.FSMStateMsg fsmState) { this.abilityVersion = abilityVersion; this.chassisStates = chassisStates; this.oldSteering = oldSteering; @@ -69,6 +86,7 @@ public class LaunchConditionData { this.statusInfo = statusInfo; this.ssmInfo = ssmInfo; this.fsmStatusReasonRespond = fsmStatusReasonRespond; + this.fsmState = fsmState; createTime = System.currentTimeMillis(); } @@ -78,9 +96,10 @@ public class LaunchConditionData { jsonObject.put("ability_version", abilityVersion); jsonObject.put("create_object_time", createTime); JSONArray array = new JSONArray(); - JSONObject chassisStatesObject = new JSONObject(); - chassisStatesObject.put("name", "ChassisStates"); + JSONObject chassisStatesObject = null; if (chassisStates != null) { + chassisStatesObject = new JSONObject(); + chassisStatesObject.put("name", "ChassisStates"); if (chassisStates.hasHeader()) { chassisStatesObject.put("data_header", TextFormat.printer().escapingNonAscii(false).printToString(chassisStates.getHeader())); } @@ -120,18 +139,27 @@ public class LaunchConditionData { } //车灯 if (light != null) { + if (chassisStatesObject == null) { + chassisStatesObject = new JSONObject(); + chassisStatesObject.put("name", "ChassisStates"); + } chassisStatesObject.put("light", light.name()); } - array.put(chassisStatesObject); + if (chassisStatesObject != null) { + array.put(chassisStatesObject); + } if (statusInfo != null) { - arrayPut(array, statusInfo.getClass().getName(), statusInfo.getAutoPilotReady(), statusInfo.getAutoPilotUnreadyReason()); + arrayPut(array, statusInfo); } if (ssmInfo != null) { - arrayPut(array, ssmInfo.getClass().getName(), ssmInfo.getAutoPilotReady(), ssmInfo.getAutoPilotUnreadyReason()); + arrayPut(array, ssmInfo); } if (fsmStatusReasonRespond != null) { arrayPut(array, fsmStatusReasonRespond); } + if (fsmState != null) { + arrayPut(array, fsmState); + } if (array.length() > 0) { jsonObject.put("data", array); } diff --git a/libraries/mogo-adas-data/src/main/java/com/zhjt/mogo/adas/data/bean/UnableLaunchReason.java b/libraries/mogo-adas-data/src/main/java/com/zhjt/mogo/adas/data/bean/UnableLaunchReason.java index e0b11d41ed..f966ea314e 100644 --- a/libraries/mogo-adas-data/src/main/java/com/zhjt/mogo/adas/data/bean/UnableLaunchReason.java +++ b/libraries/mogo-adas-data/src/main/java/com/zhjt/mogo/adas/data/bean/UnableLaunchReason.java @@ -14,6 +14,7 @@ public class UnableLaunchReason { CHASSIS,//底盤 SSM, FSM, + FSM2024, } /** @@ -31,7 +32,9 @@ public class UnableLaunchReason { CHASSIS_HAZARD_LIGHTS,//危险报警灯 SSM_OFFER,//SSM提供的原因 FSM_OFFER,//FSM提供的原因 - SSM_TIMEOUT//SSM超时 + FSM2024_OFFER,//FSM2024提供的原因 + SSM_TIMEOUT,//SSM超时 + FSM2024_TIMEOUT//FSM2024超时 } /** diff --git a/libraries/mogo-adas-data/src/main/proto/fsm2024.proto b/libraries/mogo-adas-data/src/main/proto/fsm2024.proto new file mode 100644 index 0000000000..ec22d1cc38 --- /dev/null +++ b/libraries/mogo-adas-data/src/main/proto/fsm2024.proto @@ -0,0 +1,53 @@ +syntax = "proto2"; +package fsm; + +import "header.proto"; + +enum State { + OFF = 0; + STANDBY = 1; //Standby 未来会弃用, 并入OFF + ACTIVATING = 2; + PILOT_DRIVING_ACTIVE = 3; //未来会被弃用，所有激活状态统一在Active状态下作为子状态管理 + PARALLEL_DRIVING_ACTIVE = 4; //未来会被弃用，所有激活状态统一在Active状态下作为子状态管理 + SAFETY_STOP = 5; + ACTIVE = 6; +} + +enum SafetyStopMode { + NOT_NEEDED = 0; + PLANNER_STOP = 1; + CONTROLLER_COMFORT_STOP = 2; + CONTROLLER_EMERGENCY_STOP = 3; +} + +enum ActiveMode { + NOT_ACTIVE= 0; + PILOT_ACTIVE= 1; + PARALLEL_ACTIVE= 2; + SIMULATOR_ACTIVE= 3; //模拟器驾驶， M1上专用 +} + +message FSMStateMsg { + required common.Header header = 1; + required State function_state = 2; + required SafetyStopMode fsm_safety_stop_mode = 4; //FSM如果进入safety stop，是planning 进行停车，还是control进行舒适停车，还是control进行紧急停车 + + required ActiveMode active_mode = 6; + + required bool new_msg_flag = 10; + + required bool pilot_standby_flag = 11; + required bool parallel_standby_flag = 12; + required bool simulator_standby_flag = 13; + + optional string pilot_not_standby_reason= 21; //FSM 无法自驾无法就绪的原因。 + optional string parallel_not_standby_reason= 22; //FSM 无法平行即使就绪原因 + optional string simulator_not_standby_reason= 23; //FSM 无法模拟器驾驶原因 + + optional uint64 fail_to_pilot_session_id= 36; + optional uint64 fail_to_parallel_session_id= 37; + optional uint64 fail_to_simulator_session_id= 38; + + optional string abnormal_state_trans_reason= 41; //FSM 异常退出ACTIVATING/自驾/平行驾驶/模拟器驾驶原因 （包括接管） + +} \ No newline at end of file diff --git a/libraries/mogo-adas-data/src/main/proto/message_pad.proto b/libraries/mogo-adas-data/src/main/proto/message_pad.proto index 2cce48229f..85d0e289e4 100644 --- a/libraries/mogo-adas-data/src/main/proto/message_pad.proto +++ b/libraries/mogo-adas-data/src/main/proto/message_pad.proto @@ -35,6 +35,7 @@ enum MessageType MsgTypeM1StitchedVideo = 0x1000e; //m1拼接视频 定频10hz MsgTypeSSMState = 0x1000f; //ssm 系统状态 定频1hz hq m1 MAP350开始支持，其他车型MAP360开始支持 MsgTypeFMState = 0x10010; //FM状态 定频1hz bus和清扫车是MAP370开始支持，其他车型MAP360开始支持 + MsgTypeFSM2024State = 0x10011; //20240531 FSM状态 定频10hz （telematics做状态变化判断，无变化10hz上报，有变化立即上报） BUS MAP440开始支持，其他车型暂未支持 MsgTypeLocState = 0x10012; //定位呈现状态透传 用于pad图标显示 1hz 所有车型MAP440开始支持 //### 以下消息全部不定频 ### @@ -879,5 +880,8 @@ message SessionInfo //message definition for MsgTypeFMState //refer to fm_info.proto for details +//message definition for MsgTypeFSM2024State +//refer to fsm2024.proto + //message definition for MsgTypeLocState //refer to loc_state.proto 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 2b764d3dc7..22a0b6ab0f 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 @@ -30,6 +30,7 @@ import chassis.Chassis; import chassis.ChassisStatesOuterClass; import chassis.VehicleStateOuterClass; import fault_management.FmInfo; +import fsm.Fsm2024; import function_state_management.FunctionStates; import localization.LocState; import mogo.telematics.pad.MessagePad; @@ -238,6 +239,14 @@ public interface OnAdasListener { */ void onFaultManagementState(MessagePad.Header header, @NonNull FmInfo.FaultResultMsg fmInfo); + /** + * FSM状态 + * + * @param header 头 + * @param fsmState 数据 + */ + void onFSM2024State(@NonNull MessagePad.Header header, @NonNull Fsm2024.FSMStateMsg fsmState); + /** * 定位状态 * 定位呈现状态透传 用于pad图标显示 1hz 所有车型MAP440开始支持 @@ -531,6 +540,14 @@ public interface OnAdasListener { */ void onSsmReceiveTimeout(boolean isTimeout); + /** + * 域控FSM接口接收超时 + * 状态变动时才会回调，默认FSM状态正常 前提是存在FSM接口 + * + * @param isTimeout true：FSM接口接收超时 false：FSM接口恢复正常 + */ + void onFsm2024ReceiveTimeout(boolean isTimeout); + /** * 是否有能力启动自动驾驶 * diff --git a/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/common/autopilot/ability/AutopilotAbility440.java b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/common/autopilot/ability/AutopilotAbility440.java new file mode 100644 index 0000000000..b3de145659 --- /dev/null +++ b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/common/autopilot/ability/AutopilotAbility440.java @@ -0,0 +1,62 @@ +package com.zhidao.support.adas.high.common.autopilot.ability; + +import androidx.annotation.NonNull; + +import com.zhjt.mogo.adas.data.bean.LaunchConditionData; +import com.zhjt.mogo.adas.data.bean.UnableLaunchReason; + +import java.util.ArrayList; + +import fsm.Fsm2024; + +/** + * 是否可以启动自动驾驶能力检测 工控机版本>=4400&&(isJinlv||isJinlvM1||isJinlvM) 使用此类 + * 目前监控了FSM2024状态 + */ +public class AutopilotAbility440 { + private final String TAG = this.getClass().getSimpleName(); + + @NonNull + private final AutopilotAbilityManager manager; + private OnAutopilotAbilityListener listener; + + + public AutopilotAbility440(@NonNull AutopilotAbilityManager manager) { + this.manager = manager; + } + + + public void setFSMState(Fsm2024.FSMStateMsg fsmState) { + onCallbackFsm(fsmState); + } + + protected void onCallFsmTimeout() { + onCallbackFsm(null); + } + + protected void onCallbackFsm(Fsm2024.FSMStateMsg fsmState) { + ArrayList unableAutopilotReasons = null;//不能启动自动驾驶原因 + if (fsmState == null) { + unableAutopilotReasons = manager.addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.LIB, UnableLaunchReason.UnableType.FSM2024_TIMEOUT, "FSM超时无响应"); + } else { + if (!fsmState.getPilotStandbyFlag()) { + unableAutopilotReasons = manager.addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.FSM2024, UnableLaunchReason.UnableType.FSM2024_OFFER, fsmState.getPilotNotStandbyReason()); + } + } + if (listener != null) { + boolean isAutopilotAbility = unableAutopilotReasons == null || unableAutopilotReasons.isEmpty();//是否能启动自动驾驶 + listener.onAutopilotAbility(isAutopilotAbility, new LaunchConditionData(this.getClass().getSimpleName(), fsmState), unableAutopilotReasons); + } + } + + + public synchronized void start(OnAutopilotAbilityListener listener) { + this.listener = listener; + + } + + public synchronized void stop() { + 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 cf32f031a1..8cf5651d40 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 @@ -18,6 +18,7 @@ import java.util.concurrent.atomic.AtomicBoolean; import chassis.Chassis; import chassis.ChassisStatesOuterClass; +import fsm.Fsm2024; import function_state_management.FSMStatusReasonQueryOuterClass; import mogo.telematics.pad.MessagePad; import system_master.SsmInfo; @@ -32,7 +33,7 @@ import system_master.SystemStatusInfo; */ public class AutopilotAbilityManager implements OnAutopilotAbilityListener { private static final String TAG = AutopilotAbilityManager.class.getSimpleName(); - protected static final long DEFAULT_SSM_TIMEOUT = 5000L;//SSM超时时间 + protected static final long DEFAULT_TIMEOUT = 5000L;//SSM和FSM超时时间 protected static final long DEFAULT_DETECTION_TIME = 3 * 1000L;//默认检测周期 protected static final String[] NODE_INFO_STATE = {"未知状态", "依赖未就绪", "启动中", "运行", "停止", "无法启动状态", "人为启动状态", "人为关闭状态"}; protected static final String[] NODE_INFO_STATE_FIXED_FREQUENCY = {"未知状态", "依赖未就绪", "启动中", "运行", "停止", "无法启动状态", "非自动启动状态", "非自动关闭状态"}; @@ -41,24 +42,29 @@ public class AutopilotAbilityManager implements OnAutopilotAbilityListener { private OnAdasListener listener; private int mapVersion = -1;//工控机版本 private boolean isFutianSweeper = false;//是否是福田清扫车 + private boolean isJinlv = false;//是否是小巴 private boolean isJinlvM1 = false;//是否是M1 + private boolean isJinlvM2 = false;//是否是M2 private boolean isHQ = false;//是否是HQ private AutopilotAbility230 autopilotAbility230; private AutopilotAbility250 autopilotAbility250; private AutopilotAbility330 autopilotAbility330; private AutopilotAbility350And360 autopilotAbility350And360; private AutopilotAbility360 autopilotAbility360; + private AutopilotAbility440 autopilotAbility440; private Timer startTimer; - private Timer ssmTimeoutTimer;//SSM超时计时器 + private Timer timeoutTimer;//SSM和FSM超时计时器 private long ssmReceiveTime;//SSM接收时间 + private long fsmReceiveTime;//FSM接收时间 private final AtomicBoolean isOldSsmTimeout = new AtomicBoolean(false);//SSM是否超时 老状态 + private final AtomicBoolean isOldFsmTimeout = new AtomicBoolean(false);//FSM是否超时 老状态 private final AtomicBoolean isInitCarConfig = new AtomicBoolean(false);//车辆信息是否初始化 /** * 能启动自动驾驶的档位 */ private Set launchAutopilotGear; - + private boolean isSupportFSM2024 = false; private AutopilotAbilityManager() { } @@ -109,7 +115,9 @@ public class AutopilotAbilityManager implements OnAutopilotAbilityListener { isInitCarConfig.set(true); mapVersion = version; isFutianSweeper = carConfig.getIsFutianSweeper(); + isJinlv = carConfig.getIsJinlv(); isJinlvM1 = carConfig.getIsJinlvM1(); + isJinlvM2 = carConfig.getIsJinlvM2(); isHQ = carConfig.getIsHQ(); initAutopilotAbility(); if (autopilotAbility230 != null) { @@ -165,6 +173,14 @@ public class AutopilotAbilityManager implements OnAutopilotAbilityListener { } } + public void setFSM2024State(Fsm2024.FSMStateMsg fsmState) { + fsmReceiveTime = System.currentTimeMillis(); + onCallFSMTimeout(false); + if (autopilotAbility440 != null) { + autopilotAbility440.setFSMState(fsmState); + } + } + /** * 底盘状态更新 * @@ -197,13 +213,20 @@ public class AutopilotAbilityManager implements OnAutopilotAbilityListener { private void initAutopilotAbility() { stopAllTimer(); - if (mapVersion >= 30600 && isFutianSweeper) { + isSupportFSM2024 = false; + if (mapVersion >= 40400 && (isJinlv || isJinlvM1 || isJinlvM2)) { + isSupportFSM2024 = true; + CupidLogUtils.log(TAG, "能否启动自驾能力检测使用版本：440"); + if (autopilotAbility440 == null) { + autopilotAbility440 = new AutopilotAbility440(this); + autopilotAbility440.start(this); + } + } else if (mapVersion >= 30600 && isFutianSweeper) { CupidLogUtils.log(TAG, "能否启动自驾能力检测使用版本：360清扫车专用"); if (autopilotAbility360 == null) { autopilotAbility360 = new AutopilotAbility360(this); autopilotAbility360.start(this); } - } else if ((mapVersion >= 30500 && (isJinlvM1 || isHQ)) || mapVersion >= 30600) { CupidLogUtils.log(TAG, "能否启动自驾能力检测使用版本：350和360共用"); if (autopilotAbility350And360 == null) { @@ -231,7 +254,7 @@ public class AutopilotAbilityManager implements OnAutopilotAbilityListener { } } if (autopilotAbility230 == null) { - startSsmTimeoutTimer();//MAP230及以下没有SSM所以不需要超时 + startTimeoutTimer();//MAP230及以下没有SSM和FSM所以不需要超时 } } @@ -270,6 +293,13 @@ public class AutopilotAbilityManager implements OnAutopilotAbilityListener { } } + private void stop440() { + if (autopilotAbility440 != null) { + autopilotAbility440.stop(); + autopilotAbility440 = null; + } + } + private void stopTimer() { if (startTimer != null) { startTimer.cancel(); @@ -298,12 +328,14 @@ public class AutopilotAbilityManager implements OnAutopilotAbilityListener { } - private void stopSsmTimeoutTimer() { - if (ssmTimeoutTimer != null) { + private void stopTimeoutTimer() { + if (timeoutTimer != null) { isOldSsmTimeout.set(false); + isOldFsmTimeout.set(false); ssmReceiveTime = 0; - ssmTimeoutTimer.cancel(); - ssmTimeoutTimer = null; + fsmReceiveTime = 0; + timeoutTimer.cancel(); + timeoutTimer = null; } } @@ -311,15 +343,17 @@ public class AutopilotAbilityManager implements OnAutopilotAbilityListener { * 连接工控机成功调用此函数，如果dockerVersion还未获取到将启动最低版本的启动自动驾驶能力检测 * 此函数为保险措施 以防无法获取工控机版本时 也能 正常执行逻辑 */ - public synchronized void startSsmTimeoutTimer() { - if (ssmTimeoutTimer == null) { + public synchronized void startTimeoutTimer() { + if (timeoutTimer == null) { ssmReceiveTime = System.currentTimeMillis(); - ssmTimeoutTimer = new Timer(); - ssmTimeoutTimer.schedule(new TimerTask() { + fsmReceiveTime = System.currentTimeMillis(); + timeoutTimer = new Timer(); + timeoutTimer.schedule(new TimerTask() { @Override public void run() { + //SSM超时检测 long timeDifference = System.currentTimeMillis() - ssmReceiveTime; - if (timeDifference >= DEFAULT_SSM_TIMEOUT) { + if (timeDifference >= DEFAULT_TIMEOUT) { onCallSSMTimeout(true); //超时 if (autopilotAbility250 != null) { @@ -335,8 +369,18 @@ public class AutopilotAbilityManager implements OnAutopilotAbilityListener { autopilotAbility360.onCallTimeout(); } } + //FSM超时检测 + if (isSupportFSM2024) { + timeDifference = System.currentTimeMillis() - fsmReceiveTime; + if (timeDifference >= DEFAULT_TIMEOUT) { + onCallFSMTimeout(true); + if (autopilotAbility440 != null) { + autopilotAbility440.onCallFsmTimeout(); + } + } + } } - }, 1000L, DEFAULT_SSM_TIMEOUT); + }, 1000L, DEFAULT_TIMEOUT); } } @@ -347,14 +391,22 @@ public class AutopilotAbilityManager implements OnAutopilotAbilityListener { } } + private synchronized void onCallFSMTimeout(boolean isTimeout) { + if (isTimeout != isOldFsmTimeout.get()) { + isOldFsmTimeout.set(isTimeout); + listener.onFsm2024ReceiveTimeout(isTimeout); + } + } + private void stopAllTimer() { - stopSsmTimeoutTimer(); + stopTimeoutTimer(); stopTimer(); stop230(); stop250(); stop330(); stop350And360(); stop360(); + stop440(); } public synchronized void stop() { @@ -363,7 +415,10 @@ public class AutopilotAbilityManager implements OnAutopilotAbilityListener { mapVersion = -1; isFutianSweeper = false; isHQ = false; + isJinlv = false; isJinlvM1 = false; + isJinlvM2 = false; + isSupportFSM2024 = false; } } 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 new file mode 100644 index 0000000000..82821b0bfb --- /dev/null +++ b/libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/msg/FSM2024StateMessage.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 fsm.Fsm2024; + +/** + * FSM状态 + */ +public class FSM2024StateMessage extends MyAbstractMessageHandler { + + @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); + long nowTime = 0; + if (CupidLogUtils.isEnableLog()) + nowTime = SystemClock.elapsedRealtime(); + if (adasListener != null) { + adasListener.onFSM2024State(raw.getHeader(), fsmState); + } + AdasChannel.calculateTimeConsumingBusiness("FSM状态", nowTime); + } + +} 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 3843b8093f..6945df42d2 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 @@ -32,6 +32,7 @@ public class MyMessageFactory implements IMyMessageFactory { private IMsg statusQueryRespMessage;//状态查询应答 private IMsg systemStatusMessage;//定频SSM private IMsg faultManagementMessage;//FM状态 + private IMsg fSM2024StateMessage;//FSM状态 private IMsg localizationStateMessage;//定位状态 private IMsg recordDataConfigRespMessage;//数据采集配置应答 private IMsg planningDecisionStateMessage;//planning决策状态 @@ -181,6 +182,12 @@ public class MyMessageFactory implements IMyMessageFactory { faultManagementMessage = new FaultManagementMessage(); } return faultManagementMessage; + } else if (messageType == MessageType.TYPE_RECEIVE_FSM2024_STATE.typeCode) { + //FSM状态 + if (fSM2024StateMessage == null) { + fSM2024StateMessage = new FSM2024StateMessage(); + } + return fSM2024StateMessage; } else if (messageType == MessageType.TYPE_RECEIVE_LOC_STATE.typeCode) { //定位状态 if (localizationStateMessage == null) {