yangyakun921/MoGoEagleEye
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Merge branch 'dev_robo_240612_6.5.0_tmp' of gitlab.zhidaoauto.com:SCA/L4HA/AndroidApp/MoGoEagleEye into dev_robo_240612_6.5.0_tmp

Browse Source 
# 请输入一个提交信息以解释此合并的必要性，尤其是将一个更新后的上游分支
# 合并到主题分支。
#
# 以 '#' 开始的行将被忽略，而空的提交说明将终止提交。
This commit is contained in:
aibingbing
2024-06-14 19:56:52 +08:00
parent 7bd544f387 7d45a59bdb
commit b6bae8e06b
21 changed files with 525 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files

41
core/function-impl/mogo-core-function-datacenter/src/main/java/com/mogo/eagle/core/function/datacenter/autopilot/adapter/MoGoAdasListenerImpl.kt
View File

@@ -74,7 +74,9 @@ 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
import mogo.telematics.pad.MessagePad.TrackedObject
import mogo.v2x.MogoV2X
@@ -491,6 +493,29 @@ 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开始支持
* 详细解释：http://wiki.zhidaohulian.com/pages/viewpage.action?pageId=131757484
*
* @param header 头
* @param locState 数据
*/
override fun onLocalizationState(header: MessagePad.Header?, locState: LocState.loc_state) {
CallerLocalizationStateListenerManager.invokeLocalizationState(locState)
}
/**
* 数据采集配置应答
*/
@@ -984,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

8
core/mogo-core-function-api/src/main/java/com/mogo/eagle/core/function/api/autopilot/IMoGoAutopilotStatusListener.kt
View File

@@ -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()，后会收到如下回调
*/

18
core/mogo-core-function-api/src/main/java/com/mogo/eagle/core/function/api/autopilot/IMoGoFsm2024Listener.kt Normal file
View File

@@ -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)
}

19
core/mogo-core-function-api/src/main/java/com/mogo/eagle/core/function/api/autopilot/IMoGoLocalizationStateListener.kt Normal file
View File

@@ -0,0 +1,19 @@
package com.mogo.eagle.core.function.api.autopilot
import localization.LocState
/**
*定位状态新接口
* 定位呈现状态透传 用于pad图标显示 1hz 所有车型MAP440开始支持
* 详细解释：http://wiki.zhidaohulian.com/pages/viewpage.action?pageId=131757484
*/
interface IMoGoLocalizationStateListener {
/**
* 定位状态
* @param locState 数据
*/
fun onLocalizationState(locState: LocState.loc_state)
}

13
core/mogo-core-function-call/src/main/java/com/mogo/eagle/core/function/call/autopilot/CallerAutoPilotStatusListenerManager.kt
View File

@@ -236,6 +236,19 @@ object CallerAutoPilotStatusListenerManager : CallerBase<IMoGoAutopilotStatusLis
}
}
/**
* 域控FSM接口接收超时
* 状态变动时才会回调，默认FSM状态正常 前提是存在FSM接口
*
* @param isTimeout true：FSM接口接收超时 false：FSM接口恢复正常
*/
fun invokeFsmReceiveTimeout(isTimeout: Boolean) {
M_LISTENERS.forEach {
val listener = it.value
listener.onFsmReceiveTimeout(isTimeout)
}
}
/**
* 主动调查询接口：AdasManager#sendStatusQueryReq(), 会收到以下回调
*/

25
core/mogo-core-function-call/src/main/java/com/mogo/eagle/core/function/call/autopilot/CallerFsm2024ListenerManager.kt Normal file
View File

@@ -0,0 +1,25 @@
package com.mogo.eagle.core.function.call.autopilot
import com.mogo.eagle.core.function.api.autopilot.IMoGoFsm2024Listener
import com.mogo.eagle.core.function.api.autopilot.IMoGoV2nNioEventListener
import com.mogo.eagle.core.function.call.base.CallerBase
import fsm.Fsm2024
import mogo.telematics.pad.MessagePad
/**
* 新版FSM状态
*/
object CallerFsm2024ListenerManager : CallerBase<IMoGoFsm2024Listener>() {
/**
* 绿波通行（单路口）事件推送, 透传
*/
fun invokeFSM2024State(fsmState: Fsm2024.FSMStateMsg) {
M_LISTENERS.forEach {
val listener = it.value
listener.onFSM2024State(fsmState)
}
}
}

23
core/mogo-core-function-call/src/main/java/com/mogo/eagle/core/function/call/autopilot/CallerLocalizationStateListenerManager.kt Normal file
View File

