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

[644][adas] 更改能否启动自驾检测频率，1Hz改为10Hz

Browse Source
This commit is contained in:
xinfengkun
2024-06-05 18:09:13 +08:00
parent b4e105602c
commit cd9a6452af
10 changed files with 282 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
OCH/common/common/src/main/java/com/mogo/och/common/module/manager/autopilot/OCHAdasAbilityManager.java
View File

@@ -102,6 +102,10 @@ public class OCHAdasAbilityManager implements IMoGoAutopilotActionsListener, IMo
CallerParallelDrivingActionsListenerManager.INSTANCE.removeListener(TAG);
}
@Override
public void onAutopilotAbilityHighFrequency(boolean isAutopilotAbility, @Nullable LaunchConditionData launchConditionData, @Nullable ArrayList<UnableLaunchReason> unableAutopilotReasons) {
}
@Override
public void onAutopilotAbility(boolean isAutopilotAbility, @Nullable LaunchConditionData launchConditionData, @Nullable ArrayList<UnableLaunchReason> unableAutopilotReasons) {
this.isAutopilotAbility = isAutopilotAbility;

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

@@ -941,9 +941,11 @@ class MoGoAdasListenerImpl : OnAdasListener {
launchConditionData: LaunchConditionData,
unableAutopilotReasons: ArrayList<UnableLaunchReason>?
) {
if (unableAutopilotReasons != null && unableAutopilotReasons.size > 0) {
autopilotAbilityCheck(isAutopilotAbility, unableAutopilotReasons.toString(), launchConditionData)
var reason = ""
if (unableAutopilotReasons != null) {
reason = unableAutopilotReasons.toString()
}
autopilotAbilityCheck(isAutopilotAbility, reason, launchConditionData)
invokeAutopilotAbility(isAutopilotAbility, launchConditionData, unableAutopilotReasons)
}
@@ -967,7 +969,7 @@ class MoGoAdasListenerImpl : OnAdasListener {
linkChainLog = CHAIN_TYPE_SOCKET_AUTOPILOT,
linkCode = CHAIN_SOURCE_ADAS,
nodeAliasCode = CHAIN_CODE_ADAS_ABILITY,
paramIndexes = [0, 1]
paramIndexes = [0, 1, 2]
)
private fun autopilotAbilityCheck(isAutopilotAbility: Boolean, reason: String, launchConditionData: LaunchConditionData) {

2
core/function-impl/mogo-core-function-devatools/src/main/java/com/zhjt/mogo_core_function_devatools/status/flow/autopilot/AcceleratorImpl.kt
View File

@@ -47,7 +47,7 @@ internal class AcceleratorImpl(ctx: Context): IFlow<AcceleratorStatus>(ctx), IMo
}
}
override fun onAutopilotAbility(isAutopilotAbility: Boolean, launchConditionData: LaunchConditionData?, unableAutopilotReasons: ArrayList<UnableLaunchReason>?) {
override fun onAutopilotAbilityHighFrequency(isAutopilotAbility: Boolean, launchConditionData: LaunchConditionData?, unableAutopilotReasons: ArrayList<UnableLaunchReason>?) {
if (!isAutopilotAbility) {
val target = unableAutopilotReasons?.find { it.unableType == CHASSIS_THROTTLE }?.also { itx ->
isError = true

2
core/function-impl/mogo-core-function-devatools/src/main/java/com/zhjt/mogo_core_function_devatools/status/flow/autopilot/BrakeImpl.kt
View File

@@ -46,7 +46,7 @@ internal class BrakeImpl(ctx: Context): IFlow<BrakeStatus>(ctx), IMoGoChassisBra
}
}
override fun onAutopilotAbility(isAutopilotAbility: Boolean, launchConditionData: LaunchConditionData?, unableAutopilotReasons: ArrayList<UnableLaunchReason>?) {
override fun onAutopilotAbilityHighFrequency(isAutopilotAbility: Boolean, launchConditionData: LaunchConditionData?, unableAutopilotReasons: ArrayList<UnableLaunchReason>?) {
if (!isAutopilotAbility) {
val target = unableAutopilotReasons?.find { it.unableType == CHASSIS_BRAKE }?.also { itx ->
isError = true

2
core/function-impl/mogo-core-function-devatools/src/main/java/com/zhjt/mogo_core_function_devatools/status/flow/autopilot/DoubleFlashImpl.kt
View File

@@ -52,7 +52,7 @@ internal class DoubleFlashImpl(ctx: Context): IFlow<DoubleFlashStatus>(ctx), IMo
}
override fun onAutopilotAbility(isAutopilotAbility: Boolean, launchConditionData: LaunchConditionData?, unableAutopilotReasons: ArrayList<UnableLaunchReason>?) {
override fun onAutopilotAbilityHighFrequency(isAutopilotAbility: Boolean, launchConditionData: LaunchConditionData?, unableAutopilotReasons: ArrayList<UnableLaunchReason>?) {
if (!isAutopilotAbility) {
val target = unableAutopilotReasons?.find { it.unableType == CHASSIS_HAZARD_LIGHTS }?.also { itx ->
isError = true

2
core/function-impl/mogo-core-function-devatools/src/main/java/com/zhjt/mogo_core_function_devatools/status/flow/autopilot/GearImpl.kt
View File

@@ -46,7 +46,7 @@ internal class GearImpl(ctx: Context): IFlow<GearStatus>(ctx), IMoGoChassisGearS
}
}
override fun onAutopilotAbility(isAutopilotAbility: Boolean, launchConditionData: LaunchConditionData?, unableAutopilotReasons: ArrayList<UnableLaunchReason>?) {
override fun onAutopilotAbilityHighFrequency(isAutopilotAbility: Boolean, launchConditionData: LaunchConditionData?, unableAutopilotReasons: ArrayList<UnableLaunchReason>?) {
if (!isAutopilotAbility) {
val target = unableAutopilotReasons?.find { it.unableType == CHASSIS_GEAR }?.also { itx ->
isError = true

2
core/function-impl/mogo-core-function-devatools/src/main/java/com/zhjt/mogo_core_function_devatools/status/flow/autopilot/SteerImpl.kt
View File

@@ -45,7 +45,7 @@ internal class SteerImpl(ctx: Context): IFlow<SteerStatus>(ctx), IMoGoChassisSte
}
}
override fun onAutopilotAbility(isAutopilotAbility: Boolean, launchConditionData: LaunchConditionData?, unableAutopilotReasons: ArrayList<UnableLaunchReason>?) {
override fun onAutopilotAbilityHighFrequency(isAutopilotAbility: Boolean, launchConditionData: LaunchConditionData?, unableAutopilotReasons: ArrayList<UnableLaunchReason>?) {
Logger.d(TAG, "onAutopilotAbility->($isAutopilotAbility, $launchConditionData, ${unableAutopilotReasons?.joinToString(",")}")
if (!isAutopilotAbility) {
val target = unableAutopilotReasons?.find { it.unableType == CHASSIS_STEERING }?.also { itx ->

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

@@ -9,5 +9,21 @@ import com.zhjt.mogo.adas.data.bean.UnableLaunchReason
*/
interface IMoGoAutopilotActionsListener {
fun onAutopilotAbility(isAutopilotAbility: Boolean, launchConditionData: LaunchConditionData?, unableAutopilotReasons: ArrayList<UnableLaunchReason>?)
/**
* 高频
*/
fun onAutopilotAbilityHighFrequency(
isAutopilotAbility: Boolean,
launchConditionData: LaunchConditionData?,
unableAutopilotReasons: ArrayList<UnableLaunchReason>?
){}
/**
* 数据变动才会调用
*/
fun onAutopilotAbility(
isAutopilotAbility: Boolean,
launchConditionData: LaunchConditionData?,
unableAutopilotReasons: ArrayList<UnableLaunchReason>?
){}
}

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

@@ -5,6 +5,7 @@ import chassis.VehicleStateOuterClass
import com.google.protobuf.TextFormat
import com.mogo.eagle.core.function.api.autopilot.IMoGoAutopilotActionsListener
import com.mogo.eagle.core.function.call.base.CallerBase
import com.mogo.eagle.core.utilcode.mogo.logger.Logger
import com.zhjt.mogo.adas.data.bean.LaunchConditionData
import com.zhjt.mogo.adas.data.bean.UnableLaunchReason
import org.json.JSONException
@@ -223,6 +224,21 @@ object CallerAutopilotActionsListenerManager : CallerBase<IMoGoAutopilotActionsL
unableAutopilotReasons: ArrayList<UnableLaunchReason>?
) {
this.launchConditionData = launchCondition
this.isAutopilotAbility = isAutopilotAbility
this.unableAutopilotReasons = unableAutopilotReasons
Logger.d(
"CallerAutopilotActionsListenerManager",
"高频 是否可以启动自动驾驶=" + isAutopilotAbility + " 原因=" + (unableAutopilotReasons?.toString()) + " 原始数据=" + launchConditionData?.json
)
M_LISTENERS.forEach {
val listener = it.value
listener.onAutopilotAbilityHighFrequency(
CallerAutopilotActionsListenerManager.isAutopilotAbility,
launchConditionData,
CallerAutopilotActionsListenerManager.unableAutopilotReasons
)
}
if (isConnected) {
var isEquals = true
if (unableAutopilotReasons != null && this.unableAutopilotReasons != null) {
@@ -236,14 +252,16 @@ object CallerAutopilotActionsListenerManager : CallerBase<IMoGoAutopilotActionsL
} else isEquals =
!(unableAutopilotReasons != null || this.unableAutopilotReasons != null)
if (this.isAutopilotAbility != isAutopilotAbility || !isEquals) {
this.isAutopilotAbility = isAutopilotAbility
this.unableAutopilotReasons = unableAutopilotReasons
notification()
}
}
}
private fun notification() {
Logger.d(
"CallerAutopilotActionsListenerManager",
"低频 是否可以启动自动驾驶=" + isAutopilotAbility + " 原因=" + (unableAutopilotReasons?.toString()) + " 原始数据=" + launchConditionData?.json
)
M_LISTENERS.forEach {
val listener = it.value
listener.onAutopilotAbility(

251
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 android.util.Log;
import androidx.annotation.NonNull;
import com.zhidao.support.adas.high.AdasManager;
@@ -8,7 +10,14 @@ import com.zhjt.mogo.adas.data.bean.LaunchConditionData;
import com.zhjt.mogo.adas.data.bean.UnableLaunchReason;
import java.util.ArrayList;
import java.util.Set;
import java.util.Timer;
import java.util.TimerTask;
import java.util.concurrent.atomic.AtomicBoolean;
import chassis.Chassis;
import chassis.ChassisStatesOuterClass;
import mogo.telematics.pad.MessagePad;
import system_master.SsmInfo;
/**
@@ -17,22 +26,113 @@ import system_master.SsmInfo;
* 没有使用监控事件报告的原因是因为，部分异常没进行正常恢复通知，例如收到了异常监控数据，但是异常恢复之后没有恢复的通知
* <p>
*/
public class AutopilotAbility350And360 extends BaseAutopilotAbilityChassis {
public class AutopilotAbility350And360 {
private 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
private final AutopilotAbilityManager manager;
private volatile SsmInfo.SsmStatusInf statusInfo;
private volatile ChassisStatesOuterClass.ChassisStates chassisStates;
private OnAutopilotAbilityListener listener;
private int mapVersion = -1;//工控机版本
private boolean isJinlv = false;//是否是金旅小巴
private boolean isJinlvM1 = false;//是否是M1
private boolean isJinlvM2 = false;//是否是M2
private boolean isHQ = false;//是否是HQ
private boolean isDF = false;//是否是DF
private volatile Timer turningTimer;//方向盘检测timer
private final AtomicBoolean isTurning = new AtomicBoolean(false);//方向盘是否正转动
/**
* 能启动自动驾驶的档位
*/
private Set<Chassis.GearPosition> launchAutopilotGear;
protected float oldSteering = Float.MAX_VALUE;//方向盘转角
private int masterVersion = -1;//Master版本
private long seqNum = -1;//消息条数
public AutopilotAbility350And360(@NonNull AutopilotAbilityManager manager) {
super(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;
seqNum++;
startTurningTimer();
//降频10Hz
if (seqNum % 2 == 0) {
onCallbackChassis(chassisStates, light);
}
}
protected void setStatusInfo(SsmInfo.SsmStatusInf statusInfo) {
onCallback(statusInfo);
/**
* Taxi无人化相关，从MAP360开始去掉P档限制和手刹限制
*/
private void taxiUnmanned() {
if ((isHQ || isDF) && mapVersion < 30600) {
if (launchAutopilotGear != null) {
launchAutopilotGear.remove(Chassis.GearPosition.GEAR_P);
}
}
}
public void setLaunchAutopilotGear(Set<Chassis.GearPosition> launchAutopilotGear) {
this.launchAutopilotGear = launchAutopilotGear;
taxiUnmanned();
}
public void setStatusInfo(SsmInfo.SsmStatusInf statusInfo) {
this.statusInfo = statusInfo;
}
private void onCallback(SsmInfo.SsmStatusInf statusInfo) {
ArrayList<UnableLaunchReason> unableAutopilotReasons = null;//不能启动自动驾驶原因
/**
* 获取当前档位是否能启动自动驾驶 如果不传递默认可以启动
* 不能启动自驾档位规则：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 是否能启动自驾
*/
private boolean isLaunchAutopilotLight(Chassis.LightSwitch currentLight) {
if (isJinlv || isJinlvM1 || isJinlvM2) {
return currentLight != Chassis.LightSwitch.LIGHT_FLASH;
}
return true;
}
private ArrayList<UnableLaunchReason> onSsmCallback(ArrayList<UnableLaunchReason> unableAutopilotReasons) {
//检测节点状态相关
if (statusInfo != null) {
if (masterVersion == -1 && statusInfo.hasMasterVersion()) {
@@ -84,27 +184,138 @@ public class AutopilotAbility350And360 extends BaseAutopilotAbilityChassis {
} else {
unableAutopilotReasons = manager.addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.LIB, UnableLaunchReason.UnableType.SSM_TIMEOUT, "SSM超时无响应");
}
//检测底盘相关
unableAutopilotReasons = onCallbackChassis(unableAutopilotReasons);
if (listener != null) {
boolean isAutopilotAbility = unableAutopilotReasons == null || unableAutopilotReasons.isEmpty();//是否能启动自动驾驶
listener.onAutopilotAbility(isAutopilotAbility, new LaunchConditionData(this.getClass().getSimpleName(), chassisStates, oldSteering, getLightSwitch(), null, statusInfo, null), unableAutopilotReasons);
return unableAutopilotReasons;
}
protected void onCallTimeout() {
statusInfo = null;
}
private void stopTurningTimer() {
if (turningTimer != null) {
turningTimer.cancel();
turningTimer = null;
}
}
@Override
private void startTurningTimer() {
if (turningTimer == null) {
turningTimer = new Timer();
turningTimer.schedule(new TimerTask() {
@Override
public void run() {
if (chassisStates != null && chassisStates.hasSteerSystemStates()) {
ChassisStatesOuterClass.SteerSystemStates steerSystemStates = chassisStates.getSteerSystemStates();
if (steerSystemStates.hasSteeringWheelAngle()) {
float steering = steerSystemStates.getSteeringWheelAngle();
if (oldSteering != Float.MAX_VALUE) {
CupidLogUtils.log(TAG, "方向盘当前角度=" + oldSteering + " 方向盘1秒前角度=" + steering);
isTurning.set(Math.abs(steering - oldSteering) > 15.0F);
CupidLogUtils.log(TAG, "方向盘是否转动=" + isTurning);
}
oldSteering = steering;
}
}
}
}, 0, 500L);
}
}
/**
* 检测底盘相关
*
* @param chassisStates
* @param currentLight
* @return
*/
protected void onCallbackChassis(ChassisStatesOuterClass.ChassisStates chassisStates, Chassis.LightSwitch currentLight) {
ArrayList<UnableLaunchReason> unableAutopilotReasons = null;//不能启动自动驾驶原因
//SSM检测 1.5s后开始检测ssm 避免由于底盘频率高于ssm频率时 刚开始产生的超时异常
if (seqNum > 30) {
unableAutopilotReasons = onSsmCallback(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(currentLight)) {
unableAutopilotReasons = manager.addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.CHASSIS, UnableLaunchReason.UnableType.CHASSIS_HAZARD_LIGHTS, REASON_CHASSIS_HAZARD_LIGHTS);
}
//延迟太高改用timer
// //方向盘
// if (chassisStates.hasSteerSystemStates()) {
// ChassisStatesOuterClass.SteerSystemStates steerSystemStates = chassisStates.getSteerSystemStates();
// //降频20Hz
// if (seqNum % 10 == 0) {
// if (steerSystemStates.hasSteeringWheelAngle()) {
// float steering = steerSystemStates.getSteeringWheelAngle();
// if (oldSteering != Float.MAX_VALUE) {
// CupidLogUtils.log(TAG, "方向盘当前角度=" + oldSteering + " 方向盘1秒前角度=" + steering);
// isTurning = Math.abs(steering - oldSteering) > 15.0F;
// CupidLogUtils.log(TAG, "方向盘是否转动=" + isTurning);
// }
// oldSteering = steering;
// }
// }
//
//// //暂时无法使用 红旗 东风未赋值，bus也需再优化
//// if (steerSystemStates.hasSteeringWheelSpeed()) {
//// if (steerSystemStates.getSteeringWheelSpeed() == 0) {
//// Log.i(TAG, "方向盘速度=" + steerSystemStates.getSteeringWheelSpeed());
//// isTurning = false;
//// }
//// }
// }
Log.i(TAG, "方向盘是否在转动=" + isTurning.get());
if (isTurning.get()) {
unableAutopilotReasons = manager.addUnableAutopilotReason(unableAutopilotReasons, UnableLaunchReason.SourceType.CHASSIS, UnableLaunchReason.UnableType.CHASSIS_STEERING, REASON_CHASSIS_STEERING);
}
}
if (listener != null) {
boolean isAutopilotAbility = unableAutopilotReasons == null || unableAutopilotReasons.isEmpty();//是否能启动自动驾驶
listener.onAutopilotAbility(isAutopilotAbility, new LaunchConditionData(this.getClass().getSimpleName(), chassisStates, oldSteering, currentLight, null, statusInfo, null), unableAutopilotReasons);
}
}
public synchronized void start(OnAutopilotAbilityListener listener) {
super.start(listener);
this.listener = listener;
}
@Override
protected synchronized void stop() {
public synchronized void stop() {
this.listener = null;
stopTurningTimer();
mapVersion = -1;
masterVersion = -1;
super.stop();
}
protected void onCallTimeout() {
onCallback(null);
seqNum = -1;
isHQ = false;
isJinlv = false;
isJinlvM1 = false;
isJinlvM2 = false;
isTurning.set(false);
}
}