Commits

Cliff Biffle committed 39ee8c5

Initial support for receiving and parsing nav sensor data from the drone.

  • Participants
  • Parent commits 7e44c25

Comments (0)

Files changed (9)

src/org/mg8/ardrone/comms/navdata/AttitudeListener.java

+/*
+ * Copyright 2010 Cliff L. Biffle.  All Rights Reserved.
+ * Use of this source code is governed by a BSD-style license that can be found
+ * in the LICENSE file.
+ */
+package org.mg8.ardrone.comms.navdata;
+
+/**
+ * Listener interface for attitude updates.
+ */
+public interface AttitudeListener {
+  /**
+   * Called when the drone's attitude is updated.
+   * 
+   * @param pitch degrees of front/back rotation (around X).  Negative indicates
+   *     a nose-down attitude; positive, a nose-up attitude.
+   * @param roll degrees of left/right rotation (around Y).  Negative indicates
+   *     banking to the left; positive, banking to the right.
+   * @param yaw degrees of yaw (around Z).  Negative indicates a turn to the
+   *     left; positive, a turn to the right.
+   * @param altitude distance from the surface directly beneath the drone, in
+   *     millimeters.
+   */
+  void attitudeUpdated(float pitch, float roll, float yaw, int altitude);
+
+}

src/org/mg8/ardrone/comms/navdata/BatteryListener.java

+/*
+ * Copyright 2010 Cliff L. Biffle.  All Rights Reserved.
+ * Use of this source code is governed by a BSD-style license that can be found
+ * in the LICENSE file.
+ */
+package org.mg8.ardrone.comms.navdata;
+
+/**
+ * Listener interface for updates on the drone's battery charge level.
+ */
+public interface BatteryListener {
+  void batteryLevelChanged(int percentage);
+}

src/org/mg8/ardrone/comms/navdata/DroneState.java

+/*
+ * Copyright 2010 Cliff L. Biffle.  All Rights Reserved.
+ * Use of this source code is governed by a BSD-style license that can be found
+ * in the LICENSE file.
+ */
+package org.mg8.ardrone.comms.navdata;
+
+/**
+ * Represents the drone's current system state, a collection of flags.
+ */
+public class DroneState {
+  private final int bits;
+  
+  public DroneState(int bits) {
+    this.bits = bits;
+  }
+  
+  public boolean isFlying() {
+    return bit(0);
+  }
+  
+  public boolean isVideoEnabled() {
+    return bit(1);
+  }
+  
+  public boolean isVisionEnabled() {
+    return bit(2);
+  }
+  
+  /** The alternative is Euler angle control. */
+  public boolean isControllingAngularSpeed() {
+    return bit(3);
+  }
+  
+  public boolean isControllingAltitude() {
+    return bit(4);
+  }
+  
+  public boolean isStartButtonPressed() {
+    return bit(5);
+  }
+  
+  public boolean hasReceivedControlAck() {
+    return bit(6);
+  }
+  
+  public boolean isNavDataDemoEnabled() {
+    return bit(10);
+  }
+  
+  public boolean isNavDataSuppressed() {
+    return bit(11);
+  }
+  
+  public boolean hasMotorProblem() {
+    return bit(12);
+  }
+  
+  public boolean hasCommunicationProblem() {
+    return bit(13);
+  }
+  
+  public boolean hasBatteryWarning() {
+    return bit(15);
+  }
+  
+  public boolean hasUserEmergency() {
+    return bit(16);
+  }
+  
+  public boolean hasTimerElapsed() {
+    return bit(17);
+  }
+  
+  public boolean hasExceededAngleLimits() {
+    return bit(19);
+  }
+  
+  public boolean hasUltrasonicProblem() {
+    return bit(21);
+  }
+  
+  public boolean hasAutomaticCutoutEngaged() {
+    return bit(22);
+  }
+  
+  public boolean isPicVersionOkay() {
+    return bit(23);
+  }
+  
+  public boolean isAtCodecThreadRunning() {
+    return bit(24);
+  }
+  
+  public boolean isNavDataThreadRunning() {
+    return bit(25);
+  }
+  
+  public boolean isVideoThreadRunning() {
+    return bit(26);
+  }
+  
+  public boolean isAcquisitionThreadRunning() {
+    return bit(27);
+  }
+  
+  public boolean isControlExecutionDelayed() {
+    return bit(28);
+  }
+  
+  public boolean hasAdcProblem() {
+    return bit(29);
+  }
+  
+  public boolean isCommunicationsWatchdogTripped() {
+    return bit(30);
+  }
+  
+  public boolean hasEmergency() {
+    return bit(31);
+  }
+  
+  @Override public String toString() {
+    return "DroneState(" + Integer.toHexString(bits) + ")";
+  }
+  
+  @Override public boolean equals(Object o) {
+    if (o == null || o.getClass() != getClass()) return false;
+    
+    return bits == ((DroneState) o).bits;
+  }
+  
+  @Override public int hashCode() {
+    return 31 * bits;
+  }
+  
+  private boolean bit(int index) {
+    return (bits & (1 << index)) != 0;
+  }
+}

