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

[640][adas] 能否启动自驾条件新增 油门方向盘危险报警灯状态判断，抽离底盘相关原因父类；添加车灯状态单独接口，用于统一解析后的车灯状态，目前输出：左右转灯和双闪灯

Browse Source
This commit is contained in:
xinfengkun
2024-04-10 17:29:21 +08:00
parent a6487c4966
commit 81389d56ab
27 changed files with 486 additions and 333 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
app/script/vehicleFlavors/ochDFHQ.gradle
View File

@@ -16,8 +16,8 @@ project.android.productFlavors {
// ②构建的是否要动态更换模型
buildConfigField 'boolean', 'IS_CAR_MODEL_CHANGE', 'true'
// ③不能启动自驾的档位
buildConfigField 'java.util.Set<chassis.Chassis.GearPosition>', 'UNABLE_LAUNCH_AUTOPILOT_GEAR', 'new java.util.HashSet<chassis.Chassis.GearPosition>(){{add(chassis.Chassis.GearPosition.GEAR_P);add(chassis.Chassis.GearPosition.GEAR_R);}}'
// ③能启动自驾的档位 MAP<360代码中会移除P档
buildConfigField 'java.util.Set<chassis.Chassis.GearPosition>', 'LAUNCH_AUTOPILOT_GEAR', 'new java.util.HashSet<chassis.Chassis.GearPosition>(){{add(chassis.Chassis.GearPosition.GEAR_D);add(chassis.Chassis.GearPosition.GEAR_N);add(chassis.Chassis.GearPosition.GEAR_P);}}'
}

4
app/script/vehicleFlavors/ochFT.gradle
View File

@@ -15,7 +15,7 @@ project.android.productFlavors {
// ②构建的是否要动态更换模型
buildConfigField 'boolean', 'IS_CAR_MODEL_CHANGE', 'true'
// ③不能启动自驾的档位
buildConfigField 'java.util.Set<chassis.Chassis.GearPosition>', 'UNABLE_LAUNCH_AUTOPILOT_GEAR', 'null'
// ③能启动自驾的档位
buildConfigField 'java.util.Set<chassis.Chassis.GearPosition>', 'LAUNCH_AUTOPILOT_GEAR', 'null'
}
}

4
app/script/vehicleFlavors/ochJL.gradle
View File

@@ -16,7 +16,7 @@ project.android.productFlavors {
// ②构建的是否要动态更换模型
buildConfigField 'boolean', 'IS_CAR_MODEL_CHANGE', 'true'
// ③不能启动自驾的档位
buildConfigField 'java.util.Set<chassis.Chassis.GearPosition>', 'UNABLE_LAUNCH_AUTOPILOT_GEAR', 'new java.util.HashSet<chassis.Chassis.GearPosition>(){{add(chassis.Chassis.GearPosition.GEAR_N);add(chassis.Chassis.GearPosition.GEAR_R);}}'
// ③能启动自驾的档位
buildConfigField 'java.util.Set<chassis.Chassis.GearPosition>', 'LAUNCH_AUTOPILOT_GEAR', 'new java.util.HashSet<chassis.Chassis.GearPosition>(){{add(chassis.Chassis.GearPosition.GEAR_D);}}'
}
}

5
app/script/vehicleFlavors/ochM1.gradle
View File

@@ -13,9 +13,8 @@ project.android.productFlavors {
// ①标识构建的应用身份类型，具体查看 README.md APP_IDENTITY_MODE规则
buildConfigField 'String', 'APP_IDENTITY_MODE_TAIL', "\"M1\""
// ②构建的是否要动态更换模型
buildConfigField 'java.util.Set<chassis.Chassis.GearPosition>', 'UNABLE_LAUNCH_AUTOPILOT_GEAR', 'null'
// ②M1能启动自驾的档位
buildConfigField 'java.util.Set<chassis.Chassis.GearPosition>', 'LAUNCH_AUTOPILOT_GEAR', 'new java.util.HashSet<chassis.Chassis.GearPosition>(){{add(chassis.Chassis.GearPosition.GEAR_D);}}'
// ③不能启动自驾的档位
buildConfigField 'boolean', 'IS_CAR_MODEL_CHANGE', 'false'

4
app/script/vehicleFlavors/ochM2.gradle
View File

@@ -16,7 +16,7 @@ project.android.productFlavors {
// ②构建的是否要动态更换模型
buildConfigField 'boolean', 'IS_CAR_MODEL_CHANGE', 'false'
// ③M2不能启动自驾的档位
buildConfigField 'java.util.Set<chassis.Chassis.GearPosition>', 'UNABLE_LAUNCH_AUTOPILOT_GEAR', 'new java.util.HashSet<chassis.Chassis.GearPosition>(){{add(chassis.Chassis.GearPosition.GEAR_N);add(chassis.Chassis.GearPosition.GEAR_P);add(chassis.Chassis.GearPosition.GEAR_R);}}'
// ③M2能启动自驾的档位
buildConfigField 'java.util.Set<chassis.Chassis.GearPosition>', 'LAUNCH_AUTOPILOT_GEAR', 'new java.util.HashSet<chassis.Chassis.GearPosition>(){{add(chassis.Chassis.GearPosition.GEAR_D);}}'
}
}

2
app/src/main/java/com/mogo/launcher/startup/ConfigStartUp.kt
View File

