diff --git a/OCH/common/common/src/main/java/com/mogo/och/common/module/manager/autopilot/OCHAdasAbilityManager.java b/OCH/common/common/src/main/java/com/mogo/och/common/module/manager/autopilot/OCHAdasAbilityManager.java index cb43850345..c54ba736fc 100644 --- a/OCH/common/common/src/main/java/com/mogo/och/common/module/manager/autopilot/OCHAdasAbilityManager.java +++ b/OCH/common/common/src/main/java/com/mogo/och/common/module/manager/autopilot/OCHAdasAbilityManager.java @@ -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 unableAutopilotReasons) { + + } @Override public void onAutopilotAbility(boolean isAutopilotAbility, @Nullable LaunchConditionData launchConditionData, @Nullable ArrayList unableAutopilotReasons) { this.isAutopilotAbility = isAutopilotAbility; diff --git a/core/function-impl/mogo-core-function-datacenter/src/main/java/com/mogo/eagle/core/function/datacenter/autopilot/adapter/MoGoAdasListenerImpl.kt b/core/function-impl/mogo-core-function-datacenter/src/main/java/com/mogo/eagle/core/function/datacenter/autopilot/adapter/MoGoAdasListenerImpl.kt index 0940b65459..8a6df5ac1d 100644 --- a/core/function-impl/mogo-core-function-datacenter/src/main/java/com/mogo/eagle/core/function/datacenter/autopilot/adapter/MoGoAdasListenerImpl.kt +++ b/core/function-impl/mogo-core-function-datacenter/src/main/java/com/mogo/eagle/core/function/datacenter/autopilot/adapter/MoGoAdasListenerImpl.kt @@ -941,9 +941,11 @@ class MoGoAdasListenerImpl : OnAdasListener { launchConditionData: LaunchConditionData, unableAutopilotReasons: ArrayList? ) { - 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) { diff --git a/core/function-impl/mogo-core-function-devatools/src/main/java/com/zhjt/mogo_core_function_devatools/status/flow/autopilot/AcceleratorImpl.kt b/core/function-impl/mogo-core-function-devatools/src/main/java/com/zhjt/mogo_core_function_devatools/status/flow/autopilot/AcceleratorImpl.kt index 2fa136ef90..b55c9cdc31 100644 --- a/core/function-impl/mogo-core-function-devatools/src/main/java/com/zhjt/mogo_core_function_devatools/status/flow/autopilot/AcceleratorImpl.kt +++ b/core/function-impl/mogo-core-function-devatools/src/main/java/com/zhjt/mogo_core_function_devatools/status/flow/autopilot/AcceleratorImpl.kt @@ -47,7 +47,7 @@ internal class AcceleratorImpl(ctx: Context): IFlow(ctx), IMo } } - override fun onAutopilotAbility(isAutopilotAbility: Boolean, launchConditionData: LaunchConditionData?, unableAutopilotReasons: ArrayList?) { + override fun onAutopilotAbilityHighFrequency(isAutopilotAbility: Boolean, launchConditionData: LaunchConditionData?, unableAutopilotReasons: ArrayList?) { if (!isAutopilotAbility) { val target = unableAutopilotReasons?.find { it.unableType == CHASSIS_THROTTLE }?.also { itx -> isError = true diff --git a/core/function-impl/mogo-core-function-devatools/src/main/java/com/zhjt/mogo_core_function_devatools/status/flow/autopilot/BrakeImpl.kt b/core/function-impl/mogo-core-function-devatools/src/main/java/com/zhjt/mogo_core_function_devatools/status/flow/autopilot/BrakeImpl.kt index 33f90890c9..2ccd958427 100644 --- a/core/function-impl/mogo-core-function-devatools/src/main/java/com/zhjt/mogo_core_function_devatools/status/flow/autopilot/BrakeImpl.kt +++ b/core/function-impl/mogo-core-function-devatools/src/main/java/com/zhjt/mogo_core_function_devatools/status/flow/autopilot/BrakeImpl.kt @@ -46,7 +46,7 @@ internal class BrakeImpl(ctx: Context): IFlow(ctx), IMoGoChassisBra } } - override fun onAutopilotAbility(isAutopilotAbility: Boolean, launchConditionData: LaunchConditionData?, unableAutopilotReasons: ArrayList?) { + override fun onAutopilotAbilityHighFrequency(isAutopilotAbility: Boolean, launchConditionData: LaunchConditionData?, unableAutopilotReasons: ArrayList?) { if (!isAutopilotAbility) { val target = unableAutopilotReasons?.find { it.unableType == CHASSIS_BRAKE }?.also { itx -> isError = true diff --git a/core/function-impl/mogo-core-function-devatools/src/main/java/com/zhjt/mogo_core_function_devatools/status/flow/autopilot/DoubleFlashImpl.kt b/core/function-impl/mogo-core-function-devatools/src/main/java/com/zhjt/mogo_core_function_devatools/status/flow/autopilot/DoubleFlashImpl.kt index b3ae5fca5d..b2d89dd5ec 100644 --- a/core/function-impl/mogo-core-function-devatools/src/main/java/com/zhjt/mogo_core_function_devatools/status/flow/autopilot/DoubleFlashImpl.kt +++ b/core/function-impl/mogo-core-function-devatools/src/main/java/com/zhjt/mogo_core_function_devatools/status/flow/autopilot/DoubleFlashImpl.kt @@ -52,7 +52,7 @@ internal class DoubleFlashImpl(ctx: Context): IFlow(ctx), IMo } - override fun onAutopilotAbility(isAutopilotAbility: Boolean, launchConditionData: LaunchConditionData?, unableAutopilotReasons: ArrayList?) { + override fun onAutopilotAbilityHighFrequency(isAutopilotAbility: Boolean, launchConditionData: LaunchConditionData?, unableAutopilotReasons: ArrayList?) { if (!isAutopilotAbility) { val target = unableAutopilotReasons?.find { it.unableType == CHASSIS_HAZARD_LIGHTS }?.also { itx -> isError = true diff --git a/core/function-impl/mogo-core-function-devatools/src/main/java/com/zhjt/mogo_core_function_devatools/status/flow/autopilot/GearImpl.kt b/core/function-impl/mogo-core-function-devatools/src/main/java/com/zhjt/mogo_core_function_devatools/status/flow/autopilot/GearImpl.kt index 90d4d632ee..bd9e7d0205 100644 --- a/core/function-impl/mogo-core-function-devatools/src/main/java/com/zhjt/mogo_core_function_devatools/status/flow/autopilot/GearImpl.kt +++ b/core/function-impl/mogo-core-function-devatools/src/main/java/com/zhjt/mogo_core_function_devatools/status/flow/autopilot/GearImpl.kt @@ -46,7 +46,7 @@ internal class GearImpl(ctx: Context): IFlow(ctx), IMoGoChassisGearS } } - override fun onAutopilotAbility(isAutopilotAbility: Boolean, launchConditionData: LaunchConditionData?, unableAutopilotReasons: ArrayList?) { + override fun onAutopilotAbilityHighFrequency(isAutopilotAbility: Boolean, launchConditionData: LaunchConditionData?, unableAutopilotReasons: ArrayList?) { if (!isAutopilotAbility) { val target = unableAutopilotReasons?.find { it.unableType == CHASSIS_GEAR }?.also { itx -> isError = true diff --git a/core/function-impl/mogo-core-function-devatools/src/main/java/com/zhjt/mogo_core_function_devatools/status/flow/autopilot/SteerImpl.kt b/core/function-impl/mogo-core-function-devatools/src/main/java/com/zhjt/mogo_core_function_devatools/status/flow/autopilot/SteerImpl.kt index 7db90a24ba..206f5da959 100644 --- a/core/function-impl/mogo-core-function-devatools/src/main/java/com/zhjt/mogo_core_function_devatools/status/flow/autopilot/SteerImpl.kt +++ b/core/function-impl/mogo-core-function-devatools/src/main/java/com/zhjt/mogo_core_function_devatools/status/flow/autopilot/SteerImpl.kt @@ -45,7 +45,7 @@ internal class SteerImpl(ctx: Context): IFlow(ctx), IMoGoChassisSte } } - override fun onAutopilotAbility(isAutopilotAbility: Boolean, launchConditionData: LaunchConditionData?, unableAutopilotReasons: ArrayList?) { + override fun onAutopilotAbilityHighFrequency(isAutopilotAbility: Boolean, launchConditionData: LaunchConditionData?, unableAutopilotReasons: ArrayList?) { Logger.d(TAG, "onAutopilotAbility->($isAutopilotAbility, $launchConditionData, ${unableAutopilotReasons?.joinToString(",")}") if (!isAutopilotAbility) { val target = unableAutopilotReasons?.find { it.unableType == CHASSIS_STEERING }?.also { itx -> diff --git a/core/mogo-core-function-api/src/main/java/com/mogo/eagle/core/function/api/autopilot/IMoGoAutopilotActionsListener.kt b/core/mogo-core-function-api/src/main/java/com/mogo/eagle/core/function/api/autopilot/IMoGoAutopilotActionsListener.kt index 5cf8930342..52c8f6c42e 100644 --- a/core/mogo-core-function-api/src/main/java/com/mogo/eagle/core/function/api/autopilot/IMoGoAutopilotActionsListener.kt +++ b/core/mogo-core-function-api/src/main/java/com/mogo/eagle/core/function/api/autopilot/IMoGoAutopilotActionsListener.kt @@ -9,5 +9,21 @@ import com.zhjt.mogo.adas.data.bean.UnableLaunchReason */ interface IMoGoAutopilotActionsListener { - fun onAutopilotAbility(isAutopilotAbility: Boolean, launchConditionData: LaunchConditionData?, unableAutopilotReasons: ArrayList?) + /** + * 高频 + */ + fun onAutopilotAbilityHighFrequency( + isAutopilotAbility: Boolean, + launchConditionData: LaunchConditionData?, + unableAutopilotReasons: ArrayList? + ){} + + /** + * 数据变动才会调用 + */ + fun onAutopilotAbility( + isAutopilotAbility: Boolean, + launchConditionData: LaunchConditionData?, + unableAutopilotReasons: ArrayList? + ){} } \ No newline at end of file diff --git a/core/mogo-core-function-call/src/main/java/com/mogo/eagle/core/function/call/autopilot/CallerAutopilotActionsListenerManager.kt b/core/mogo-core-function-call/src/main/java/com/mogo/eagle/core/function/call/autopilot/CallerAutopilotActionsListenerManager.kt index 88ecc1b0e2..4b57197f5b 100644 --- a/core/mogo-core-function-call/src/main/java/com/mogo/eagle/core/function/call/autopilot/CallerAutopilotActionsListenerManager.kt +++ b/core/mogo-core-function-call/src/main/java/com/mogo/eagle/core/function/call/autopilot/CallerAutopilotActionsListenerManager.kt @@ -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? ) { 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 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 launchAutopilotGear) { + this.launchAutopilotGear = launchAutopilotGear; + taxiUnmanned(); + } + + public void setStatusInfo(SsmInfo.SsmStatusInf statusInfo) { + this.statusInfo = statusInfo; } - private void onCallback(SsmInfo.SsmStatusInf statusInfo) { - ArrayList 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 onSsmCallback(ArrayList 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 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); } }