From 77e7f4ca6e145652a765c05af70c5d606cf282a8 Mon Sep 17 00:00:00 2001 From: "Ilia O." Date: Sun, 28 Jan 2024 00:08:30 -0800 Subject: [PATCH] YDLIDAR X2 works --- README.md | 9 +- keywords.txt | 1 + library.properties | 2 +- src/LDS_YDLIDAR_X2.cpp | 348 +++++++++++++++++++++++++++++++++++++++++ src/LDS_YDLIDAR_X2.h | 153 ++++++++++++++++++ src/LDS_YDLIDAR_X4.cpp | 21 ++- src/LDS_YDLIDAR_X4.h | 2 + src/LDS_all_models.h | 18 +++ 8 files changed, 543 insertions(+), 11 deletions(-) create mode 100644 src/LDS_YDLIDAR_X2.cpp create mode 100644 src/LDS_YDLIDAR_X2.h create mode 100644 src/LDS_all_models.h diff --git a/README.md b/README.md index 19872a2..ccd505b 100644 --- a/README.md +++ b/README.md @@ -1,8 +1,9 @@ # LDS/LiDAR Library for Arduino Laser distance scan sensor (LDS/LIDAR) Arduino wrapper/controller for [kaia.ai](https://kaia.ai) platform. The library supports - YDLIDAR X4 -- Xiaomi LDS02RR -- Neato XV/Botvac +- YDLIDAR X2 +- Xiaomi Mi LDS02RR +- TODO Neato XV/Botvac Note: LDS02RR, Neato LDS require an additional motor control board to operate. You can find the board schematics at [Makerspet repo](https://github.com/makerspet/makerspet_snoopy/) under `kicad` or purchase it at [makerspet.com](https://makerspet.com) when it becomes available. @@ -13,6 +14,10 @@ You can find the board schematics at [Makerspet repo](https://github.com/makersp ## Release notes +### v0.3.1 +- added YDLIDAR X2 +- measure RPM for YDLIDAR X4, YDLIDAR X2 + ### v0.3.0 - virtual class methods - ESP32 crash workaround by moving init code from constructor out to init() diff --git a/keywords.txt b/keywords.txt index 860e925..aab11ac 100644 --- a/keywords.txt +++ b/keywords.txt @@ -8,6 +8,7 @@ LDS KEYWORD1 LDS_YDLIDAR_X4 KEYWORD1 +LDS_YDLIDAR_X2 KEYWORD1 LDS_LDS02RR KEYWORD1 ####################################### diff --git a/library.properties b/library.properties index 5431a50..5b8c077 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=LDS -version=0.3.0 +version=0.3.1 author=Ilia O. maintainer=Ilia O. sentence=Laser distance scan sensors (LDS/LIDAR) wrapper/controller for kaia.ai robotics platform diff --git a/src/LDS_YDLIDAR_X2.cpp b/src/LDS_YDLIDAR_X2.cpp new file mode 100644 index 0000000..b13928b --- /dev/null +++ b/src/LDS_YDLIDAR_X2.cpp @@ -0,0 +1,348 @@ +// Copyright 2023-2024 REMAKE.AI, KAIA.AI, MAKERSPET.COM +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Based on +// https://github.com/YDLIDAR/lidarCar/ +// https://github.com/YDLIDAR/ydlidar_ros2_driver +// https://github.com/YDLIDAR/YDLidar-SDK + +#include "LDS_YDLIDAR_X2.h" + +void LDS_YDLIDAR_X2::init() { + ring_start_ms[0] = ring_start_ms[1] = 0; + enableMotor(false); +} + +LDS::result_t LDS_YDLIDAR_X2::start() { + enableMotor(true); + postInfo(INFO_MODEL, "YDLIDAR X2"); + postInfo(INFO_SAMPLING_RATE, String(getSamplingRateHz())); + postInfo(INFO_DEFAULT_TARGET_SCAN_FREQ_HZ, String(getTargetScanFreqHz())); + return LDS::RESULT_OK; +} + +uint32_t LDS_YDLIDAR_X2::getSerialBaudRate() { + return 115200; +} + +float LDS_YDLIDAR_X2::getCurrentScanFreqHz() { + unsigned long int scan_period_ms = ring_start_ms[0] - ring_start_ms[1]; + return (scan_period_ms == 0) ? 0 : 1000.0f/float(scan_period_ms); +} + +float LDS_YDLIDAR_X2::getTargetScanFreqHz() { + return 7; +} + +int LDS_YDLIDAR_X2::getSamplingRateHz() { + return 3000; +} + +void LDS_YDLIDAR_X2::stop() { + enableMotor(false); +} + +void LDS_YDLIDAR_X2::enableMotor(bool enable) { + motor_enabled = enable; + + setMotorPin(DIR_INPUT, LDS_MOTOR_PWM_PIN); + setMotorPin(DIR_OUTPUT_CONST, LDS_MOTOR_EN_PIN); + + // Entire LDS on/off + setMotorPin(enable ? VALUE_HIGH : VALUE_LOW, LDS_MOTOR_EN_PIN); +} + +bool LDS_YDLIDAR_X2::isActive() { + return motor_enabled; +} + +LDS::result_t LDS_YDLIDAR_X2::setScanTargetFreqHz(float freq) { + return ERROR_NOT_IMPLEMENTED; +} + +LDS::result_t LDS_YDLIDAR_X2::setScanPIDSamplePeriodMs(uint32_t sample_period_ms) { + return ERROR_NOT_IMPLEMENTED; +} + +LDS::result_t LDS_YDLIDAR_X2::setScanPIDCoeffs(float Kp, float Ki, float Kd) { + return ERROR_NOT_IMPLEMENTED; +} + +LDS::result_t LDS_YDLIDAR_X2::waitScanDot() { + static int recvPos = 0; + static uint8_t package_Sample_Num = 0; + static int package_recvPos = 0; + static int package_sample_sum = 0; + static int currentByte = 0; + + static node_package package; + static uint8_t *packageBuffer = (uint8_t*)&package.package_Head; + + static uint16_t package_Sample_Index = 0; + static float IntervalSampleAngle = 0; + static float IntervalSampleAngle_LastPackage = 0; + static uint16_t FirstSampleAngle = 0; + static uint16_t LastSampleAngle = 0; + static uint16_t CheckSum = 0; + + static uint16_t CheckSumCal = 0; + static uint16_t SampleNumlAndCTCal = 0; + static uint16_t LastSampleAngleCal = 0; + static bool CheckSumResult = true; + static uint16_t Valu8Tou16 = 0; + + static uint8_t state = 0; + + 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: + 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)) + 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 == CT_RING_START) + markScanTime(); + if ((currentByte != CT_NORMAL) && (currentByte != CT_RING_START)) { + recvPos = 0; + continue; + } + break; + case 3: + SampleNumlAndCTCal += (currentByte<>1; + break; + case 6: + if (currentByte & LIDAR_RESP_MEASUREMENT_CHECKBIT) { + LastSampleAngle = currentByte; + } else { + recvPos = 0; + continue; + } + break; + case 7: + LastSampleAngle += (currentByte<>1; + if (package_Sample_Num == 1) { + IntervalSampleAngle = 0; + } else { + if (LastSampleAngle < FirstSampleAngle) { + if ((FirstSampleAngle > 17280) && (LastSampleAngle < 5760)) { + IntervalSampleAngle = ((float)(23040 + 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< 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<<1; + + // Read in samples, 2 bytes each + while (true) { +state2: + currentByte = readSerial(); + if (currentByte < 0){ + state = 2; + return ERROR_NOT_READY; + } + + if ((recvPos & 1) == 1) { + Valu8Tou16 += (currentByte< 23040) { + node.angle_q6_checkbit = ((uint16_t)((FirstSampleAngle + sampleAngle + + AngleCorrectForDistance - 23040))<> LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f; + uint8_t point_quality = (node.sync_quality>>LIDAR_RESP_MEASUREMENT_QUALITY_SHIFT); + bool point_startBit = (node.sync_quality & LIDAR_RESP_MEASUREMENT_SYNCBIT); + + postScanPoint(point_angle, point_distance_mm, point_quality, point_startBit); + + // 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; +} + +void LDS_YDLIDAR_X2::loop() { + result_t result = waitScanDot(); + if (result < 0) + postError(result, "waitScanDot()"); +} + +void LDS_YDLIDAR_X2::markScanTime() { + ring_start_ms[1] = ring_start_ms[0]; + ring_start_ms[0] = millis(); +} \ No newline at end of file diff --git a/src/LDS_YDLIDAR_X2.h b/src/LDS_YDLIDAR_X2.h new file mode 100644 index 0000000..fd1ac56 --- /dev/null +++ b/src/LDS_YDLIDAR_X2.h @@ -0,0 +1,153 @@ +// Copyright 2023-2024 REMAKE.AI, KAIA.AI, MAKERSPET.COM +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Based on +// https://github.com/YDLIDAR/lidarCar/ +// https://github.com/YDLIDAR/ydlidar_ros2_driver +// https://github.com/YDLIDAR/YDLidar-SDK + +#pragma once +#include "LDS.h" + + +class LDS_YDLIDAR_X2 : public LDS { + public: + void init() override; + + result_t start() override; + void stop() override; + void loop() override; // Call loop() frequently from Arduino loop() + + uint32_t getSerialBaudRate() override; + float getCurrentScanFreqHz() override; + float getTargetScanFreqHz() override; + int getSamplingRateHz() override; + bool isActive() override; + + result_t setScanTargetFreqHz(float freq) override; + result_t setScanPIDCoeffs(float Kp, float Ki, float Kd) override; + result_t setScanPIDSamplePeriodMs(uint32_t sample_period_ms) override; + + protected: + bool motor_enabled; + unsigned long int ring_start_ms[2]; + + protected: + static const uint32_t DEFAULT_TIMEOUT_MS = 500; + static const uint8_t PACKAGE_SAMPLE_MAX_LENGTH = 40; // 0x80 + + typedef enum { + CT_NORMAL = 0, + CT_RING_START = 1, + CT_TAIL, + } CT; + + struct node_info { + uint8_t sync_quality; + uint16_t angle_q6_checkbit; + uint16_t distance_q2; + } __attribute__((packed)) ; + + struct device_info{ + uint8_t model; + uint16_t firmware_version; + uint8_t hardware_version; + uint8_t serialnum[16]; + } __attribute__((packed)) ; + + struct device_health { + uint8_t status; + uint16_t error_code; + } __attribute__((packed)) ; + + struct cmd_packet { + uint8_t syncByte; + uint8_t cmd_flag; + uint8_t size; + uint8_t data; + } __attribute__((packed)) ; + + struct lidar_ans_header { + uint8_t syncByte1; + uint8_t syncByte2; + uint32_t size:30; + uint32_t subType:2; + uint8_t type; + } __attribute__((packed)); + + struct node_package { + uint16_t package_Head; + uint8_t package_CT; + uint8_t nowPackageNum; + uint16_t packageFirstSampleAngle; + uint16_t packageLastSampleAngle; + uint16_t checkSum; + uint16_t packageSampleDistance[PACKAGE_SAMPLE_MAX_LENGTH]; + } __attribute__((packed)) ; + + protected: + void enableMotor(bool enable); + LDS::result_t waitScanDot(); // wait for one sample package to arrive + void setupPins(); + void markScanTime(); + + protected: + // Scan start packet: 2 bytes + // Samples: 16 packets, up to 90B each total + // 10 bytes header + 40*2=70 bytes samples + // At 7Hz max total 1,442 bytes per scan + static const uint8_t LIDAR_CMD_STOP = 0x65; + static const uint8_t LIDAR_CMD_SCAN = 0x60; + static const uint8_t LIDAR_CMD_FORCE_SCAN = 0x61; + static const uint8_t LIDAR_CMD_RESET = 0x80; + static const uint8_t LIDAR_CMD_FORCE_STOP = 0x00; + static const uint8_t LIDAR_CMD_GET_EAI = 0x55; + static const uint8_t LIDAR_CMD_GET_DEVICE_INFO = 0x90; + static const uint8_t LIDAR_CMD_GET_DEVICE_HEALTH = 0x92; + static const uint8_t LIDAR_CMD_SYNC_BYTE = 0xA5; + static const uint16_t LIDAR_CMDFLAG_HAS_PAYLOAD = 0x8000; + + static const uint8_t LIDAR_ANS_TYPE_DEVINFO = 0x4; + static const uint8_t LIDAR_ANS_TYPE_DEVHEALTH = 0x6; + static const uint8_t LIDAR_ANS_SYNC_BYTE1 = 0xA5; + static const uint8_t LIDAR_ANS_SYNC_BYTE2 = 0x5A; + static const uint8_t LIDAR_ANS_TYPE_MEASUREMENT = 0x81; + + static const uint8_t LIDAR_RESP_MEASUREMENT_SYNCBIT = (0x1<<0); + static const uint8_t LIDAR_RESP_MEASUREMENT_QUALITY_SHIFT = 2; + static const uint8_t LIDAR_RESP_MEASUREMENT_CHECKBIT = (0x1<<0); + static const uint8_t LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT = 1; + static const uint8_t LIDAR_RESP_MEASUREMENT_ANGLE_SAMPLE_SHIFT = 8; + + static const uint8_t LIDAR_CMD_RUN_POSITIVE = 0x06; + static const uint8_t LIDAR_CMD_RUN_INVERSION = 0x07; + static const uint8_t LIDAR_CMD_SET_AIMSPEED_ADDMIC = 0x09; + static const uint8_t LIDAR_CMD_SET_AIMSPEED_DISMIC = 0x0A; + static const uint8_t LIDAR_CMD_SET_AIMSPEED_ADD = 0x0B; + static const uint8_t LIDAR_CMD_SET_AIMSPEED_DIS = 0x0C; + static const uint8_t LIDAR_CMD_GET_AIMSPEED = 0x0D; + static const uint8_t LIDAR_CMD_SET_SAMPLING_RATE = 0xD0; + static const uint8_t LIDAR_CMD_GET_SAMPLING_RATE = 0xD1; + + static const uint8_t LIDAR_STATUS_OK = 0x0; + static const uint8_t LIDAR_STATUS_WARNING = 0x1; + static const uint8_t LIDAR_STATUS_ERROR = 0x2; + + static const uint8_t PACKAGE_SAMPLE_BYTES = 2; + static const uint16_t NODE_DEFAULT_QUALITY = (10<<2); + static const uint8_t NODE_SYNC = 1; + static const uint8_t NODE_NOT_SYNC = 2; + static const uint8_t PACKAGE_PAID_BYTES = 10; + static const uint16_t PH = 0x55AA; // Packet Header +}; diff --git a/src/LDS_YDLIDAR_X4.cpp b/src/LDS_YDLIDAR_X4.cpp index 855e1f5..831fa80 100644 --- a/src/LDS_YDLIDAR_X4.cpp +++ b/src/LDS_YDLIDAR_X4.cpp @@ -19,8 +19,8 @@ #include "LDS_YDLIDAR_X4.h" void LDS_YDLIDAR_X4::init() { - motor_enabled = false; target_scan_freq = sampling_rate = ERROR_UNKNOWN; + ring_start_ms[0] = ring_start_ms[1] = 0; enableMotor(false); } @@ -28,6 +28,8 @@ LDS::result_t LDS_YDLIDAR_X4::start() { // Initialize enableMotor(false); + abort(); + device_info deviceinfo; if (getDeviceInfo(deviceinfo, 500) != LDS::RESULT_OK) return ERROR_DEVICE_INFO; @@ -99,7 +101,8 @@ uint32_t LDS_YDLIDAR_X4::getSerialBaudRate() { } float LDS_YDLIDAR_X4::getCurrentScanFreqHz() { - return ERROR_NOT_IMPLEMENTED; + unsigned long int scan_period_ms = ring_start_ms[0] - ring_start_ms[1]; + return (scan_period_ms == 0) ? 0 : 1000.0f/float(scan_period_ms); } float LDS_YDLIDAR_X4::getTargetScanFreqHz() { @@ -142,6 +145,11 @@ LDS::result_t LDS_YDLIDAR_X4::setScanPIDCoeffs(float Kp, float Ki, float Kd) { return ERROR_NOT_IMPLEMENTED; } +void LDS_YDLIDAR_X4::markScanTime() { + ring_start_ms[1] = ring_start_ms[0]; + ring_start_ms[0] = millis(); +} + LDS::result_t LDS_YDLIDAR_X4::waitScanDot() { static int recvPos = 0; static uint8_t package_Sample_Num = 0; @@ -187,9 +195,6 @@ LDS::result_t LDS_YDLIDAR_X4::waitScanDot() { if (currentByte<0) { state = 1; return ERROR_NOT_READY; - } else { - //uint8_t charBuf = (uint8_t)currentByte; - //postPacket(&charBuf, 1, false); } switch (recvPos) { @@ -206,6 +211,8 @@ LDS::result_t LDS_YDLIDAR_X4::waitScanDot() { break; case 2: SampleNumlAndCTCal = currentByte; + if (currentByte == CT_RING_START) + markScanTime(); if ((currentByte != CT_NORMAL) && (currentByte != CT_RING_START)) { recvPos = 0; continue; @@ -288,10 +295,8 @@ LDS::result_t LDS_YDLIDAR_X4::waitScanDot() { if (currentByte < 0){ state = 2; return ERROR_NOT_READY; - } else { - //uint8_t charBuf = (uint8_t)currentByte; - //postPacket(&charBuf, 1, false); } + if ((recvPos & 1) == 1) { Valu8Tou16 += (currentByte< +#include +#include