@@ -45,7 +45,7 @@ object ConfigStartUp {
// 各个module需要的url
FunctionBuildConfig.urlJson = GsonUtils.fromJson(BuildConfig.URLs, UrlConfig::class.java)
//不能启动自动驾驶的档位
FunctionBuildConfig.unableLaunchAutopilotGear = BuildConfig.UNABLE_LAUNCH_AUTOPILOT_GEAR
FunctionBuildConfig.launchAutopilotGear = BuildConfig.LAUNCH_AUTOPILOT_GEAR
// 各车型宣传视频本地配置json
FunctionBuildConfig.mediaUrlConfig = BuildConfig.mediaUrlConfig
FunctionBuildConfig.musicUrlConfig = BuildConfig.musicUrlConfig

6
core/function-impl/mogo-core-function-datacenter/src/main/java/com/mogo/eagle/core/function/datacenter/autopilot/MoGoAutopilotControlProvider.kt
View File

@@ -132,7 +132,7 @@ class MoGoAutopilotControlProvider :
.setEnableCertification(SharedPrefsMgr.getInstance().getBoolean("${MoGoConfig.AUTOPILOT_CERTIFICATION}-${DebugConfig.getNetMode()}", MoGoConfig.AUTOPILOT_CERTIFICATION_DEFAULT_VALUE))
.setRootCrt(CallerCloudCertManager.getRootCrtFDecode())
.setDeviceCrt(CallerCloudCertManager.getDeviceCrtFDecode())
.setUnableLaunchAutopilotGear(FunctionBuildConfig.unableLaunchAutopilotGear)
.setLaunchAutopilotGear(FunctionBuildConfig.launchAutopilotGear)
// .setSubscribeInterfaceOptions(subscribeInterfaceOptions)//
.build()
@@ -228,7 +228,7 @@ class MoGoAutopilotControlProvider :
val options = AdasOptions
.newBuilder()
.setPassenger(true)
.setUnableLaunchAutopilotGear(FunctionBuildConfig.unableLaunchAutopilotGear)
.setLaunchAutopilotGear(FunctionBuildConfig.launchAutopilotGear)
.build()
AdasManager.getInstance()
.create(mContext, options, MoGoAdasMsgConnectStatusListenerImpl())
@@ -263,7 +263,7 @@ class MoGoAutopilotControlProvider :
.setEnableCertification(SharedPrefsMgr.getInstance().getBoolean("${MoGoConfig.AUTOPILOT_CERTIFICATION}-${DebugConfig.getNetMode()}", MoGoConfig.AUTOPILOT_CERTIFICATION_DEFAULT_VALUE))
.setRootCrt(CallerCloudCertManager.getRootCrtFDecode())
.setDeviceCrt(CallerCloudCertManager.getDeviceCrtFDecode())
.setUnableLaunchAutopilotGear(FunctionBuildConfig.unableLaunchAutopilotGear)
.setLaunchAutopilotGear(FunctionBuildConfig.launchAutopilotGear)
.build()
AdasManager.getInstance().create(mContext, options, MoGoAdasMsgConnectStatusListenerImpl())
//////////////////////////////////注意先后顺序，AdasManager.getInstance().create后才可以设置监听/////////////////////////////////////////////

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

@@ -1,6 +1,7 @@
package com.mogo.eagle.core.function.datacenter.autopilot.adapter
import bag_manager.BagManagerOuterClass
import chassis.Chassis
import chassis.ChassisStatesOuterClass
import chassis.VehicleStateOuterClass
import com.mogo.eagle.core.data.app.AppConfigInfo
@@ -169,7 +170,7 @@ class MoGoAdasListenerImpl : OnAdasListener {
) {
if (vehicleState != null) {
//转向灯数据
CallerChassisLamplightListenerManager.invokeAutopilotLightSwitchData(vehicleState.light)
// CallerChassisLamplightListenerManager.invokeAutopilotLightSwitchData(vehicleState.light)
//刹车灯数据
CallerChassisLamplightListenerManager.invokeAutopilotBrakeLightData(vehicleState.brakeLightStatus)
//方向盘转向角数据
@@ -231,10 +232,10 @@ class MoGoAdasListenerImpl : OnAdasListener {
) {
if (chassisStates != null) {
chassisStates.bcmSystemStates?.let { bcmSystemStates ->
bcmSystemStates.turnLightState?.let {
//转向灯数据
CallerChassisLamplightListenerManager.invokeAutopilotLightSwitchData(it)
}
// bcmSystemStates.turnLightState?.let {
// //转向灯数据
// CallerChassisLamplightListenerManager.invokeAutopilotLightSwitchData(it)
// }
//刹车灯数据
CallerChassisLamplightListenerManager.invokeAutopilotBrakeLightData(bcmSystemStates.brakeLightState != 0)
}
@@ -277,6 +278,19 @@ class MoGoAdasListenerImpl : OnAdasListener {
}
}
/**
* 底盘车灯状态 转换过的可以直接使用
* 例如：
* 原始右转数据： 0 2 0 2 0 2。。。
* 转换之后数据： 2 2 2 2 2 2。。。
*
* @param light 车灯 目前域控发送车灯有 左传{@link Chassis.LightSwitch#LIGHT_LEFT} 右转{@link Chassis.LightSwitch#LIGHT_RIGHT} 危险报警灯{@link Chassis.LightSwitch#LIGHT_FLASH}
*/
override fun onLightSwitch(light: Chassis.LightSwitch) {
//转向灯数据
CallerChassisLamplightListenerManager.invokeAutopilotLightSwitchData(light)
}
//自动驾驶状态
@ChainLog(
linkChainLog = CHAIN_TYPE_SOCKET_AUTOPILOT,

2
core/mogo-core-data/src/main/java/com/mogo/eagle/core/data/config/FunctionBuildConfig.kt
View File

@@ -302,7 +302,7 @@ object FunctionBuildConfig {
*/
@Volatile
@JvmField
var unableLaunchAutopilotGear: Set<Chassis.GearPosition>? = null
var launchAutopilotGear: Set<Chassis.GearPosition>? = null
/**
* 当前应用是否支持patch升级

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

@@ -17,10 +17,10 @@ object CallerChassisLamplightListenerManager : CallerBase<IMoGoChassisLamplightL
* @param lightSwitch
*/
fun invokeAutopilotLightSwitchData(lightSwitch: Chassis.LightSwitch) {
val switch = getTurnLightState(lightSwitch)
// val switch = getTurnLightState(lightSwitch)
M_LISTENERS.forEach {
val listener = it.value
listener.onAutopilotLightSwitchData(switch)
listener.onAutopilotLightSwitchData(lightSwitch)
}
}

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

@@ -29,7 +29,6 @@ public class UnableLaunchReason {
CHASSIS_THROTTLE,//油门
CHASSIS_STEERING,//方向盘
CHASSIS_HAZARD_LIGHTS,//危险报警灯
PARKING_BRAKE,//制动系统，手刹，电子驻车制动系统
SSM_OFFER,//SSM提供的原因
FSM_OFFER,//FSM提供的原因
SSM_TIMEOUT//SSM超时

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

@@ -207,7 +207,7 @@ public class AdasChannel implements IAdasNetCommApi, FpgaSocket.IWebSocketConnec
this.adasOptions = options;
}
ReceiveTimeoutManager.getInstance().setEnable(adasOptions.isEnableTimeoutDetection(), getIpcConnectionStatus());
setUnableLaunchAutopilotGear(adasOptions.getUnableLaunchAutopilotGear());
setLaunchAutopilotGear(adasOptions.getLaunchAutopilotGear());
}
/**
@@ -850,6 +850,7 @@ public class AdasChannel implements IAdasNetCommApi, FpgaSocket.IWebSocketConnec
ipcConnectedIp = (String) args[0];
ipcConnectedPort = (int) args[1];
}
myMessageFactory.initTurnLightState();
seqSpecialVehicle = 0;
AutopilotAbilityManager.getInstance().start();
ParallelDrivingManager.getInstance().start();
@@ -1119,9 +1120,14 @@ public class AdasChannel implements IAdasNetCommApi, FpgaSocket.IWebSocketConnec
adasOptionsBuild(adasOptionsToBuilder().setAutoConnect(enable));
}
/**
* 设置可以启动自动驾驶的档位
*
* @param launchAutopilotGear 能起自驾档位 null表示所有档位都可启动
*/
@Override
public void setUnableLaunchAutopilotGear(Set<Chassis.GearPosition> unableLaunchAutopilotGear) {
AutopilotAbilityManager.getInstance().setUnableLaunchAutopilotGear(unableLaunchAutopilotGear);
public void setLaunchAutopilotGear(@Nullable Set<Chassis.GearPosition> launchAutopilotGear) {
AutopilotAbilityManager.getInstance().setLaunchAutopilotGear(launchAutopilotGear);
}
/**

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

@@ -4,6 +4,7 @@ import android.content.Context;
import android.text.TextUtils;
import androidx.annotation.NonNull;
import androidx.annotation.Nullable;
import com.google.protobuf.ByteString;
import com.zhidao.support.adas.high.bean.VersionCompatibility;
@@ -1794,13 +1795,13 @@ public class AdasManager implements IAdasNetCommApi {
}
/**
* 设置不能启动自动驾驶的档位
* 设置可以启动自动驾驶的档位
*
* @param unableLaunchAutopilotGear 不能起自驾档位
* @param launchAutopilotGear 能起自驾档位 null表示所有档位都可启动
*/
@Override
public void setUnableLaunchAutopilotGear(Set<Chassis.GearPosition> unableLaunchAutopilotGear) {
AutopilotAbilityManager.getInstance().setUnableLaunchAutopilotGear(unableLaunchAutopilotGear);
public void setLaunchAutopilotGear(@Nullable Set<Chassis.GearPosition> launchAutopilotGear) {
AutopilotAbilityManager.getInstance().setLaunchAutopilotGear(launchAutopilotGear);
}
/**

24
libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/AdasOptions.java
View File

@@ -51,7 +51,7 @@ public class AdasOptions {
private final String deviceCrt;
private final boolean isAutoConnect;
private final Set<SubscribeInterfaceOption> subscribeInterfaceOptions;
private final Set<Chassis.GearPosition> unableLaunchAutopilotGear;
private final Set<Chassis.GearPosition> launchAutopilotGear;
private AdasOptions(Builder builder) {
@@ -66,7 +66,7 @@ public class AdasOptions {
this.deviceCrt = builder.deviceCrt;
this.isAutoConnect = builder.isAutoConnect;
this.subscribeInterfaceOptions = builder.subscribeInterfaceOptions;
this.unableLaunchAutopilotGear = builder.unableLaunchAutopilotGear;
this.launchAutopilotGear = builder.launchAutopilotGear;
}
public boolean isPassenger() {
@@ -113,8 +113,8 @@ public class AdasOptions {
return subscribeInterfaceOptions;
}
public Set<Chassis.GearPosition> getUnableLaunchAutopilotGear() {
return unableLaunchAutopilotGear;
public Set<Chassis.GearPosition> getLaunchAutopilotGear() {
return launchAutopilotGear;
}
public static Builder newBuilder() {
@@ -141,7 +141,7 @@ public class AdasOptions {
private String deviceCrt;
private boolean isAutoConnect = DEFAULT.IS_AUTO_CONNECT;
private Set<SubscribeInterfaceOption> subscribeInterfaceOptions;
private Set<Chassis.GearPosition> unableLaunchAutopilotGear;
private Set<Chassis.GearPosition> launchAutopilotGear;
private Builder() {
@@ -159,7 +159,7 @@ public class AdasOptions {
this.deviceCrt = options.deviceCrt;
this.isAutoConnect = options.isAutoConnect;
this.subscribeInterfaceOptions = options.subscribeInterfaceOptions;
this.unableLaunchAutopilotGear = options.unableLaunchAutopilotGear;
this.launchAutopilotGear = options.launchAutopilotGear;
}
@@ -276,8 +276,8 @@ public class AdasOptions {
/**
* 设备证书
* 启用认证后如果不传递，必须在调用以下方法时传递
* {@link AdasManager#sendBasicInfoResp(String, AdasConstants.Environment, AdasConstants.TerminalRole, String)}
* {@link AdasManager#sendBasicInfoResp(String, AdasConstants.Environment, AdasConstants.TerminalRole, int, String, String)}
* {@link AdasManager#sendBasicInfoResp(String, int, int, String)}
* {@link AdasManager#sendBasicInfoResp(String, int, int, int, String, String)}
*
* @param deviceCrt root证书
* @return Builder
@@ -325,13 +325,13 @@ public class AdasOptions {
}
/**
* 不能启动自驾的档位
* 能启动自驾的档位
*
* @param unableLaunchAutopilotGear 档位
* @param launchAutopilotGear 能起自驾档位 null表示所有档位都可启动
* @return Builder
*/
public Builder setUnableLaunchAutopilotGear(Set<Chassis.GearPosition> unableLaunchAutopilotGear) {
this.unableLaunchAutopilotGear = unableLaunchAutopilotGear;
public Builder setLaunchAutopilotGear(Set<Chassis.GearPosition> launchAutopilotGear) {
this.launchAutopilotGear = launchAutopilotGear;
return this;
}

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

@@ -1,6 +1,7 @@
package com.zhidao.support.adas.high;
import androidx.annotation.NonNull;
import androidx.annotation.Nullable;
import com.zhidao.support.adas.high.bean.VersionCompatibility;
import com.zhidao.support.adas.high.common.Constants;
@@ -145,11 +146,11 @@ public interface IAdasNetCommApi {
void setAutoConnect(boolean enable);
/**
* 设置不能启动自动驾驶的档位
* 设置可以启动自动驾驶的档位
*
* @param unableLaunchAutopilotGear 不能起自驾档位
* @param launchAutopilotGear 能起自驾档位 null表示所有档位都可启动
*/
void setUnableLaunchAutopilotGear(Set<Chassis.GearPosition> unableLaunchAutopilotGear);
void setLaunchAutopilotGear(@Nullable Set<Chassis.GearPosition> launchAutopilotGear);
/**
* 自动驾驶设备基础信息应答

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

@@ -26,6 +26,7 @@ import com.zhjt.mogo.adas.data.sweeper.task.stop.SweeperTaskStop;
import java.util.ArrayList;
import bag_manager.BagManagerOuterClass;
import chassis.Chassis;
import chassis.ChassisStatesOuterClass;
import chassis.VehicleStateOuterClass;
import fault_management.FmInfo;
@@ -98,6 +99,16 @@ public interface OnAdasListener {
*/
void onChassisStates(MessagePad.Header header, ChassisStatesOuterClass.ChassisStates chassisStates);
/**
* 底盘车灯状态 转换过的可以直接使用
* 例如：
* 原始右转数据： 0 2 0 2 0 2。。。
* 转换之后数据： 2 2 2 2 2 2。。。
*
* @param light 车灯 目前域控发送车灯有 左传{@link Chassis.LightSwitch#LIGHT_LEFT} 右转{@link Chassis.LightSwitch#LIGHT_RIGHT} 危险报警灯{@link Chassis.LightSwitch#LIGHT_FLASH}
*/
void onLightSwitch(@NonNull Chassis.LightSwitch light);
/**
* 自动驾驶状态
*

34
libraries/mogo-adas/src/main/java/com/zhidao/support/adas/high/common/TurnLightState.java Normal file
View File

@@ -0,0 +1,34 @@
package com.zhidao.support.adas.high.common;
public class TurnLightState {
private boolean m_state;
private double m_lastOnTime;
public TurnLightState() {
init();
}
public void init() {
this.m_state = false;
this.m_lastOnTime = 0.0;
}
public boolean update(boolean state) {
long t = System.currentTimeMillis();
if (state) {
// on!
m_state = true;
m_lastOnTime = t;
} else {
// off?
if (m_state && (t - m_lastOnTime > 500)) {
m_state = false;
}
}
return m_state;
}
public boolean getState() {
return m_state;
}
}

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

@@ -1,5 +1,7 @@
package com.zhidao.support.adas.high.common.autopilot.ability;
import androidx.annotation.NonNull;
import com.zhjt.mogo.adas.data.bean.UnableLaunchData;
import com.zhjt.mogo.adas.data.bean.UnableLaunchReason;
@@ -7,9 +9,6 @@ import java.util.ArrayList;
import java.util.Timer;
import java.util.TimerTask;
import chassis.Chassis;
import chassis.ChassisStatesOuterClass;
/**
* 是否可以启动自动驾驶能力检测 工控机版本>=230&& <250 使用此类
* 目前监控了底盘的一些状态和查询节点状态应答的数据
@@ -17,58 +16,27 @@ import chassis.ChassisStatesOuterClass;
* <p>
* 此定时器不能停止 鹰眼中存在UI更新依赖循环查询系统状态
*/
public class AutopilotAbility230 {
private static final String TAG = AutopilotAbility230.class.getSimpleName();
public class AutopilotAbility230 extends BaseAutopilotAbilityChassis {
private volatile Timer timer;
private ChassisStatesOuterClass.ChassisStates chassisStates;
private OnAutopilotAbilityListener listener;
protected AutopilotAbility230() {
}
protected void setChassisStates(ChassisStatesOuterClass.ChassisStates chassisStates) {
this.chassisStates = chassisStates;
public AutopilotAbility230(@NonNull AutopilotAbilityManager manager) {
super(manager);
}
private void onCallback() {
boolean isAutopilotAbility = true;//是否能启动自动驾驶
ArrayList<UnableLaunchReason> unableAutopilotReasons = null;//不能启动自动驾驶原因
//检测底盘相关
if (chassisStates != null) {
if (chassisStates.hasBrakeSystemStates()) {
float brake = chassisStates.getBrakeSystemStates().getBrakePedalResponsePosition();
if (brake > 0) {
isAutopilotAbility = false;
unableAutopilotReasons = AutopilotAbilityManager.getInstance().addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.CHASSIS, UnableLaunchReason.UnableType.CHASSIS_BRAKE, AutopilotAbilityManager.REASON_CHASSIS_BRAKE);
}
}
if (chassisStates.hasGearSystemStates()) {
Chassis.GearPosition gear = chassisStates.getGearSystemStates().getGearPosition();
if (!AutopilotAbilityManager.getInstance().isLaunchAutopilot(gear)) {
isAutopilotAbility = false;
unableAutopilotReasons = AutopilotAbilityManager.getInstance().addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.CHASSIS, UnableLaunchReason.UnableType.CHASSIS_GEAR, AutopilotAbilityManager.REASON_CHASSIS_GEAR);
}
}
//TODO 关于手刹：不同车型的实现不同所以目前没法使用此字段
// //电子驻车制动系统
// if (chassisStates.hasEpbSystemStates()) {
// ChassisStatesOuterClass.EPBSystemStates epb = chassisStates.getEpbSystemStates();
// if (epb.hasEpbEnableState()){
// epb.getEpbWorkState();
// }
// }
}
unableAutopilotReasons = onCallbackChassis(unableAutopilotReasons);
if (listener != null) {
boolean isAutopilotAbility = unableAutopilotReasons == null || unableAutopilotReasons.isEmpty();//是否能启动自动驾驶
listener.onAutopilotAbility(isAutopilotAbility, new UnableLaunchData(this.getClass().getSimpleName(), null, null, null), unableAutopilotReasons);
}
}
protected synchronized void start(OnAutopilotAbilityListener listener) {
this.listener = listener;
@Override
public synchronized void start(OnAutopilotAbilityListener listener) {
super.start(listener);
if (timer == null) {
timer = new Timer();
timer.schedule(new TimerTask() {
@@ -80,12 +48,13 @@ public class AutopilotAbility230 {
}
}
@Override
protected synchronized void stop() {
if (timer != null) {
timer.cancel();
timer = null;
}
this.chassisStates = null;
this.listener = null;
super.stop();
}
}

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

@@ -1,5 +1,7 @@
package com.zhidao.support.adas.high.common.autopilot.ability;
import androidx.annotation.NonNull;
import com.zhidao.support.adas.high.AdasManager;
import com.zhidao.support.adas.high.common.CupidLogUtils;
import com.zhjt.mogo.adas.data.bean.UnableLaunchData;
@@ -10,9 +12,6 @@ import java.util.List;
import java.util.Timer;
import java.util.TimerTask;
import chassis.Chassis;
import chassis.ChassisStatesOuterClass;
import chassis.VehicleStateOuterClass;
import system_master.SystemStatusInfo;
/**
@@ -22,24 +21,13 @@ import system_master.SystemStatusInfo;
* <p>
* 此定时器不能停止 鹰眼中存在UI更新依赖循环查询系统状态
*/
public class AutopilotAbility250 {
private static final String TAG = AutopilotAbility250.class.getSimpleName();
public class AutopilotAbility250 extends BaseAutopilotAbilityChassis {
private volatile Timer timer;
private ChassisStatesOuterClass.ChassisStates chassisStates;
private VehicleStateOuterClass.VehicleState vehicleState;
private int mapVersion = -1;//MAP版本
private int masterVersion = -1;//Master版本
private boolean isHQ = false;//是否是HQ
private boolean isDF = false;//是否是DF
private boolean isM1 = false;//是否是M1
private OnAutopilotAbilityListener listener;
protected AutopilotAbility250(int mapVersion, boolean isHQ, boolean isDF, boolean isM1) {
this.mapVersion = mapVersion;
this.isHQ = isHQ;
this.isDF = isDF;
this.isM1 = isM1;
this.masterVersion = -1;
public AutopilotAbility250(@NonNull AutopilotAbilityManager manager) {
super(manager);
}
@@ -47,16 +35,8 @@ public class AutopilotAbility250 {
onCallback(statusInfo);
}
protected void setChassisStates(ChassisStatesOuterClass.ChassisStates chassisStates) {
this.chassisStates = chassisStates;
}
protected void setVehicleState(VehicleStateOuterClass.VehicleState vehicleState) {
this.vehicleState = vehicleState;
}
private void onCallback(SystemStatusInfo.StatusInfo statusInfo) {
boolean isAutopilotAbility = true;//是否能启动自动驾驶
ArrayList<UnableLaunchReason> unableAutopilotReasons = null;//不能启动自动驾驶原因
//检测节点状态相关
if (statusInfo != null) {
@@ -67,13 +47,11 @@ public class AutopilotAbility250 {
CupidLogUtils.i(TAG, "MasterVersion=" + masterVersion);
//SSM 3版本兼容
if (mapVersion >= 30400 && masterVersion > 2 && statusInfo.hasAutoPilotReady() && statusInfo.hasAutoPilotUnreadyReason()) {
isAutopilotAbility = statusInfo.getAutoPilotReady();
if (!isAutopilotAbility) {
unableAutopilotReasons = AutopilotAbilityManager.getInstance().addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.SSM, UnableLaunchReason.UnableType.SSM_OFFER, statusInfo.getAutoPilotUnreadyReason());
if (!statusInfo.getAutoPilotReady()) {
unableAutopilotReasons = manager.addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.SSM, UnableLaunchReason.UnableType.SSM_OFFER, statusInfo.getAutoPilotUnreadyReason());
}
} else if (mapVersion >= 21000 && masterVersion > 1 && statusInfo.hasAutoPilotReady()) {//如果 maser version 大于1，还需要判断AutoPilotReady字段是否存在，以确保MAP版本和SSM Maser版本不陪配情况逻辑能正常执行
isAutopilotAbility = statusInfo.getAutoPilotReady();
if (!isAutopilotAbility) {
if (!statusInfo.getAutoPilotReady()) {
SystemStatusInfo.NodeFaultList nodeFaultList = statusInfo.getAutoPilotUnreadyList();
if (nodeFaultList.getSum() > 0) {
List<SystemStatusInfo.NodeInfo> list = nodeFaultList.getNodeList();
@@ -81,20 +59,20 @@ public class AutopilotAbility250 {
String nodeName = info.getNodeName();
int state = info.getState();
if (state < AutopilotAbilityManager.NODE_INFO_STATE.length) {
unableAutopilotReasons = AutopilotAbilityManager.getInstance().addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.SSM, UnableLaunchReason.UnableType.SSM_OFFER, nodeName + AutopilotAbilityManager.NODE_INFO_STATE[state]);
unableAutopilotReasons = manager.addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.SSM, UnableLaunchReason.UnableType.SSM_OFFER, nodeName + AutopilotAbilityManager.NODE_INFO_STATE[state]);
} else {
unableAutopilotReasons = AutopilotAbilityManager.getInstance().addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.SSM, UnableLaunchReason.UnableType.SSM_ERROR, nodeName + "未知异常");
unableAutopilotReasons = manager.addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.SSM, UnableLaunchReason.UnableType.SSM_ERROR, nodeName + "未知异常");
}
}
} else {
unableAutopilotReasons = AutopilotAbilityManager.getInstance().addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.SSM, UnableLaunchReason.UnableType.SSM_ERROR, "未知异常节点");
unableAutopilotReasons = manager.addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.SSM, UnableLaunchReason.UnableType.SSM_ERROR, "未知异常节点");
}
}
} else {
SystemStatusInfo.SystemState systemState = statusInfo.getSysState();
// 目前已知可以下发启动自驾命令的状态： SystemState.SYS_RUNNING、SystemState.PILOT_READY、SystemState.AUTO_PILOT_STARTING、SystemState.AUTO_PILOT_RUNNING
if (systemState != SystemStatusInfo.SystemState.SYS_RUNNING && systemState != SystemStatusInfo.SystemState.PILOT_READY) {
isAutopilotAbility = false;
boolean isAutopilotAbility = false;
String unableAutopilotReason = null;
if (systemState == SystemStatusInfo.SystemState.SYS_STARTING) {
unableAutopilotReason = "系统正在启动";
@@ -118,51 +96,24 @@ public class AutopilotAbility250 {
unableAutopilotReason = "未知系统状态";
}
if (!isAutopilotAbility) {
unableAutopilotReasons = AutopilotAbilityManager.getInstance().addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.SSM, UnableLaunchReason.UnableType.SSM_OFFER, unableAutopilotReason);
unableAutopilotReasons = manager.addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.SSM, UnableLaunchReason.UnableType.SSM_OFFER, unableAutopilotReason);
}
}
}
} else {
isAutopilotAbility = false;//是否能启动自动驾驶
unableAutopilotReasons = AutopilotAbilityManager.getInstance().addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.LIB, UnableLaunchReason.UnableType.SSM_TIMEOUT, "SSM查询超时无响应");
unableAutopilotReasons = manager.addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.LIB, UnableLaunchReason.UnableType.SSM_TIMEOUT, "SSM查询超时无响应");
}
//检测底盘相关
if (chassisStates != null) {
if (chassisStates.hasBrakeSystemStates()) {
float brake = chassisStates.getBrakeSystemStates().getBrakePedalResponsePosition();
if (brake > 0) {
isAutopilotAbility = false;
unableAutopilotReasons = AutopilotAbilityManager.getInstance().addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.CHASSIS, UnableLaunchReason.UnableType.CHASSIS_BRAKE, AutopilotAbilityManager.REASON_CHASSIS_BRAKE);
}
}
if (chassisStates.hasGearSystemStates()) {
Chassis.GearPosition gear = chassisStates.getGearSystemStates().getGearPosition();
if (!AutopilotAbilityManager.getInstance().isLaunchAutopilot(gear)) {
isAutopilotAbility = false;
unableAutopilotReasons = AutopilotAbilityManager.getInstance().addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.CHASSIS, UnableLaunchReason.UnableType.CHASSIS_GEAR, AutopilotAbilityManager.REASON_CHASSIS_GEAR);
}
}
}
if ((!isHQ && !isDF && !isM1) || mapVersion < 30600) {
if (vehicleState != null) {
//TODO 关于手刹：目前只有老底盘中存在这个字段，df360开始，其他车型未知
//电子驻车制动系统
if (vehicleState.hasParkingBrake()) {
boolean isBrake = vehicleState.getParkingBrake();
if (isBrake) {
isAutopilotAbility = false;
unableAutopilotReasons = AutopilotAbilityManager.getInstance().addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.CHASSIS, UnableLaunchReason.UnableType.PARKING_BRAKE, AutopilotAbilityManager.REASON_CHASSIS_PARKING_BRAKE);
}
}
}
}
unableAutopilotReasons = onCallbackChassis(unableAutopilotReasons);
if (listener != null) {
boolean isAutopilotAbility = unableAutopilotReasons == null || unableAutopilotReasons.isEmpty();//是否能启动自动驾驶
listener.onAutopilotAbility(isAutopilotAbility, new UnableLaunchData(this.getClass().getSimpleName(), statusInfo, null, null), unableAutopilotReasons);
}
}
protected synchronized void start(OnAutopilotAbilityListener listener) {
this.listener = listener;
@Override
public synchronized void start(OnAutopilotAbilityListener listener) {
super.start(listener);
if (timer == null) {
timer = new Timer();
timer.schedule(new TimerTask() {
@@ -174,17 +125,17 @@ public class AutopilotAbility250 {
}
}
@Override
protected synchronized void stop() {
if (timer != null) {
timer.cancel();
timer = null;
}
this.chassisStates = null;
this.masterVersion = -1;
this.listener = null;
masterVersion = -1;
super.stop();
}
protected void onCallTimeout() {
onCallback(null);
}

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

@@ -2,6 +2,8 @@ package com.zhidao.support.adas.high.common.autopilot.ability;
import android.text.TextUtils;
import androidx.annotation.NonNull;
import com.zhidao.support.adas.high.AdasManager;
import com.zhidao.support.adas.high.common.CupidLogUtils;
import com.zhjt.mogo.adas.data.bean.UnableLaunchData;
@@ -25,6 +27,8 @@ import system_master.SystemStatusInfo;
*/
public class AutopilotAbility330 {
private static final String TAG = AutopilotAbility330.class.getSimpleName();
@NonNull
private final AutopilotAbilityManager manager;
private volatile Timer timer;
private volatile FSMStatusReasonQueryOuterClass.FSMStatusReasonRespond fsmStatusReasonRespond;//自动驾驶状态为OFF的原因
@@ -32,7 +36,8 @@ public class AutopilotAbility330 {
private int masterVersion = -1;//Master版本
private OnAutopilotAbilityListener listener;
protected AutopilotAbility330(int mapVersion) {
protected AutopilotAbility330(@NonNull AutopilotAbilityManager manager, int mapVersion) {
this.manager = manager;
this.mapVersion = mapVersion;
this.masterVersion = -1;
}
@@ -60,7 +65,7 @@ public class AutopilotAbility330 {
if (mapVersion >= 30400 && masterVersion > 2 && statusInfo.hasAutoPilotReady() && statusInfo.hasAutoPilotUnreadyReason()) {
isAutopilotAbility = statusInfo.getAutoPilotReady();
if (!isAutopilotAbility) {
unableAutopilotReasons = AutopilotAbilityManager.getInstance().addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.SSM, UnableLaunchReason.UnableType.SSM_OFFER, statusInfo.getAutoPilotUnreadyReason());
unableAutopilotReasons = manager.addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.SSM, UnableLaunchReason.UnableType.SSM_OFFER, statusInfo.getAutoPilotUnreadyReason());
}
} else if (mapVersion >= 21000 && masterVersion > 1 && statusInfo.hasAutoPilotReady()) {//如果 maser version 大于1，还需要判断AutoPilotReady字段是否存在，以确保MAP版本和SSM Maser版本不陪配情况逻辑能正常执行
isAutopilotAbility = statusInfo.getAutoPilotReady();
@@ -72,13 +77,13 @@ public class AutopilotAbility330 {
String nodeName = info.getNodeName();
int state = info.getState();
if (state < AutopilotAbilityManager.NODE_INFO_STATE.length) {
unableAutopilotReasons = AutopilotAbilityManager.getInstance().addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.SSM, UnableLaunchReason.UnableType.SSM_OFFER, nodeName + AutopilotAbilityManager.NODE_INFO_STATE[state]);
unableAutopilotReasons = manager.addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.SSM, UnableLaunchReason.UnableType.SSM_OFFER, nodeName + AutopilotAbilityManager.NODE_INFO_STATE[state]);
} else {
unableAutopilotReasons = AutopilotAbilityManager.getInstance().addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.SSM, UnableLaunchReason.UnableType.SSM_ERROR, nodeName + "未知异常");
unableAutopilotReasons = manager.addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.SSM, UnableLaunchReason.UnableType.SSM_ERROR, nodeName + "未知异常");
}
}
} else {
unableAutopilotReasons = AutopilotAbilityManager.getInstance().addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.SSM, UnableLaunchReason.UnableType.SSM_ERROR, "未知异常节点");
unableAutopilotReasons = manager.addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.SSM, UnableLaunchReason.UnableType.SSM_ERROR, "未知异常节点");
}
}
} else {
@@ -109,13 +114,13 @@ public class AutopilotAbility330 {
unableAutopilotReason = "未知系统状态";
}
if (!isAutopilotAbility) {
unableAutopilotReasons = AutopilotAbilityManager.getInstance().addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.SSM, UnableLaunchReason.UnableType.SSM_OFFER, unableAutopilotReason);
unableAutopilotReasons = manager.addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.SSM, UnableLaunchReason.UnableType.SSM_OFFER, unableAutopilotReason);
}
}
}
} else {
isAutopilotAbility = false;//是否能启动自动驾驶
unableAutopilotReasons = AutopilotAbilityManager.getInstance().addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.LIB, UnableLaunchReason.UnableType.SSM_TIMEOUT, "SSM查询超时无响应");
unableAutopilotReasons = manager.addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.LIB, UnableLaunchReason.UnableType.SSM_TIMEOUT, "SSM查询超时无响应");
}
if (fsmStatusReasonRespond != null) {
int count = fsmStatusReasonRespond.getFsmStatusReasonRespondCount();
@@ -124,11 +129,11 @@ public class AutopilotAbility330 {
for (int i = 0; i < count; i++) {
String respond = fsmStatusReasonRespond.getFsmStatusReasonRespond(i);
if (!TextUtils.isEmpty(respond)) {
unableAutopilotReasons = AutopilotAbilityManager.getInstance().addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.FSM, UnableLaunchReason.UnableType.FSM_OFFER, respond);
unableAutopilotReasons = manager.addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.FSM, UnableLaunchReason.UnableType.FSM_OFFER, respond);
}
}
if (unableAutopilotReasons == null || unableAutopilotReasons.isEmpty()) {
unableAutopilotReasons = AutopilotAbilityManager.getInstance().addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.LIB, UnableLaunchReason.UnableType.FSM_ERROR, "FSM数据异常");
unableAutopilotReasons = manager.addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.LIB, UnableLaunchReason.UnableType.FSM_ERROR, "FSM数据异常");
}
}
}

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

