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

[650][adas] 新增FSM2024接口;新增FSM2024超时接口，FSM数据接入新增检测能否启动自驾440兼容版本；

Browse Source
This commit is contained in:
xinfengkun
2024-06-14 15:15:40 +08:00
parent 75d18a6ea7
commit 39f6f085fc
15 changed files with 377 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -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 mogo.telematics.pad.MessagePad;
import mogo.v2x.MogoV2X;
@@ -237,6 +238,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);
/**
* 数据采集配置应答
*
@@ -488,6 +497,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);
}
}

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

@@ -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 recordDataConfigRespMessage;//数据采集配置应答
private IMsg planningDecisionStateMessage;//planning决策状态
private IMsg obuWarningDataMessage;//工控机透传OBU V2I数据
@@ -176,6 +177,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_RECORD_DATA_CONFIG_RESP.typeCode) {
//数据采集配置应答
if (recordDataConfigRespMessage == null) {