Skip to content

Commit

Permalink
SCL builds
Browse files Browse the repository at this point in the history
  • Loading branch information
Ilia O. authored and Ilia O. committed Aug 20, 2024
1 parent faeba48 commit f60ae78
Show file tree
Hide file tree
Showing 3 changed files with 243 additions and 1 deletion.
216 changes: 216 additions & 0 deletions src/LDS_YDLIDAR_SCL.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -97,4 +97,220 @@ LDS::result_t LDS_YDLIDAR_SCL::setScanPIDCoeffs(float Kp, float Ki, float Kd) {
LDS::result_t LDS_YDLIDAR_SCL::setScanPIDSamplePeriodMs(uint32_t sample_period_ms) {
scanFreqPID.SetSampleTime(sample_period_ms);
return RESULT_OK;
}

LDS::result_t LDS_YDLIDAR_SCL::waitScanDot() {
switch(state) {
case 1:
goto state1;
case 2:
goto state2;
}

if (package_Sample_Index == 0) {

package_Sample_Num = 0;
package_recvPos = 0;
recvPos = 0; // Packet start

// Read in 10-byte packet header
while (true) {
state1:
int currentByte = readSerial();
if (currentByte < 0) {
state = 1;
return ERROR_NOT_READY;
}

switch (recvPos) {
case 0: // 0xAA 1st byte of package header
if (currentByte != (PH&0xFF)) {
checkInfo(currentByte);
continue;
}
break;
case 1: // 0x55 2nd byte of package header
CheckSumCal = PH;
if (currentByte != (PH>>8)) {
recvPos = 0;
continue;
}
break;
case 2:
SampleNumlAndCTCal = currentByte;
if ((currentByte & 0x01) == CT_RING_START)
markScanTime();
//if ((currentByte != CT_NORMAL) && (currentByte != CT_RING_START)) {
// recvPos = 0;
// continue;
//}
break;
case 3:
SampleNumlAndCTCal += (currentByte << 8);
package_Sample_Num = currentByte;
break;
case 4:
if (currentByte & 0x01) {
FirstSampleAngle = currentByte;
} else {
recvPos = 0;
continue;
}
break;
case 5:
FirstSampleAngle += (currentByte << 8);
CheckSumCal ^= FirstSampleAngle;
FirstSampleAngle = FirstSampleAngle>>1; // degrees x64
break;
case 6:
if (currentByte & 0x01) {
LastSampleAngle = currentByte;
} else {
recvPos = 0;
continue;
}
break;
case 7:
LastSampleAngle += (currentByte << 8);
LastSampleAngleCal = LastSampleAngle;
LastSampleAngle = LastSampleAngle >> 1;
if (package_Sample_Num == 1) {
IntervalSampleAngle = 0;
} else {
if (LastSampleAngle < FirstSampleAngle) {
if ((FirstSampleAngle > 270*64) && (LastSampleAngle < 90*64)) {
IntervalSampleAngle = ((float)(360*64 + LastSampleAngle - FirstSampleAngle))/(package_Sample_Num-1);
IntervalSampleAngle_LastPackage = IntervalSampleAngle;
} else {
IntervalSampleAngle = IntervalSampleAngle_LastPackage;
}
} else {
IntervalSampleAngle = ((float)(LastSampleAngle - FirstSampleAngle))/(package_Sample_Num-1);
IntervalSampleAngle_LastPackage = IntervalSampleAngle;
}
}
break;
case 8:
CheckSum = currentByte;
break;
case 9:
CheckSum += (currentByte << 8);
break;
}
packageBuffer[recvPos++] = currentByte;

if (recvPos == PACKAGE_PAID_BYTES ) {
package_recvPos = recvPos;
break;
}
}

// Check buffer overflow
if (package_Sample_Num > PACKAGE_SAMPLE_MAX_LENGTH)
return ERROR_INVALID_PACKET;

if (PACKAGE_PAID_BYTES == recvPos) {
// Packet header looks valid size-wise
recvPos = 0;
package_sample_sum = package_Sample_Num * 3;

// Read in samples, 3 bytes each
while (true) {
state2:
int currentByte = readSerial();
if (currentByte < 0){
state = 2;
return ERROR_NOT_READY;
}

if ((recvPos & 1) == 1) {
Valu8Tou16 += (currentByte << 8);
CheckSumCal ^= Valu8Tou16;
} else {
Valu8Tou16 = currentByte;
}

packageBuffer[package_recvPos + recvPos] = currentByte;
recvPos++;
if (package_sample_sum == recvPos) {
package_recvPos += recvPos;
break;
}
}

if (package_sample_sum != recvPos) {
state = 0;
return ERROR_INVALID_PACKET;
}
} else {
// Packet length is off
state = 0;
return ERROR_INVALID_PACKET;
}
CheckSumCal ^= SampleNumlAndCTCal;
CheckSumCal ^= LastSampleAngleCal;

CheckSumResult = CheckSumCal == CheckSum;
}

scan_completed = false;
if (CheckSumResult) {
scan_completed = (package.package_CT & 0x01) == 0x01;
if (scan_completed)
scan_freq_hz = float(package.package_CT >> 1)*0.1f;

postPacket(packageBuffer, PACKAGE_PAID_BYTES+package_sample_sum, scan_completed);
}

// Process the buffered packet
while(true) {
node_info_scl_t node;
cloud_point_scl_t point;

if (CheckSumResult == true) {
int32_t AngleCorrectForDistance;
point = ((node_package_scl_t*)&package)->packageSampleDistance[package_Sample_Index];
node.distance_mm = (point.distance0 >> 2) + (point.distance1 << 8);
node.intensity = point.intensity;
node.quality_flag = point.distance0 && 0x03;

if (node.distance_mm != 0) {
AngleCorrectForDistance = (int32_t)(atan(17.8f/((float)node.distance_mm)) *3666.929888837269f ); // *64*360/2/pi
} else {
AngleCorrectForDistance = 0;
}

float sampleAngle = IntervalSampleAngle * package_Sample_Index; // x64, before correction

node.angle_deg = (FirstSampleAngle + sampleAngle + AngleCorrectForDistance) * 0.015625f; // /64
if (node.angle_deg < 0) {
node.angle_deg = node.angle_deg + 360;
} else {
if (node.angle_deg > 360) {
node.angle_deg = node.angle_deg - 360;
}
}
} else {
// Invalid checksum
node.angle_deg = 0;
node.distance_mm = 0;
package_Sample_Index = 0;
state = 0;
return ERROR_CHECKSUM;
}

// Dump out processed data
postScanPoint(node.angle_deg, node.distance_mm, node.quality_flag, scan_completed);
scan_completed = false;

// Dump finished?
package_Sample_Index++;
uint8_t nowPackageNum = package.nowPackageNum;
if (package_Sample_Index >= nowPackageNum) {
package_Sample_Index = 0;
break;
}
}
state = 0;
return LDS::RESULT_OK;
}
26 changes: 26 additions & 0 deletions src/LDS_YDLIDAR_SCL.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,7 @@ class LDS_YDLIDAR_SCL : public LDS_YDLIDAR_X4 {
virtual result_t stop() override;
virtual const char* getModelName() override;
virtual void loop() override;
virtual LDS::result_t waitScanDot() override;

virtual float getCurrentScanFreqHz() override;
virtual uint32_t getSerialBaudRate() override;
Expand All @@ -38,6 +39,31 @@ class LDS_YDLIDAR_SCL : public LDS_YDLIDAR_X4 {
protected:
static constexpr float DEFAULT_SCAN_FREQ_HZ = 5.0f;
static constexpr float PWM_START = 0.25f;

struct node_info_scl_t {
//uint8_t sync_quality;
float angle_deg;
uint16_t distance_mm;
uint8_t intensity;
uint8_t quality_flag;
} __attribute__((packed)) ;

struct cloud_point_scl_t {
uint8_t intensity;
uint8_t distance0;
uint8_t distance1;
} __attribute__((packed));

struct node_package_scl_t {
uint16_t package_Head;
uint8_t package_CT; // package type
uint8_t nowPackageNum; // distance sample count
uint16_t packageFirstSampleAngle;
uint16_t packageLastSampleAngle;
uint16_t checkSum;
cloud_point_scl_t packageSampleDistance[PACKAGE_SAMPLE_MAX_LENGTH];
} __attribute__((packed));

virtual void enableMotor(bool enable) override;
String receiveInfo(uint32_t timeout_ms);

Expand Down
2 changes: 1 addition & 1 deletion src/LDS_YDLIDAR_X4.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -88,7 +88,7 @@ class LDS_YDLIDAR_X4 : public LDS {
uint16_t packageFirstSampleAngle;
uint16_t packageLastSampleAngle;
uint16_t checkSum;
uint16_t packageSampleDistance[PACKAGE_SAMPLE_MAX_LENGTH];
uint16_t packageSampleDistance[PACKAGE_SAMPLE_MAX_LENGTH + (PACKAGE_SAMPLE_MAX_LENGTH>>1)]; // SCL hack
} __attribute__((packed)) ;

protected:
Expand Down

0 comments on commit f60ae78

Please sign in to comment.