diff --git a/src/LDS_YDLIDAR_SCL.cpp b/src/LDS_YDLIDAR_SCL.cpp index 3eff106..aa8ff8e 100644 --- a/src/LDS_YDLIDAR_SCL.cpp +++ b/src/LDS_YDLIDAR_SCL.cpp @@ -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; } \ No newline at end of file diff --git a/src/LDS_YDLIDAR_SCL.h b/src/LDS_YDLIDAR_SCL.h index 293c414..f4f9400 100644 --- a/src/LDS_YDLIDAR_SCL.h +++ b/src/LDS_YDLIDAR_SCL.h @@ -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; @@ -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); diff --git a/src/LDS_YDLIDAR_X4.h b/src/LDS_YDLIDAR_X4.h index f8b7f32..2226377 100644 --- a/src/LDS_YDLIDAR_X4.h +++ b/src/LDS_YDLIDAR_X4.h @@ -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: