137 lines
6.9 KiB
Plaintext
137 lines
6.9 KiB
Plaintext
import processing.serial.*;
|
|
|
|
class Radar {
|
|
static final byte REQUEST_TYPE_PDAT = 0x04;
|
|
static final byte REQUEST_TYPE_TDAT = 0x08;
|
|
static final byte BASE_FREQUENCY_LOW = 0x00;
|
|
static final byte BASE_FREQUENCY_MIDDLE = 0x01;
|
|
static final byte BASE_FREQUENCY_HIGH = 0x02;
|
|
static final byte MAXIMUM_SPEED_12_5 = 0x00;
|
|
static final byte MAXIMUM_SPEED_25 = 0x01;
|
|
static final byte MAXIMUM_SPEED_50 = 0x02;
|
|
static final byte MAXIMUM_SPEED_100 = 0x03;
|
|
static final byte MAXIMUM_RANGE_5 = 0x00;
|
|
static final byte MAXIMUM_RANGE_10 = 0x01;
|
|
static final byte MAXIMUM_RANGE_30 = 0x02;
|
|
static final byte MAXIMUM_RANGE_100 = 0x03;
|
|
static final byte TRACKING_FILTER_TYPE_STANDARD = 0x00;
|
|
static final byte TRACKING_FILTER_TYPE_FAST_DETECTION = 0x01;
|
|
static final byte TRACKING_FILTER_TYPE_LONG_VISIBILITY = 0x02;
|
|
static final byte DETECTION_DIRECTION_RECEDING = 0x00;
|
|
static final byte DETECTION_DIRECTION_APPROACHING = 0x01;
|
|
static final byte DETECTION_DIRECTION_BOTH = 0x02;
|
|
static final byte DIGITAL_OUTPUT_DIRECTION = 0x00;
|
|
static final byte DIGITAL_OUTPUT_ANGLE = 0x01;
|
|
static final byte DIGITAL_OUTPUT_RANGE = 0x02;
|
|
static final byte DIGITAL_OUTPUT_SPEED = 0x03;
|
|
static final byte DIGITAL_OUTPUT_MICRO_DETECTION = 0x04;
|
|
static final byte RETRIGGER_OFF = 0x00;
|
|
static final byte RETRIGGER_ON = 0x01;
|
|
|
|
Serial serial;
|
|
public Radar(k_ld7 parent, String port) {
|
|
serial = new Serial(parent, port, 115200, 'E', 8, 1.0);
|
|
sendCommandAndWaitForResponse("INIT", new byte[]{0x00, 0x00, 0x00, 0x00});
|
|
}
|
|
|
|
public void stop() {
|
|
serial.stop();
|
|
}
|
|
|
|
private void sendCommand(String command, byte[] bytes) {
|
|
if (logSerial) System.out.printf("Sending command %s...\n", command);
|
|
serial.write(command);
|
|
int len = (bytes == null) ? 0 : bytes.length;
|
|
serial.write(new byte[] {(byte)(len & 0xFF), (byte)((len >> 8) & 0xFF), (byte)((len >> 16) & 0xFF), (byte)((len >> 24) & 0xFF)});
|
|
if (len > 0) serial.write(bytes);
|
|
}
|
|
|
|
private void sendCommandAndWaitForResponse(String command, byte[] bytes) {
|
|
sendCommand(command, bytes);
|
|
while(!readResponse().equals("RESP")) {try{Thread.sleep(100);}catch(Exception e){}};
|
|
}
|
|
|
|
private byte[] readBytes(int count) {
|
|
byte[] result = serial.readBytes(count);
|
|
if (logSerial) {
|
|
for (int i=0; i<count; i++) {
|
|
System.out.printf("0x%02X ", result[i]);
|
|
}
|
|
System.out.println();
|
|
}
|
|
return result;
|
|
}
|
|
|
|
public String readResponse() {
|
|
if (logSerial) System.out.printf("readResponse(): %d bytes available\n", serial.available());
|
|
if (serial.available() < 8) {
|
|
return "";
|
|
}
|
|
String command = new String(readBytes(4));
|
|
byte[] l = readBytes(4);
|
|
int len = l[0] + (l[1]<<8) + (l[2]<<16) + (l[3]<<24);
|
|
if (logSerial) System.out.printf("Receiving %s with %d Bytes payload...", command, len);
|
|
byte[] payload = readBytes(len);
|
|
if (logSerial) System.out.println(" complete.");
|
|
|
|
if (command.equals("RESP") && len==1) {
|
|
switch (payload[0]) {
|
|
case 0: break; // OK
|
|
case 1: System.out.println("Unknown command"); break;
|
|
case 2: System.out.println("Invalid parameter value"); break;
|
|
case 3: System.out.println("Invalid RPST version"); break;
|
|
case 4: System.out.println("Uart error (parity, framing, noise)"); break;
|
|
case 5: System.out.println("Sensor busy"); break;
|
|
case 6: System.out.println("Timeout error"); break;
|
|
default: System.out.printf("Unknown error code %d\n", payload[0]); break;
|
|
}
|
|
} else if (command.equals("PDAT")) {
|
|
if (len > 0) {
|
|
detections = parseDetectionData(payload);
|
|
}
|
|
waitingForData = false;
|
|
dataSets++;
|
|
} else if (command.equals("TDAT")) {
|
|
if (len > 0) {
|
|
detections = parseDetectionData(payload);
|
|
}
|
|
waitingForData = false;
|
|
dataSets++;
|
|
} else if (command.equals("RPST")) {
|
|
for(byte b : payload) {
|
|
System.out.printf("%02X ", b);
|
|
}
|
|
System.out.println();
|
|
}
|
|
|
|
return command;
|
|
}
|
|
|
|
public void requestDataFrame(byte type) {
|
|
sendCommand("GNFD", new byte[] {type, 0, 0, 0});
|
|
}
|
|
|
|
public Radar setBaseFrequency(byte type) { sendCommandAndWaitForResponse("RBFR", new byte[]{type, 0, 0, 0}); return this;}
|
|
public Radar setMaximumSpeed(byte type) { sendCommandAndWaitForResponse("RSPI", new byte[]{type, 0, 0, 0}); return this;}
|
|
public Radar setMaximumRange(byte type) { sendCommandAndWaitForResponse("RRAI", new byte[]{type, 0, 0, 0}); return this;}
|
|
public Radar setThresholdOffset(byte value) { sendCommandAndWaitForResponse("THOF", new byte[]{value, 0, 0, 0}); return this;}
|
|
public Radar setTrackingFilterType(byte type) { sendCommandAndWaitForResponse("TRFT", new byte[]{type, 0, 0, 0}); return this;}
|
|
public Radar setVibrationSuppression(byte value) { sendCommandAndWaitForResponse("VISU", new byte[]{value, 0, 0, 0}); return this;}
|
|
public Radar setMinimumDetectionDistance(byte type) { sendCommandAndWaitForResponse("MIRA", new byte[]{type, 0, 0, 0}); return this;}
|
|
public Radar setMaximumDetectionDistance(byte type) { sendCommandAndWaitForResponse("MARA", new byte[]{type, 0, 0, 0}); return this;}
|
|
public Radar setMinimumDetectionAngle(byte value) { sendCommandAndWaitForResponse("MIAN", new byte[]{value, 0, 0, 0}); return this;}
|
|
public Radar setMaximumDetectionAngle(byte value) { sendCommandAndWaitForResponse("MAAN", new byte[]{value, 0, 0, 0}); return this;}
|
|
public Radar setMinimumDetectionSpeed(byte value) { sendCommandAndWaitForResponse("MISP", new byte[]{value, 0, 0, 0}); return this;}
|
|
public Radar setMaximumDetectionSpeed(byte value) { sendCommandAndWaitForResponse("MASP", new byte[]{value, 0, 0, 0}); return this;}
|
|
public Radar setDetectionDirection(byte type) { sendCommandAndWaitForResponse("DEDI", new byte[]{type, 0, 0, 0}); return this;}
|
|
public Radar setRangeThreshold(byte value) { sendCommandAndWaitForResponse("RATH", new byte[]{value, 0, 0, 0}); return this;}
|
|
public Radar setAngleThreshold(byte value) { sendCommandAndWaitForResponse("ANTH", new byte[]{value, 0, 0, 0}); return this;}
|
|
public Radar setSpeedThreshold(byte value) { sendCommandAndWaitForResponse("SPTH", new byte[]{value, 0, 0, 0}); return this;}
|
|
public Radar setDigitalOutput1(byte type) { sendCommandAndWaitForResponse("DIG1", new byte[]{type, 0, 0, 0}); return this;}
|
|
public Radar setDigitalOutput2(byte type) { sendCommandAndWaitForResponse("DIG2", new byte[]{type, 0, 0, 0}); return this;}
|
|
public Radar setDigitalOutput3(byte type) { sendCommandAndWaitForResponse("DIG3", new byte[]{type, 0, 0, 0}); return this;}
|
|
public Radar setHoldTime(int value) { sendCommandAndWaitForResponse("DIG1", new byte[]{(byte)(value & 0xFF), (byte)((value >> 8) & 0xFF), 0, 0}); return this;}
|
|
public Radar setMicroDetectionRetrigger(byte type) { sendCommandAndWaitForResponse("MIDE", new byte[]{type, 0, 0, 0}); return this;}
|
|
public Radar setMicroDetectionSensitivity(byte type) { sendCommandAndWaitForResponse("MIDS", new byte[]{type, 0, 0, 0}); return this;}
|
|
}
|