src/org/mg8/ardrone/comms/navdata/NavDataException.java

+/*
+ * Copyright 2010 Cliff L. Biffle.  All Rights Reserved.
+ * Use of this source code is governed by a BSD-style license that can be found
+ * in the LICENSE file.
+ */
+package org.mg8.ardrone.comms.navdata;
+
+/**
+ * Indicates a failure decoding nav data.
+ */
+public class NavDataException extends Exception {
+  private static final long serialVersionUID = 6472545302468239466L;
+
+  public NavDataException(String message) {
+    super(message);
+  }
+
+}

src/org/mg8/ardrone/comms/navdata/NavDataParser.java

+/*
+ * Copyright 2010 Cliff L. Biffle.  All Rights Reserved.
+ * Use of this source code is governed by a BSD-style license that can be found
+ * in the LICENSE file.
+ */
+package org.mg8.ardrone.comms.navdata;
+
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+
+/**
+ * Parses nav data, distributing the results to registered listeners.
+ */
+public class NavDataParser {
+  private AttitudeListener attitudeListener;
+  private StateListener stateListener;
+  private VelocityListener velocityListener;
+  private BatteryListener batteryListener;
+  
+  private long lastSequenceNumber = 1;
+
+  public void setBatteryListener(BatteryListener batteryListener) {
+    this.batteryListener = batteryListener;
+  }
+  public void setAttitudeListener(AttitudeListener attitudeListener) {
+    this.attitudeListener = attitudeListener;
+  }
+
+  public void setStateListener(StateListener stateListener) {
+    this.stateListener = stateListener;
+  }
+
+  public void setVelocityListener(VelocityListener velocityListener) {
+    this.velocityListener = velocityListener;
+  }
+
+  public void parseNavData(ByteBuffer buffer) throws NavDataException {
+    buffer.order(ByteOrder.LITTLE_ENDIAN);
+    int magic = buffer.getInt();
+    requireEquals("Magic must be correct", 0x55667788, magic);
+    
+    int state = buffer.getInt();
+    long sequence = buffer.getInt() & 0xFFFFFFFFL;
+    @SuppressWarnings("unused")  // TODO: figure out what to do with this.
+    int vision = buffer.getInt();
+    
+    if (sequence < lastSequenceNumber && sequence != 1) {
+      return;
+    }
+    lastSequenceNumber = sequence;
+    
+    if (stateListener != null) {
+      stateListener.stateChanged(new DroneState(state));
+    }
+    
+    while (buffer.position() < buffer.limit()) {
+      /* Option size includes the header (determined empirically). */
+      int tag = buffer.getShort() & 0xFFFF;
+      int payloadSize = (buffer.getShort() & 0xFFFF) - 4;
+      ByteBuffer optionData = buffer.slice().order(ByteOrder.LITTLE_ENDIAN);
+      optionData.limit(payloadSize);
+      buffer.position(buffer.position() + payloadSize);
+      
+      dispatch(tag, optionData);
+    }
+  }
+  
+  private void dispatch(int tag, ByteBuffer optionData) {
+    switch (tag) {
+      case 0:  // NAVDATA_DEMO_TAG
+        processNavDataDemo(optionData);
+        break;
+    }
+  }
+
+  private void processNavDataDemo(ByteBuffer optionData) {
+    @SuppressWarnings("unused")  // TODO undocumented
+    int controlState = optionData.getInt();
+    int batteryPercentage = optionData.getInt();
+    
+    // Attitude is in millidegrees, stored as an integral float.  How odd.
+    float theta = optionData.getFloat() / 1000;
+    float phi = optionData.getFloat() / 1000;
+    float psi = optionData.getFloat() / 1000;
+    
+    // Altitude is in millimeters.
+    int altitude = optionData.getInt();
+    
+    float vx = optionData.getFloat();
+    float vy = optionData.getFloat();
+    float vz = optionData.getFloat();
+    
+    if (batteryListener != null) {
+      batteryListener.batteryLevelChanged(batteryPercentage);
+    }
+    
+    if (attitudeListener != null) {
+      attitudeListener.attitudeUpdated(theta, phi, psi, altitude);
+    }
+    
+    if (velocityListener != null) {
+      velocityListener.velocityChanged(vx, vy, vz);
+    }
+  }
+
+  private void requireEquals(String message, int expected, int actual)
+      throws NavDataException {
+    if (expected != actual) {
+      throw new NavDataException(message
+          + ": expected " + expected + ", was " + actual);
+    }
+  }
+  
+}

