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

[660][adas][data-center] FSM与Can Adapter自动驾驶状态兼容，鹰眼全局自动驾驶状态新增来源字段，新增Can Adapter自驾状态回调接口

Browse Source
This commit is contained in:
xinfengkun
2024-08-23 15:21:14 +08:00
parent 9b39ee7244
commit 45d5576288
12 changed files with 111 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -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);
/**
* 定位状态

11
libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/common/AutopilotReview.java
View File

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

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

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

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

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

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

@@ -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<Integer, Boolean> 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() {

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

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