示例#1
0
  protected void initIMU() {

    // The nav6 IMU serial port configuration is 8 data bits, no parity, one stop bit.
    // No flow control is used.
    // Conveniently, these are the defaults used by the WPILib's SerialPort class.
    //
    // In addition, the WPILib's SerialPort class also defaults to:
    //
    // Timeout period of 5 seconds
    // Termination ('\n' character)
    // Transmit immediately

    initializeYawHistory();
    user_yaw_offset = 0;

    // set the nav6 into the desired update mode
    byte stream_command_buffer[] = new byte[256];
    int packet_length =
        IMUProtocol.encodeStreamCommand(stream_command_buffer, update_type, update_rate_hz);
    try {
      serial_port.write(stream_command_buffer, packet_length);
    } catch (RuntimeException ex) {
      ex.printStackTrace();
    }
  }
示例#2
0
  public void run() {

    stop = false;
    boolean stream_response_received = false;
    double last_valid_packet_time = 0.0;
    int partial_binary_packet_count = 0;
    int stream_response_receive_count = 0;
    int timeout_count = 0;
    int discarded_bytes_count = 0;
    int port_reset_count = 0;
    double last_stream_command_sent_timestamp = 0.0;
    int updates_in_last_second = 0;
    double last_second_start_time = 0;
    try {
      serial_port.setReadBufferSize(512);
      serial_port.setTimeout(1.0);
      serial_port.enableTermination('\n');
      serial_port.flush();
      serial_port.reset();
    } catch (RuntimeException ex) {
      ex.printStackTrace();
    }

    IMUProtocol.StreamResponse response = new IMUProtocol.StreamResponse();

    byte[] stream_command = new byte[256];

    int cmd_packet_length =
        IMUProtocol.encodeStreamCommand(stream_command, update_type, update_rate_hz);
    try {
      serial_port.reset();
      serial_port.write(stream_command, cmd_packet_length);
      serial_port.flush();
      port_reset_count++;
      // SmartDashboard.putNumber("nav6_PortResets", (double)port_reset_count);
      last_stream_command_sent_timestamp = Timer.getFPGATimestamp();
    } catch (RuntimeException ex) {
      ex.printStackTrace();
    }

    while (!stop) {
      try {

        // Wait, with delays to conserve CPU resources, until
        // bytes have arrived.

        while (!stop && (serial_port.getBytesReceived() < 1)) {
          Timer.delay(1.0 / update_rate_hz);
        }

        int packets_received = 0;
        byte[] received_data = serial_port.read(256);
        int bytes_read = received_data.length;
        if (bytes_read > 0) {
          byte_count += bytes_read;
          int i = 0;
          // Scan the buffer looking for valid packets
          while (i < bytes_read) {

            // Attempt to decode a packet

            int bytes_remaining = bytes_read - i;

            if (received_data[i] != IMUProtocol.PACKET_START_CHAR) {
              /* Skip over received bytes until a packet start is detected. */
              i++;
              discarded_bytes_count++;
              // SmartDashboard.putNumber("nav6 Discarded Bytes", (double)discarded_bytes_count);
              continue;
            } else {
              if ((bytes_remaining > 2) && (received_data[i + 1] == (byte) '#')) {
                /* Binary packet received; next byte is packet length-2 */
                byte total_expected_binary_data_bytes = received_data[i + 2];
                total_expected_binary_data_bytes += 2;
                while (bytes_remaining < total_expected_binary_data_bytes) {

                  /* This binary packet contains an embedded     */
                  /* end-of-line character.  Continue to receive */
                  /* more data until entire packet is received.  */
                  byte[] additional_received_data = serial_port.read(256);
                  byte_count += additional_received_data.length;
                  bytes_remaining += additional_received_data.length;

                  /* Resize array to hold existing and new data */
                  byte[] c = new byte[received_data.length + additional_received_data.length];
                  if (c.length > 0) {
                    System.arraycopy(received_data, 0, c, 0, received_data.length);
                    System.arraycopy(
                        additional_received_data,
                        0,
                        c,
                        received_data.length,
                        additional_received_data.length);
                    received_data = c;
                  } else {
                    /* Timeout waiting for remainder of binary packet */
                    i++;
                    bytes_remaining--;
                    partial_binary_packet_count++;
                    // SmartDashboard.putNumber("nav6 Partial Binary Packets",
                    // (double)partial_binary_packet_count);
                    continue;
                  }
                }
              }
            }

            int packet_length = decodePacketHandler(received_data, i, bytes_remaining);
            if (packet_length > 0) {
              packets_received++;
              update_count++;
              last_valid_packet_time = Timer.getFPGATimestamp();
              updates_in_last_second++;
              if ((last_valid_packet_time - last_second_start_time) > 1.0) {
                // SmartDashboard.putNumber("nav6 UpdatesPerSec", (double)updates_in_last_second);
                updates_in_last_second = 0;
                last_second_start_time = last_valid_packet_time;
              }
              i += packet_length;
            } else {
              packet_length =
                  IMUProtocol.decodeStreamResponse(received_data, i, bytes_remaining, response);
              if (packet_length > 0) {
                packets_received++;
                setStreamResponse(response);
                stream_response_received = true;
                i += packet_length;
                stream_response_receive_count++;
                // SmartDashboard.putNumber("nav6 Stream Responses",
                // (double)stream_response_receive_count);
              } else {
                // current index is not the start of a valid packet; increment
                i++;
              }
            }
          }

          if ((packets_received == 0) && (bytes_read == 256)) {
            // Workaround for issue found in SerialPort implementation:
            // No packets received and 256 bytes received; this
            // condition occurs in the SerialPort.  In this case,
            // reset the serial port.
            serial_port.reset();
            port_reset_count++;
            // SmartDashboard.putNumber("nav6_PortResets", (double)port_reset_count);
          }

          // If a stream configuration response has not been received within three seconds
          // of operation, (re)send a stream configuration request

          if (!stream_response_received
              && ((Timer.getFPGATimestamp() - last_stream_command_sent_timestamp) > 3.0)) {
            cmd_packet_length =
                IMUProtocol.encodeStreamCommand(stream_command, update_type, update_rate_hz);
            try {
              last_stream_command_sent_timestamp = Timer.getFPGATimestamp();
              serial_port.write(stream_command, cmd_packet_length);
              serial_port.flush();
            } catch (RuntimeException ex2) {
              ex2.printStackTrace();
            }
          } else {
            // If no bytes remain in the buffer, and not awaiting a response, sleep a bit
            if (stream_response_received && (serial_port.getBytesReceived() == 0)) {
              Timer.delay(1.0 / update_rate_hz);
            }
          }

          /* If receiving data, but no valid packets have been received in the last second */
          /* the navX MXP may have been reset, but no exception has been detected.         */
          /* In this case , trigger transmission of a new stream_command, to ensure the    */
          /* streaming packet type is configured correctly.                                */

          if ((Timer.getFPGATimestamp() - last_valid_packet_time) > 1.0) {
            stream_response_received = false;
          }
        }
      } catch (RuntimeException ex) {
        // This exception typically indicates a Timeout
        stream_response_received = false;
        timeout_count++;
        // SmartDashboard.putNumber("nav6 Serial Port Timeouts", (double)timeout_count);
        // SmartDashboard.putString("LastNavExceptionBacktrace",ex.getStackTrace().toString());
        // SmartDashboard.putString("LastNavException", ex.getMessage() + "; " + ex.toString());
      }
    }
  }