package com.aros.apron.manager; import static com.aros.apron.tools.Utils.getIDJIErrorMsg; import static dji.sdk.keyvalue.key.KeyTools.createKey; import android.os.Handler; import android.os.Looper; import android.text.TextUtils; import android.util.Log; import androidx.annotation.NonNull; import androidx.annotation.Nullable; import com.aros.apron.base.BaseManager; import com.aros.apron.constant.AMSConfig; import com.aros.apron.entity.ApronExecutionStatus; import com.aros.apron.entity.MessageDown; import com.aros.apron.entity.Movement; import com.aros.apron.tools.AlternateArucoDetect; import com.aros.apron.tools.ApronArucoDetect; import com.aros.apron.tools.DroneHelper; import com.aros.apron.tools.LocationUtils; import com.aros.apron.tools.LogUtil; import com.aros.apron.tools.MqttManager; import com.aros.apron.tools.PreferenceUtils; import com.google.gson.Gson; import org.greenrobot.eventbus.EventBus; import java.util.List; import dji.sdk.keyvalue.key.AirLinkKey; import dji.sdk.keyvalue.key.FlightControllerKey; import dji.sdk.keyvalue.key.KeyTools; import dji.sdk.keyvalue.key.ProductKey; import dji.sdk.keyvalue.key.RtkMobileStationKey; import dji.sdk.keyvalue.value.common.Attitude; import dji.sdk.keyvalue.value.common.EmptyMsg; import dji.sdk.keyvalue.value.common.LocationCoordinate2D; import dji.sdk.keyvalue.value.common.LocationCoordinate3D; import dji.sdk.keyvalue.value.common.Velocity3D; import dji.sdk.keyvalue.value.flightcontroller.FailsafeAction; import dji.sdk.keyvalue.value.flightcontroller.FlightMode; import dji.sdk.keyvalue.value.flightcontroller.GPSSignalLevel; import dji.sdk.keyvalue.value.flightcontroller.GoHomeState; import dji.sdk.keyvalue.value.flightcontroller.LEDsSettings; import dji.sdk.keyvalue.value.flightcontroller.PropellerRotationCommand; import dji.sdk.keyvalue.value.flightcontroller.PropellerRotationCommandResult; import dji.sdk.keyvalue.value.flightcontroller.PropellerRotationStatus; import dji.sdk.keyvalue.value.flightcontroller.RemoteControllerFlightMode; import dji.sdk.keyvalue.value.flightcontroller.RidWorkingStatusPushMsg; import dji.sdk.keyvalue.value.flightcontroller.WindDirection; import dji.sdk.keyvalue.value.product.ProductType; import dji.sdk.keyvalue.value.rtkmobilestation.RTKTakeoffAltitudeInfo; import dji.v5.common.callback.CommonCallbacks; import dji.v5.common.error.IDJIError; import dji.v5.common.utils.GpsUtils; import dji.v5.manager.KeyManager; import dji.v5.manager.aircraft.perception.data.PerceptionInfo; import dji.v5.manager.aircraft.perception.listener.PerceptionInformationListener; import dji.v5.manager.aircraft.virtualstick.VirtualStickManager; import dji.v5.manager.aircraft.waypoint3.WaypointMissionManager; import dji.v5.manager.diagnostic.DJIDeviceHealthInfo; import dji.v5.manager.diagnostic.DJIDeviceHealthInfoChangeListener; import dji.v5.manager.diagnostic.DJIDeviceStatus; import dji.v5.manager.diagnostic.DJIDeviceStatusChangeListener; import dji.v5.manager.interfaces.IDeviceHealthManager; import dji.v5.manager.interfaces.IDeviceStatusManager; import dji.v5.manager.interfaces.IPerceptionManager; import dji.v5.manager.interfaces.IWaypointMissionManager; public class FlightManager extends BaseManager { private IDeviceHealthManager iDeviceHealthManager; private IDeviceStatusManager iDeviceStatusManager; private boolean isFlying; private boolean isMotorsOn; private FlightManager() { } private static class FlightControlHolder { private static final FlightManager INSTANCE = new FlightManager(); } public static FlightManager getInstance() { return FlightControlHolder.INSTANCE; } public void initFlightInfo() { Boolean isConnect = KeyManager.getInstance().getValue(createKey(FlightControllerKey.KeyConnection)); //添加飞机连接状态的mode KeyManager.getInstance().listen(createKey(FlightControllerKey.KeyConnection), this, new CommonCallbacks.KeyListener() { @Override public void onValueChange(@Nullable Boolean oldValue, @Nullable Boolean newValue) { if (newValue != null) { if (!newValue) { LogUtil.log(TAG,"遥控器断开连接"); // 飞机未连接时设置未连接状态 Movement.getInstance().setMode_code(14); OSDManager.getInstance().pushFlightAttitude(); Movement.getInstance().setMissionFinish(true); }else{ LogUtil.log(TAG,"setMode_code1(0)"); //连上就待机 Movement.getInstance().setMissionFinish(false); Movement.getInstance().setMode_code(0); OSDManager.getInstance().pushFlightAttitude(); } } } }); //重新复制一下如果一直断开这个回调会不执行 if(isConnect){ LogUtil.log(TAG,"setMode_code2(0)"); Movement.getInstance().setMode_code(0); }else{ Movement.getInstance().setMode_code(14); } if (isConnect != null && isConnect) { if (!TextUtils.isEmpty(PreferenceUtils.getInstance().getAlternatePointLon()) && !TextUtils.isEmpty(PreferenceUtils.getInstance().getAlternatePointLat())) { Movement.getInstance().setAlternatePointLon(PreferenceUtils.getInstance().getAlternatePointLon()); Movement.getInstance().setAlternatePointLat(PreferenceUtils.getInstance().getAlternatePointLat()); } iDeviceStatusManager = dji.v5.manager.diagnostic.DeviceStatusManager.getInstance(); iDeviceStatusManager.addDJIDeviceStatusChangeListener(new DJIDeviceStatusChangeListener() { @Override public void onDeviceStatusUpdate(DJIDeviceStatus from, DJIDeviceStatus to) { if (to != null && !TextUtils.isEmpty(to.description())) { Movement.getInstance().setPlaneMessage(to.description()); pushFlightAttitude(); } } }); //避障状态 IPerceptionManager perceptionManager = dji.v5.manager.aircraft.perception.PerceptionManager.getInstance(); perceptionManager.addPerceptionInformationListener(new PerceptionInformationListener() { @Override public void onUpdate(@NonNull PerceptionInfo information) { if (information != null) { if (information.getDownwardObstacleAvoidanceWorking()!=null){ Movement.getInstance().setDownside(information.getDownwardObstacleAvoidanceWorking() ? 1 : 0); } Movement.getInstance().setHorizon(information.isHorizontalObstacleAvoidanceEnabled() ? 1 : 0); if (information.getUpwardObstacleAvoidanceWorking()!=null){ Movement.getInstance().setUpside(information.getUpwardObstacleAvoidanceWorking() ? 1 : 0); } pushFlightAttitude(); } } }); iDeviceHealthManager = dji.v5.manager.diagnostic.DeviceHealthManager.getInstance(); iDeviceHealthManager.addDJIDeviceHealthInfoChangeListener(new DJIDeviceHealthInfoChangeListener() { @Override public void onDeviceHealthInfoUpdate(List infos) { if (infos != null && infos.size() > 0) { String warningMessage = infos.get(0).description(); if (!TextUtils.isEmpty(warningMessage)) { Movement.getInstance().setWarningMessage(warningMessage); pushFlightAttitude(); } } else { Log.e(TAG, "监听设备健康,无异常"); Movement.getInstance().setWarningMessage(""); pushFlightAttitude(); } } }); KeyManager.getInstance().listen(createKey(FlightControllerKey.KeyIsFlying), this, new CommonCallbacks.KeyListener() { @Override public void onValueChange(@Nullable Boolean oldValue, @Nullable Boolean newValue) { if (newValue != null) { if (newValue) { ApronExecutionStatus.getInstance().setAircraftWaitShutDown(false); //起飞了这个状态就看这里 if(Movement.getInstance().getFlightmode()==1){ Movement.getInstance().setMode_code(5); }else if(Movement.getInstance().getFlightmode()==2){ Movement.getInstance().setMode_code(17); } }else{ LogUtil.log(TAG,"setMode_code3(0)"); Movement.getInstance().setMode_code(0); OSDManager.getInstance().pushFlightAttitude(); } isFlying = newValue; Movement.getInstance().setPlaneWing(newValue); pushFlightAttitude(); } } }); KeyManager.getInstance().listen(createKey(FlightControllerKey.KeyAreMotorsOn), this, new CommonCallbacks.KeyListener() { @Override public void onValueChange(@Nullable Boolean oldValue, @Nullable Boolean newValue) { if (newValue != null) { isMotorsOn = newValue; pushFlightAttitude(); Movement.getInstance().setMotorsOn(newValue); } } }); //挡位 KeyManager.getInstance().listen(createKey(FlightControllerKey.KeyRemoteControllerFlightMode), this, new CommonCallbacks.KeyListener() { @Override public void onValueChange(@Nullable RemoteControllerFlightMode remoteControllerFlightMode, @Nullable RemoteControllerFlightMode t1) { if (remoteControllerFlightMode != null) { Movement.getInstance().setGear(remoteControllerFlightMode.value()); pushFlightAttitude(); } } }); KeyManager.getInstance().listen(KeyTools.createKey(ProductKey.KeyProductType), this, new CommonCallbacks.KeyListener() { @Override public void onValueChange(@Nullable ProductType productType, @Nullable ProductType t1) { if (t1 != null) { Movement.getInstance().setProductName(t1.name()); pushFlightAttitude(); } } }); //飞机SN号 KeyManager.getInstance().listen(createKey(FlightControllerKey.KeySerialNumber), this, new CommonCallbacks.KeyListener() { @Override public void onValueChange(@Nullable String s, @Nullable String t1) { if (t1 != null) { Movement.getInstance().setFirmware_version(t1); pushFlightAttitude(); } } }); //RID状态 KeyManager.getInstance().listen(createKey(FlightControllerKey.KeyRidWorkingStatusPush), this, new CommonCallbacks.KeyListener() { @Override public void onValueChange(@Nullable RidWorkingStatusPushMsg ridWorkingStatusPushMsg, @Nullable RidWorkingStatusPushMsg t1) { if (t1 != null) { Movement.getInstance().setRid_state(t1.getIsRidSupport()); pushFlightAttitude(); } } }); //rtk起飞高度(计算海拔高度使用) KeyManager.getInstance().listen(createKey(RtkMobileStationKey.KeyRTKTakeoffAltitudeInfo), this, new CommonCallbacks.KeyListener() { @Override public void onValueChange(@Nullable RTKTakeoffAltitudeInfo rtkTakeoffAltitudeInfo, @Nullable RTKTakeoffAltitudeInfo t1) { if (t1 != null) { Movement.getInstance().setRtk_takeoff_altitude(t1.getAltitude()); pushFlightAttitude(); } } }); KeyManager.getInstance().listen(createKey(FlightControllerKey.KeyAircraftLocation3D), this, new CommonCallbacks.KeyListener() { @Override public void onValueChange(@Nullable LocationCoordinate3D oldValue, @Nullable LocationCoordinate3D newValue) { if (newValue != null) { if (newValue.getAltitude() != null) { Movement.getInstance().setElevation(newValue.getAltitude()); Movement.getInstance().setTask_height(newValue.getAltitude()); } double distance = LocationUtils.getDistance(String.valueOf(Movement.getInstance().getHomepoint_longitude()), String.valueOf(Movement.getInstance().getHomepoint_latitude()), String.valueOf(newValue.getLongitude()), String.valueOf(newValue.getLatitude())); Movement.getInstance().setHome_distance(distance); Movement.getInstance().setHeight(GpsUtils.egm96Altitude((Movement.getInstance().getRtk_takeoff_altitude() + Movement.getInstance().getElevation()), newValue.getLatitude(), newValue.getLongitude())); Movement.getInstance().setLatitude(newValue.getLatitude()); Movement.getInstance().setLongitude(newValue.getLongitude()); pushFlightAttitude(); } } }); //水平&垂直速度 KeyManager.getInstance().listen(createKey(FlightControllerKey.KeyAircraftVelocity), this, new CommonCallbacks.KeyListener() { @Override public void onValueChange(@Nullable Velocity3D oldValue, @Nullable Velocity3D newValue) { if (newValue != null) { if (newValue.getZ() != null) { Movement.getInstance().setVertical_speed(newValue.getZ()); } if (newValue.getY() != null && newValue.getX() != null) { Movement.getInstance().setHorizontal_speed(Math.abs(Math.sqrt((newValue.getX() * newValue.getX()) + (newValue.getY() * newValue.getY())))); } pushFlightAttitude(); } } }); //卫星数 KeyManager.getInstance().listen(createKey(FlightControllerKey.KeyGPSSatelliteCount), this, new CommonCallbacks.KeyListener() { @Override public void onValueChange(@Nullable Integer oldValue, @Nullable Integer newValue) { if (newValue != null) { Movement.getInstance().setGps_number(newValue); Movement.getInstance().setRtk_number(newValue); pushFlightAttitude(); } } }); KeyManager.getInstance().listen(createKey(FlightControllerKey.KeyGPSSignalLevel), this, new CommonCallbacks.KeyListener() { @Override public void onValueChange(@Nullable GPSSignalLevel gpsSignalLevel, @Nullable GPSSignalLevel t1) { if (t1 != null) { Movement.getInstance().setQuality(t1.value()); pushFlightAttitude(); } } }); //返航位置 KeyManager.getInstance().listen(createKey(FlightControllerKey.KeyHomeLocation), this, new CommonCallbacks.KeyListener() { @Override public void onValueChange(@Nullable LocationCoordinate2D oldValue, @Nullable LocationCoordinate2D newValue) { if (newValue != null) { Movement.getInstance().setHomepoint_latitude(newValue.getLatitude()); Movement.getInstance().setHomepoint_longitude(newValue.getLongitude()); pushFlightAttitude(); } } }); KeyManager.getInstance().listen(createKey(FlightControllerKey.KeyDistanceLimitEnabled), this, new CommonCallbacks.KeyListener() { @Override public void onValueChange(@Nullable Boolean aBoolean, @Nullable Boolean t1) { if (t1 != null) { Movement.getInstance().setDistanceLimitEnabled(t1); pushFlightAttitude(); } } }); KeyManager.getInstance().listen(createKey(FlightControllerKey.KeyWindSpeed), this, new CommonCallbacks.KeyListener() { @Override public void onValueChange(@Nullable Integer oldValue, @Nullable Integer newValue) { if (newValue != null) { Movement.getInstance().setWind_speed(newValue); pushFlightAttitude(); } } }); KeyManager.getInstance().listen(createKey(FlightControllerKey.KeyWindDirection), this, new CommonCallbacks.KeyListener() { @Override public void onValueChange(@Nullable WindDirection windDirection, @Nullable WindDirection t1) { if (t1!=null){ Movement.getInstance().setWind_direction(t1.value()); pushFlightAttitude(); } } }); KeyManager.getInstance().listen(createKey(FlightControllerKey.KeyCompassHeading), this, new CommonCallbacks.KeyListener() { @Override public void onValueChange(@Nullable Double oldValue, @Nullable Double newValue) { if (newValue != null) { Movement.getInstance().setAngleYaw(newValue.intValue()); pushFlightAttitude(); } } }); //飞行器夜航灯状态 KeyManager.getInstance().listen(createKey(FlightControllerKey.KeyLEDsSettings), this, new CommonCallbacks.KeyListener() { @Override public void onValueChange(@Nullable LEDsSettings leDsSettings, @Nullable LEDsSettings t1) { if (t1 != null) { Movement.getInstance().setNight_lights_state(t1.getNavigationLEDsOn() ? 1 : 0); pushFlightAttitude(); } } }); //飞行器状态 (未找到类似上云api的枚举格式，先用FlightMode替代) KeyManager.getInstance().listen(createKey(FlightControllerKey.KeyFlightMode), this, new CommonCallbacks.KeyListener() { @Override public void onValueChange(@Nullable FlightMode oldValue, @Nullable FlightMode newValue) { if (newValue != null) { if (newValue == FlightMode.MOTOR_START) { //刚起飞时，重置保存的状态 PreferenceUtils.getInstance().setNeedTriggerAlterArucoLand(false); PreferenceUtils.getInstance().setNeedTriggerApronArucoLand(false); PreferenceUtils.getInstance().setTriggerToAlternatePoint(false); } Movement.getInstance().setPlaneMode(newValue.name()); pushFlightAttitude(); switch (newValue){ case TAKE_OFF_READY: //Movement.getInstance().setMode_code(2); break; case VIRTUAL_STICK: //Movement.getInstance().setMode_code(3); break; case AUTO_TAKE_OFF: //我觉得可能会重复设置 //Movement.getInstance().setMode_code(4); break; case WAYPOINT: //Movement.getInstance().setMode_code(5); break; case PANO: Movement.getInstance().setMode_code(6); break; case FOLLOW_ME: Movement.getInstance().setMode_code(7); break; case AUTO_AVOIDANCE: Movement.getInstance().setMode_code(8); break; case GO_HOME: Movement.getInstance().setMode_code(9); break; case AUTO_LANDING: Movement.getInstance().setMode_code(10); //重置这个飞行状态 Movement.getInstance().setFlightmode(0); break; case FORCE_LANDING: Movement.getInstance().setMode_code(11); break; case POI: Movement.getInstance().setMode_code(20); break; } } } }); KeyManager.getInstance().listen(createKey(FlightControllerKey.KeyAircraftAttitude), this, new CommonCallbacks.KeyListener() { @Override public void onValueChange(@Nullable Attitude attitude, @Nullable Attitude t1) { if (t1 != null) { Movement.getInstance().setAttitude_pitch(t1.getPitch()); Movement.getInstance().setAttitude_head(t1.getYaw()); Movement.getInstance().setTask_attitude_head(t1.getYaw()); Movement.getInstance().setAttitude_roll(t1.getRoll()); } pushFlightAttitude(); } }); KeyManager.getInstance().listen(createKey(AirLinkKey.KeyUpLinkQuality), this, new CommonCallbacks.KeyListener() { @Override public void onValueChange(@Nullable Integer oldValue, @Nullable Integer newValue) { if (newValue != null) { Movement.getInstance().setRemoteControlSignal(newValue); } pushFlightAttitude(); } }); KeyManager.getInstance().listen(createKey(AirLinkKey.KeyDownLinkQuality), this, new CommonCallbacks.KeyListener() { @Override public void onValueChange(@Nullable Integer oldValue, @Nullable Integer newValue) { if (newValue != null) { Movement.getInstance().setPictureBiographySignal(newValue); } pushFlightAttitude(); } }); GoHomeState goHomeState = KeyManager.getInstance().getValue(createKey(FlightControllerKey.KeyGoHomeStatus)); if (goHomeState != null) { Movement.getInstance().setGoHomeState(goHomeState.value()); } KeyManager.getInstance().listen(createKey(FlightControllerKey.KeyGoHomeStatus), this, new CommonCallbacks.KeyListener() { @Override public void onValueChange(@Nullable GoHomeState oldValue, @Nullable GoHomeState newValue) { if (newValue != null) { Movement.getInstance().setGoHomeState(newValue.value()); LogUtil.log(TAG, "GoHomeStatus:" + newValue.name()); goHomeExecutionState = newValue.value(); //返航或降落中不允许恢复断点航线 if (newValue.value() == 1 || newValue.value() == 2 || newValue.value() == 3) { Movement.getInstance().setCurrentWaypointIndex(0); } //返航后触发可入库条件 if (newValue.value() == 2) { triggerLandOrGoHome = true; Movement.getInstance().setVirtualStickQuitMission(false); } pushFlightAttitude(); } } }); KeyManager.getInstance().listen(createKey(FlightControllerKey.KeyUltrasonicHeight), this, new CommonCallbacks.KeyListener() { @Override public void onValueChange(@Nullable Integer oldValue, @Nullable Integer newValue) { if (newValue != null) { Movement.getInstance().setUltrasonicHeight(newValue); } pushFlightAttitude(); } }); //总里程(单位m) KeyManager.getInstance().listen(createKey(FlightControllerKey.KeyAircraftTotalFlightDistance), this, new CommonCallbacks.KeyListener() { @Override public void onValueChange(@Nullable Double aDouble, @Nullable Double t1) { if (t1 != null) { Movement.getInstance().setTotal_flight_distance(t1); } pushFlightAttitude(); } }); //航迹 ID KeyManager.getInstance().listen(createKey(FlightControllerKey.KeyTrackId), this, new CommonCallbacks.KeyListener() { @Override public void onValueChange(@Nullable String s, @Nullable String t1) { if (t1 != null) { Movement.getInstance().setTrack_id(t1); Movement.getInstance().setTask_track_id(t1); } pushFlightAttitude(); } }); //总航时 KeyManager.getInstance().listen(createKey(FlightControllerKey.KeyAircraftTotalFlightDuration), this, new CommonCallbacks.KeyListener() { @Override public void onValueChange(@Nullable Double aDouble, @Nullable Double t1) { if (t1 != null) { Movement.getInstance().setTotal_flight_time(t1); } pushFlightAttitude(); } }); //总架次 KeyManager.getInstance().listen(createKey(FlightControllerKey.KeyAircraftTotalFlightTimes), this, new CommonCallbacks.KeyListener() { @Override public void onValueChange(@Nullable Integer integer, @Nullable Integer t1) { if (t1 != null) { Movement.getInstance().setTotal_flight_sorties(t1); } pushFlightAttitude(); } }); //失联动作 KeyManager.getInstance().listen(createKey(FlightControllerKey.KeyFailsafeAction), this, new CommonCallbacks.KeyListener() { @Override public void onValueChange(@Nullable FailsafeAction failsafeAction, @Nullable FailsafeAction t1) { if (t1 != null) { Movement.getInstance().setRc_lost_action(t1.value()); } pushFlightAttitude(); } }); //返航高度 KeyManager.getInstance().listen(createKey(FlightControllerKey.KeyGoHomeHeight), this, new CommonCallbacks.KeyListener() { @Override public void onValueChange(@Nullable Integer integer, @Nullable Integer t1) { if (t1 != null) { Movement.getInstance().setRth_altitude(t1); } pushFlightAttitude(); } }); //限高 KeyManager.getInstance().listen(createKey(FlightControllerKey.KeyHeightLimit), this, new CommonCallbacks.KeyListener() { @Override public void onValueChange(@Nullable Integer integer, @Nullable Integer t1) { if (t1 != null) { Movement.getInstance().setHeight_limit(t1); } pushFlightAttitude(); } }); //是否接近限高 KeyManager.getInstance().listen(createKey(FlightControllerKey.KeyIsNearHeightLimit), this, new CommonCallbacks.KeyListener() { @Override public void onValueChange(@Nullable Boolean aBoolean, @Nullable Boolean t1) { if (t1 != null) { Movement.getInstance().setIs_near_height_limit(t1 ? 1 : 0); } pushFlightAttitude(); } }); //是否接近设定的限制距离 KeyManager.getInstance().listen(createKey(FlightControllerKey.KeyIsNearDistanceLimit), this, new CommonCallbacks.KeyListener() { @Override public void onValueChange(@Nullable Boolean aBoolean, @Nullable Boolean t1) { if (t1 != null) { Movement.getInstance().setIs_near_distance_limit(t1 ? 1 : 0); Movement.getInstance().setIs_near_area_limit(t1 ? 1 : 0); } pushFlightAttitude(); } }); //是否开启限远 KeyManager.getInstance().listen(createKey(FlightControllerKey.KeyDistanceLimitEnabled), this, new CommonCallbacks.KeyListener() { @Override public void onValueChange(@Nullable Boolean aBoolean, @Nullable Boolean t1) { if (t1 != null) { Movement.getInstance().setIs_near_distance_limit(t1 ? 1 : 0); } pushFlightAttitude(); } }); //限远 KeyManager.getInstance().listen(createKey(FlightControllerKey.KeyDistanceLimit), this, new CommonCallbacks.KeyListener() { @Override public void onValueChange(@Nullable Integer integer, @Nullable Integer t1) { if (t1 != null) { Movement.getInstance().setDistance_limit(t1); } pushFlightAttitude(); } }); KeyManager.getInstance().listen(createKey(FlightControllerKey.KeyLowBatteryWarningThreshold), this, new CommonCallbacks.KeyListener() { @Override public void onValueChange(@Nullable Integer integer, @Nullable Integer t1) { if (t1 != null) { Movement.getInstance().setLowBatteryWarningThreshold(t1); pushFlightAttitude(); } } }); KeyManager.getInstance().listen(createKey(FlightControllerKey.KeySeriousLowBatteryWarningThreshold), this, new CommonCallbacks.KeyListener() { @Override public void onValueChange(@Nullable Integer integer, @Nullable Integer t1) { if (t1 != null) { Movement.getInstance().setSeriousLowBatteryWarningThreshold(t1); pushFlightAttitude(); } } }); KeyManager.getInstance().listen(createKey(FlightControllerKey.KeyLowBatteryRTHEnabled), this, new CommonCallbacks.KeyListener() { @Override public void onValueChange(@Nullable Boolean aBoolean, @Nullable Boolean t1) { if (t1 != null) { Movement.getInstance().setLowBatteryRTHEnabled(t1 ? 1 : 0); pushFlightAttitude(); } } }); } else { LogUtil.log(TAG, "初始化飞控失败" + "flight controller is null"); } } //标识是否降落后触发关舱门 private boolean triggerLandOrGoHome; //标识飞机是否执行完任务处于landing或gohome，避免刚飞出去就执行精准降落 private int goHomeExecutionState; //确保每次流程只发送触发降落一次，降落完成后设置为false，而不是最后开始landing时触发，如果不再landing时设置为false，那么在landing的途中，也可能再次触发landing private boolean isSendDetect; //飞机飞走后是否发送关舱门(确保只发送一次) private boolean sendCloseCabinDoorMsg; //飞机飞回后是否发送开舱门(确保只发送一次) private boolean sendOpenCabinDoorMsg; //飞机是否在返航,处理云台归中逻辑(确保只发送一次) public boolean isGimbalReset; //飞机是否在降落,处理云台朝下逻辑(确保只发送一次) public boolean isGimbalDownwards; //(决定飞机触发最后landing的重要因素)是否触发最后一步Landing，如果触发过，确保landing时不再触发landing public boolean isTriggerLanding; //飞机电机起转后发送一次开始飞行 private boolean sendStartTakeOffMsg; public boolean isSendDetect() { return isSendDetect; } public void setSendDetect(boolean sendDetect) { isSendDetect = sendDetect; } private void pushFlightAttitude() { //强制返航 batteryLowLanding(); //关仓门 closeCabinDoor(); //开舱门 openCabinDoor(); //降落时将云台朝下 gimbalDownwards(); //返航时将云台归中,曝光ISO降低 gimbalAndCameraReset(); //开始视觉识别降落 checkAndStartVisionLanding(); //触发入库 droneStorage(); } //(决定飞机触发电量低的返航) public boolean isTriggerRoomBattrryLanding; private void batteryLowLanding() { if (isTriggerRoomBattrryLanding){ return; } int forcedBattery = Integer.valueOf(PreferenceUtils.getInstance().getForcedBattery()); Integer value = KeyManager.getInstance().getValue(createKey(FlightControllerKey. KeyBatteryPowerPercent, 0)); String missionState = Movement.getInstance().getWaypointMissionExecuteState(); boolean isMissionExecuting = (!TextUtils.isEmpty(missionState) && (missionState.equals("EXECUTING") || missionState.equals("ENTER_WAYLINE")) ||(!TextUtils.isEmpty(Movement.getInstance().getPlaneMode()) &&Movement.getInstance().getPlaneMode().equals("WAYPOINT"))); // 获取飞行状态和航线状态 boolean isFlyingAndBatteryOk = isFlying &&value!=null&&value< forcedBattery&&isMissionExecuting; boolean isAlterLandStatus=(PreferenceUtils.getInstance().getNeedTriggerAlterArucoLand()||PreferenceUtils.getInstance().getTriggerToAlternatePoint()); if (isFlyingAndBatteryOk&& !isTriggerRoomBattrryLanding&&!isAlterLandStatus) { isTriggerRoomBattrryLanding = true; KeyManager.getInstance().performAction(createKey(FlightControllerKey.KeyStartGoHome), new CommonCallbacks.CompletionCallbackWithParam() { @Override public void onSuccess(EmptyMsg emptyMsg) { sendEvent2Server("电量低于阈值，强制返航",2); } @Override public void onFailure(@NonNull IDJIError error) { sendEvent2Server("电量低于阈值，返航失败",2); } }); } } private void closeCabinDoor() { // 获取飞行状态和航线状态 boolean isFlyingAndHeightOk = isFlying && Movement.getInstance().getElevation() > 10; boolean isDebugMode = PreferenceUtils.getInstance().getIsDebugMode(); String missionState = Movement.getInstance().getWaypointMissionExecuteState(); boolean isMissionExecuting = (!TextUtils.isEmpty(missionState) && (missionState.equals("EXECUTING") || missionState.equals("ENTER_WAYLINE")) || (!TextUtils.isEmpty(Movement.getInstance().getPlaneMode()) && Movement.getInstance().getPlaneMode().equals("WAYPOINT"))); // 当飞机在飞行，高度足够，且航线状态为EXECUTING或ENTER_WAYLINE时，触发关舱门，开启水平避障 if (!PreferenceUtils.getInstance().getTriggerToAlternatePoint() && isFlyingAndHeightOk && !isDebugMode && isMissionExecuting && !sendCloseCabinDoorMsg) { sendCloseCabinDoorMsg = true; DockCloseManager.getInstance().sendDockCloseMsg2Server(); PerceptionManager.getInstance().setPerceptionEnable(true); } } private static final int FLYING_HEIGHT_THRESHOLD = 10; // 开舱门飞行高度阈值 private static final int DISTANCE_THRESHOLD = 500; // 返航距离阈值 private void openCabinDoor() { boolean isReturningHome = goHomeExecutionState == GoHomeState.RETURNING_TO_HOME.value() || goHomeExecutionState == GoHomeState.LANDING.value(); double distance = Movement.getInstance().getHome_distance(); double flyingHeight = Movement.getInstance().getElevation(); boolean isDebugMode = PreferenceUtils.getInstance().getIsDebugMode(); boolean isDistanceAndHeightValid = distance < DISTANCE_THRESHOLD && flyingHeight > FLYING_HEIGHT_THRESHOLD && !sendOpenCabinDoorMsg; if (!PreferenceUtils.getInstance().getTriggerToAlternatePoint() && isReturningHome && isDistanceAndHeightValid && !isDebugMode) { LogUtil.log(TAG, "返航距离:" + distance + "---当前高度:" + flyingHeight); sendOpenCabinDoorMsg = true; DockOpenManager.getInstance().sendDockOpenMsg2Server(); } } //降落时将云台朝下 private void gimbalDownwards() { if (goHomeExecutionState == GoHomeState.LANDING.value() && Movement.getInstance().getElevation() > 11 && !isGimbalDownwards) { DroneHelper.getInstance().setGimbalPitchDegree(); //将镜头设置为自动对焦 DroneHelper.getInstance().setCameraFocusMode(); isGimbalDownwards = true; PerceptionManager.getInstance().setPerceptionEnable(false); if (Movement.getInstance().getIsRecording()==1){ CameraManager.getInstance().stopRecordVideo(null); } } } private boolean shouldResetGimbalAndCamera() { return goHomeExecutionState == GoHomeState.RETURNING_TO_HOME.value() && Movement.getInstance().getElevation() > 20 && !isGimbalReset; } private void gimbalAndCameraReset() { if (shouldResetGimbalAndCamera()) { GimbalManager.getInstance().gimbalReset(); CameraManager.getInstance().resumeLensToWideISOManual(); isGimbalReset = true; } } // 检查是否满足开始视觉识别降落的条件 private void checkAndStartVisionLanding() { boolean shouldStartVisionLanding = (PreferenceUtils.getInstance().getLandType() == 2); if (shouldStartVisionLanding) { startVisionLanding(); // 检查是否满足降落条件 checkLandingConditions(); } } private static final double FLYING_HEIGHT_THRESHOLD_MAX = 13.0; private static final double FLYING_HEIGHT_THRESHOLD_MAX_ALTERNATE = 15.0; private static final double FLYING_HEIGHT_THRESHOLD_MIN = -2; private static final double FLYING_HEIGHT_THRESHOLD_MIN_ALTERNATE = 2.0; private void startVisionLanding() { boolean isDebugMode = PreferenceUtils.getInstance().getIsDebugMode(); boolean triggerToAlternatePoint = PreferenceUtils.getInstance().getTriggerToAlternatePoint(); boolean needTriggerApronArucoLand = PreferenceUtils.getInstance().getNeedTriggerApronArucoLand(); boolean needTriggerAlterArucoLand = PreferenceUtils.getInstance().getNeedTriggerAlterArucoLand(); double thresholdMax = triggerToAlternatePoint ? FLYING_HEIGHT_THRESHOLD_MAX_ALTERNATE : FLYING_HEIGHT_THRESHOLD_MAX; if (isFlying && Movement.getInstance().getElevation() < thresholdMax && !isSendDetect) { double flyingHeight = Movement.getInstance().getElevation(); double thresholdMin = triggerToAlternatePoint ? FLYING_HEIGHT_THRESHOLD_MIN_ALTERNATE : FLYING_HEIGHT_THRESHOLD_MIN; if (flyingHeight > thresholdMin) { boolean shouldTriggerDetection; if (isDebugMode) { shouldTriggerDetection = goHomeExecutionState == 2; } else { shouldTriggerDetection = goHomeExecutionState == 2 || needTriggerApronArucoLand || needTriggerAlterArucoLand; } if (shouldTriggerDetection) { triggerArucoDetection(); } } } } private void triggerArucoDetection() { //当电池电量大于40时，允许复降次数设置为10次 if (Movement.getInstance().getBattery_a_capacity_percent() > 40) { AMSConfig.getInstance().setAlternateLandingTimes(10 + ""); } if (PreferenceUtils.getInstance().getTriggerToAlternatePoint()) { LogUtil.log(TAG, "识别AlterTag:" + PreferenceUtils.getInstance().getNeedTriggerAlterArucoLand()); EventBus.getDefault().post(FLAG_START_DETECT_ARUCO_ALTERNATE); PreferenceUtils.getInstance().setNeedTriggerAlterArucoLand(true); PreferenceUtils.getInstance().setNeedTriggerApronArucoLand(false); LogUtil.log(TAG, "开始识别备降点二维码,椭球高度:" + Movement.getInstance().getElevation() + "米" + "--超声波高度:" + Movement.getInstance().getUltrasonicHeight() + "分米"); sendEvent2Server("开始备降点视觉降落",1); } else { LogUtil.log(TAG, "识别ApronTag:" + PreferenceUtils.getInstance().getNeedTriggerApronArucoLand()); EventBus.getDefault().post(FLAG_START_DETECT_ARUCO_APRON); PreferenceUtils.getInstance().setNeedTriggerAlterArucoLand(false); PreferenceUtils.getInstance().setNeedTriggerApronArucoLand(true); LogUtil.log(TAG, "开始识别机库二维码,椭球高度:" + Movement.getInstance().getElevation() + "米" + "--超声波高度:" + Movement.getInstance().getUltrasonicHeight() + "分米"); sendEvent2Server("开始视觉降落",1); } isSendDetect = true; PerceptionManager.getInstance().setPerceptionEnable(false); } // 定义常量用于EventBus public static final String FLAG_DOWN_LAND = "FLAG_DOWN_LAND"; public static final String FLAG_START_DETECT_ARUCO_APRON = "FLAG_START_DETECT_ARUCO_APRON"; public static final String FLAG_START_DETECT_ARUCO_ALTERNATE = "FLAG_START_DETECT_ARUCO_ALTERNATE"; public static final String FLAG_STOP_ARUCO = "FLAG_STOP_ARUCO"; // 检查是否满足降落条件，并触发相应的降落逻辑 public void checkLandingConditions() { //到达备降点触发了直接降落高度或备降点未识别到二维码 if (PreferenceUtils.getInstance().getNeedTriggerAlterArucoLand() && shouldStopVisionAndLanding()) { stopArucoDetectAndLanding(3); LogUtil.log(TAG, "备降点直接降落"); } else { if (shouldStopVisionAndLanding()) { stopArucoDetectAndLanding(2); } } } public void stopArucoDetectAndLanding(int i) { logLandingHeight(i); DroneHelper.getInstance().exitVirtualStickMode(); EventBus.getDefault().post(FLAG_DOWN_LAND); PreferenceUtils.getInstance().setNeedTriggerApronArucoLand(false); PreferenceUtils.getInstance().setNeedTriggerAlterArucoLand(false); PreferenceUtils.getInstance().setTriggerToAlternatePoint(false); isGimbalReset = false; isTriggerLanding = true; isGimbalDownwards = false; } /** * 打印这里的条件，判断是否满足降落停浆 * * @return */ private boolean shouldStopVisionAndLanding() { //记录isFlying || isMotorsOn可能桨叶在转但是飞机不在飞的情况，可能导致不触发landing if (PreferenceUtils.getInstance().getNeedTriggerAlterArucoLand()) { return !isTriggerLanding && (isFlying || isMotorsOn) && AlternateArucoDetect.getInstance().isCanLanding(); } else { return !isTriggerLanding && (isFlying || isMotorsOn) && ( ApronArucoDetect.getInstance().isCanLanding()|| (ApronArucoDetect.getInstance().isStartFastStick() && ApronArucoDetect.getInstance().getCheckThrowingErrorsTimes() > 100)); } } private void logLandingHeight(int i) { LogUtil.log(TAG, "降落高度" + Movement.getInstance().getElevation() + "米---" + Movement.getInstance().getUltrasonicHeight() + "分米"); } private void droneStorage() { boolean isDebugMode = PreferenceUtils.getInstance().getIsDebugMode(); // 检查无人机是否满足降落和入库的条件 if (triggerLandOrGoHome && !isMotorsOn && !isFlying && Movement.getInstance().getElevation() <= 0.0) { // 重置降落或返航的触发标志 triggerLandOrGoHome = false; // 禁用触发和检测标志 isSendDetect = false; isTriggerLanding = false; sendCloseCabinDoorMsg = false; ApronArucoDetect.getInstance().setCanLanding(false); // 发布事件，通知其他组件停止Aruco检测 EventBus.getDefault().post(FLAG_STOP_ARUCO); //调试模式下不上传媒体文件 不入库 if (!isDebugMode) { //这里可能也会触发备降点关舱门的逻辑 if (!PreferenceUtils.getInstance().getNeedTriggerAlterArucoLand()) { // 发送无人机入库消息到服务器********************待修改************************ ApronExecutionStatus.getInstance().setAircraftWaitShutDown(false); DockStorageManager.getInstance().sendDockStorageMsg2Server( ); } // 上传媒体文件 SystemManager.getInstance().upLoadMedia(MqttManager.getInstance().mqttAndroidClient); } // 避免在下次起飞时触发视觉识别 PreferenceUtils.getInstance().setNeedTriggerApronArucoLand(false); PreferenceUtils.getInstance().setNeedTriggerAlterArucoLand(false); PreferenceUtils.getInstance().setTriggerToAlternatePoint(false); // 延迟1秒设置任务完成标志(防止他不上报我的待机状态) // new Handler(Looper.getMainLooper()).postDelayed(() -> { //Movement.getInstance().setMissionFinish(true); // }, 1000); //原有那个替换 Movement.getInstance().setMissionFinish1(true); Movement.getInstance().setTask_current_step(25); sendOpenCabinDoorMsg=false; } } //返航 public void startGoHome(MessageDown message) { FlightMode flightMode = KeyManager.getInstance().getValue(createKey(FlightControllerKey.KeyFlightMode)); if (Movement.getInstance().isPlaneWing() && Movement.getInstance().getHome_distance() < 20 && Movement.getInstance().getElevation() < 88 && Movement.getInstance().getBattery_a_capacity_percent() > 25 && ((flightMode != null && flightMode == FlightMode.WAYPOINT) || (flightMode != null && flightMode == FlightMode.AUTO_TAKE_OFF))) { WayLineExecutingInterruptManager.getInstance().onExecutingInterruptToDo(); } else { Boolean isConnect = KeyManager.getInstance().getValue(createKey(FlightControllerKey.KeyConnection)); if (isConnect != null && isConnect) { KeyManager.getInstance().performAction(createKey(FlightControllerKey.KeyStartGoHome), new CommonCallbacks.CompletionCallbackWithParam() { @Override public void onSuccess(EmptyMsg emptyMsg) { sendMsg2Server( message); } @Override public void onFailure(@NonNull IDJIError error) { sendFailMsg2Server( message, "返航执行失败:" + getIDJIErrorMsg(error)); } }); } else { sendFailMsg2Server( message, "返航执行失败:飞控未连接"); } } } //取消返航 public void stopGoHome(MessageDown message) { Boolean isConnect = KeyManager.getInstance().getValue(createKey(FlightControllerKey.KeyConnection)); if (isConnect != null && isConnect) { KeyManager.getInstance().performAction(createKey(FlightControllerKey.KeyStopGoHome), new CommonCallbacks.CompletionCallbackWithParam() { @Override public void onSuccess(EmptyMsg emptyMsg) { Movement.getInstance().setMode_code(3); sendMsg2Server( message); } @Override public void onFailure(@NonNull IDJIError error) { sendFailMsg2Server( message, "取消返航执行失败:" + getIDJIErrorMsg(error)); } }); } else { sendFailMsg2Server( message, "飞控未连接"); } } private void resetAircrftLandingStatus() { // 避免在下次起飞时触发视觉识别 PreferenceUtils.getInstance().setNeedTriggerApronArucoLand(false); PreferenceUtils.getInstance().setNeedTriggerAlterArucoLand(false); PreferenceUtils.getInstance().setTriggerToAlternatePoint(false); //设置为未触发开始识别二维码状态 isSendDetect = false; EventBus.getDefault().post(FLAG_STOP_ARUCO); } //开启低速转浆 public void startPropellerRotation(MessageDown message) { KeyManager.getInstance().performAction(createKey(FlightControllerKey.KeyPropellerRotation), PropellerRotationCommand.LOW_SPEED_ROTATION, new CommonCallbacks.CompletionCallbackWithParam() { @Override public void onSuccess(PropellerRotationCommandResult propellerRotationCommandResult) { sendMsg2Server(message); if (propellerRotationCommandResult.getStatus() == PropellerRotationStatus.ALL_MOTOR_IN_LOW_SPEED_ROTATION) { Movement.getInstance().setPropellerRotation(true); } LogUtil.log(TAG, "开始低速转浆结果:" + new Gson().toJson(propellerRotationCommandResult)); } @Override public void onFailure(@NonNull IDJIError error) { LogUtil.log(TAG, "开始低速转浆失败:" + getIDJIErrorMsg(error)); sendFailMsg2Server(message, "开始低速转浆失败:" + getIDJIErrorMsg(error)); } }); } //停止低速转浆 public void stopPropellerRotation(MessageDown message) { KeyManager.getInstance().performAction(createKey(FlightControllerKey.KeyPropellerRotation), PropellerRotationCommand.EXIT_LOW_SPEED_ROTATION, new CommonCallbacks.CompletionCallbackWithParam() { @Override public void onSuccess(PropellerRotationCommandResult propellerRotationCommandResult) { sendMsg2Server(message); if (propellerRotationCommandResult.getStatus() == PropellerRotationStatus.NONE_MOTOR_IN_LOW_SPEED_ROTATION) { Movement.getInstance().setPropellerRotation(false); } LogUtil.log(TAG, "停止低速转浆结果:" + new Gson().toJson(propellerRotationCommandResult)); } @Override public void onFailure(@NonNull IDJIError error) { LogUtil.log(TAG, "停止低速转浆失败:" + new Gson().toJson(error)); sendFailMsg2Server(message, "停止低速转浆失败:" + getIDJIErrorMsg(error)); } }); } //降落 public void startAutoLanding(MessageDown message) { Boolean isConnect = KeyManager.getInstance().getValue(createKey(FlightControllerKey.KeyConnection)); if (isConnect != null && isConnect) { KeyManager.getInstance().performAction(createKey(FlightControllerKey.KeyStartAutoLanding), new CommonCallbacks.CompletionCallbackWithParam() { @Override public void onSuccess(EmptyMsg emptyMsg) { if (message!=null){ sendMsg2Server( message); } } @Override public void onFailure(@NonNull IDJIError error) { LogUtil.log(TAG, "降落失败:" + new Gson().toJson(error)); if (message!=null){ sendFailMsg2Server( message, "降落失败:" + getIDJIErrorMsg(error)); } } }); } else { sendFailMsg2Server( message, "飞控未连接"); } } /** * 紧急悬停 */ public void emergencyHover(MessageDown message) { FlightMode flightMode = KeyManager.getInstance().getValue(createKey(FlightControllerKey.KeyFlightMode)); if (flightMode != null) { switch (flightMode) { case GO_HOME: KeyManager.getInstance().performAction(createKey(FlightControllerKey.KeyStopGoHome), new CommonCallbacks.CompletionCallbackWithParam() { @Override public void onSuccess(EmptyMsg emptyMsg) { LogUtil.log(TAG, "紧急悬停，取消返航成功"); sendMsg2Server( message); resetAircrftLandingStatus(); } @Override public void onFailure(@NonNull IDJIError error) { sendFailMsg2Server( message, "紧急悬停，取消返航失败:" + getIDJIErrorMsg(error)); } }); break; case WAYPOINT: IWaypointMissionManager missionManager = WaypointMissionManager.getInstance(); missionManager.stopMission(TextUtils.isEmpty(Movement.getInstance().getMissionName()) ? "aros" : Movement.getInstance().getMissionName(), new CommonCallbacks.CompletionCallback() { @Override public void onSuccess() { LogUtil.log(TAG, "紧急悬停，终止任务成功"); sendMsg2Server( message); resetAircrftLandingStatus(); } @Override public void onFailure(@NonNull IDJIError error) { sendFailMsg2Server( message, "紧急悬停，终止任务失败:" + getIDJIErrorMsg(error)); } }); break; case AUTO_LANDING: KeyManager.getInstance().performAction(createKey(FlightControllerKey.KeyStopAutoLanding), new CommonCallbacks.CompletionCallbackWithParam() { @Override public void onSuccess(EmptyMsg emptyMsg) { sendMsg2Server( message); resetAircrftLandingStatus(); } @Override public void onFailure(@NonNull IDJIError error) { LogUtil.log(TAG, "紧急悬停，取消降落失败:" + new Gson().toJson(error)); sendFailMsg2Server( message, "紧急悬停，取消降落失败:" + getIDJIErrorMsg(error)); } }); break; case VIRTUAL_STICK: VirtualStickManager.getInstance().disableVirtualStick(new CommonCallbacks.CompletionCallback() { @Override public void onSuccess() { LogUtil.log(TAG, "紧急悬停，控制权释放成功"); sendMsg2Server( message); resetAircrftLandingStatus(); } @Override public void onFailure(@NonNull IDJIError error) { sendFailMsg2Server( message, "紧急悬停，控制权失败:" + getIDJIErrorMsg(error)); } }); break; } } } }