@@ -0,0 +1,23 @@
package com.mogo.eagle.core.function.call.autopilot
import com.mogo.eagle.core.function.api.autopilot.IMoGoLocalizationStateListener
import com.mogo.eagle.core.function.call.base.CallerBase
import localization.LocState
/**
*定位状态新接口
* 定位呈现状态透传 用于pad图标显示 1hz 所有车型MAP440开始支持
* 详细解释：http://wiki.zhidaohulian.com/pages/viewpage.action?pageId=131757484
*/
object CallerLocalizationStateListenerManager : CallerBase<IMoGoLocalizationStateListener>() {
fun invokeLocalizationState(locState: LocState.loc_state) {
M_LISTENERS.forEach {
val listener = it.value
listener.onLocalizationState(locState)
}
}
}

2
libraries/mogo-adas-data/src/main/java/com/zhjt/mogo/adas/common/MessageType.java
View File

@@ -30,6 +30,8 @@ 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, "自动驾驶设备基础信息请求"),
TYPE_SEND_BASIC_INFO_RESP(MessagePad.MessageType.MsgTypeBasicInfoResp, "自动驾驶设备基础信息应答"),

40
libraries/mogo-adas-data/src/main/java/com/zhjt/mogo/adas/data/bean/LaunchConditionData.java
View File

@@ -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);
}

15
libraries/mogo-adas-data/src/main/java/com/zhjt/mogo/adas/data/bean/MogoReport.java
View File

