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

2
OCH/common/common/src/debug/java/com/mogo/och/common/module/debug/DebugDataDispatch.kt
View File

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

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

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

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

@@ -82,6 +82,7 @@ class MoGoAdasMsgConnectStatusListenerImpl :
//与工控机断开连接，需要重置自动驾驶状态(包括上传至云平台缓存信息)，等待连接成功后同步状态信息
CallerAutoPilotStatusListenerManager.getAutoPilotStatusInfo().state = 0
CallerAutoPilotStatusListenerManager.getAutoPilotStatusInfo().pilotmode = 0
CallerAutoPilotStatusListenerManager.getAutoPilotStatusInfo().autopilotStateSource = 0
isReceivedBasicInfoReq.set(false)
isSentBasicInfoReq.set(false)
}

6
core/mogo-core-data/src/main/java/com/mogo/eagle/core/data/autopilot/AutopilotStatusInfo.kt
View File

@@ -58,6 +58,12 @@ open class AutopilotStatusInfo : Serializable, Cloneable {
@Volatile
var state = 0
/**
* 自动驾驶状态数据来源 0：默认/未知；1：底盘；2：FSM；
*/
@Volatile
var autopilotStateSource = 0
/**
* 底盘的自动驾驶状态 0非自动驾驶，1自动驾驶
*/

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

@@ -20,6 +20,13 @@ interface IMoGoAutopilotStatusListener {
*/
fun onAutopilotStatusResponse(state: Int) {}
/**
* 自动驾驶状态信息 数据源来自底盘 不管是否存在FSM 都将进行底盘自驾状态的转发
*
* @param state 状态信息
*/
fun onAutopilotStatusResponseFromCan(state: Int) {}
/**
* 自动驾驶docker信息
*

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

@@ -145,9 +145,23 @@ object CallerAutoPilotStatusListenerManager : CallerBase<IMoGoAutopilotStatusLis
mAutopilotStatusInfo.locationStatus = true
invokeAutoPilotStatus()
}
private var autopilotStateFromCan: Int by Delegates.observable(0) { _, oldValue, newValue ->
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()

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