Android Open Source - base_controller Kobuki Packet Reader

• Back to Hardware/controller  ↑

From Project

Back to project page base_controller.

License

The source code is released under:

Apache License

Java Source Code

/*
  Copyright (c) 2013 - York College of Pennsylvania, Patrick J. Martin
  The MIT License/* w  w  w  .  j av a 2s.c o m*/
  See license.txt for details.
*/

package com.github.c77.base_driver.kobuki;
import org.apache.commons.codec.binary.Hex;
import org.apache.commons.logging.Log;
import org.apache.commons.logging.LogFactory;

import java.nio.ByteBuffer;
import java.util.Arrays;

public class KobukiPacketReader {

    Log log = LogFactory.getLog(KobukiPacketReader.class);

    //kobuki sensors read from here to update. Accessible fields
  private byte[] sensorPacket = new byte[15];
  private byte[] inertialSensorPacket = new byte[4];
  private byte[] dockingIRPacket = new byte[3];
  private byte[] wheelCurrentPacket = new byte[4];
  private enum parserState {
    INIT, PARTIAL, COMPLETE;
  }
  //Fields for class
  private parserState curState;
  private volatile byte[] stored;
  private int bytesStillNeeded;
  private int totalSize;
  private int bytesStoredIndex;

  public KobukiPacketReader() {
    curState = parserState.INIT;
    stored = new byte[500]; // Should be big enough for the biggest packets
    bytesStillNeeded = 0;
    totalSize = 0;
    bytesStoredIndex = 0;
  }

  public final void newPacket(ByteBuffer buff) { // look for main headers in buffer
    if(curState == parserState.INIT || curState == parserState.COMPLETE) {
      for(int i=0; i < buff.array().length-2; i++){ // currently ignoring -86|85 split case... should be rare
        if(buff.array()[i] == -86 && buff.array()[i+1] == 85) { // Header 0 and Header 1 means packet
          curState = parserState.PARTIAL;
          if((buff.array().length-i) >= (buff.array()[i+2]+4)) { // Whole packet is in the buffer. +3 accounts for headers and size. Don't think this is right. Change(+4 for checksum)
            byte[] packet = new byte[byteToInt(buff.array()[i+2])+2]; // make byte array of proper size. +1 just in case!!!! Needs to be + 2 to account for checksum and length
            //If x + 2 is the length of the payload, the length of payload + length + check sum = length of payload + 2
            System.arraycopy(buff.array(), i + 2, packet, 0, byteToInt(buff.array()[i+2]) + 2); //copy in data needed
            if(verifyChecksum(byteToInt(buff.array()[i+2]) + 2, packet)) {
              System.arraycopy(packet, 1, packet, 0, packet.length - 1);
              goodPacket(byteToInt(buff.array()[i+2]) ,packet); // send goodPacket the array and length of array.  At this point, the packet contains ONLY the payload...
            } else {
              //packet not valid
            }
            curState = parserState.COMPLETE;
          } else {
            System.arraycopy(buff.array(), i+2, stored, 0, (buff.array().length-(i+2))); //Copy the partial packet into the stored array (only for partial packets)
            bytesStillNeeded = (buff.array()[i+2] + 2) - (buff.array().length-(i+2));
            totalSize = buff.array()[i+2] + 2;  //This is the payload length + 2.  This accounts for both the length of the payload byte as well as the checksum byte
            bytesStoredIndex = buff.array().length - (i+2); //Basically, in the case of a partial packet, this variable stores the length of the partial packet
          }
        }
      }
    } else {
      System.arraycopy(buff.array(), 0, stored, bytesStoredIndex, bytesStillNeeded);  //Copy in the "rest" of the packet
      if(verifyChecksum(totalSize, stored)) {
        System.arraycopy(stored, 1, stored, 0, stored.length - 1);  //Get the array without the checksum or length
        goodPacket(totalSize - 2, stored);
      } else {
        //packet not valid
      }
      curState = parserState.COMPLETE;
    }
  }

  private final boolean verifyChecksum(int length, byte[] packet) { //Method for verifying checksum

    byte checksum = 0;

    for(int i = 0; i < length - 1; i++) {  //length - 1 so that checksum is not included in total!

      checksum ^= packet[i];   //XOR's all values in packet EXCEPT: HEADERS AND CHECKSUM
    }

    if(packet[length - 1] == checksum) { //Ensures that calculated checksum is equal to the checksum found in the packet
      return true;
    } else {
      return false;
    }
  }

  private final void goodPacket(int length ,byte[] packet) { // Without first 2 headers, length, or checksum!
    //header
    //size
    //payload

    int curPlace = 0;
    byte[] toSend = new byte[100];
    System.arraycopy(packet, 0, toSend, 0, (packet[1]+2));
    curPlace = packet[1] + 2;
    sortParts(toSend);

    for(int i = 0; i < 6; i++) { // seven total packets six left
       Arrays.fill(toSend, (byte)0);
       //Changed from curPlace + 1 to curPlace. Changed last field from curPlace + 2 to packet[curPlace + 1]
       System.arraycopy(packet, curPlace, toSend, 0, packet[curPlace+1] + 2); //Curplace is current index that needs to be accessed. packet[curPlace + 1] + 2 accounts for the size of the payload AND the index + payload size bytes
       curPlace = curPlace + packet[curPlace+1] + 2; //Essentially, the index of the element we just accessed. + the length of the payload + 2 for the index and payload size
       sortParts(toSend);  //Sorts packet array into individual packets (see method)
    }
  }

  private final void sortParts(byte[] packet){
    switch(packet[0]){ // Sort by packet[0] (The index of the packet. See kobuki driver page for more info).
      case 1:
        sensorPacket(packet); // Sorts sensor packet
        break;
      case 3:
        dockingIRPacket(packet); //sorts dockingIR packet
        break;
      case 4:
        inertialSensorPacket(packet); //Sorts inertial sensor packet
        break;
      case 6:
        wheelCurrentPacket(packet); //Sorts wheel current packet
        break;
      default:
        break;
    }
  }

  private final int byteToInt(byte b) {
    return b & 0xFF;
  }

  public final void sensorPacket(byte[] packet) { // Store sensor values from packet
    System.arraycopy(packet, 2, sensorPacket, 0, 15); //Copy packet from incoming byte array
  }

  private final void inertialSensorPacket(byte[] packet) {
    System.arraycopy(packet, 2, inertialSensorPacket, 0, 4); //Copy packet from incoming byte array
  }

  private final void dockingIRPacket(byte[] packet) {  //Sorts the IR docking packet
    System.arraycopy(packet, 2, dockingIRPacket, 0, 3); //Copy packet from incoming byte array
  }

  private final void wheelCurrentPacket(byte[] packet) {  //Sorts wheel current data packet
    System.arraycopy(packet, 2, wheelCurrentPacket, 0, 2); //Copy packet from incoming byte array
  }

  /**
   * @return the sensorPacket
   */
  public final byte[] getSensorPacket() {
    return sensorPacket;
  }

  /**
   * @return the inertialSensorPacket
   */
  public final byte[] getInertialSensorPacket() {
    return inertialSensorPacket;
  }

  /**
   * @return the dockingIRPacket
   */
  public final byte[] getDockingIRPacket() {
    return dockingIRPacket;
  }

  /**
   * @return the wheelCurrentPacket
   */
  public final byte[] getWheelCurrentPacket() {
    return wheelCurrentPacket;
  }
}

Java Source Code List

• Back to Hardware/controller  ↑