diff --git a/app/src/main/java/com/aros/apron/base/BaseManager.java b/app/src/main/java/com/aros/apron/base/BaseManager.java index f93d8674..ccdbe3ae 100644 --- a/app/src/main/java/com/aros/apron/base/BaseManager.java +++ b/app/src/main/java/com/aros/apron/base/BaseManager.java @@ -5,6 +5,7 @@ import static com.aros.apron.tools.Utils.getIDJIErrorMsg; import android.os.Handler; import android.os.Looper; import android.text.TextUtils; +import android.util.Log; import androidx.annotation.NonNull; @@ -204,20 +205,32 @@ public abstract class BaseManager { final FlightTaskProgress.Data.Output.Ext ext = new FlightTaskProgress.Data.Output.Ext(); ext.setCurrent_waypoint_index(Movement.getInstance().getTask_current_waypoint_index()); ext.setFlight_id(PreferenceUtils.getInstance().getFlightId()); + //媒体文件数量 ext.setMedia_count(Movement.getInstance().getTask_media_count()); ext.setTrack_id(Movement.getInstance().getTrack_id()); ext.setWayline_id(Movement.getInstance().getTask_wayline_id()); ext.setWayline_mission_state(Movement.getInstance().getTask_wayline_mission_state()); final FlightTaskProgress.Data.Output.Progress progress = new FlightTaskProgress.Data.Output.Progress(); + + + LogUtil.log(TAG,"CurrentWayline"+CurrentWayline.getInstance().getWaypoints()+"size"+CurrentWayline.getInstance().getWaypoints().size() + +"getTask_wayline_mission_state"+Movement.getInstance().getTask_wayline_mission_state()); if (CurrentWayline.getInstance().getWaypoints() != null && CurrentWayline.getInstance().getWaypoints().size() > 0 && Movement.getInstance().getTask_wayline_mission_state() == 6) { + //这个没执行到 progress.setPercent((100 * (Movement.getInstance().getCurrentWaypointIndex() + 1) / CurrentWayline.getInstance().getWaypoints().size())); + + } + progress.setCurrent_step(Movement.getInstance().getTask_current_step()); + + + if (Movement.getInstance().isMissionFinish1()) { // 是结束状态 → 查完断点再发 WaypointMissionManager.getInstance().queryBreakPointInfoFromAircraft( @@ -264,6 +277,7 @@ public abstract class BaseManager { breakPoint.setLatitude(breakPointInfo.getLocation().getLatitude()); breakPoint.setLongitude(breakPointInfo.getLocation().getLongitude()); + breakPoint.setProgress(Movement.getInstance().getTask_progress()); breakPoint.setState(Movement.getInstance().getState()); breakPoint.setWayline_id(Movement.getInstance().getTask_wayline_id()); @@ -380,6 +394,7 @@ public abstract class BaseManager { Movement.getInstance().setHeight(breakPointInfo.getLocation().getAltitude()); Movement.getInstance().setTask_latitude(breakPointInfo.getLocation().getLatitude()); Movement.getInstance().setTask_longitude(breakPointInfo.getLocation().getLongitude()); + Movement.getInstance().setTask_progress(breakPointInfo.getSegmentProgress()); Movement.getInstance().setTask_state(0); Movement.getInstance().setTask_wayline_id(breakPointInfo.getWaylineID()); diff --git a/app/src/main/java/com/aros/apron/callback/MqttCallBack.java b/app/src/main/java/com/aros/apron/callback/MqttCallBack.java index 0c138272..f75da6c4 100644 --- a/app/src/main/java/com/aros/apron/callback/MqttCallBack.java +++ b/app/src/main/java/com/aros/apron/callback/MqttCallBack.java @@ -6,6 +6,7 @@ import android.os.Looper; import android.util.Log; import com.aros.apron.app.ApronApp; +import com.aros.apron.base.BaseManager; import com.aros.apron.constant.AMSConfig; import com.aros.apron.constant.Constant; import com.aros.apron.entity.ApronExecutionStatus; @@ -31,7 +32,7 @@ import org.eclipse.paho.client.mqttv3.IMqttDeliveryToken; import org.eclipse.paho.client.mqttv3.MqttCallbackExtended; import org.eclipse.paho.client.mqttv3.MqttMessage; -public class MqttCallBack implements MqttCallbackExtended { +public class MqttCallBack extends BaseManager implements MqttCallbackExtended { private String TAG = "MqttCallBack"; @@ -141,7 +142,7 @@ public class MqttCallBack implements MqttCallbackExtended { LogUtil.log(TAG, "收到：航线" + jsonString); //设置modecode Movement.getInstance().setMode_code(1); - OSDManager.getInstance().pushFlightAttitude(); + sendFlightTaskProgress2Server(); //设置标志为 Movement.getInstance().setFlightmode(1); MissionV3Manager.getInstance().taskExecute(message); @@ -159,6 +160,7 @@ public class MqttCallBack implements MqttCallbackExtended { LogUtil.log(TAG, "收到：返航" + jsonString); //自动返航 (如果调用方法失败了 这个设置就有一个问题但是为了方便看懂我就放这里了没放成功的回调里面) Movement.getInstance().setMode_code(9); + sendFlightTaskProgress2Server(); FlightManager.getInstance().startGoHome(message); break; case Constant.RETURN_HOME_CANCEL: @@ -185,7 +187,7 @@ public class MqttCallBack implements MqttCallbackExtended { LogUtil.log(TAG, "收到：一键起飞" + jsonString); //设置modecode Movement.getInstance().setMode_code(1); - OSDManager.getInstance().pushFlightAttitude(); + sendFlightTaskProgress2Server(); //设置标志为 Movement.getInstance().setFlightmode(2); diff --git a/app/src/main/java/com/aros/apron/entity/MessageDown.java b/app/src/main/java/com/aros/apron/entity/MessageDown.java index 8af3b483..72b37ce5 100644 --- a/app/src/main/java/com/aros/apron/entity/MessageDown.java +++ b/app/src/main/java/com/aros/apron/entity/MessageDown.java @@ -110,6 +110,7 @@ public class MessageDown { private float width; + public float getWidth() { return width; } diff --git a/app/src/main/java/com/aros/apron/manager/FlightManager.java b/app/src/main/java/com/aros/apron/manager/FlightManager.java index ca1bb27b..71d9454e 100644 --- a/app/src/main/java/com/aros/apron/manager/FlightManager.java +++ b/app/src/main/java/com/aros/apron/manager/FlightManager.java @@ -96,17 +96,15 @@ public class FlightManager extends BaseManager { LogUtil.log(TAG,"遥控器断开连接"); // 飞机未连接时设置未连接状态 Movement.getInstance().setMode_code(14); - OSDManager.getInstance().pushFlightAttitude(); - + sendFlightTaskProgress2Server(); Movement.getInstance().setMissionFinish(true); }else{ - LogUtil.log(TAG,"setMode_code1(0)"); //连上就待机 Movement.getInstance().setMissionFinish(false); Movement.getInstance().setMode_code(0); - OSDManager.getInstance().pushFlightAttitude(); + sendFlightTaskProgress2Server(); } } @@ -114,10 +112,11 @@ public class FlightManager extends BaseManager { }); //重新复制一下如果一直断开这个回调会不执行 if(isConnect){ - LogUtil.log(TAG,"setMode_code2(0)"); Movement.getInstance().setMode_code(0); + sendFlightTaskProgress2Server(); }else{ Movement.getInstance().setMode_code(14); + sendFlightTaskProgress2Server(); } @@ -191,12 +190,10 @@ public class FlightManager extends BaseManager { }else if(Movement.getInstance().getFlightmode()==2){ Movement.getInstance().setMode_code(17); } + sendFlightTaskProgress2Server(); }else{ - LogUtil.log(TAG,"setMode_code3(0)"); - Movement.getInstance().setMode_code(0); - OSDManager.getInstance().pushFlightAttitude(); - + sendFlightTaskProgress2Server(); } isFlying = newValue; @@ -459,8 +456,8 @@ public class FlightManager extends BaseManager { case POI: Movement.getInstance().setMode_code(20); break; - } + sendFlightTaskProgress2Server(); } } }); @@ -992,6 +989,7 @@ public class FlightManager extends BaseManager { // 上传媒体文件 SystemManager.getInstance().upLoadMedia(MqttManager.getInstance().mqttAndroidClient); } + // 避免在下次起飞时触发视觉识别 PreferenceUtils.getInstance().setNeedTriggerApronArucoLand(false); PreferenceUtils.getInstance().setNeedTriggerAlterArucoLand(false); @@ -1042,13 +1040,13 @@ public class FlightManager extends BaseManager { //取消返航 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); + sendFlightTaskProgress2Server(); sendMsg2Server( message); } diff --git a/app/src/main/java/com/aros/apron/manager/FlightTaskProgressManager.java b/app/src/main/java/com/aros/apron/manager/FlightTaskProgressManager.java index c02f4f3a..4dd22180 100644 --- a/app/src/main/java/com/aros/apron/manager/FlightTaskProgressManager.java +++ b/app/src/main/java/com/aros/apron/manager/FlightTaskProgressManager.java @@ -38,20 +38,15 @@ public class FlightTaskProgressManager extends BaseManager { // sendFlightTaskProgress2Server(); // // } - //只要一个满足就上报 if (Movement.getInstance().isPlaneWing() || Movement.getInstance().isMotorsOn()) { sendFlightTaskProgress2Server(); } - - if (Movement.getInstance().isMissionFinish1()) { sendFlightTaskProgress2Server(); handler.removeCallbacks(this); return; } - - // 始终基于“实际执行时间”来调度 handler.postDelayed(this, INTERVAL); } diff --git a/app/src/main/java/com/aros/apron/manager/FlyToPointManager.java b/app/src/main/java/com/aros/apron/manager/FlyToPointManager.java index 06cbf642..b043bb1c 100644 --- a/app/src/main/java/com/aros/apron/manager/FlyToPointManager.java +++ b/app/src/main/java/com/aros/apron/manager/FlyToPointManager.java @@ -176,7 +176,7 @@ public class FlyToPointManager extends BaseManager { sendFailMsg2Server(message,"指点航线上传失败:"+ Utils.getIDJIErrorMsg(error)); //待机 Movement.getInstance().setMode_code(0); - OSDManager.getInstance().pushFlightAttitude(); + sendFlightTaskProgress2Server(); } }); } @@ -204,6 +204,7 @@ public class FlyToPointManager extends BaseManager { //指令飞行( 指点前置不需要起飞准备这些因为没有指点起飞的航线) Movement.getInstance().setMode_code(17); + sendFlightTaskProgress2Server(); } @Override @@ -211,7 +212,7 @@ public class FlyToPointManager extends BaseManager { sendFailMsg2Server(message,"指点航线执行失败:" + new Gson().toJson(error)); //待机 Movement.getInstance().setMode_code(0); - OSDManager.getInstance().pushFlightAttitude(); + sendFlightTaskProgress2Server(); } }; diff --git a/app/src/main/java/com/aros/apron/manager/MediaManager.java b/app/src/main/java/com/aros/apron/manager/MediaManager.java index 11baf5e0..d9ee1849 100644 --- a/app/src/main/java/com/aros/apron/manager/MediaManager.java +++ b/app/src/main/java/com/aros/apron/manager/MediaManager.java @@ -19,6 +19,7 @@ import com.amazonaws.services.s3.model.ProgressListener; import com.amazonaws.services.s3.model.PutObjectRequest; import com.aros.apron.base.BaseManager; import com.aros.apron.entity.ApronExecutionStatus; +import com.aros.apron.entity.Movement; import com.aros.apron.tools.LogUtil; import com.aros.apron.tools.PreferenceUtils; import com.autonavi.base.amap.mapcore.FileUtil; @@ -140,6 +141,9 @@ public class MediaManager extends BaseManager { if (mState == MediaFileListState.UP_TO_DATE) { mediaFiles = MediaDataCenter.getInstance().getMediaManager().getMediaFileListData().getData(); + //倒是看会不会上传 + Movement.getInstance().setTask_media_count(mediaFiles.size()); + if (mediaFiles != null&&mediaFiles.size()>0) { if (Build.VERSION.SDK_INT >= Build.VERSION_CODES.O) { pullOriginalMediaFileFromCamera(); diff --git a/app/src/main/java/com/aros/apron/manager/MissionV3Manager.java b/app/src/main/java/com/aros/apron/manager/MissionV3Manager.java index 1f21e5a1..8d6b6aa2 100644 --- a/app/src/main/java/com/aros/apron/manager/MissionV3Manager.java +++ b/app/src/main/java/com/aros/apron/manager/MissionV3Manager.java @@ -601,7 +601,7 @@ public class MissionV3Manager extends BaseManager { //自主起飞 Movement.getInstance().setMode_code(4); - OSDManager.getInstance().pushFlightAttitude(); + sendFlightTaskProgress2Server(); Movement.getInstance().setTask_current_step(22); startMission(message); @@ -630,7 +630,7 @@ public class MissionV3Manager extends BaseManager { } else { //待机 Movement.getInstance().setMode_code(0); - OSDManager.getInstance().pushFlightAttitude(); + sendFlightTaskProgress2Server(); sendEvent2Server("航线第" + pushKMZFileTimes + "次上传失败,直接关机", 2); TaskFailManager.getInstance().sendTaskFailMsg2Server(-1); @@ -702,7 +702,7 @@ public class MissionV3Manager extends BaseManager { //待机 Movement.getInstance().setMode_code(0); - OSDManager.getInstance().pushFlightAttitude(); + sendFlightTaskProgress2Server(); sendEvent2Server("航线第" + startMissionFailTimes + "次开始失败,直接关机:" + "---" + new Gson().toJson(error) + "--" + Movement.getInstance().getQuality(), 2); TaskFailManager.getInstance().sendTaskFailMsg2Server(-1); diff --git a/app/src/main/java/com/aros/apron/manager/OSDManager.java b/app/src/main/java/com/aros/apron/manager/OSDManager.java index 180c30ec..5d0fa357 100644 --- a/app/src/main/java/com/aros/apron/manager/OSDManager.java +++ b/app/src/main/java/com/aros/apron/manager/OSDManager.java @@ -68,10 +68,14 @@ public class OSDManager extends BaseManager { } lastExecuteTime = now; Boolean isConnect = KeyManager.getInstance().getValue(createKey(FlightControllerKey.KeyConnection)); - if (isConnect != null && isConnect && !Movement.getInstance().isMissionFinish()) { - pushFlightAttitude(); - }else{ - LogUtil.log(TAG,"osd stop:flight controller is null"); + if (!Movement.getInstance().isMissionFinish()) { + if (isConnect != null && isConnect) { + pushFlightAttitude(); + } else { + LogUtil.log(TAG, "osd stop:flight controller is null"); + } + } else { + LogUtil.log(TAG, "osd stop:flight controller is null"); } // 始终基于“实际执行时间”来调度 handler.postDelayed(this, INTERVAL); diff --git a/app/src/main/java/com/aros/apron/manager/TakeOffToPointManager.java b/app/src/main/java/com/aros/apron/manager/TakeOffToPointManager.java index fc2563ed..7d674c14 100644 --- a/app/src/main/java/com/aros/apron/manager/TakeOffToPointManager.java +++ b/app/src/main/java/com/aros/apron/manager/TakeOffToPointManager.java @@ -343,7 +343,7 @@ public class TakeOffToPointManager extends BaseManager { public void onSuccess() { //起飞准备完毕 Movement.getInstance().setMode_code(2); - + sendFlightTaskProgress2Server(); sendEvent2Server("航线上传成功,准备执行任务", 1); isPushKMZSuccess = true; mainHandler.postDelayed(new Runnable() { @@ -354,7 +354,7 @@ public class TakeOffToPointManager extends BaseManager { */ //自主起飞 Movement.getInstance().setMode_code(4); - OSDManager.getInstance().pushFlightAttitude(); + sendFlightTaskProgress2Server(); Movement.getInstance().setTask_current_step(22); startMission(message); pushKMZFileTimes = 0; @@ -382,7 +382,7 @@ public class TakeOffToPointManager extends BaseManager { sendEvent2Server("航线第" + pushKMZFileTimes + "次上传失败,直接关机",2); //待机 Movement.getInstance().setMode_code(0); - OSDManager.getInstance().pushFlightAttitude(); + sendFlightTaskProgress2Server(); TaskFailManager.getInstance().sendTaskFailMsg2Server(-1); } @@ -452,7 +452,7 @@ public class TakeOffToPointManager extends BaseManager { if (!Movement.getInstance().isPlaneWing()) { //待机 Movement.getInstance().setMode_code(0); - OSDManager.getInstance().pushFlightAttitude(); + sendFlightTaskProgress2Server(); sendEvent2Server("航线第" + startMissionFailTimes + "次开始失败,直接关机:" + "---" + new Gson().toJson(error) + "--" + Movement.getInstance().getQuality(),2); TaskFailManager.getInstance().sendTaskFailMsg2Server(-1); diff --git a/app/src/main/java/com/aros/apron/tools/ApronArucoDetect.java b/app/src/main/java/com/aros/apron/tools/ApronArucoDetect.java index f0235f93..d9a9f284 100644 --- a/app/src/main/java/com/aros/apron/tools/ApronArucoDetect.java +++ b/app/src/main/java/com/aros/apron/tools/ApronArucoDetect.java @@ -1,14 +1,12 @@ -package com.aros.apron.tools; +package com.aros.apron.tools; import android.os.Handler; import android.os.Looper; - import com.aros.apron.constant.AMSConfig; import com.aros.apron.entity.ArucoMarker; import com.aros.apron.entity.ArucoMarkerDimensions; import com.aros.apron.entity.Movement; import com.aros.apron.manager.AlternateLandingManager; - import org.opencv.aruco.Aruco; import org.opencv.aruco.Dictionary; import org.opencv.calib3d.Calib3d; @@ -298,63 +296,6 @@ public class ApronArucoDetect { if (Movement.getInstance().getElevation() <= 1.5 || Movement.getInstance().getUltrasonicHeight() < 15) { if (isDoublePayload) { - if (mFindArucoList.isEmpty()) { - for (int i = 0; i < idArray.length; i++) { - if (idArray[i] == 21) { - detectedSmallMarkers = true; - detectedSmallMarkerId = 21; - mFindArucoList.add(new ArucoMarker(idArray[i], mArucoCornerList.get(i), 0.03f)); - return; - } - if (idArray[i] == 22 && detectedSmallMarkerId != 21) { - detectedSmallMarkers = true; - detectedSmallMarkerId = 22; - mFindArucoList.add(new ArucoMarker(idArray[i], mArucoCornerList.get(i), 0.03f)); - return; - } - if (idArray[i] == 23 && detectedSmallMarkerId != 21 && detectedSmallMarkerId != 22) { - detectedSmallMarkers = true; - detectedSmallMarkerId = 23; - mFindArucoList.add(new ArucoMarker(idArray[i], mArucoCornerList.get(i), 0.03f)); - return; - } - - if (idArray[i] == 24 && detectedSmallMarkerId != 21 && detectedSmallMarkerId != 22 && detectedSmallMarkerId != 23) { - detectedSmallMarkers = true; - detectedSmallMarkerId = 24; - mFindArucoList.add(new ArucoMarker(idArray[i], mArucoCornerList.get(i), 0.03f)); - return; - } - - if (idArray[i] == 19 && detectedSmallMarkerId != 21 && detectedSmallMarkerId != 22 && detectedSmallMarkerId != 23 && detectedSmallMarkerId != 24) { - detectedSmallMarkers = true; - detectedSmallMarkerId = 19; - mFindArucoList.add(new ArucoMarker(idArray[i], mArucoCornerList.get(i), 0.03f)); - return; - } - - if (idArray[i] == 20 && detectedSmallMarkerId != 21 && detectedSmallMarkerId != 22 && detectedSmallMarkerId != 23 && detectedSmallMarkerId != 24 && detectedSmallMarkerId != 19) { - detectedSmallMarkers = true; - detectedSmallMarkerId = 20; - mFindArucoList.add(new ArucoMarker(idArray[i], mArucoCornerList.get(i), 0.03f)); - return; - } - if (idArray[i] == 17 && detectedSmallMarkerId != 21 && detectedSmallMarkerId != 22 && detectedSmallMarkerId != 23 && detectedSmallMarkerId != 24 && detectedSmallMarkerId != 19 && detectedSmallMarkerId != 20) { - detectedSmallMarkers = true; - detectedSmallMarkerId = 17; - mFindArucoList.add(new ArucoMarker(idArray[i], mArucoCornerList.get(i), 0.03f)); - return; - } - if (idArray[i] == 18 && detectedSmallMarkerId != 21 && detectedSmallMarkerId != 22 && detectedSmallMarkerId != 23 && detectedSmallMarkerId != 24 && detectedSmallMarkerId != 19 && detectedSmallMarkerId != 20 && detectedSmallMarkerId != 17) { - detectedSmallMarkers = true; - detectedSmallMarkerId = 18; - mFindArucoList.add(new ArucoMarker(idArray[i], mArucoCornerList.get(i), 0.03f)); - return; - } - } - } - - } else { if (mFindArucoList.isEmpty()) { for (int i = 0; i < idArray.length; i++) { if (idArray[i] == 19) { @@ -369,38 +310,127 @@ public class ApronArucoDetect { mFindArucoList.add(new ArucoMarker(idArray[i], mArucoCornerList.get(i), 0.03f)); return; } - if (idArray[i] == 21 && detectedSmallMarkerId != 19 && detectedSmallMarkerId != 20) { - detectedSmallMarkers = true; - detectedSmallMarkerId = 21; - mFindArucoList.add(new ArucoMarker(idArray[i], mArucoCornerList.get(i), 0.03f)); - return; - } - if (idArray[i] == 22 && detectedSmallMarkerId != 19 && detectedSmallMarkerId != 20 && detectedSmallMarkerId != 21) { - detectedSmallMarkers = true; - detectedSmallMarkerId = 22; - mFindArucoList.add(new ArucoMarker(idArray[i], mArucoCornerList.get(i), 0.03f)); - return; - } - - if (idArray[i] == 17 && detectedSmallMarkerId != 19 && detectedSmallMarkerId != 20 && detectedSmallMarkerId != 21 && detectedSmallMarkerId != 22) { + if (idArray[i] == 17 && detectedSmallMarkerId != 20 && detectedSmallMarkerId != 19) { detectedSmallMarkers = true; detectedSmallMarkerId = 17; mFindArucoList.add(new ArucoMarker(idArray[i], mArucoCornerList.get(i), 0.03f)); return; } - if (idArray[i] == 18 && detectedSmallMarkerId != 19 && detectedSmallMarkerId != 20 && detectedSmallMarkerId != 21 && detectedSmallMarkerId != 22 && detectedSmallMarkerId != 17) { + + if (idArray[i] == 18 && detectedSmallMarkerId != 20 && detectedSmallMarkerId != 19&& detectedSmallMarkerId != 17) { detectedSmallMarkers = true; detectedSmallMarkerId = 18; mFindArucoList.add(new ArucoMarker(idArray[i], mArucoCornerList.get(i), 0.03f)); return; } - if (idArray[i] == 23 && detectedSmallMarkerId != 19 && detectedSmallMarkerId != 20 && detectedSmallMarkerId != 21 && detectedSmallMarkerId != 22 && detectedSmallMarkerId != 17 && detectedSmallMarkerId != 18) { + + if (idArray[i] == 15 && detectedSmallMarkerId != 18&& detectedSmallMarkerId != 20 && detectedSmallMarkerId != 19&& detectedSmallMarkerId != 17) { + detectedSmallMarkers = true; + detectedSmallMarkerId = 15; + mFindArucoList.add(new ArucoMarker(idArray[i], mArucoCornerList.get(i), 0.03f)); + return; + } + if (idArray[i] == 16 && detectedSmallMarkerId != 15&& detectedSmallMarkerId != 18&& detectedSmallMarkerId != 20 && detectedSmallMarkerId != 19&& detectedSmallMarkerId != 17) { + detectedSmallMarkers = true; + detectedSmallMarkerId = 16; + mFindArucoList.add(new ArucoMarker(idArray[i], mArucoCornerList.get(i), 0.03f)); + return; + } + if (idArray[i] == 24 && detectedSmallMarkerId != 16&& detectedSmallMarkerId != 15&& detectedSmallMarkerId != 18&& detectedSmallMarkerId != 20 && detectedSmallMarkerId != 19&& detectedSmallMarkerId != 17) { + detectedSmallMarkers = true; + detectedSmallMarkerId = 24; + mFindArucoList.add(new ArucoMarker(idArray[i], mArucoCornerList.get(i), 0.03f)); + return; + } + if (idArray[i] == 25 && detectedSmallMarkerId != 24&& detectedSmallMarkerId != 16&& detectedSmallMarkerId != 15&& detectedSmallMarkerId != 18&& detectedSmallMarkerId != 20 && detectedSmallMarkerId != 19&& detectedSmallMarkerId != 17) { + detectedSmallMarkers = true; + detectedSmallMarkerId = 25; + mFindArucoList.add(new ArucoMarker(idArray[i], mArucoCornerList.get(i), 0.03f)); + return; + } + if (idArray[i] == 23 && detectedSmallMarkerId != 25&& detectedSmallMarkerId != 24&& detectedSmallMarkerId != 16&& detectedSmallMarkerId != 15&& detectedSmallMarkerId != 18&& detectedSmallMarkerId != 20 && detectedSmallMarkerId != 19&& detectedSmallMarkerId != 17) { detectedSmallMarkers = true; detectedSmallMarkerId = 23; mFindArucoList.add(new ArucoMarker(idArray[i], mArucoCornerList.get(i), 0.03f)); return; } - if (idArray[i] == 24 && detectedSmallMarkerId != 19 && detectedSmallMarkerId != 20 && detectedSmallMarkerId != 21 && detectedSmallMarkerId != 22 && detectedSmallMarkerId != 17 && detectedSmallMarkerId != 18 && detectedSmallMarkerId != 23) { + if (idArray[i] == 13 && detectedSmallMarkerId != 23 && detectedSmallMarkerId != 25&& detectedSmallMarkerId != 24&& detectedSmallMarkerId != 16&& detectedSmallMarkerId != 15&& detectedSmallMarkerId != 18&& detectedSmallMarkerId != 20 && detectedSmallMarkerId != 19&& detectedSmallMarkerId != 17) { + detectedSmallMarkers = true; + detectedSmallMarkerId = 13; + mFindArucoList.add(new ArucoMarker(idArray[i], mArucoCornerList.get(i), 0.03f)); + return; + } + if (idArray[i] == 14 && detectedSmallMarkerId != 13&& detectedSmallMarkerId != 23 && detectedSmallMarkerId != 25&& detectedSmallMarkerId != 24&& detectedSmallMarkerId != 16&& detectedSmallMarkerId != 15&& detectedSmallMarkerId != 18&& detectedSmallMarkerId != 20 && detectedSmallMarkerId != 19&& detectedSmallMarkerId != 17) { + detectedSmallMarkers = true; + detectedSmallMarkerId = 14; + mFindArucoList.add(new ArucoMarker(idArray[i], mArucoCornerList.get(i), 0.03f)); + return; + } + if (idArray[i] == 22 && detectedSmallMarkerId != 14&& detectedSmallMarkerId != 13&& detectedSmallMarkerId != 23 && detectedSmallMarkerId != 25&& detectedSmallMarkerId != 24&& detectedSmallMarkerId != 16&& detectedSmallMarkerId != 15&& detectedSmallMarkerId != 18&& detectedSmallMarkerId != 20 && detectedSmallMarkerId != 19&& detectedSmallMarkerId != 17) { + detectedSmallMarkers = true; + detectedSmallMarkerId = 22; + mFindArucoList.add(new ArucoMarker(idArray[i], mArucoCornerList.get(i), 0.03f)); + return; + } + } + } + + } else { + if (mFindArucoList.isEmpty()) { + for (int i = 0; i < idArray.length; i++) { + if (idArray[i] == 15) { + detectedSmallMarkers = true; + detectedSmallMarkerId = 15; + mFindArucoList.add(new ArucoMarker(idArray[i], mArucoCornerList.get(i), 0.03f)); + return; + } + if (idArray[i] == 16 && detectedSmallMarkerId != 15) { + detectedSmallMarkers = true; + detectedSmallMarkerId = 16; + mFindArucoList.add(new ArucoMarker(idArray[i], mArucoCornerList.get(i), 0.03f)); + return; + } + + if (idArray[i] == 13&&detectedSmallMarkerId!=15&&detectedSmallMarkerId!=16) { + detectedSmallMarkers = true; + detectedSmallMarkerId = 13; + mFindArucoList.add(new ArucoMarker(idArray[i], mArucoCornerList.get(i), 0.03f)); + return; + } + + if (idArray[i] == 14&&detectedSmallMarkerId!=15&&detectedSmallMarkerId!=16&&detectedSmallMarkerId!=13) { + detectedSmallMarkers = true; + detectedSmallMarkerId = 14; + mFindArucoList.add(new ArucoMarker(idArray[i], mArucoCornerList.get(i), 0.03f)); + return; + } + + if (idArray[i] == 17&&detectedSmallMarkerId!=15&&detectedSmallMarkerId!=16&&detectedSmallMarkerId!=14&&detectedSmallMarkerId!=13) { + detectedSmallMarkers = true; + detectedSmallMarkerId = 17; + mFindArucoList.add(new ArucoMarker(idArray[i], mArucoCornerList.get(i), 0.03f)); + return; + } + + if (idArray[i] == 18&&detectedSmallMarkerId!=17&&detectedSmallMarkerId!=15&&detectedSmallMarkerId!=16&&detectedSmallMarkerId!=14&&detectedSmallMarkerId!=13) { + detectedSmallMarkers = true; + detectedSmallMarkerId = 18; + mFindArucoList.add(new ArucoMarker(idArray[i], mArucoCornerList.get(i), 0.03f)); + return; + } + if (idArray[i] == 23&&detectedSmallMarkerId!=18&&detectedSmallMarkerId!=17&&detectedSmallMarkerId!=15&&detectedSmallMarkerId!=16&&detectedSmallMarkerId!=14&&detectedSmallMarkerId!=13) { + detectedSmallMarkers = true; + detectedSmallMarkerId = 23; + mFindArucoList.add(new ArucoMarker(idArray[i], mArucoCornerList.get(i), 0.03f)); + return; + } + if (idArray[i] == 22&&detectedSmallMarkerId!=23&&detectedSmallMarkerId!=18&&detectedSmallMarkerId!=17&&detectedSmallMarkerId!=15&&detectedSmallMarkerId!=16&&detectedSmallMarkerId!=14&&detectedSmallMarkerId!=13) { + detectedSmallMarkers = true; + detectedSmallMarkerId = 22; + mFindArucoList.add(new ArucoMarker(idArray[i], mArucoCornerList.get(i), 0.03f)); + return; + } + if (idArray[i] == 24&&detectedSmallMarkerId!=22&&detectedSmallMarkerId!=24&&detectedSmallMarkerId!=18&&detectedSmallMarkerId!=17&&detectedSmallMarkerId!=15&&detectedSmallMarkerId!=16&&detectedSmallMarkerId!=14&&detectedSmallMarkerId!=13) { detectedSmallMarkers = true; detectedSmallMarkerId = 24; mFindArucoList.add(new ArucoMarker(idArray[i], mArucoCornerList.get(i), 0.03f)); @@ -486,11 +516,12 @@ public class ApronArucoDetect { } } else { + //测试是否因为数组过长，导致的飞机椭球返回很高，但实际距离很近识别到多个码，帧数据处理不过来 if (idArray.length > 5) { idArray = Arrays.copyOf(idArray, 5); } for (int i = 0; i < idArray.length; i++) { - mFindArucoList.add(new ArucoMarker(idArray[i], mArucoCornerList.get(i), ArucoMarkerDimensions.getSizeById(idArray[i]))); + mFindArucoList.add(new ArucoMarker(idArray[i], mArucoCornerList.get(i), ArucoMarkerDimensions.getSizeById(idArray[i]))); } } @@ -591,6 +622,7 @@ public class ApronArucoDetect { List eulerAngles = RotationConversion.INSTANCE.rotationMatrixToEulerAngles(camR); //欧拉角转飞机偏航角度 double yawCamera = MathUtils.toDegree(eulerAngles.get(2)); +// LogUtil.log(TAG,"偏航角度："+yawCamera); if (yawCamera < 0) { if (yawCamera < -15 && z < 9 && z > 3) { if (yawCamera < -80) { @@ -814,19 +846,19 @@ public class ApronArucoDetect { } } -// LogUtil.log(TAG, -// " 杆量x=" + outX + -// " 偏移x=" + offsetX + -// " 杆量y=" + outY + -// " 偏移y=" + offsetY + -// " Id=" + id + -// " Size=" + arucoMarkers.size() + -// " 宽度=" + arucoWidth + -// " 椭球=" + Movement.getInstance().getElevation() + -// " 融合=" + ultrasonicHeight + -// " X=" + x + -// " Z=" + z -// ); + LogUtil.log(TAG, + " 杆量x=" + outX + + " 偏移x=" + offsetX + + " 杆量y=" + outY + + " 偏移y=" + offsetY + + " Id=" + id + + " Size=" + arucoMarkers.size() + + " 宽度=" + arucoWidth + + " 椭球=" + Movement.getInstance().getElevation() + + " 融合=" + ultrasonicHeight + + " X=" + x + + " Z=" + z + ); if ((Math.abs(outX)>0.3||Math.abs(outY)>0.3)&&(Movement.getInstance().getElevation()<3.5)){ if (ultrasonicHeight<=25){ outX=outX>0?0.125:-0.125; @@ -859,7 +891,7 @@ public class ApronArucoDetect { if (!startFastStick) { // if (absX <= 250 && absY <= 300 && ultrasonicHeight <= 4 && flyingHeight <= 3) { - if (absX <= 230 && absY <= 250 && ultrasonicHeight <= 2 && flyingHeight <= 3) { + if (absX <= 230 && absY <= 250 && arucoWidth >= 230 && ultrasonicHeight <= 3 && flyingHeight <= 3) { logMessage = "参考融合高度降落:" + id + " arucoW" + arucoWidth + " Flying Height:" + flyingHeight + "--" + " Ultrasonic Height:" + ultrasonicHeight; @@ -928,7 +960,7 @@ public class ApronArucoDetect { }; private void performOperation() { -// LogUtil.log(TAG,"快速下拉中..."+handlerCallbackCount); + LogUtil.log(TAG,"快速下拉中..."+handlerCallbackCount); DroneHelper.getInstance().moveVxVyYawrateHeight(0f, 0f, 0f, -4); handlerCallbackCount++; // 增加计数器 } @@ -943,4 +975,4 @@ public class ApronArucoDetect { } -} \ No newline at end of file +}