Initial commit
This commit is contained in:
parent
8ba24da704
commit
b897cf9734
3
.gitignore
vendored
3
.gitignore
vendored
@ -10,3 +10,6 @@ application.windows64
|
|||||||
application.macosx
|
application.macosx
|
||||||
out
|
out
|
||||||
|
|
||||||
|
.*~
|
||||||
|
*.tsv
|
||||||
|
*.ods
|
||||||
|
|||||||
32
detection.pde
Normal file
32
detection.pde
Normal file
@ -0,0 +1,32 @@
|
|||||||
|
class Detection {
|
||||||
|
public int distance, x, y;
|
||||||
|
public float speed, angle, magnitude, radAngle, distPx;
|
||||||
|
public boolean freshData;
|
||||||
|
|
||||||
|
public Detection(byte[] data) {
|
||||||
|
ByteBuffer bb = ByteBuffer.wrap(data);
|
||||||
|
bb.order(ByteOrder.LITTLE_ENDIAN);
|
||||||
|
distance = bb.getShort();
|
||||||
|
speed = bb.getShort() / 100.0;
|
||||||
|
angle = bb.getShort() / 100.0;
|
||||||
|
magnitude = bb.getShort() / 100.0;
|
||||||
|
radAngle = radians(angle);
|
||||||
|
distPx = 500.0 / max_distance * distance;
|
||||||
|
x = round(w_1_2 + sin(radAngle) * distPx);
|
||||||
|
y = round(500 - abs(cos(radAngle)) * distPx);
|
||||||
|
freshData = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
public String toString() {
|
||||||
|
return String.format("%d\t%3.1f\t%5.2f\t%4.2f\t%5.2f\t%d\t%d", millis(), distance/100.0, speed, angle, magnitude, x, y);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
public Detection[] parseDetectionData(byte[] data) {
|
||||||
|
int sets = data.length / 8;
|
||||||
|
Detection[] results = new Detection[sets];
|
||||||
|
for(int i=0; i<sets; i++) {
|
||||||
|
results[i] = new Detection(subset(data, i*8, 8));
|
||||||
|
}
|
||||||
|
return results;
|
||||||
|
}
|
||||||
121
k_ld7.pde
Normal file
121
k_ld7.pde
Normal file
@ -0,0 +1,121 @@
|
|||||||
|
import java.nio.ByteBuffer;
|
||||||
|
import java.nio.ByteOrder;
|
||||||
|
|
||||||
|
// Maximum distance displayed.
|
||||||
|
int max_distance = 3000;
|
||||||
|
|
||||||
|
// The serial port used.
|
||||||
|
String port = "/dev/tty.usbserial-00000000";
|
||||||
|
|
||||||
|
// Request method
|
||||||
|
// PDAT returns all detected points, including reflections and noise
|
||||||
|
// TDAT tries to track the strongest signal and returns only one point per request
|
||||||
|
byte request_method = Radar.REQUEST_TYPE_TDAT;
|
||||||
|
|
||||||
|
int w_1_2 = 383;
|
||||||
|
boolean waitingForData = false;
|
||||||
|
boolean logSerial = false;
|
||||||
|
int dataSets = 0;
|
||||||
|
PrintWriter tsv;
|
||||||
|
Radar radar;
|
||||||
|
Detection[] detections;
|
||||||
|
|
||||||
|
void settings() {
|
||||||
|
size(w_1_2*2, 500);
|
||||||
|
}
|
||||||
|
|
||||||
|
void setup() {
|
||||||
|
frameRate(10);
|
||||||
|
radar = new Radar(this, port);
|
||||||
|
radar
|
||||||
|
.setMaximumSpeed(Radar.MAXIMUM_SPEED_100)
|
||||||
|
.setMaximumRange(Radar.MAXIMUM_RANGE_30)
|
||||||
|
.setThresholdOffset((byte)20)
|
||||||
|
.setTrackingFilterType(Radar.TRACKING_FILTER_TYPE_FAST_DETECTION)
|
||||||
|
.setMaximumDetectionDistance((byte)100)
|
||||||
|
.setRangeThreshold((byte)0)
|
||||||
|
.setSpeedThreshold((byte)100)
|
||||||
|
.setHoldTime(1);
|
||||||
|
|
||||||
|
tsv = createWriter("log.tsv");
|
||||||
|
|
||||||
|
background(200);
|
||||||
|
fill(255);
|
||||||
|
distanceArc(max_distance, PIE);
|
||||||
|
noFill();
|
||||||
|
for (int i = 1000; i<max_distance; i+=1000) {
|
||||||
|
distanceArc(i, OPEN);
|
||||||
|
}
|
||||||
|
tsv.println("timestamp\tdistance\tspeed\tangle\tmagnitude\tx\ty");
|
||||||
|
System.out.println("timestamp\tdistance\tspeed\tangle\tmagnitude\tx\ty");
|
||||||
|
}
|
||||||
|
|
||||||
|
void stop() {
|
||||||
|
radar.stop();
|
||||||
|
tsv.close();
|
||||||
|
}
|
||||||
|
|
||||||
|
void draw() {
|
||||||
|
fill(200);
|
||||||
|
rect(0, 0, 20, 20);
|
||||||
|
if (dataSets % 4 == 0) {
|
||||||
|
circle(5, 5, 5);
|
||||||
|
}
|
||||||
|
if (dataSets % 4 == 1) {
|
||||||
|
circle(10, 5, 5);
|
||||||
|
}
|
||||||
|
if (dataSets % 4 == 2) {
|
||||||
|
circle(10, 10, 5);
|
||||||
|
}
|
||||||
|
if (dataSets % 4 == 3) {
|
||||||
|
circle(5, 10, 5);
|
||||||
|
}
|
||||||
|
|
||||||
|
radar.readResponse();
|
||||||
|
drawData();
|
||||||
|
|
||||||
|
if (!waitingForData) {
|
||||||
|
try { Thread.sleep(10); } catch(InterruptedException e) {}
|
||||||
|
requestData();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void drawData() {
|
||||||
|
if (detections!=null && detections.length > 0) { //<>//
|
||||||
|
for (Detection d: detections) {
|
||||||
|
if (d.freshData) {
|
||||||
|
System.out.println(d.toString());
|
||||||
|
tsv.println(d.toString());
|
||||||
|
tsv.flush();
|
||||||
|
drawPoint(d.x, d.y, d.speed, d.magnitude);
|
||||||
|
d.freshData = false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void distanceArc(int distance, int mode) {
|
||||||
|
arc(w_1_2, 500, 500.0/max_distance*distance*2, 500.0/max_distance*distance*2, radians(-40 - 90), radians(40 - 90), mode);
|
||||||
|
}
|
||||||
|
|
||||||
|
void requestData() {
|
||||||
|
radar.requestDataFrame(request_method);
|
||||||
|
waitingForData = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
void drawPoint(int x, int y, float speed, float magnitude) {
|
||||||
|
int dia;
|
||||||
|
if (magnitude < 20) {
|
||||||
|
noFill();
|
||||||
|
dia = 2;
|
||||||
|
} else {
|
||||||
|
fill(220 - (magnitude-20)*10);
|
||||||
|
dia = 15;
|
||||||
|
}
|
||||||
|
circle(x, y, dia);
|
||||||
|
fill(0);
|
||||||
|
textAlign(CENTER, BOTTOM);
|
||||||
|
//text(speed, x, y-7);
|
||||||
|
}
|
||||||
136
radar.pde
Normal file
136
radar.pde
Normal file
@ -0,0 +1,136 @@
|
|||||||
|
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;}
|
||||||
|
}
|
||||||
Loading…
Reference in New Issue
Block a user