@@ -1,5 +1,7 @@
package com.zhidao.support.adas.high.common.autopilot.ability;
import androidx.annotation.NonNull;
import com.zhidao.support.adas.high.AdasManager;
import com.zhidao.support.adas.high.common.CupidLogUtils;
import com.zhjt.mogo.adas.data.bean.UnableLaunchData;
@@ -7,9 +9,6 @@ import com.zhjt.mogo.adas.data.bean.UnableLaunchReason;
import java.util.ArrayList;
import chassis.Chassis;
import chassis.ChassisStatesOuterClass;
import chassis.VehicleStateOuterClass;
import system_master.SsmInfo;
/**
@@ -18,42 +17,21 @@ import system_master.SsmInfo;
* 没有使用监控事件报告的原因是因为，部分异常没进行正常恢复通知，例如收到了异常监控数据，但是异常恢复之后没有恢复的通知
* <p>
*/
public class AutopilotAbility350And360 {
private static final String TAG = AutopilotAbility250.class.getSimpleName();
private ChassisStatesOuterClass.ChassisStates chassisStates;
private VehicleStateOuterClass.VehicleState vehicleState;
private int mapVersion = -1;//MAP版本
public class AutopilotAbility350And360 extends BaseAutopilotAbilityChassis {
private int masterVersion = -1;//Master版本
private boolean isHQ = false;//是否是HQ
private boolean isDF = false;//是否是DF
private boolean isM1 = false;//是否是M1
private OnAutopilotAbilityListener listener;
protected AutopilotAbility350And360(int mapVersion, boolean isHQ, boolean isDF, boolean isM1) {
this.mapVersion = mapVersion;
this.isHQ = isHQ;
this.isDF = isDF;
this.isM1 = isM1;
this.masterVersion = -1;
public AutopilotAbility350And360(@NonNull AutopilotAbilityManager manager) {
super(manager);
}
protected void setStatusInfo(SsmInfo.SsmStatusInf statusInfo) {
onCallback(statusInfo);
}
protected void setChassisStates(ChassisStatesOuterClass.ChassisStates chassisStates) {
this.chassisStates = chassisStates;
}
protected void setVehicleState(VehicleStateOuterClass.VehicleState vehicleState) {
this.vehicleState = vehicleState;
}
private void onCallback(SsmInfo.SsmStatusInf statusInfo) {
boolean isAutopilotAbility = true;//是否能启动自动驾驶
ArrayList<UnableLaunchReason> unableAutopilotReasons = null;//不能启动自动驾驶原因
//检测节点状态相关
if (statusInfo != null) {
@@ -64,13 +42,11 @@ public class AutopilotAbility350And360 {
CupidLogUtils.i(TAG, "MasterVersion=" + masterVersion);
//SSM 3版本兼容
if (masterVersion > 2 && statusInfo.hasAutoPilotReady() && statusInfo.hasAutoPilotUnreadyReason()) {
isAutopilotAbility = statusInfo.getAutoPilotReady();
if (!isAutopilotAbility) {
unableAutopilotReasons = AutopilotAbilityManager.getInstance().addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.SSM, UnableLaunchReason.UnableType.SSM_OFFER, statusInfo.getAutoPilotUnreadyReason());
if (!statusInfo.getAutoPilotReady()) {
unableAutopilotReasons = manager.addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.SSM, UnableLaunchReason.UnableType.SSM_OFFER, statusInfo.getAutoPilotUnreadyReason());
}
} else if (masterVersion > 1 && statusInfo.hasAutoPilotReady()) {//如果 maser version 大于1，还需要判断AutoPilotReady字段是否存在，以确保MAP版本和SSM Maser版本不陪配情况逻辑能正常执行
isAutopilotAbility = statusInfo.getAutoPilotReady();
if (!isAutopilotAbility) {
if (!statusInfo.getAutoPilotReady()) {
int count = statusInfo.getAutoPilotUnreadyListCount();
if (count > 0) {
for (int i = 0; i < count; i++) {
@@ -78,19 +54,18 @@ public class AutopilotAbility350And360 {
String nodeName = info.getNodeName();
int state = info.getState().getNumber();
if (state < AutopilotAbilityManager.NODE_INFO_STATE_FIXED_FREQUENCY.length) {
unableAutopilotReasons = AutopilotAbilityManager.getInstance().addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.SSM, UnableLaunchReason.UnableType.SSM_OFFER, nodeName + AutopilotAbilityManager.NODE_INFO_STATE_FIXED_FREQUENCY[state]);
unableAutopilotReasons = manager.addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.SSM, UnableLaunchReason.UnableType.SSM_OFFER, nodeName + AutopilotAbilityManager.NODE_INFO_STATE_FIXED_FREQUENCY[state]);
} else {
unableAutopilotReasons = AutopilotAbilityManager.getInstance().addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.SSM, UnableLaunchReason.UnableType.SSM_ERROR, nodeName + "未知异常");
unableAutopilotReasons = manager.addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.SSM, UnableLaunchReason.UnableType.SSM_ERROR, nodeName + "未知异常");
}
}
} else {
unableAutopilotReasons = AutopilotAbilityManager.getInstance().addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.SSM, UnableLaunchReason.UnableType.SSM_ERROR, "未知异常节点");
unableAutopilotReasons = manager.addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.SSM, UnableLaunchReason.UnableType.SSM_ERROR, "未知异常节点");
}
}
} else {
SsmInfo.ModeState modeState = statusInfo.getModeState();
if (modeState != SsmInfo.ModeState.MODE_RUN_UNREADY && modeState != SsmInfo.ModeState.MODE_RUN_READY) {
isAutopilotAbility = false;
String unableAutopilotReason = null;
if (modeState == SsmInfo.ModeState.MODE_STOP_UNREADY) {
unableAutopilotReason = "系统处于停止模式（未就绪）";
@@ -103,56 +78,29 @@ public class AutopilotAbility350And360 {
} else {
unableAutopilotReason = "未知系统模式";
}
unableAutopilotReasons = AutopilotAbilityManager.getInstance().addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.SSM, UnableLaunchReason.UnableType.SSM_OFFER, unableAutopilotReason);
unableAutopilotReasons = manager.addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.SSM, UnableLaunchReason.UnableType.SSM_OFFER, unableAutopilotReason);
}
}
} else {
isAutopilotAbility = false;//是否能启动自动驾驶
unableAutopilotReasons = AutopilotAbilityManager.getInstance().addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.LIB, UnableLaunchReason.UnableType.SSM_TIMEOUT, "SSM超时无响应");
unableAutopilotReasons = manager.addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.LIB, UnableLaunchReason.UnableType.SSM_TIMEOUT, "SSM超时无响应");
}
//检测底盘相关
if (chassisStates != null) {
if (chassisStates.hasBrakeSystemStates()) {
float brake = chassisStates.getBrakeSystemStates().getBrakePedalResponsePosition();
if (brake > 0) {
isAutopilotAbility = false;
unableAutopilotReasons = AutopilotAbilityManager.getInstance().addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.CHASSIS, UnableLaunchReason.UnableType.CHASSIS_BRAKE, AutopilotAbilityManager.REASON_CHASSIS_BRAKE);
}
}
if (chassisStates.hasGearSystemStates()) {
Chassis.GearPosition gear = chassisStates.getGearSystemStates().getGearPosition();
if (!AutopilotAbilityManager.getInstance().isLaunchAutopilot(gear)) {
isAutopilotAbility = false;
unableAutopilotReasons = AutopilotAbilityManager.getInstance().addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.CHASSIS, UnableLaunchReason.UnableType.CHASSIS_GEAR, AutopilotAbilityManager.REASON_CHASSIS_GEAR);
}
}
}
if ((!isHQ && !isDF && !isM1) || mapVersion < 30600) {
if (vehicleState != null) {
//TODO 关于手刹：目前只有老底盘中存在这个字段，df360开始，其他车型未知
//电子驻车制动系统
if (vehicleState.hasParkingBrake()) {
boolean isBrake = vehicleState.getParkingBrake();
if (isBrake) {
isAutopilotAbility = false;
unableAutopilotReasons = AutopilotAbilityManager.getInstance().addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.CHASSIS, UnableLaunchReason.UnableType.PARKING_BRAKE, AutopilotAbilityManager.REASON_CHASSIS_PARKING_BRAKE);
}
}
}
}
unableAutopilotReasons = onCallbackChassis(unableAutopilotReasons);
if (listener != null) {
boolean isAutopilotAbility = unableAutopilotReasons == null || unableAutopilotReasons.isEmpty();//是否能启动自动驾驶
listener.onAutopilotAbility(isAutopilotAbility, new UnableLaunchData(this.getClass().getSimpleName(), null, statusInfo, null), unableAutopilotReasons);
}
}
protected void start(OnAutopilotAbilityListener listener) {
this.listener = listener;
@Override
public synchronized void start(OnAutopilotAbilityListener listener) {
super.start(listener);
}
protected void stop() {
this.chassisStates = null;
this.masterVersion = -1;
this.listener = null;
@Override
protected synchronized void stop() {
masterVersion = -1;
super.stop();
}
protected void onCallTimeout() {

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

@@ -2,6 +2,8 @@ package com.zhidao.support.adas.high.common.autopilot.ability;
import android.text.TextUtils;
import androidx.annotation.NonNull;
import com.zhidao.support.adas.high.AdasManager;
import com.zhidao.support.adas.high.common.CupidLogUtils;
import com.zhjt.mogo.adas.data.bean.UnableLaunchData;
@@ -20,12 +22,15 @@ import system_master.SsmInfo;
*/
public class AutopilotAbility360 {
private static final String TAG = AutopilotAbility360.class.getSimpleName();
@NonNull
private final AutopilotAbilityManager manager;
private volatile FSMStatusReasonQueryOuterClass.FSMStatusReasonRespond fsmStatusReasonRespond;//自动驾驶状态为OFF的原因
private int masterVersion = -1;//Master版本
private OnAutopilotAbilityListener listener;
protected AutopilotAbility360() {
protected AutopilotAbility360(@NonNull AutopilotAbilityManager manager) {
this.manager = manager;
this.masterVersion = -1;
}
@@ -52,7 +57,7 @@ public class AutopilotAbility360 {
if (masterVersion > 2 && statusInfo.hasAutoPilotReady() && statusInfo.hasAutoPilotUnreadyReason()) {
isAutopilotAbility = statusInfo.getAutoPilotReady();
if (!isAutopilotAbility) {
unableAutopilotReasons = AutopilotAbilityManager.getInstance().addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.SSM, UnableLaunchReason.UnableType.SSM_OFFER, statusInfo.getAutoPilotUnreadyReason());
unableAutopilotReasons = manager.addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.SSM, UnableLaunchReason.UnableType.SSM_OFFER, statusInfo.getAutoPilotUnreadyReason());
}
} else if (masterVersion > 1 && statusInfo.hasAutoPilotReady()) {//如果 maser version 大于1，还需要判断AutoPilotReady字段是否存在，以确保MAP版本和SSM Maser版本不陪配情况逻辑能正常执行
isAutopilotAbility = statusInfo.getAutoPilotReady();
@@ -64,13 +69,13 @@ public class AutopilotAbility360 {
String nodeName = info.getNodeName();
int state = info.getState().getNumber();
if (state < AutopilotAbilityManager.NODE_INFO_STATE_FIXED_FREQUENCY.length) {
unableAutopilotReasons = AutopilotAbilityManager.getInstance().addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.SSM, UnableLaunchReason.UnableType.SSM_OFFER, nodeName + AutopilotAbilityManager.NODE_INFO_STATE_FIXED_FREQUENCY[state]);
unableAutopilotReasons = manager.addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.SSM, UnableLaunchReason.UnableType.SSM_OFFER, nodeName + AutopilotAbilityManager.NODE_INFO_STATE_FIXED_FREQUENCY[state]);
} else {
unableAutopilotReasons = AutopilotAbilityManager.getInstance().addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.SSM, UnableLaunchReason.UnableType.SSM_ERROR, nodeName + "未知异常");
unableAutopilotReasons = manager.addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.SSM, UnableLaunchReason.UnableType.SSM_ERROR, nodeName + "未知异常");
}
}
} else {
unableAutopilotReasons = AutopilotAbilityManager.getInstance().addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.SSM, UnableLaunchReason.UnableType.SSM_ERROR, "未知异常节点");
unableAutopilotReasons = manager.addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.SSM, UnableLaunchReason.UnableType.SSM_ERROR, "未知异常节点");
}
}
} else {
@@ -89,12 +94,12 @@ public class AutopilotAbility360 {
} else {
unableAutopilotReason = "未知系统模式";
}
unableAutopilotReasons = AutopilotAbilityManager.getInstance().addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.SSM, UnableLaunchReason.UnableType.SSM_OFFER, unableAutopilotReason);
unableAutopilotReasons = manager.addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.SSM, UnableLaunchReason.UnableType.SSM_OFFER, unableAutopilotReason);
}
}
} else {
isAutopilotAbility = false;//是否能启动自动驾驶
unableAutopilotReasons = AutopilotAbilityManager.getInstance().addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.LIB, UnableLaunchReason.UnableType.SSM_TIMEOUT, "SSM超时无响应");
unableAutopilotReasons = manager.addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.LIB, UnableLaunchReason.UnableType.SSM_TIMEOUT, "SSM超时无响应");
}
if (fsmStatusReasonRespond != null) {
int count = fsmStatusReasonRespond.getFsmStatusReasonRespondCount();
@@ -103,11 +108,11 @@ public class AutopilotAbility360 {
for (int i = 0; i < count; i++) {
String respond = fsmStatusReasonRespond.getFsmStatusReasonRespond(i);
if (!TextUtils.isEmpty(respond)) {
unableAutopilotReasons = AutopilotAbilityManager.getInstance().addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.FSM, UnableLaunchReason.UnableType.FSM_OFFER, respond);
unableAutopilotReasons = manager.addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.FSM, UnableLaunchReason.UnableType.FSM_OFFER, respond);
}
}
if (unableAutopilotReasons == null || unableAutopilotReasons.isEmpty()) {
unableAutopilotReasons = AutopilotAbilityManager.getInstance().addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.LIB, UnableLaunchReason.UnableType.FSM_ERROR, "FSM数据异常");
unableAutopilotReasons = manager.addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.LIB, UnableLaunchReason.UnableType.FSM_ERROR, "FSM数据异常");
}
}
}

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

