Skip to content

Commit

Permalink
Delta2G checksum works
Browse files Browse the repository at this point in the history
  • Loading branch information
@kaiaai
kaiaai committed Feb 11, 2024
1 parent 26ff417 commit 174a816
Show file tree
Hide file tree
Showing 6 changed files with 112 additions and 74 deletions.
2 changes: 1 addition & 1 deletion src/LDS.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -118,7 +118,7 @@ String LDS::resultCodeToString(result_t code) {
return "Timeout error";
case ERROR_INVALID_PACKET:
return "Invalid Packet error";
case ERROR_CRC:
case ERROR_CHECKSUM:
return "CRC error";
case ERROR_NOT_READY:
return "Not ready error";
Expand Down
2 changes: 1 addition & 1 deletion src/LDS.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,7 +27,7 @@ class LDS {
RESULT_OK = 0,
ERROR_TIMEOUT = -1,
ERROR_INVALID_PACKET = -2,
ERROR_CRC = -3,
ERROR_CHECKSUM = -3,
ERROR_NOT_READY = -4,
ERROR_NOT_IMPLEMENTED = -5,
ERROR_NOT_CONFIGURED = -6,
Expand Down
161 changes: 99 additions & 62 deletions src/LDS_DELTA_2G.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,12 +17,12 @@
void LDS_DELTA_2G::init() {
motor_enabled = false;
pwm_val = 0.5;
scan_rpm = 0;
scan_rpm_setpoint = DEFAULT_SCAN_RPM;
scan_freq_hz = 0;
scan_freq_hz_setpoint = DEFAULT_SCAN_FREQ_HZ;
parser_state = 0;
checksum = 0;

scanFreqPID.init(&scan_rpm, &pwm_val, &scan_rpm_setpoint, 3.0e-3, 1.0e-3, 0.0, PID_v1::DIRECT);
scanFreqPID.init(&scan_freq_hz, &pwm_val, &scan_freq_hz_setpoint, 3.0e-3, 1.0e-3, 0.0, PID_v1::DIRECT);
scanFreqPID.SetOutputLimits(0, 1.0);
scanFreqPID.SetSampleTime(20);
scanFreqPID.SetMode(PID_v1::AUTOMATIC);
Expand All @@ -40,7 +40,7 @@ uint32_t LDS_DELTA_2G::getSerialBaudRate() {
}

float LDS_DELTA_2G::getTargetScanFreqHz() {
return scan_rpm_setpoint / 60.0f;
return scan_freq_hz_setpoint / 60.0f;
}

int LDS_DELTA_2G::getSamplingRateHz() {
Expand All @@ -58,7 +58,7 @@ LDS::result_t LDS_DELTA_2G::setScanPIDSamplePeriodMs(uint32_t sample_period_ms)
}

float LDS_DELTA_2G::getCurrentScanFreqHz() {
return scan_rpm/60.0f;
return scan_freq_hz;
}

void LDS_DELTA_2G::stop() {
Expand All @@ -82,16 +82,15 @@ bool LDS_DELTA_2G::isActive() {
}

LDS::result_t LDS_DELTA_2G::setScanTargetFreqHz(float freq) {
float rpm = freq * 60.0f;
if (rpm <= 0) {
scan_rpm_setpoint = DEFAULT_SCAN_RPM;
if (freq <= 0) {
scan_freq_hz_setpoint = DEFAULT_SCAN_FREQ_HZ;
return RESULT_OK;
}

if (rpm <= DEFAULT_SCAN_RPM*0.9f || rpm >= DEFAULT_SCAN_RPM*1.1f)
if (freq <= DEFAULT_SCAN_FREQ_HZ*0.9f || freq >= DEFAULT_SCAN_FREQ_HZ*1.1f)
return ERROR_INVALID_VALUE;

scan_rpm_setpoint = rpm;
scan_freq_hz_setpoint = freq;
return RESULT_OK;
}

Expand All @@ -117,13 +116,10 @@ void LDS_DELTA_2G::loop() {
}

LDS::result_t LDS_DELTA_2G::processByte(uint8_t c) {
uint16_t packet_length = 0;
uint16_t data_length = 0;
LDS::result_t result = RESULT_OK;
// parse packet header
// get N samples
// postPacket()
// postScanPoint()
uint8_t * rx_buffer = (uint8_t *)&scan_packet;
// parser_idx = parser_idx >= sizeof(scan_packet_t) ? 0 : parser_idx;

if (parser_idx >= sizeof(scan_packet_t)) {
parser_idx = 0;
Expand All @@ -132,89 +128,121 @@ LDS::result_t LDS_DELTA_2G::processByte(uint8_t c) {

rx_buffer[parser_idx++] = c;
checksum += c;

/*
if (c < 16)
Serial.print(" 0");
else
Serial.print(' ');
Serial.print(c, HEX);
*/
switch (parser_idx) {
case 0:
case 1:
if (c != START_BYTE)
result = ERROR_INVALID_VALUE;
result = ERROR_INVALID_VALUE;
else
checksum = 0;
break;
checksum = c;
break;

case 1: // packet length MSB
case 2:
break;

case 2: // packet length LSB
if (scan_packet.packet_length >
sizeof(scan_packet_t) - sizeof(scan_packet.checksum))
case 3:
packet_length = decodeUInt16(scan_packet.packet_length);
if (packet_length > sizeof(scan_packet_t) - sizeof(scan_packet.checksum)) {
result = ERROR_INVALID_PACKET;
Serial.println("packet_length too long");
}
break;

case 3:
case 4:
if (c != PROTOCOL_VERSION)
result = ERROR_INVALID_VALUE;
result = ERROR_INVALID_VALUE;
break;

case 4:
case 5:
if (c != PACKET_TYPE)
result = ERROR_INVALID_VALUE;
break;

case 5:
case 6:
if (c != DATA_TYPE_RPM_AND_MEAS && c != DATA_TYPE_RPM_ONLY)
result = ERROR_INVALID_VALUE;
break;

case 6: // data length MSB
case 7: // data length MSB
break;

case 7: // data length LSB
if (scan_packet.data_length == 0 ||
scan_packet.data_length > MAX_DATA_BYTE_LEN)
case 8: // data length LSB
data_length = decodeUInt16(scan_packet.data_length);
if (data_length == 0 || data_length > MAX_DATA_BYTE_LEN)
result = ERROR_INVALID_PACKET;
break;

default:
// Keep reading
if (parser_idx >= scan_packet.packet_length + sizeof(scan_packet.checksum)) {
packet_length = decodeUInt16(scan_packet.packet_length);
if (parser_idx != packet_length + 2)
break;

// Got checksum
scan_packet.checksum += c << 8;
uint16_t checksum_adjusted = checksum;
checksum_adjusted += checksum & 0xFF;
checksum_adjusted += (checksum >> 8) & 0xFF;
//Serial.println();
uint16_t pkt_checksum = (rx_buffer[parser_idx-2] << 8) + rx_buffer[parser_idx-1];
/*
Serial.print("pars idx ");
Serial.println(parser_idx, HEX);
Serial.print("Pkt len ");
Serial.println(packet_length, HEX);
Serial.print("Prot ver ");
Serial.println(scan_packet.protocol_version, HEX);
Serial.print("Pkt type ");
Serial.println(scan_packet.packet_type, HEX);
Serial.print("Data typ ");
Serial.println(scan_packet.data_type, HEX);
Serial.print("Data len ");
Serial.println(data_length, HEX);
Serial.print("Scan frq ");
Serial.println(scan_packet.scan_freq_x20, HEX);
Serial.print("Offs ang ");
Serial.println(decodeUInt16(scan_packet.offset_angle_x100), HEX);
Serial.print("Strt ang ");
Serial.println(decodeUInt16(scan_packet.start_angle_x100), HEX);
Serial.print("Checksum ");
Serial.println(decodeUInt16(scan_packet.checksum), HEX);
*/

pkt_checksum += rx_buffer[parser_idx-2];
pkt_checksum += rx_buffer[parser_idx-1];
if (checksum != pkt_checksum) {
result = ERROR_CHECKSUM;
break;
}

if (scan_packet.checksum != checksum_adjusted) {
result = ERROR_CRC;
break;
}
scan_freq_hz = scan_packet.scan_freq_x20 * 0.05;

scan_rpm = scan_packet.scan_freq_x20 * 0.05;
if (scan_packet.data_type == DATA_TYPE_RPM_AND_MEAS) {
if (scan_packet.data_type == DATA_TYPE_RPM_AND_MEAS) {
uint16_t start_angle_x100 = decodeUInt16(scan_packet.start_angle_x100);
unsigned int packet_index = (start_angle_x100 * PACKETS_PER_SCAN) % 36000;
bool scan_completed = packet_index == 0;

unsigned int packet_index = (scan_packet.start_angle_x100 * PACKETS_PER_SCAN) % 36000;
bool scan_completed = packet_index == 0;
uint16_t sample_count = (data_length - 5) / 3;
if (sample_count > MAX_DATA_SAMPLES) {
result = ERROR_INVALID_PACKET;
break;
}

uint16_t sample_count = (scan_packet.data_length - 5) / 3;
if (sample_count > MAX_DATA_SAMPLES) {
result = ERROR_INVALID_PACKET;
break;
}
postPacket(rx_buffer, parser_idx, scan_completed);

postPacket(rx_buffer, parser_idx, scan_completed);
float start_angle = start_angle_x100 * 0.01;
float coeff = DEG_PER_PACKET / (float)sample_count;
for (uint16_t idx = 0; idx < sample_count; idx++) {
float angle_deg = start_angle + idx * coeff;

float start_angle = scan_packet.start_angle_x100 * 0.01;
float coeff = DEG_PER_PACKET / (float)sample_count;
for (uint16_t idx = 0; idx < sample_count; idx++) {
float angle_deg = start_angle + idx * coeff;
float distance_mm = scan_packet.sample[idx].distance_mm_x4 * 0.25;
float quality = scan_packet.sample[idx].quality;
postScanPoint(angle_deg, distance_mm, quality, scan_completed);
}
uint16_t distance_mm_x4 = decodeUInt16(scan_packet.sample[idx].distance_mm_x4);
float distance_mm = distance_mm_x4 * 0.25;
float quality = scan_packet.sample[idx].quality;
postScanPoint(angle_deg, distance_mm, quality, scan_completed);
}
parser_idx = 0;
break;
}
parser_idx = 0;
break;
}

Expand All @@ -224,4 +252,13 @@ LDS::result_t LDS_DELTA_2G::processByte(uint8_t c) {
return result;
}

uint16_t LDS_DELTA_2G::decodeUInt16(const uint16_t value) const {
union {
uint16_t i;
char c[2];
} bint = {0x0102};

return bint.c[0] == 0x01 ? value : (value << 8) + (value >> 8);
}

const char* LDS_DELTA_2G::getModelName() { return "3irobotics Delta-2G"; }
17 changes: 9 additions & 8 deletions src/LDS_DELTA_2G.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,15 +36,15 @@ class LDS_DELTA_2G : public LDS {
virtual result_t setScanPIDSamplePeriodMs(uint32_t sample_period_ms) override;

protected:
static constexpr float DEFAULT_SCAN_RPM = 315.0f;
static constexpr float DEFAULT_SCAN_FREQ_HZ = 5.25f;
static const uint8_t START_BYTE = 0xAA;
static const uint8_t PROTOCOL_VERSION = 0x01;
static const uint8_t PACKET_TYPE = 0x61;
static const uint8_t DATA_TYPE_RPM_AND_MEAS = 0xAE;
static const uint8_t DATA_TYPE_RPM_ONLY = 0xAD;
static const uint8_t PACKETS_PER_SCAN = 15;
static constexpr float DEG_PER_PACKET = 1.0f / (float)PACKETS_PER_SCAN;
static const uint8_t MAX_DATA_SAMPLES = 360/PACKETS_PER_SCAN;
static const uint8_t MAX_DATA_SAMPLES = 28; //360/PACKETS_PER_SCAN;
static const uint8_t PACKET_HEADER_BYTE_LEN = 10;

struct meas_sample_t {
Expand All @@ -62,26 +62,27 @@ class LDS_DELTA_2G : public LDS {
// 0xAD -> data_length -> scan_freq_x20 -> data
uint16_t data_length; // n_samples = (data_length - 5)/3;
uint8_t scan_freq_x20;
int16_t offset_angle_x100; // signed
uint16_t start_angle_x100; // unsigned?
int16_t offset_angle_x100; // signed
uint16_t start_angle_x100; // unsigned?

meas_sample_t sample[MAX_DATA_SAMPLES];
meas_sample_t sample[MAX_DATA_SAMPLES]; // 3*28=84

uint16_t checksum;
} __attribute__((packed));
} __attribute__((packed)); // 8 + 5 + 84 + 2 = 97
// 15 scan steps per revolution
// float angle = start_angle + 22.5 * sample_idx / n_samples;
// float angle = start_angle + 24 * sample_idx / n_samples;

virtual void enableMotor(bool enable);
LDS::result_t processByte(uint8_t c);
uint16_t decodeUInt16(const uint16_t value) const;

float scan_rpm_setpoint;
float scan_freq_hz_setpoint;
bool motor_enabled;
uint8_t parser_state;
float pwm_val;
float pwm_last;
float scan_rpm;
float scan_freq_hz;
PID_v1 scanFreqPID;

scan_packet_t scan_packet;
Expand Down
2 changes: 1 addition & 1 deletion src/LDS_NEATO_XV11.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -229,7 +229,7 @@ LDS::result_t LDS_NEATO_XV11::processByte(int inByte) {

} else {
// Bad packet
result = ERROR_CRC;
result = ERROR_CHECKSUM;
}

clearVars(); // initialize a bunch of stuff before we switch back to State 1
Expand Down
2 changes: 1 addition & 1 deletion src/LDS_YDLIDAR_X4.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -341,7 +341,7 @@ LDS::result_t LDS_YDLIDAR_X4::waitScanDot() {
node.distance_q2 = 0;
package_Sample_Index = 0;
state = 0;
return ERROR_CRC;
return ERROR_CHECKSUM;
}

// Dump out processed data
Expand Down

0 comments on commit 174a816

Please sign in to comment.