src/org/mg8/ardrone/comms/navdata/StateListener.java

+/*
+ * Copyright 2010 Cliff L. Biffle.  All Rights Reserved.
+ * Use of this source code is governed by a BSD-style license that can be found
+ * in the LICENSE file.
+ */
+package org.mg8.ardrone.comms.navdata;
+
+/**
+ * Listener interface for notifications about the drone's system state.
+ */
+public interface StateListener {
+  void stateChanged(DroneState state);
+}

src/org/mg8/ardrone/comms/navdata/VelocityListener.java

+/*
+ * Copyright 2010 Cliff L. Biffle.  All Rights Reserved.
+ * Use of this source code is governed by a BSD-style license that can be found
+ * in the LICENSE file.
+ */
+package org.mg8.ardrone.comms.navdata;
+
+/**
+ * Listener interface for updates on the drone's estimated velocity.
+ */
+public interface VelocityListener {
+  void velocityChanged(float vx, float vy, float vz);
+}

test/org/mg8/ardrone/comms/navdata/NavDataDemo.java

+/*
+ * Copyright 2010 Cliff L. Biffle.  All Rights Reserved.
+ * Use of this source code is governed by a BSD-style license that can be found
+ * in the LICENSE file.
+ */
+package org.mg8.ardrone.comms.navdata;
+
+import java.io.IOException;
+import java.net.DatagramPacket;
+import java.net.DatagramSocket;
+import java.nio.ByteBuffer;
+
+import org.mg8.ardrone.comms.DroneControlSocket;
+
+import com.google.common.net.InetAddresses;
+
+/**
+ * @author cbiffle
+ *
+ */
+public class NavDataDemo {
+  private final DroneControlSocket control;
+  private final DatagramSocket navdataSocket;
+  
+  public NavDataDemo(DroneControlSocket control, DatagramSocket navdataSocket) {
+    this.control = control;
+    this.navdataSocket = navdataSocket;
+  }
+  
+  public void run() throws IOException, NavDataException {
+    tickleNavdataPort();
+    enableDemoData();
+    tickleNavdataPort();
+    sendControlAck();
+    NavDataParser parser = new NavDataParser();
+    
+    parser.setAttitudeListener(new AttitudeListener() {
+      @Override public void attitudeUpdated(float theta, float phi, float psi,
+          int altitude) {
+        System.out.printf("Attitude: theta=%f phi=%f psi=%f altitude=%d\n", theta, phi, psi, altitude);
+      }});
+    
+    parser.setStateListener(new StateListener() {
+      @Override public void stateChanged(DroneState state) {
+        System.out.println("State: " + state);
+      }});
+    
+    while (true) {
+      tickleNavdataPort();
+      DatagramPacket packet = new DatagramPacket(new byte[1024], 1024);
+      navdataSocket.receive(packet);
+      System.out.println("Received packet");
+      
+      ByteBuffer buffer = ByteBuffer.wrap(packet.getData(), 0, packet.getLength());
+      parser.parseNavData(buffer);
+    }
+  }
+
+  private void tickleNavdataPort() throws IOException {
+    byte[] buf = new byte[1];
+    buf[0] = '\n';
+    DatagramPacket packet = new DatagramPacket(buf, 1);
+    navdataSocket.send(packet);
+  }
+  
+  private void enableDemoData() throws IOException {
+    control.newMessage().addConfigCommand("general:navdata_demo", "TRUE").addFlatTrimCommand().send();
+  }
+  
+  private void sendControlAck() throws IOException {
+    control.newMessage().addModeMessage(0).send();
+  }
+
+  public static void main(String[] args) throws Exception {
+    System.out.println("Navdata demo running.");
+    
+    DroneControlSocket dcs = DroneControlSocket.create();
+    DatagramSocket navdata = new DatagramSocket(5554);
+    navdata.connect(InetAddresses.forString("192.168.1.1"), 5554);
+    
+    new NavDataDemo(dcs, navdata).run();
+  }
+}

test/org/mg8/ardrone/comms/navdata/NavDataParserTest.java

