YDLIDAR receive info

kaiaai · Aug 20, 2024 · faeba48 · faeba48
1 parent 4b5899f
commit faeba48
Show file tree

Hide file tree

Showing 8 changed files with 149 additions and 19 deletions.
diff --git a/src/LDS_NEATO_XV11.h b/src/LDS_NEATO_XV11.h
@@ -63,7 +63,7 @@ class LDS_NEATO_XV11 : public LDS {
     bool isValidPacket();
     void clearVars();
 
-    float scan_rpm_setpoint; // desired scan RPM  
+    float scan_rpm_setpoint; // desired scan RPM
     bool motor_enabled;
     int eState;
 
@@ -100,7 +100,7 @@ class LDS_NEATO_XV11 : public LDS {
 
     float pwm_val;
     float pwm_last;
-    float scan_rpm;    
+    float scan_rpm;
     PID_v1 scanFreqPID;
 
     uint16_t aryDist[N_DATA_QUADS];    // there are (4) distances, one for each data quad

diff --git a/src/LDS_YDLIDAR_SCL.cpp b/src/LDS_YDLIDAR_SCL.cpp
@@ -18,6 +18,18 @@
 
 const char* LDS_YDLIDAR_SCL::getModelName() { return "YDLIDAR SCL"; }
 
+void LDS_YDLIDAR_SCL::init() {
+  LDS_YDLIDAR_X4::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.SetOutputLimits(0, 1.0);
+  scanFreqPID.SetSampleTime(20);
+  scanFreqPID.SetMode(PID_v1::AUTOMATIC);
+}
+
 LDS::result_t LDS_YDLIDAR_SCL::start() {
   enableMotor(true);
   postInfo(INFO_MODEL, getModelName());
@@ -28,16 +40,12 @@ uint32_t LDS_YDLIDAR_SCL::getSerialBaudRate() {
   return 115200;
 }
 
-float LDS_YDLIDAR_SCL::getTargetScanFreqHz() {
-  return DEFAULT_VALUE;
-}
-
 int LDS_YDLIDAR_SCL::getSamplingRateHz() {
   return 3000;
 }
 
 float LDS_YDLIDAR_SCL::getCurrentScanFreqHz() {
-  return LDS_YDLIDAR_X4::getCurrentScanFreqHz();
+  return motor_enabled ? scan_freq_hz : 0;
 }
 
 LDS::result_t LDS_YDLIDAR_SCL::stop() {
@@ -49,15 +57,44 @@ void LDS_YDLIDAR_SCL::enableMotor(bool enable) {
   motor_enabled = enable;
 
   if (enable) {
-    // Drive PWM
-    //setMotorPin(DIR_INPUT, LDS_MOTOR_PWM_PIN);
-    setMotorPin(DIR_OUTPUT_CONST, LDS_MOTOR_PWM_PIN);
-    setMotorPin(VALUE_HIGH, LDS_MOTOR_PWM_PIN);
+    //setMotorPin(DIR_OUTPUT_CONST, LDS_MOTOR_PWM_PIN);
+    //setMotorPin(VALUE_HIGH, LDS_MOTOR_PWM_PIN);
+    setMotorPin(DIR_OUTPUT_PWM, LDS_MOTOR_PWM_PIN);
+    setMotorPin(pwm_val, LDS_MOTOR_PWM_PIN);
   } else {
     setMotorPin(DIR_OUTPUT_CONST, LDS_MOTOR_PWM_PIN);
     setMotorPin(VALUE_LOW, LDS_MOTOR_PWM_PIN);
   }
+}
+
+void LDS_YDLIDAR_SCL::loop() {
+  LDS_YDLIDAR_X4::loop();
+
+  if (motor_enabled) {
+    scanFreqPID.Compute();
+
+    if (pwm_val != pwm_last) {
+      setMotorPin(pwm_val, LDS_MOTOR_PWM_PIN);
+      pwm_last = pwm_val;
+    }
+  }
+}
+
+LDS::result_t LDS_YDLIDAR_SCL::setScanTargetFreqHz(float freq) {
+  scan_freq_setpoint_hz = freq <= 0 ? DEFAULT_SCAN_FREQ_HZ : freq;
+  return RESULT_OK;
+}
+
+float LDS_YDLIDAR_SCL::getTargetScanFreqHz() {
+  return scan_freq_setpoint_hz;
+}
+
+LDS::result_t LDS_YDLIDAR_SCL::setScanPIDCoeffs(float Kp, float Ki, float Kd) {
+  scanFreqPID.SetTunings(Kp, Ki, Kd);
+  return RESULT_OK;
+}
 
-  // Entire LDS on/off
-  //setMotorPin(enable ? VALUE_HIGH : VALUE_LOW, LDS_MOTOR_PWM_PIN);
+LDS::result_t LDS_YDLIDAR_SCL::setScanPIDSamplePeriodMs(uint32_t sample_period_ms) {
+  scanFreqPID.SetSampleTime(sample_period_ms);
+  return RESULT_OK;
 }
diff --git a/src/LDS_YDLIDAR_SCL.h b/src/LDS_YDLIDAR_SCL.h
@@ -16,17 +16,33 @@
 
 #pragma once
 #include "LDS_YDLIDAR_X4.h"
+#include "PID_v1_0_0.h"
 
 class LDS_YDLIDAR_SCL : public LDS_YDLIDAR_X4 {
   public:
+    void init() override;
     virtual result_t start() override;
     virtual result_t stop() override;
     virtual const char* getModelName() override;
+    virtual void loop() override;
 
     virtual float getCurrentScanFreqHz() override;
     virtual uint32_t getSerialBaudRate() override;
     virtual float getTargetScanFreqHz() override;
     virtual int getSamplingRateHz() override;
+
+    virtual result_t setScanTargetFreqHz(float freq) override;
+    virtual result_t setScanPIDCoeffs(float Kp, float Ki, float Kd) override;
+    virtual result_t setScanPIDSamplePeriodMs(uint32_t sample_period_ms) override;
+
   protected:
+    static constexpr float DEFAULT_SCAN_FREQ_HZ = 5.0f;
+    static constexpr float PWM_START = 0.25f;
     virtual void enableMotor(bool enable) override;
+    String receiveInfo(uint32_t timeout_ms);
+
+    float scan_freq_setpoint_hz; // desired scan frequency
+    float pwm_val;
+    float pwm_last;
+    PID_v1 scanFreqPID;
 };
diff --git a/src/LDS_YDLIDAR_X2_X2L.cpp b/src/LDS_YDLIDAR_X2_X2L.cpp
@@ -51,8 +51,9 @@ void LDS_YDLIDAR_X2_X2L::enableMotor(bool enable) {
   if (enable) {
     setMotorPin(DIR_INPUT, LDS_MOTOR_PWM_PIN);
   } else {
+    // Cannot stop X2L completely
     setMotorPin(DIR_OUTPUT_CONST, LDS_MOTOR_PWM_PIN);
-    setMotorPin(VALUE_LOW, LDS_MOTOR_PWM_PIN);
+    setMotorPin(VALUE_HIGH, LDS_MOTOR_PWM_PIN);
   }
 
   // Entire LDS on/off

diff --git a/src/LDS_YDLIDAR_X3.cpp b/src/LDS_YDLIDAR_X3.cpp
@@ -17,5 +17,17 @@
 const char* LDS_YDLIDAR_X3::getModelName() { return "YDLIDAR X3"; }
 
 float LDS_YDLIDAR_X3::getCurrentScanFreqHz() {
-  return motor_enabled ? float(scan_freq)*0.1f : 0;
+  return motor_enabled ? scan_freq_hz : 0;
+}
+
+void LDS_YDLIDAR_X3::enableMotor(bool enable) {
+  motor_enabled = enable;
+
+  if (enable) {
+    setMotorPin(DIR_INPUT, LDS_MOTOR_PWM_PIN);
+  } else {
+    // Cannot stop X3 completely
+    setMotorPin(DIR_OUTPUT_CONST, LDS_MOTOR_PWM_PIN);
+    setMotorPin(VALUE_LOW, LDS_MOTOR_PWM_PIN);
+  }
 }
diff --git a/src/LDS_YDLIDAR_X3.h b/src/LDS_YDLIDAR_X3.h
@@ -19,4 +19,7 @@ class LDS_YDLIDAR_X3 : public LDS_YDLIDAR_X2_X2L {
   public:
     virtual const char* getModelName() override;
     virtual float getCurrentScanFreqHz() override;
+
+  protected:
+    virtual void enableMotor(bool enable) override;
 };
diff --git a/src/LDS_YDLIDAR_X4.cpp b/src/LDS_YDLIDAR_X4.cpp
@@ -18,7 +18,7 @@
 
 void LDS_YDLIDAR_X4::init() {
   ring_start_ms[0] = ring_start_ms[1] = 0;
-  scan_freq = 0;
+  scan_freq_hz = 0;
   scan_completed = false;
   enableMotor(false);
 }
@@ -116,6 +116,61 @@ void LDS_YDLIDAR_X4::markScanTime() {
   ring_start_ms[0] = millis();
 }
 
+void LDS_YDLIDAR_X4::checkInfo(int currentByte) {
+  static String s;
+  static uint8_t state = 0;
+  const uint8_t header[] = {0xA5, 0x5A, 0x14, 0x00, 0x00, 0x00, 0x04};
+
+  switch (state) {
+    case 0:
+    case 1:
+    case 2:
+    case 3:
+    case 4:
+    case 5:
+    case 6:
+      if (currentByte == header[state]) {
+         state++;
+      } else {
+        state = 0;
+        s = "";
+      }
+      break;
+    case 27:
+      postInfo(INFO_MODEL, s);
+      state = 0;
+      s = "";
+      break;
+    case 7:
+      s = "Model 0x";
+      if (currentByte < 16)
+        s = s + '0';
+      s = s + String(currentByte, HEX);
+      state++;
+      break;
+    case 8:
+      s = s + ", firmware v" + String(currentByte);
+      state++;
+      break;
+    case 9:
+      s = s + '.' + String(currentByte);
+      state++;
+      break;
+    case 10:
+      s = s + ", hardware v" + String(currentByte);
+      state++;
+      break;
+    case 11:
+      s = s + ", S/N " + String(currentByte);
+      state++;
+      break;
+    default:
+      s = s + String(currentByte);
+      state++;
+      break;
+  }
+}
+
 LDS::result_t LDS_YDLIDAR_X4::waitScanDot() {
   switch(state) {
     case 1:
@@ -141,8 +196,10 @@ LDS::result_t LDS_YDLIDAR_X4::waitScanDot() {
 
       switch (recvPos) {
         case 0: // 0xAA 1st byte of package header
-          if (currentByte != (PH&0xFF))
+          if (currentByte != (PH&0xFF)) {
+            checkInfo(currentByte);
             continue;
+          }
           break;
         case 1: // 0x55 2nd byte of package header
           CheckSumCal = PH;
@@ -277,7 +334,10 @@ LDS::result_t LDS_YDLIDAR_X4::waitScanDot() {
   if (CheckSumResult) {
     scan_completed = (package.package_CT & 0x01) == CT_RING_START;
     if (scan_completed)
-      scan_freq = package.package_CT >> 1;
+      //scan_freq = package.package_CT >> 1;
+      //F = CT[bit(7:1)]/10 (when CT[bit(7:1)] = 1).
+      scan_freq_hz = float(package.package_CT >> 1)*0.1f;
+
     postPacket(packageBuffer, PACKAGE_PAID_BYTES+package_sample_sum, scan_completed);
   }
 

diff --git a/src/LDS_YDLIDAR_X4.h b/src/LDS_YDLIDAR_X4.h
@@ -101,6 +101,7 @@ class LDS_YDLIDAR_X4 : public LDS {
     LDS::result_t sendCommand(uint8_t cmd, const void * payload = NULL, size_t payloadsize = 0);
     LDS::result_t waitResponseHeader(ans_header_t * header, uint32_t timeout = DEFAULT_TIMEOUT_MS);
     void markScanTime();
+    void checkInfo(int currentByte);
 
   protected:
     // Scan start packet: 2 bytes
@@ -160,6 +161,6 @@ class LDS_YDLIDAR_X4 : public LDS {
 
     uint8_t state = 0;
 
-    uint8_t scan_freq = 0;
+    float scan_freq_hz = 0;
     bool scan_completed = false;
 };