@@ -7,7 +7,8 @@ import java.util.Map;
/**
* 监控事件报告中定义的事件以及解释
* 根据MAP4.2.0事件定义编写 最后修改于2024-04-07
* 根据诊断事件定义表 第143版 编写 最后修改于2024-06-13
* 由于 MAP420及以后版本 诊断事件定义表 中不会标记 新增版本以及弃用版本，所以将不再使用 @ReportState进行版本注解
*/
public class MogoReport {
public interface Result {
@@ -341,6 +342,8 @@ public class MogoReport {
@ReportState("4.2.0")
String HOST_MEMORY_EXCEED = "EMAP_HOST_MEMORY_EXCEED";//系统内存占用过高
String ENTRY_AUTOPILOT_FOR_GEAR_NOT_READY = "EMAP_ENTRY_AUTOPILOT_FOR_GEAR_NOT_READY";//档位不正确未进自驾
}
/**
@@ -583,6 +586,14 @@ public class MogoReport {
@ReportState("4.1.0")
String RADAR_CHECK_FAILED = "ECLB_RADAR_CHECK_FAILED";//radar标定自检算法失败，无法判定外参是否正常，建议重试
}
/**
* EFSM(功能状态管理)
*/
interface EFSM {
@ReportState("4.4.0")
String ENTRY_AUTOPILOT = "EFSM_ENTRY_AUTOPILOT";//FSM因为某些原因未进自驾
}
}
/**
@@ -973,6 +984,8 @@ public class MogoReport {
String REMOTE_PILOT_STATE_CHANGED = "IFSM_REMOTE_PILOT_STATE_CHANGED";//平行驾驶状态机变化
@ReportState("3.5.0")
String TELECTRL_PILOT_STATE_CHANGED = "IFSM_TELECTRL_PILOT_STATE_CHANGED";//遥感驾驶状态机变化
@ReportState("4.4.0")
String RECEIVE_AP_REQUEST = "IFSM_RECEIVE_AP_REQUEST";//FSM收到自驾或平行驾驶或手动驾驶请求信号
}
/**

5
libraries/mogo-adas-data/src/main/java/com/zhjt/mogo/adas/data/bean/UnableLaunchReason.java
View File

@@ -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超时
}
/**

53
libraries/mogo-adas-data/src/main/proto/fsm2024.proto Normal file
View File

@@ -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/自驾/平行驾驶/模拟器驾驶原因 （包括接管）
}

15
libraries/mogo-adas-data/src/main/proto/loc_state.proto Normal file
View File

@@ -0,0 +1,15 @@
syntax = "proto2";
package localization;
import "header.proto";
message loc_state {
optional common.Header header = 1;
optional double rtk_time = 2;//rtk 时间戳
optional int32 autopilot_state = 3;//0非自驾，1自驾状态
optional int32 loc_status = 4;//loc 定位状态 0 正常 1 DR模式 2异常（对应430统计异常值）(对应鹰眼端显示0 蓝色、1 黄色、2异常红色)
optional string loc_sensors_state= 5;//子模块异常状态
optional string loc_current_sensor=6;//当前定位使用的主要定位消息源
optional string reserved = 7;
}

10
libraries/mogo-adas-data/src/main/proto/message_pad.proto
View File

@@ -35,6 +35,8 @@ 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开始支持
//### 以下消息全部不定频 ###
MsgTypeBasicInfoReq = 0x10100; //自动驾驶设备基础信息请求
@@ -257,7 +259,7 @@ enum AdditionalAttribute
message SubSource
{
uint32 source = 1; // TrackedSource=1:1-lidar 2-camera 3-radar 4-vidar 5-falcon
// TrackedSource=2:1-v2v_bsm 2-v2i_rsm 3-v2v_ssm 4-v2n_rsm 5-v2n_rsi 6-v2i_ssm 7-v2i_rsi
// TrackedSource=2:1-v2v_bsm 2-v2i_rsm 3-v2v_ssm 4-v2n_rsm 5-v2n_rsi 6-v2i_ssm 7-v2i_rsi 8-v2n_rsm_2
string id = 2; //HEX_string -bsm_id
}
@@ -877,3 +879,9 @@ 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

28
libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/OnAdasListener.java
View File

@@ -30,7 +30,9 @@ 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;
import mogo.v2x.MogoV2X;
import mogo.v2x.RoadOverviewEvents;
@@ -237,6 +239,24 @@ 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开始支持
* 详细解释：http://wiki.zhidaohulian.com/pages/viewpage.action?pageId=131757484
*
* @param header 头
* @param locState 数据
*/
void onLocalizationState(MessagePad.Header header, @NonNull LocState.loc_state locState);
/**
* 数据采集配置应答
*
@@ -520,6 +540,14 @@ public interface OnAdasListener {
*/
void onSsmReceiveTimeout(boolean isTimeout);
/**
* 域控FSM接口接收超时
* 状态变动时才会回调，默认FSM状态正常 前提是存在FSM接口
*
* @param isTimeout true：FSM接口接收超时 false：FSM接口恢复正常
*/
void onFsm2024ReceiveTimeout(boolean isTimeout);
/**
* 是否有能力启动自动驾驶
*

62
libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/common/autopilot/ability/AutopilotAbility440.java Normal file
View File

@@ -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<UnableLaunchReason> 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;
}
}

89
libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/common/autopilot/ability/AutopilotAbilityManager.java
View File

@@ -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<Chassis.GearPosition> 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;
}
}

33
libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/msg/FSM2024StateMessage.java Normal file
View File

@@ -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);
}
}

33
libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/msg/LocalizationStateMessage.java Normal file
View File

@@ -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.protocol.RawData;
import localization.LocState;
/**
* 定位状态
* 定位呈现状态透传 用于pad图标显示 1hz 所有车型MAP440开始支持
* 详细解释：http://wiki.zhidaohulian.com/pages/viewpage.action?pageId=131757484
*/
public class LocalizationStateMessage extends MyAbstractMessageHandler {
@Override
public void handlerMsg(RawData raw, OnAdasListener adasListener) throws InvalidProtocolBufferException {
LocState.loc_state locState = LocState.loc_state.parser().parseFrom(raw.originalData.toByteArray(), raw.getOffsetValue(), raw.getPackageLengthValue() - raw.getOffsetValue());
AdasChannel.calculateTimeConsumingOnDispatchRaw("定位状态", raw.receiveTime);
long nowTime = 0;
if (CupidLogUtils.isEnableLog())
nowTime = SystemClock.elapsedRealtime();
if (adasListener != null) {
adasListener.onLocalizationState(raw.getHeader(), locState);
}
AdasChannel.calculateTimeConsumingBusiness("定位状态", nowTime);
}
}

14
libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/msg/MyMessageFactory.java
View File

@@ -32,6 +32,8 @@ 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决策状态
private IMsg obuWarningDataMessage;//工控机透传OBU V2I数据
@@ -180,6 +182,18 @@ 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) {
localizationStateMessage = new LocalizationStateMessage();
}
return localizationStateMessage;
} else if (messageType == MessageType.TYPE_RECEIVE_RECORD_DATA_CONFIG_RESP.typeCode) {
//数据采集配置应答
if (recordDataConfigRespMessage == null) {

5
libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/msg/ReportMessage.java
View File

@@ -61,6 +61,11 @@ public class ReportMessage extends MyAbstractMessageHandler {
startAutopilotFailCode.add(MogoReport.Code.Error.EMAP.ENTRY_AUTOPILOT_FOR_CHASSIS_FAULT);//控制判断底盘异常，需要重启车辆
//不知道确切的失败原因，需要联系控制进一步排查
startAutopilotFailCode.add(MogoReport.Code.Error.EMAP.ENTRY_AUTOPILOT_FOR_UNKNOWN);//不知道确切的失败原因，需要联系控制进一步排查
startAutopilotFailCode.add(MogoReport.Code.Error.ESYS.ROUTING_RESPONSE_FAIL);//算路异常，拒绝自动驾驶
startAutopilotFailCode.add(MogoReport.Code.Error.ESYS.TRAJECTORY_AGENT_NOT_READY);//轨迹下载客户端未就绪拒绝轨迹下载请求
startAutopilotFailCode.add(MogoReport.Code.Error.EFSM.ENTRY_AUTOPILOT);//FSM因为某些原因未进自驾
}
@Override