DJI　mobileSDKのflightcontrollerを使ってドローンを動かしたい。

前提・実現したいこと

プログラミングはこれが初めての経験ですので初歩的な質問だと思うのですが助言いただけると幸いです。

実現したい事はflightcontrollerのvirtualstickcontrollmodeを使って任意の位置までドローンを動かせるようになることです。

発生している問題・エラーメッセージ

動作確認をするためにドローンを現在地から東に10メートル、北に10メートル先にゴール地点を設定してstartボタンを押した後、ゴール地点の緯度経度を過ぎれば停止させるようなプログラムを作りたいが、startボタンを押すと画面の操作が利かなくなり、ドローンが飛び続けてしまう。

MainActitvity.java

java
1public class MainActivity extends Activity implements View.OnClickListener {
2
3  private FlightController mFlightController;
4  protected TextView mConnectStatusTextView;
5  private Button mBtnTakeOff;
6  private Button mBtnLand;
7  private Button mBtnStart;
8  private EditText mEditText_X;
9  private EditText mEditText_Y;
10  private TextView TextView_status;
11  private TextView TextView_latitude;
12  private TextView TextView_longitude;
13  private TextView TextView_altitude;
14  private TextView TextView_sysmsg;
15
16
17  private Timer mSendVirtualStickDataTimer;
18  private SendVirtualStickDataTask mSendVirtualStickDataTask;
19
20  private float mPitch;
21  private float mRoll;
22  private float mYaw;
23  private float mThrottle;
24
25  double goal_x_double, goal_y_double;
26  double goalLng, goalLat;
27
28  private boolean isEND = false;
29
30  private double droneLocationLat = 181, droneLocationLng = 181;
31  private double HomePositionLat = 0, HomePositionLng = 0;
32  private float droneLocationAlt = 0;
33
34  @Override
35  protected void onCreate(Bundle savedInstanceState) {
36    super.onCreate(savedInstanceState);
37    checkAndRequestPermissions();
38    setContentView(R.layout.activity_main);
39
40    initUI();
41
42    // Register the broadcast receiver for receiving the device connection's changes.
43    IntentFilter filter = new IntentFilter();
44    filter.addAction(DJISimulatorApplication.FLAG_CONNECTION_CHANGE);
45    registerReceiver(mReceiver, filter);
46  }
47  
48
49  protected BroadcastReceiver mReceiver = new BroadcastReceiver() {
50
51    @Override
52    public void onReceive(Context context, Intent intent) {
53      updateTitleBar();
54    }
55  };
56
57  public void showToast(final String msg) {
58    runOnUiThread(new Runnable() {
59      public void run() {
60        Toast.makeText(MainActivity.this, msg, Toast.LENGTH_SHORT).show();
61      }
62    });
63  }
64
65  private void updateTitleBar() {
66    if (mConnectStatusTextView == null) return;
67    boolean ret = false;
68    BaseProduct product = DJISimulatorApplication.getProductInstance();
69    if (product != null) {
70      if (product.isConnected()) {
71        //The product is connected
72        mConnectStatusTextView.setText(DJISimulatorApplication.getProductInstance().getModel() + " Connected");
73        ret = true;
74      } else {
75        if (product instanceof Aircraft) {
76          Aircraft aircraft = (Aircraft) product;
77          if (aircraft.getRemoteController() != null && aircraft.getRemoteController().isConnected()) {
78            // The product is not connected, but the remote controller is connected
79            mConnectStatusTextView.setText("only RC Connected");
80            ret = true;
81          }
82        }
83      }
84    }
85
86    if (!ret) {
87      // The product or the remote controller are not connected.
88      mConnectStatusTextView.setText("Disconnected");
89    }
90  }
91
92  @Override
93  public void onResume() {
94    Log.e(TAG, "onResume");
95    super.onResume();
96    updateTitleBar();
97    initFlightController();
98    loginAccount();
99
100  }
101
102  @Override
103  public void onPause() {
104    Log.e(TAG, "onPause");
105    super.onPause();
106  }
107
108  @Override
109  public void onStop() {
110    Log.e(TAG, "onStop");
111    super.onStop();
112  }
113
114  public void onReturn(View view) {
115    Log.e(TAG, "onReturn");
116    this.finish();
117  }
118
119  @Override
120  protected void onDestroy() {
121    Log.e(TAG, "onDestroy");
122    unregisterReceiver(mReceiver);
123    if (null != mSendVirtualStickDataTimer) {
124      mSendVirtualStickDataTask.cancel();
125      mSendVirtualStickDataTask = null;
126      mSendVirtualStickDataTimer.cancel();
127      mSendVirtualStickDataTimer.purge();
128      mSendVirtualStickDataTimer = null;
129    }
130    super.onDestroy();
131  }
132
133  
134
135
136
137  private void initFlightController() {
138    Aircraft aircraft = DJISimulatorApplication.getAircraftInstance();
139    if (aircraft == null || !aircraft.isConnected()) {
140      showToast("Disconnected");
141      mFlightController = null;
142      return;
143    } else {
144      mFlightController = aircraft.getFlightController();
145      mFlightController.setRollPitchControlMode(RollPitchControlMode.VELOCITY);
146      mFlightController.setYawControlMode(YawControlMode.ANGULAR_VELOCITY);
147      mFlightController.setVerticalControlMode(VerticalControlMode.POSITION);
148      mFlightController.setRollPitchCoordinateSystem(FlightCoordinateSystem.BODY);
149      mFlightController.setStateCallback(new FlightControllerState.Callback() {
150
151        @Override
152        public void onUpdate(FlightControllerState djiFlightControllerCurrentState) {
153          new Handler(Looper.getMainLooper()).post(new Runnable() {
154            @Override
155            public void run() {
156              droneLocationLat = djiFlightControllerCurrentState.getAircraftLocation().getLatitude();
157              droneLocationLng = djiFlightControllerCurrentState.getAircraftLocation().getLongitude();
158              droneLocationAlt = djiFlightControllerCurrentState.getAircraftLocation().getAltitude();
159            }
160          });
161
162        }
163      });
164    }
165  }
166
167  private void initUI() {
168    mBtnStart = (Button) findViewById(R.id.start);
169    mEditText_X = (EditText)findViewById(R.id.GoalX);
170    mEditText_Y = (EditText)findViewById(R.id.GoalY);
171
172    mBtnStart.setOnClickListener(this);
173  }
174
175  private void getgoal(){
176    String goal_x, goal_y;
177    goal_x = mEditText_X.getText().toString();
178    goal_y = mEditText_Y.getText().toString();
179
180
181    goal_x_double = Double.parseDouble(goal_x);
182    goal_y_double = Double.parseDouble(goal_y);
183
184    show_message("goal point = x : "+ goal_x + " [m]  y : " + goal_y + "[m]");
185
186    goalLng = droneLocationLng + goal_x_double * 0.00000899;
187    goalLat = droneLocationLat + goal_y_double * 0.00001091;
188
189  }
190  private void getposition(){
191
192    if　(droneLocationLat ≻ goalLat && droneLocationLng ≻ goalLng ){
193　　　pX=0;
194　　　pY=0;
195      isEND = true;
196    }
197  }
198
199  class SendVirtualStickDataTask extends TimerTask {
200
201    @Override
202    public void run() {
203
204      if (mFlightController != null) {
205        mFlightController.sendVirtualStickFlightControlData(
206          new FlightControlData(
207            mPitch, mRoll, mYaw, mThrottle
208          ), new CommonCallbacks.CompletionCallback() {
209            @Override
210            public void onResult(DJIError djiError) {
211
212            }
213          }
214        );
215      }
216    }
217  }
218
219  @Override
220  public void onClick(View v) {
221
222    switch (v.getId()) {
223      case R.id.start:
224        show_message("Push START Button");
225        getgoal();
226　　　　float pX, pY;
227
228        float JoyControlMaxSpeed = 3;
229        float verticalJoyControlMaxAltitude = 20;
230        float PitchJoyControlMaxSpeed, RollJoyControlMaxSpeed;
231　　　　 pX = 1;
232        pY = 1;
233
234        float  r, goal_x_float, goal_y_float;
235
236        goal_x_float = (float)goal_x_double;
237        goal_y_float = (float)goal_y_double;
238
239
240        r = (float) Math.sqrt(goal_x_double*goal_x_double + goal_y_double * goal_y_double);
241
242        PitchJoyControlMaxSpeed = (JoyControlMaxSpeed / r) * goal_x_float;//東西
243        RollJoyControlMaxSpeed = (JoyControlMaxSpeed / r) * goal_y_float;//南北
244
245
246
247
248
249        while (!isEND){
250          getposition();
251          
252
253          mPitch = (float)(PitchJoyControlMaxSpeed * pX);
254
255          mRoll = (float)(RollJoyControlMaxSpeed * pY);
256
257          mThrottle = (float)(verticalJoyControlMaxAltitude);
258
259          if(null == mSendVirtualStickDataTimer) {
260            mSendVirtualStickDataTask = new SendVirtualStickDataTask();
261            mSendVirtualStickDataTimer = new Timer();
262            mSendVirtualStickDataTimer.schedule(mSendVirtualStickDataTask, 100, 200);
263          }
264        }
265
266    }
267  }
268}
269

HRSKT

2019/12/15 16:37 編集

ハード情報、どんなセンサがあって、どんな動作をするか、わからないから変な回答したくないんだけど droneLocationLat = 181, droneLocationLng = 181; の緯度経度(181,181) って何でしょう？（どこでしょう？） GPS信号で上書きされるのかな？ゴール地点の緯度経度goalLng/Latでも使用されているようですが goalLng = droneLocationLng + goal_x_double * 0.00000899; たぶん[目標緯度/経度]＝[初期緯度/経度]+[10mを角度に換算した値]？ちなみにjava全部読んでないです(得意でもないです) 数値が気になったので書きましたそのままで良い値なのかもしれません

HRSKT

2019/12/15 16:35

ちなみに緯度Latは独自の規準でなければ0～90の範囲です