Skip to content

Commit

Permalink
SCL CRC, PID work
Browse files Browse the repository at this point in the history
  • Loading branch information
Ilia O. authored and Ilia O. committed Aug 21, 2024
1 parent f60ae78 commit ca5ed47
Show file tree
Hide file tree
Showing 4 changed files with 32 additions and 40 deletions.
51 changes: 26 additions & 25 deletions src/LDS_YDLIDAR_SCL.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,7 +24,7 @@ void LDS_YDLIDAR_SCL::init() {
pwm_val = PWM_START;
scan_freq_setpoint_hz = DEFAULT_SCAN_FREQ_HZ;

scanFreqPID.init(&scan_freq_hz, &pwm_val, &scan_freq_setpoint_hz, 3.0e-3, 1.0e-3, 0.0, PID_v1::DIRECT);
scanFreqPID.init(&scan_freq_hz, &pwm_val, &scan_freq_setpoint_hz, 1.0e-2, 2.5e-2, 0.0, PID_v1::DIRECT);
scanFreqPID.SetOutputLimits(0, 1.0);
scanFreqPID.SetSampleTime(20);
scanFreqPID.SetMode(PID_v1::AUTOMATIC);
Expand Down Expand Up @@ -100,6 +100,8 @@ LDS::result_t LDS_YDLIDAR_SCL::setScanPIDSamplePeriodMs(uint32_t sample_period_m
}

LDS::result_t LDS_YDLIDAR_SCL::waitScanDot() {
uint8_t *packageBuffer = (uint8_t*)&package_scl.package_Head;

switch(state) {
case 1:
goto state1;
Expand All @@ -124,26 +126,22 @@ LDS::result_t LDS_YDLIDAR_SCL::waitScanDot() {

switch (recvPos) {
case 0: // 0xAA 1st byte of package header
if (currentByte != (PH&0xFF)) {
if (currentByte != 0xAA) { // (PH&0xFF)
checkInfo(currentByte);
continue;
}
break;
case 1: // 0x55 2nd byte of package header
CheckSumCal = PH;
if (currentByte != (PH>>8)) {
CheckSumCal = 0x55AA; // PH
if (currentByte != 0x55) { // (PH>>8)
recvPos = 0;
continue;
}
break;
case 2:
SampleNumlAndCTCal = currentByte;
if ((currentByte & 0x01) == CT_RING_START)
if ((currentByte & 0x01) == 0x01) // CT_RING_START
markScanTime();
//if ((currentByte != CT_NORMAL) && (currentByte != CT_RING_START)) {
// recvPos = 0;
// continue;
//}
break;
case 3:
SampleNumlAndCTCal += (currentByte << 8);
Expand All @@ -160,7 +158,7 @@ LDS::result_t LDS_YDLIDAR_SCL::waitScanDot() {
case 5:
FirstSampleAngle += (currentByte << 8);
CheckSumCal ^= FirstSampleAngle;
FirstSampleAngle = FirstSampleAngle>>1; // degrees x64
FirstSampleAngle = FirstSampleAngle>>1; // degrees*64
break;
case 6:
if (currentByte & 0x01) {
Expand Down Expand Up @@ -199,7 +197,7 @@ LDS::result_t LDS_YDLIDAR_SCL::waitScanDot() {
}
packageBuffer[recvPos++] = currentByte;

if (recvPos == PACKAGE_PAID_BYTES ) {
if (recvPos == PACKAGE_PAID_BYTES) {
package_recvPos = recvPos;
break;
}
Expand All @@ -223,11 +221,16 @@ LDS::result_t LDS_YDLIDAR_SCL::waitScanDot() {
return ERROR_NOT_READY;
}

if ((recvPos & 1) == 1) {
Valu8Tou16 += (currentByte << 8);
CheckSumCal ^= Valu8Tou16;
} else {
Valu8Tou16 = currentByte;
switch (recvPos % 3) {
case 0:
CheckSumCal ^= currentByte;
break;
case 1:
Valu8Tou16 = currentByte;
break;
case 2:
Valu8Tou16 += (currentByte << 8);
CheckSumCal ^= Valu8Tou16;
}

packageBuffer[package_recvPos + recvPos] = currentByte;
Expand Down Expand Up @@ -255,9 +258,9 @@ LDS::result_t LDS_YDLIDAR_SCL::waitScanDot() {

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

postPacket(packageBuffer, PACKAGE_PAID_BYTES+package_sample_sum, scan_completed);
}
Expand All @@ -268,19 +271,17 @@ LDS::result_t LDS_YDLIDAR_SCL::waitScanDot() {
cloud_point_scl_t point;

if (CheckSumResult == true) {
int32_t AngleCorrectForDistance;
point = ((node_package_scl_t*)&package)->packageSampleDistance[package_Sample_Index];
int32_t AngleCorrectForDistance = 0;
point = package_scl.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;
AngleCorrectForDistance = (int32_t)(atan(17.8f/((float)node.distance_mm))*3666.929888837269f); // *64*360/2/pi
}

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

node.angle_deg = (FirstSampleAngle + sampleAngle + AngleCorrectForDistance) * 0.015625f; // /64
if (node.angle_deg < 0) {
Expand All @@ -305,7 +306,7 @@ LDS::result_t LDS_YDLIDAR_SCL::waitScanDot() {

// Dump finished?
package_Sample_Index++;
uint8_t nowPackageNum = package.nowPackageNum;
uint8_t nowPackageNum = package_scl.nowPackageNum;
if (package_Sample_Index >= nowPackageNum) {
package_Sample_Index = 0;
break;
Expand Down
5 changes: 3 additions & 2 deletions src/LDS_YDLIDAR_SCL.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,12 +46,12 @@ class LDS_YDLIDAR_SCL : public LDS_YDLIDAR_X4 {
uint16_t distance_mm;
uint8_t intensity;
uint8_t quality_flag;
} __attribute__((packed)) ;
} __attribute__((packed));

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

struct node_package_scl_t {
Expand All @@ -66,6 +66,7 @@ class LDS_YDLIDAR_SCL : public LDS_YDLIDAR_X4 {

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

float scan_freq_setpoint_hz; // desired scan frequency
float pwm_val;
Expand Down
13 changes: 2 additions & 11 deletions src/LDS_YDLIDAR_X4.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -172,6 +172,8 @@ void LDS_YDLIDAR_X4::checkInfo(int currentByte) {
}

LDS::result_t LDS_YDLIDAR_X4::waitScanDot() {
uint8_t *packageBuffer = (uint8_t*)&package.package_Head;

switch(state) {
case 1:
goto state1;
Expand Down Expand Up @@ -326,10 +328,6 @@ LDS::result_t LDS_YDLIDAR_X4::waitScanDot() {
CheckSumResult = CheckSumCal == CheckSum;
}

//if (CheckSumResult)
// postPacket(packageBuffer, PACKAGE_PAID_BYTES+package_sample_sum,
// package.package_CT == CT_RING_START);

scan_completed = false;
if (CheckSumResult) {
scan_completed = (package.package_CT & 0x01) == CT_RING_START;
Expand All @@ -343,15 +341,8 @@ LDS::result_t LDS_YDLIDAR_X4::waitScanDot() {

// Process the buffered packet
while(true) {
// uint8_t package_CT;
node_info_t node;

//package_CT = package.package_CT;
//if ((package_CT & 0x01) == CT_NORMAL) {
// node.sync_quality = NODE_DEFAULT_QUALITY + NODE_NOT_SYNC;
//} else {
// node.sync_quality = NODE_DEFAULT_QUALITY + NODE_SYNC;
//}
node.sync_quality = NODE_DEFAULT_QUALITY;

if (CheckSumResult == true) {
Expand Down
3 changes: 1 addition & 2 deletions 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 + (PACKAGE_SAMPLE_MAX_LENGTH>>1)]; // SCL hack
uint16_t packageSampleDistance[PACKAGE_SAMPLE_MAX_LENGTH]; // SCL hack
} __attribute__((packed)) ;

protected:
Expand Down Expand Up @@ -144,7 +144,6 @@ class LDS_YDLIDAR_X4 : public LDS {
int package_sample_sum = 0;

node_package_t package;
uint8_t *packageBuffer = (uint8_t*)&package.package_Head;

uint16_t package_Sample_Index = 0;
float IntervalSampleAngle = 0;
Expand Down

0 comments on commit ca5ed47

Please sign in to comment.