+/*
+ * Copyright 2010 Cliff L. Biffle.  All Rights Reserved.
+ * Use of this source code is governed by a BSD-style license that can be found
+ * in the LICENSE file.
+ */
+package org.mg8.ardrone.comms.navdata;
+
+import static org.easymock.EasyMock.*;
+
+import java.nio.ByteBuffer;
+
+import org.junit.After;
+import org.junit.Before;
+import org.junit.Test;
+
+/**
+ * Unit test for {@link NavDataParser}.
+ */
+public class NavDataParserTest {
+  NavDataParser unit;
+  
+  AttitudeListener attitudeListener;
+  BatteryListener batteryListener;
+  StateListener stateListener;
+  VelocityListener velocityListener;
+  
+  @Before public void makeParserAndRegisterListeners() {
+    unit = new NavDataParser();
+    
+    attitudeListener = createMock(AttitudeListener.class);
+    batteryListener = createMock(BatteryListener.class);
+    stateListener = createMock(StateListener.class);
+    velocityListener = createMock(VelocityListener.class);
+    replayAll();
+    
+    unit.setAttitudeListener(attitudeListener);
+    unit.setBatteryListener(batteryListener);
+    unit.setStateListener(stateListener);
+    unit.setVelocityListener(velocityListener);
+    verifyAll();
+    resetAll();
+  }
+  
+  @After public void verifyAll() {
+    verify(attitudeListener, batteryListener, stateListener, velocityListener);
+  }
+  
+  @Test public void testParseNavData_capturedBootstrapPacket()
+      throws NavDataException {
+    byte[] packet = {
+        (byte) 0x88, 0x77, 0x66, 0x55, 0x00, 0x1c,
+        (byte) 0x82, (byte) 0xcf, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00,
+        0x00, 0x00, (byte) 0xff, (byte) 0xff, 0x08, 0x00, 0x28, 0x03,
+        0x00, 0x00,
+    };
+    
+    stateListener.stateChanged(new DroneState(0xCF821C00));
+    // No useful data is included here.
+    
+    replayAll();
+    ByteBuffer buffer = ByteBuffer.wrap(packet);
+    
+    unit.parseNavData(buffer);
+  }
+
+  @Test public void testParseNavData_capturedDemoPacket()
+      throws NavDataException {
+    byte[] packet = {
+        (byte) 0x88, 0x77, 0x66, 0x55, 0x40, 0x14,
+        (byte) 0x82, (byte) 0xcf, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00,
+        0x00, 0x00, 0x00, 0x00, (byte) 0x94, 0x00, 0x00, 0x00,
+        0x00, 0x00, 0x64, 0x00, 0x00, 0x00, 0x00, (byte) 0xb8,
+        0x4b, (byte) 0xc6, 0x00, 0x00, 0x63, (byte) 0xc4, 0x00, (byte) 0xa0,
+        (byte) 0xa8, 0x44, (byte) 0xdc, 0x00, 0x00, 0x00, 0x00, 0x00,
+        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+        0x00, 0x00, (byte) 0xf9, 0x06, 0x00, 0x00, 0x00, 0x00,
+        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, (byte) 0xd0, 0x54,
+        0x79, 0x3f, 0x00, (byte) 0x9d, (byte) 0xa3, (byte) 0xbc, 0x0f, 0x4d,
+        0x67, (byte) 0xbe, (byte) 0xbe, (byte) 0xf7,
+        (byte) 0xbb, 0x3c, 0x1a, (byte) 0xeb,
+        0x7f, 0x3f, 0x7a, (byte) 0xbf, 0x2c, 0x3c, (byte) 0xf9, 0x02,
+        0x67, 0x3e, 0x0a, 0x2a, 0x7d, (byte) 0xbc, 0x7e, 0x5e,
+        0x79, 0x3f, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x07, 0x00,
+        0x10, 0x00, 0x00, 0x00, 0x00, 0x00, (byte) 0x8b, 0x04,
+        0x27, 0x40, (byte) 0xc4, (byte) 0xab,
+        (byte) 0x92, (byte) 0xc0, 0x10, 0x00,
+        0x68, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, (byte) 0xff, (byte) 0xff,
+        0x08, 0x00, (byte) 0xff, 0x1e, 0x00, 0x00,
+    };
+
+    stateListener.stateChanged(new DroneState(0xCF821440));
+    batteryListener.batteryLevelChanged(100);
+    attitudeListener.attitudeUpdated(-13.038f, -0.908f, 1.349f, 220);
+    velocityListener.velocityChanged(0, 0, 0);
+
+    replayAll();
+    ByteBuffer buffer = ByteBuffer.wrap(packet);
+
+    unit.parseNavData(buffer);
+  }
+
+  private void replayAll() {
+    replay(attitudeListener, batteryListener, stateListener, velocityListener);
+  }
+
+  private void resetAll() {
+    reset(attitudeListener, batteryListener, stateListener, velocityListener);
+  }
+}