git.haldean.org droidcopter / 5953541
bugfixes Ben 11 years ago
5 changed file(s) with 20 addition(s) and 16 deletion(s). Raw diff Collapse all Expand all
Binary diff not shown
6060 //comm.setTelemetrySource(pic);
6161 comm.registerReceiver(IMAGE, pic);
6262 comm.registerReceiver(NAV, nav);
63 comm.registerReceiver(CSYS, nav);
6364 comm.registerReceiver(GUID, guid);
6465
6566 nav.registerReceiver(comm);
5555 private final int mDataOutPort = 7001;
5656
5757 /** How long (in ms) to wait for the first PULSE signal before assuming connectivity failure */
58 public static final int FIRST_PULSE = 30000;
58 public static final int FIRST_PULSE = 10000;
5959
6060 /** How long (in ms) to wait for subsequent PULSE signals before assuming connectivity failure. */
61 public static final int PULSE_RATE = 10000;
61 public static final int PULSE_RATE = 5000;
6262
6363 /** Tag for logging */
6464 public static final String TAG = "chopper.Comm";
258258 private void reviseMotorSpeed() {
259259 //Log.v(TAG, "START MOTOR REVISION");
260260 long starttime = System.currentTimeMillis();
261
262 //Copying motor values to temporary array for working purposes.
263 for (int i = 0; i < 4; i++) {
264 mTempMotorSpeed[i] = mMotorSpeed[i];
265 }
266
261267 boolean mStabilizing = false; //initializing value
262 //Retrieve current orientation.
263
268 //Retrieve current orientation.
264269 mAzimuth = mStatus.getReadingFieldNow(AZIMUTH, mAzimuth);
265270 mPitchDeg = -mStatus.getReadingFieldNow(PITCH, -mPitchDeg);
266271 mRollDeg = mStatus.getReadingFieldNow(ROLL, mRollDeg);
453458 mMotorSpeed[i] += Math.max(diff, -MAX_DMOTOR);
454459 }
455460 mMotorSpeed[i] = mTempMotorSpeed[i];
456
457 mTempMotorSpeed[i] = mMotorSpeed[i];
458
459461 }
460462
461463 //Send motor values to motors here:
6060 private final AtomicBoolean mAutoPilot = new AtomicBoolean(false);
6161
6262 /** Chopper's navigation status */
63 private final AtomicInteger mNavStatus = new AtomicInteger(NAV_STATUSES);
63 private final AtomicInteger mNavStatus = new AtomicInteger(NAV_STATUSES - 1);
6464
6565 /** Holds all flight plans */
6666 private Vector<NavTask> mTravelPlans = new Vector<NavTask>(); //Vector --> already thread-safe
114114 mHandler.removeMessages(EVAL_NAV);
115115 if (mAutoPilot.get()) {
116116 mHandler.sendEmptyMessage(EVAL_NAV);
117 System.out.println("Autopilot engaged");
117 Log.i(TAG, "Autopilot engaged");
118118 }
119119 }
120120
154154 setTask(BASIC_AUTO, taskList);
155155 setTask(NO_CONN, "{ VEL!0!0!-1!0!10000 }");
156156 setTask(LOW_POWER, "{ VEL!0!0!-1!0!10000 }");
157 updateStatus(BASIC_AUTO);
158 autoPilot(true);
157 //autoPilot(true);
159158 Looper.loop();
160159 }
161160 };
229228 * @param source The source of the message, if a reply is needed. May be null.
230229 */
231230 public void receiveMessage(String msg, Receivable source) {
231 Log.d(TAG, "Receiving " + msg);
232232 String[] parts = msg.split(":");
233233 if (parts[0].equals("NAV")) {
234234 if (parts[1].equals("SET")) {
265265 }
266266 if (parts[0].equals("CSYS")) {
267267 if (parts[1].equals("NOCONN")) {
268 Log.d(TAG, "no conn in Nav");
268269 updateStatus(NO_CONN);
269270 autoPilot(true);
270271 updateReceivers("GUID:AUTOMATIC");
338339
339340 NavTask myList = mTravelPlans.get(thisStatus);
340341 if (myList.isComplete()) {
341 System.out.println("Hovering");
342 Log.i(TAG, "Hovering");
342343 hover();
343344 return;
344345 }
345346 myList.getVelocity(mTempTarget);
346347 setTarget(mTempTarget);
347348
348 System.out.print("New NAV Vector: ");
349 String newNav = "New Nav Vector: ";
349350 for (int i = 0; i < 4; i++) {
350 System.out.print(mTempTarget[i] + " ");
351 }
352 System.out.println();
351 newNav += mTempTarget[i] + " ";
352 }
353 Log.i(TAG, newNav);
353354
354355 long interval = myList.getInterval();
355356