@@ -11,7 +11,6 @@ import com.zhjt.mogo.adas.data.bean.UnableLaunchData;
import com.zhjt.mogo.adas.data.bean.UnableLaunchReason;
import java.util.ArrayList;
import java.util.HashSet;
import java.util.Set;
import java.util.Timer;
import java.util.TimerTask;
@@ -19,7 +18,6 @@ import java.util.concurrent.atomic.AtomicBoolean;
import chassis.Chassis;
import chassis.ChassisStatesOuterClass;
import chassis.VehicleStateOuterClass;
import function_state_management.FSMStatusReasonQueryOuterClass;
import mogo.telematics.pad.MessagePad;
import system_master.SsmInfo;
@@ -38,17 +36,13 @@ public class AutopilotAbilityManager implements OnAutopilotAbilityListener {
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 = {"未知状态", "依赖未就绪", "启动中", "运行", "停止", "无法启动状态", "非自动启动状态", "非自动关闭状态"};
protected static final String REASON_CHASSIS_BRAKE = "请检查刹车踏板";
protected static final String REASON_CHASSIS_GEAR = "当前档位无法启动";
protected static final String REASON_CHASSIS_PARKING_BRAKE = "请检查手刹";
private static volatile AutopilotAbilityManager INSTANCE;
private OnAdasListener listener;
private int mapVersion = -1;//工控机版本
private boolean isFutianSweeper = false;//是否是福田清扫车
private boolean isJinlvM1 = false;//是否是M1
private boolean isHQ = false;//是否是HQ
private boolean isDF = false;//是否是DF
private boolean isM1 = false;//是否是M1
private AutopilotAbility230 autopilotAbility230;
private AutopilotAbility250 autopilotAbility250;
private AutopilotAbility330 autopilotAbility330;
@@ -61,9 +55,9 @@ public class AutopilotAbilityManager implements OnAutopilotAbilityListener {
private final AtomicBoolean isOldSsmTimeout = new AtomicBoolean(false);//SSM是否超时 老状态
private final AtomicBoolean isInitCarConfig = new AtomicBoolean(false);//车辆信息是否初始化
/**
* 不能启动自动驾驶的档位
* 能启动自动驾驶的档位
*/
private Set<Chassis.GearPosition> unableLaunchAutopilotGear;
private Set<Chassis.GearPosition> launchAutopilotGear;
private AutopilotAbilityManager() {
@@ -80,27 +74,17 @@ public class AutopilotAbilityManager implements OnAutopilotAbilityListener {
return INSTANCE;
}
public void setUnableLaunchAutopilotGear(Set<Chassis.GearPosition> unableLaunchAutopilotGear) {
if (unableLaunchAutopilotGear != null) {
this.unableLaunchAutopilotGear = new HashSet<>(unableLaunchAutopilotGear);
taxiUnmanned();
public void setLaunchAutopilotGear(Set<Chassis.GearPosition> launchAutopilotGear) {
this.launchAutopilotGear = launchAutopilotGear;
if (autopilotAbility230 != null) {
autopilotAbility230.setLaunchAutopilotGear(launchAutopilotGear);
} else if (autopilotAbility250 != null) {
autopilotAbility250.setLaunchAutopilotGear(launchAutopilotGear);
} else if (autopilotAbility350And360 != null) {
autopilotAbility350And360.setLaunchAutopilotGear(launchAutopilotGear);
}
}
/**
* 获取当前档位是否能启动自动驾驶 如果不传递默认可以启动
* 不能启动自驾档位规则：app/README.md/不能启动自动驾驶的档位
*
* @param currentGear 当前档位
* @return 是否能启动自驾
*/
public boolean isLaunchAutopilot(Chassis.GearPosition currentGear) {
if (unableLaunchAutopilotGear != null && !unableLaunchAutopilotGear.isEmpty()) {
return !unableLaunchAutopilotGear.contains(currentGear);
}
return true;
}
/**
* 添加不能启动自驾的原因
*
@@ -117,16 +101,6 @@ public class AutopilotAbilityManager implements OnAutopilotAbilityListener {
return unableAutopilotReasons;
}
/**
* Taxi无人化相关，从MAP360开始去掉P档限制和手刹限制
*/
private void taxiUnmanned() {
if ((isHQ || isDF) && mapVersion >= 30600) {
if (unableLaunchAutopilotGear != null) {
unableLaunchAutopilotGear.remove(Chassis.GearPosition.GEAR_P);
}
}
}
public void setCarConfig(@NonNull MessagePad.CarConfigResp carConfig) {
if (!isInitCarConfig.get()) {
@@ -137,10 +111,17 @@ public class AutopilotAbilityManager implements OnAutopilotAbilityListener {
isFutianSweeper = carConfig.getIsFutianSweeper();
isJinlvM1 = carConfig.getIsJinlvM1();
isHQ = carConfig.getIsHQ();
isDF = carConfig.getIsDF();
isM1 = carConfig.getIsJinlvM1();
taxiUnmanned();
initAutopilotAbility();
if (autopilotAbility230 != null) {
autopilotAbility230.setCarConfig(carConfig);
autopilotAbility230.setLaunchAutopilotGear(launchAutopilotGear);
} else if (autopilotAbility250 != null) {
autopilotAbility250.setCarConfig(carConfig);
autopilotAbility250.setLaunchAutopilotGear(launchAutopilotGear);
} else if (autopilotAbility350And360 != null) {
autopilotAbility350And360.setCarConfig(carConfig);
autopilotAbility350And360.setLaunchAutopilotGear(launchAutopilotGear);
}
}
}
}
@@ -189,29 +170,13 @@ public class AutopilotAbilityManager implements OnAutopilotAbilityListener {
*
* @param chassisStates 底盘
*/
public void setChassisStates(ChassisStatesOuterClass.ChassisStates chassisStates) {
public void setChassisStates(ChassisStatesOuterClass.ChassisStates chassisStates, Chassis.LightSwitch light) {
if (autopilotAbility230 != null) {
autopilotAbility230.setChassisStates(chassisStates);
}
if (autopilotAbility250 != null) {
autopilotAbility250.setChassisStates(chassisStates);
}
if (autopilotAbility350And360 != null) {
autopilotAbility350And360.setChassisStates(chassisStates);
}
}
/**
* 老底盘中手刹状态更新（新底盘PB中没有所以单独传递）
*
* @param vehicleState 底盘
*/
public void setVehicleState(VehicleStateOuterClass.VehicleState vehicleState) {
if (autopilotAbility250 != null) {
autopilotAbility250.setVehicleState(vehicleState);
}
if (autopilotAbility350And360 != null) {
autopilotAbility350And360.setVehicleState(vehicleState);
autopilotAbility230.setChassisStates(chassisStates, light);
} else if (autopilotAbility250 != null) {
autopilotAbility250.setChassisStates(chassisStates, light);
} else if (autopilotAbility350And360 != null) {
autopilotAbility350And360.setChassisStates(chassisStates, light);
}
}
@@ -235,33 +200,33 @@ public class AutopilotAbilityManager implements OnAutopilotAbilityListener {
if (mapVersion >= 30600 && isFutianSweeper) {
CupidLogUtils.log(TAG, "能否启动自驾能力检测使用版本：360清扫车专用");
if (autopilotAbility360 == null) {
autopilotAbility360 = new AutopilotAbility360();
autopilotAbility360 = new AutopilotAbility360(this);
autopilotAbility360.start(this);
}
} else if ((mapVersion >= 30500 && (isJinlvM1 || isHQ)) || mapVersion >= 30600) {
CupidLogUtils.log(TAG, "能否启动自驾能力检测使用版本：350和360共用");
if (autopilotAbility350And360 == null) {
autopilotAbility350And360 = new AutopilotAbility350And360(mapVersion, isHQ, isDF, isM1);
autopilotAbility350And360 = new AutopilotAbility350And360(this);
autopilotAbility350And360.start(this);
}
} else if (mapVersion >= 30300 && isFutianSweeper) {//目前只有MAP330的清扫车用的新的FSM状态原因查询
CupidLogUtils.log(TAG, "能否启动自驾能力检测使用版本：330清扫车专用");
if (autopilotAbility330 == null) {
autopilotAbility330 = new AutopilotAbility330(mapVersion);
autopilotAbility330 = new AutopilotAbility330(this, mapVersion);
autopilotAbility330.start(this);
}
} else if (mapVersion >= 20500) {
CupidLogUtils.log(TAG, "能否启动自驾能力检测使用版本：250");
if (autopilotAbility250 == null) {
autopilotAbility250 = new AutopilotAbility250(mapVersion, isHQ, isDF, isM1);
autopilotAbility250 = new AutopilotAbility250(this);
autopilotAbility250.start(this);
}
} else {
CupidLogUtils.log(TAG, "能否启动自驾能力检测使用版本：230");
if (autopilotAbility230 == null) {
autopilotAbility230 = new AutopilotAbility230();
autopilotAbility230 = new AutopilotAbility230(this);
autopilotAbility230.start(this);
}
}

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

@@ -0,0 +1,175 @@
package com.zhidao.support.adas.high.common.autopilot.ability;
import androidx.annotation.NonNull;
import com.zhidao.support.adas.high.common.CupidLogUtils;
import com.zhjt.mogo.adas.data.bean.UnableLaunchReason;
import java.util.ArrayList;
import java.util.Set;
import java.util.concurrent.atomic.AtomicReference;
import chassis.Chassis;
import chassis.ChassisStatesOuterClass;
import mogo.telematics.pad.MessagePad;
/**
* 所有需要使用底盘检测数据的版本父类
*/
public abstract class BaseAutopilotAbilityChassis {
protected final String TAG = this.getClass().getSimpleName();
private static final String REASON_CHASSIS_BRAKE = "刹车踏板";
private static final String REASON_CHASSIS_THROTTLE = "油门踏板";
private static final String REASON_CHASSIS_GEAR = "档位";
private static final String REASON_CHASSIS_HAZARD_LIGHTS = "危险报警灯";
private static final String REASON_CHASSIS_STEERING = "方向盘";
@NonNull
protected final AutopilotAbilityManager manager;
private ChassisStatesOuterClass.ChassisStates chassisStates;
protected OnAutopilotAbilityListener listener;
protected int mapVersion = -1;//工控机版本
protected boolean isJinlv = false;//是否是金旅小巴
protected boolean isJinlvM1 = false;//是否是M1
protected boolean isJinlvM2 = false;//是否是M2
protected boolean isHQ = false;//是否是HQ
protected boolean isDF = false;//是否是DF
private final AtomicReference<Chassis.LightSwitch> currentLight = new AtomicReference<>(Chassis.LightSwitch.LIGHT_NONE);
/**
* 能启动自动驾驶的档位
*/
private Set<Chassis.GearPosition> launchAutopilotGear;
private float oldThrottle = Float.MAX_VALUE;//方向盘转角
public BaseAutopilotAbilityChassis(@NonNull AutopilotAbilityManager manager) {
this.manager = manager;
}
public void setCarConfig(@NonNull MessagePad.CarConfigResp carConfig) {
mapVersion = carConfig.getMapVersion();
isJinlv = carConfig.getIsJinlv();
isJinlvM1 = carConfig.getIsJinlvM1();
isJinlvM2 = carConfig.getIsJinlvM2();
isHQ = carConfig.getIsHQ();
isDF = carConfig.getIsDF();
taxiUnmanned();
}
public void setChassisStates(ChassisStatesOuterClass.ChassisStates chassisStates, Chassis.LightSwitch light) {
this.chassisStates = chassisStates;
currentLight.set(light);
}
public void setLaunchAutopilotGear(Set<Chassis.GearPosition> launchAutopilotGear) {
this.launchAutopilotGear = launchAutopilotGear;
taxiUnmanned();
}
/**
* Taxi无人化相关，从MAP360开始去掉P档限制和手刹限制
*/
private void taxiUnmanned() {
if ((isHQ || isDF) && mapVersion < 30600) {
if (launchAutopilotGear != null) {
launchAutopilotGear.remove(Chassis.GearPosition.GEAR_P);
}
}
}
/**
* 获取当前档位是否能启动自动驾驶 如果不传递默认可以启动
* 不能启动自驾档位规则：app/README.md/不能启动自动驾驶的档位
*
* @param currentGear 当前档位
* @return 是否能启动自驾
*/
private boolean isLaunchAutopilotGear(Chassis.GearPosition currentGear) {
if (launchAutopilotGear != null && !launchAutopilotGear.isEmpty()) {
return launchAutopilotGear.contains(currentGear);
}
return true;
}
/**
* 获取当前是否开启危险报警灯
* 双闪处于打开状态（taxi可以进自驾，金旅bus不可以）
*
* @return 是否能启动自驾
*/
public boolean isLaunchAutopilotLight() {
if (isJinlv || isJinlvM1 || isJinlvM2) {
return currentLight.get() != Chassis.LightSwitch.LIGHT_FLASH;
}
return true;
}
protected ArrayList<UnableLaunchReason> onCallbackChassis(ArrayList<UnableLaunchReason> unableAutopilotReasons) {
//检测底盘相关
if (chassisStates != null) {
//制动踏板
if (chassisStates.hasBrakeSystemStates()) {
float brake = chassisStates.getBrakeSystemStates().getBrakePedalResponsePosition();
if (brake > 0) {
unableAutopilotReasons = manager.addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.CHASSIS, UnableLaunchReason.UnableType.CHASSIS_BRAKE, REASON_CHASSIS_BRAKE);
}
}
//油门踏板
if (chassisStates.hasDrivingSystemStates()) {
float throttle = chassisStates.getDrivingSystemStates().getThrottleResponsePosition();
if (throttle > 0) {
unableAutopilotReasons = manager.addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.CHASSIS, UnableLaunchReason.UnableType.CHASSIS_THROTTLE, REASON_CHASSIS_THROTTLE);
}
}
//档位
if (chassisStates.hasGearSystemStates()) {
Chassis.GearPosition gear = chassisStates.getGearSystemStates().getGearPosition();
if (!isLaunchAutopilotGear(gear)) {
unableAutopilotReasons = manager.addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.CHASSIS, UnableLaunchReason.UnableType.CHASSIS_GEAR, REASON_CHASSIS_GEAR);
}
}
//危险报警灯
if (!isLaunchAutopilotLight()) {
unableAutopilotReasons = manager.addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.CHASSIS, UnableLaunchReason.UnableType.CHASSIS_HAZARD_LIGHTS, REASON_CHASSIS_HAZARD_LIGHTS);
}
//方向盘
if (chassisStates.hasSteerSystemStates()) {
ChassisStatesOuterClass.SteerSystemStates steerSystemStates = chassisStates.getSteerSystemStates();
if (steerSystemStates.hasSteeringWheelAngle()) {
float throttle = steerSystemStates.getSteeringWheelAngle();
if (oldThrottle == Float.MAX_VALUE) {
oldThrottle = throttle;
} else {
CupidLogUtils.log(TAG, "方向盘当前角度=" + oldThrottle + " 方向盘1秒前角度=" + throttle);
boolean isTurning = Math.abs(throttle - oldThrottle) > 2.0F;
oldThrottle = throttle;
CupidLogUtils.log(TAG, "方向盘是否转动=" + isTurning);
if (isTurning) {
unableAutopilotReasons = manager.addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.CHASSIS, UnableLaunchReason.UnableType.CHASSIS_STEERING, REASON_CHASSIS_STEERING);
}
}
}
}
}
return unableAutopilotReasons;
}
public synchronized void start(OnAutopilotAbilityListener listener) {
this.listener = listener;
}
protected synchronized void stop() {
this.chassisStates = null;
this.listener = null;
mapVersion = -1;
isHQ = false;
isJinlv = false;
isJinlvM1 = false;
isJinlvM2 = false;
}
}

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

@@ -3,24 +3,56 @@ package com.zhidao.support.adas.high.msg;
import com.google.protobuf.InvalidProtocolBufferException;
import com.zhidao.support.adas.high.OnAdasListener;
import com.zhidao.support.adas.high.common.ParallelDrivingManager;
import com.zhidao.support.adas.high.common.TurnLightState;
import com.zhidao.support.adas.high.common.autopilot.ability.AutopilotAbilityManager;
import com.zhidao.support.adas.high.protocol.RawData;
import chassis.Chassis;
import chassis.ChassisStatesOuterClass;
/**
* 重构后的底盘状态
*/
public class ChassisStatesMessage extends MyAbstractMessageHandler {
private final TurnLightState lightLeft;
private final TurnLightState lightRight;
public ChassisStatesMessage(TurnLightState lightLeft, TurnLightState lightRight) {
this.lightLeft = lightLeft;
this.lightRight = lightRight;
}
@Override
public void handlerMsg(RawData raw, OnAdasListener adasListener) throws InvalidProtocolBufferException {
ChassisStatesOuterClass.ChassisStates chassisStates = ChassisStatesOuterClass.ChassisStates.parser().parseFrom(raw.originalData.toByteArray(), raw.getOffsetValue(), raw.getPackageLengthValue() - raw.getOffsetValue());
AutopilotAbilityManager.getInstance().setChassisStates(chassisStates);
Chassis.LightSwitch light = updateLight(chassisStates, adasListener);
AutopilotAbilityManager.getInstance().setChassisStates(chassisStates, light);
ParallelDrivingManager.getInstance().setChassisStates(chassisStates);
if (adasListener != null) {
adasListener.onChassisStates(raw.getHeader(), chassisStates);
adasListener.onLightSwitch(light);
}
}
private Chassis.LightSwitch updateLight(ChassisStatesOuterClass.ChassisStates chassisStates, OnAdasListener adasListener) {
Chassis.LightSwitch light = Chassis.LightSwitch.LIGHT_NONE;
if (chassisStates.hasBcmSystemStates()) {
ChassisStatesOuterClass.BCMSystemStates bcm = chassisStates.getBcmSystemStates();
light = bcm.getTurnLightState();
}
boolean isTurnLightLeft = lightLeft.update((light.getNumber() & Chassis.LightSwitch.LIGHT_LEFT.getNumber()) != 0);
boolean isTurnLightRight = lightRight.update((light.getNumber() & Chassis.LightSwitch.LIGHT_RIGHT.getNumber()) != 0);
if (isTurnLightLeft && isTurnLightRight) {
//双闪
light = Chassis.LightSwitch.LIGHT_FLASH;
} else if (isTurnLightLeft) {
//左传
light = Chassis.LightSwitch.LIGHT_LEFT;
} else if (isTurnLightRight) {
//右转
light = Chassis.LightSwitch.LIGHT_RIGHT;
}
return light;
}
}

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

@@ -1,6 +1,7 @@
package com.zhidao.support.adas.high.msg;
import com.zhidao.support.adas.high.common.AutopilotReview;
import com.zhidao.support.adas.high.common.TurnLightState;
import com.zhjt.mogo.adas.common.MessageType;
import mogo.telematics.pad.MessagePad;
@@ -47,11 +48,18 @@ public class MyMessageFactory implements IMyMessageFactory {
private IMsg fSMStatusReasonRespondMessage;//FSM状态原因查询
private final AutopilotReview autopilotReview;
private final TurnLightState lightLeft = new TurnLightState();
private final TurnLightState lightRight = new TurnLightState();
public MyMessageFactory(AutopilotReview autopilotReview) {
this.autopilotReview = autopilotReview;
}
public void initTurnLightState() {
lightLeft.init();
lightRight.init();
}
@Override
public IMsg createMessage(MessagePad.MessageType messageType) {
if (messageType == MessageType.TYPE_RECEIVE_TRAJECTORY.typeCode) {
@@ -75,13 +83,13 @@ public class MyMessageFactory implements IMyMessageFactory {
} else if (messageType == MessageType.TYPE_RECEIVE_VEHICLE_STATE.typeCode) {
//底盘信息, 透传底盘状态，pb参考底盘
if (vehicleStateMessage == null) {
vehicleStateMessage = new VehicleStateMessage();
vehicleStateMessage = new VehicleStateMessage(lightLeft, lightRight);
}
return vehicleStateMessage;
} else if (messageType == MessageType.TYPE_RECEIVE_CHASSIS_STATES.typeCode) {
//重构后的底盘状态
if (chassisStatesMessage == null) {
chassisStatesMessage = new ChassisStatesMessage();
chassisStatesMessage = new ChassisStatesMessage(lightLeft, lightRight);
}
return chassisStatesMessage;
} else if (messageType == MessageType.TYPE_RECEIVE_AUTOPILOT_STATE.typeCode) {

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

@@ -5,9 +5,10 @@ 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.ParallelDrivingManager;
import com.zhidao.support.adas.high.common.autopilot.ability.AutopilotAbilityManager;
import com.zhidao.support.adas.high.common.CupidLogUtils;
import com.zhidao.support.adas.high.common.ParallelDrivingManager;
import com.zhidao.support.adas.high.common.TurnLightState;
import com.zhidao.support.adas.high.common.autopilot.ability.AutopilotAbilityManager;
import com.zhidao.support.adas.high.protocol.RawData;
import chassis.Chassis;
@@ -19,6 +20,13 @@ import common.HeaderOuterClass;
* 底盘信息, 透传底盘状态，pb参考底盘
*/
public class VehicleStateMessage extends MyAbstractMessageHandler {
private final TurnLightState lightLeft;
private final TurnLightState lightRight;
public VehicleStateMessage(TurnLightState lightLeft, TurnLightState lightRight) {
this.lightLeft = lightLeft;
this.lightRight = lightRight;
}
@Override
public void handlerMsg(RawData raw, OnAdasListener adasListener) throws InvalidProtocolBufferException {
@@ -27,18 +35,40 @@ public class VehicleStateMessage extends MyAbstractMessageHandler {
long nowTime = 0;
if (CupidLogUtils.isEnableLog())
nowTime = SystemClock.elapsedRealtime();
AutopilotAbilityManager.getInstance().setVehicleState(vehicleState);
ParallelDrivingManager.getInstance().setVehicleState(vehicleState);
ChassisStatesOuterClass.ChassisStates chassisStates = compatibility(adasListener, raw, vehicleState);
Chassis.LightSwitch light = updateLight(vehicleState, adasListener);
ChassisStatesOuterClass.ChassisStates chassisStates = compatibility(vehicleState, light);
if (adasListener != null) {
//TODO 暂时关闭老底盘转发 后期 会删除 onVehicleState
//adasListener.onChassisStates(raw.getHeader(), chassisStates);
adasListener.onVehicleState(raw.getHeader(), vehicleState);
adasListener.onLightSwitch(light);
}
AdasChannel.calculateTimeConsumingBusiness("底盘信息", nowTime);
// CupidLogUtils.e("底盘信息--->" + vehicleState.toString());
}
private Chassis.LightSwitch updateLight(VehicleStateOuterClass.VehicleState vehicleState, OnAdasListener adasListener) {
Chassis.LightSwitch light = Chassis.LightSwitch.LIGHT_NONE;
if (vehicleState.hasLight()) {
light = vehicleState.getLight();
}
boolean isTurnLightLeft = lightLeft.update((light.getNumber() & Chassis.LightSwitch.LIGHT_LEFT.getNumber()) != 0);
boolean isTurnLightRight = lightRight.update((light.getNumber() & Chassis.LightSwitch.LIGHT_RIGHT.getNumber()) != 0);
if (isTurnLightLeft && isTurnLightRight) {
//双闪
light = Chassis.LightSwitch.LIGHT_FLASH;
} else if (isTurnLightLeft) {
//左传
light = Chassis.LightSwitch.LIGHT_LEFT;
} else if (isTurnLightRight) {
//右转
light = Chassis.LightSwitch.LIGHT_RIGHT;
}
return light;
}
/**
* 将老地盘数据转换成新地盘数据
* 字段注释中 鹰眼已用 的 已和底盘相关开发 确认过，其他字段均为确认对应关系
@@ -49,7 +79,7 @@ public class VehicleStateMessage extends MyAbstractMessageHandler {
* fuel_value robo_taxi_state robo_bus_state
* robo_bus_jinlv_m1_state
*/
private ChassisStatesOuterClass.ChassisStates compatibility(OnAdasListener adasListener, RawData raw, VehicleStateOuterClass.VehicleState vehicleState) throws InvalidProtocolBufferException {
private ChassisStatesOuterClass.ChassisStates compatibility(VehicleStateOuterClass.VehicleState vehicleState, Chassis.LightSwitch light) throws InvalidProtocolBufferException {
/**************老地盘转换成新地盘PB***************/
ChassisStatesOuterClass.ChassisStates.Builder builder = ChassisStatesOuterClass.ChassisStates.newBuilder();
//工控机所用Header
@@ -114,7 +144,7 @@ public class VehicleStateMessage extends MyAbstractMessageHandler {
.setSweeperFutianTaskSystemStates(ChassisStatesOuterClass.SweeperFuTianTaskSystemStates.parseFrom(bytes)));//鹰眼已用 清扫车专用
}
ChassisStatesOuterClass.ChassisStates chassisStates = builder.build();
AutopilotAbilityManager.getInstance().setChassisStates(chassisStates);
AutopilotAbilityManager.getInstance().setChassisStates(chassisStates, light);
ParallelDrivingManager.getInstance().setChassisStates(chassisStates);
return chassisStates;
}