Delta-2G in debug

README.md

@@ -27,11 +27,16 @@ Other models are in the works.
  <img src="http://img.youtube.com/vi/f8IYjfiXsMk/maxresdefault.jpg" alt="RPLIDAR A1 laser distance scan sensor runs on Arduino" width="720" height="405" border="10" />
 </a>
 
-Note: LDS02RR, Neato XV11 LDS require an additional motor control board to operate.
-You can find the board schematics [here](https://github.com/makerspet/pcb/tree/main/neato_delta_adapter) or purchase it at [makerspet.com](https://makerspet.com) when it becomes available.
+Some LiDAR/LDS models do not have built-in motor control and therefore require an additional board to operate:
+- for Xiaomi Roborock 1st gen LDS02RR use [this board](https://github.com/makerspet/pcb/tree/main/lds02rr_adapter)
+- for Neato XV11 use [this board](https://github.com/makerspet/pcb/tree/main/neato_delta_adapter)
+- for 3irobotix Delta-2A, -2B, -2C PRO, -2D, -2G use [this board](https://github.com/makerspet/pcb/tree/main/neato_delta_adapter)
 
 ## Release notes
 
+## v0.5.0 (in debug)
+- added Delta-2G
+
 ## v0.4.0
 - added Neato XV11
 - added RPLIDAR A1
@@ -52,3 +57,11 @@ You can find the board schematics [here](https://github.com/makerspet/pcb/tree/m
 
 ### v0.1.0
 - initial release
+
+## TODO
+- add Xiaomi Roborock LDS01RR
+- add LDROBOT LD14P, LD20
+- add Hitachi-LG HLS-LFCD2
+- add Dreame TBD
+- reduce raw data volume
+  - omit measurement quality since it usually does not get used

keywords.txt

@@ -14,6 +14,7 @@ LDS_YDLIDAR_X2_X2L	KEYWORD1
 LDS_NEATO_X11	KEYWORD1
 LDS_LDS02RR	KEYWORD1
 LDS_RPLIDAR_A1	KEYWORD1
+LDS_DELTA_2G	KEYWORD1
 
 #######################################
 # Methods and Functions (KEYWORD2)

library.properties

@@ -1,9 +1,9 @@
 name=LDS
-version=0.4.0
+version=0.5.0
 author=Ilia O. <[email protected]>
 maintainer=Ilia O. <[email protected]>
 sentence=Laser distance scan sensors (LDS/LIDAR) wrapper/controller for kaia.ai robotics platform
-paragraph=Supports YDLIDAR X4, X3, X3 PRO, X2/X2L, Xiaomi LDS02RR, Neato XV11, RPLIDAR A1
+paragraph=Supports YDLIDAR X4, X3, X3 PRO, X2/X2L, Xiaomi LDS02RR, Neato XV11, RPLIDAR A1, 3irobotix Delta-2G
 category=Sensors
 url=https://github.com/kaiaai/LDS
 architectures=*

src/LDS_DELTA_2G.cpp

@@ -0,0 +1,227 @@
+// 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.
+
+#include "LDS_DELTA_2G.h"
+
+void LDS_DELTA_2G::init() {
+  motor_enabled = false;
+  pwm_val = 0.5;
+  scan_rpm = 0;
+  scan_rpm_setpoint = DEFAULT_SCAN_RPM;
+  parser_state = 0;
+  checksum = 0;
+
+  scanFreqPID.init(&scan_rpm, &pwm_val, &scan_rpm_setpoint, 3.0e-3, 1.0e-3, 0.0, PID_v1::DIRECT);
+  scanFreqPID.SetOutputLimits(0, 1.0);
+  scanFreqPID.SetSampleTime(20);
+  scanFreqPID.SetMode(PID_v1::AUTOMATIC);
+  enableMotor(false);
+}
+
+LDS::result_t LDS_DELTA_2G::start() {
+  enableMotor(true);
+  postInfo(INFO_MODEL, getModelName());
+  return RESULT_OK;
+}
+
+uint32_t LDS_DELTA_2G::getSerialBaudRate() {
+  return 115200;
+}
+
+float LDS_DELTA_2G::getTargetScanFreqHz() {
+  return scan_rpm_setpoint / 60.0f;
+}
+
+int LDS_DELTA_2G::getSamplingRateHz() {
+  return 1890;
+}
+
+LDS::result_t LDS_DELTA_2G::setScanPIDCoeffs(float Kp, float Ki, float Kd) {
+  scanFreqPID.SetTunings(Kp, Ki, Kd);
+  return RESULT_OK;
+}
+
+LDS::result_t LDS_DELTA_2G::setScanPIDSamplePeriodMs(uint32_t sample_period_ms) {
+  scanFreqPID.SetSampleTime(sample_period_ms);
+  return RESULT_OK;
+}
+
+float LDS_DELTA_2G::getCurrentScanFreqHz() {
+  return scan_rpm/60.0f;
+}
+
+void LDS_DELTA_2G::stop() {
+  enableMotor(false);
+}
+
+void LDS_DELTA_2G::enableMotor(bool enable) {
+  motor_enabled = enable;
+
+  if (enable) {
+    setMotorPin(DIR_OUTPUT_PWM, LDS_MOTOR_PWM_PIN);
+    setMotorPin(enable ? pwm_val : float(VALUE_LOW), LDS_MOTOR_PWM_PIN);
+  } else {
+    setMotorPin(DIR_OUTPUT_CONST, LDS_MOTOR_PWM_PIN);
+    setMotorPin(VALUE_LOW, LDS_MOTOR_PWM_PIN);
+  }
+}
+
+bool LDS_DELTA_2G::isActive() {
+  return motor_enabled;
+}
+
+LDS::result_t LDS_DELTA_2G::setScanTargetFreqHz(float freq) {
+  float rpm = freq * 60.0f;
+  if (rpm <= 0) {
+    scan_rpm_setpoint = DEFAULT_SCAN_RPM;
+    return RESULT_OK;
+  }
+
+  if (rpm <= DEFAULT_SCAN_RPM*0.9f || rpm >= DEFAULT_SCAN_RPM*1.1f)
+    return ERROR_INVALID_VALUE;
+
+  scan_rpm_setpoint = rpm;
+  return RESULT_OK;
+}
+
+void LDS_DELTA_2G::loop() {
+  while (true) {
+    int c = readSerial();
+    if (c < 0)
+      break;
+
+    result_t result = processByte((uint8_t)c);
+    if (result < 0)
+      postError(result, "processByte()");
+  }
+
+  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_DELTA_2G::processByte(uint8_t c) {
+  LDS::result_t result = RESULT_OK;
+  // parse packet header
+  // get N samples
+  //   postPacket()
+  //   postScanPoint()
+  uint8_t * rx_buffer = (uint8_t *)&scan_packet;
+  // parser_idx = parser_idx >= sizeof(scan_packet_t) ? 0 : parser_idx;
+
+  if (parser_idx >= sizeof(scan_packet_t)) {
+    parser_idx = 0;
+    return ERROR_INVALID_PACKET;
+  }
+
+  rx_buffer[parser_idx++] = c;
+  checksum += c;
+
+  switch (parser_idx) {
+  case 0:
+    if (c != START_BYTE)
+	    result = ERROR_INVALID_VALUE;
+    else
+      checksum = 0;
+	  break;
+
+  case 1: // packet length MSB
+    break;
+
+  case 2: // packet length LSB
+    if (scan_packet.packet_length >
+      sizeof(scan_packet_t) - sizeof(scan_packet.checksum))
+      result = ERROR_INVALID_PACKET;
+    break;
+
+  case 3:
+    if (c != PROTOCOL_VERSION)
+	    result = ERROR_INVALID_VALUE;
+    break;
+
+  case 4:
+    if (c != PACKET_TYPE)
+      result = ERROR_INVALID_VALUE;
+    break;
+
+  case 5:
+    if (c != DATA_TYPE_RPM_AND_MEAS && c != DATA_TYPE_RPM_ONLY)
+      result = ERROR_INVALID_VALUE;
+    break;
+
+  case 6: // data length MSB
+    break;
+
+  case 7: // data length LSB
+    if (scan_packet.data_length == 0 ||
+      scan_packet.data_length > MAX_DATA_BYTE_LEN)
+	    result = ERROR_INVALID_PACKET;
+    break;
+
+  default:
+    // Keep reading
+    if (parser_idx >= scan_packet.packet_length + sizeof(scan_packet.checksum)) {
+
+      // Got checksum
+      scan_packet.checksum += c << 8;
+      uint16_t checksum_adjusted = checksum;
+      checksum_adjusted += checksum & 0xFF;
+      checksum_adjusted += (checksum >> 8) & 0xFF;
+
+      if (scan_packet.checksum != checksum_adjusted) {
+        result = ERROR_CRC;
+        break;
+      }
+
+      scan_rpm = scan_packet.scan_freq_x20 * 0.05;
+      if (scan_packet.data_type == DATA_TYPE_RPM_AND_MEAS) {
+
+        unsigned int packet_index = (scan_packet.start_angle_x100 * PACKETS_PER_SCAN) % 36000;
+        bool scan_completed = packet_index == 0;
+
+        uint16_t sample_count = (scan_packet.data_length - 5) / 3;
+        if (sample_count > MAX_DATA_SAMPLES) {
+          result = ERROR_INVALID_PACKET;
+          break;
+        }
+
+        postPacket(rx_buffer, parser_idx, scan_completed);
+
+        float start_angle = scan_packet.start_angle_x100 * 0.01;
+        float coeff = DEG_PER_PACKET / (float)sample_count;
+        for (uint16_t idx = 0; idx < sample_count; idx++) {
+          float angle_deg = start_angle + idx * coeff;
+          float distance_mm = scan_packet.sample[idx].distance_mm_x4 * 0.25;
+          float quality = scan_packet.sample[idx].quality;
+          postScanPoint(angle_deg, distance_mm, quality, scan_completed);
+        }
+      }
+      parser_idx = 0;
+      break;
+    }
+    break;
+  }
+
+  if (result < RESULT_OK)
+    parser_idx = 0;
+
+  return result;
+}
+
+const char* LDS_DELTA_2G::getModelName() { return "3irobotics Delta-2G"; }

src/LDS_DELTA_2G.h

@@ -0,0 +1,90 @@
+// 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.
+
+#pragma once
+#include "LDS.h"
+#include "PID_v1_0_0.h"
+
+class LDS_DELTA_2G : public LDS {
+  public:
+    virtual void init() override;
+
+    virtual result_t start() override;
+    virtual void stop() override;
+    virtual void loop() override; // Call loop() frequently from Arduino loop()
+
+    virtual uint32_t getSerialBaudRate() override;
+    virtual float getCurrentScanFreqHz() override;
+    virtual float getTargetScanFreqHz() override;
+    virtual int getSamplingRateHz() override;
+    virtual bool isActive() override;
+    virtual const char* getModelName() 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_RPM = 315.0f;
+    static const uint8_t START_BYTE = 0xAA;
+    static const uint8_t PROTOCOL_VERSION = 0x01;
+    static const uint8_t PACKET_TYPE = 0x61;
+    static const uint8_t DATA_TYPE_RPM_AND_MEAS = 0xAE;
+    static const uint8_t DATA_TYPE_RPM_ONLY = 0xAD;
+    static const uint8_t PACKETS_PER_SCAN = 15;
+    static constexpr float DEG_PER_PACKET = 1.0f / (float)PACKETS_PER_SCAN;
+    static const uint8_t MAX_DATA_SAMPLES = 360/PACKETS_PER_SCAN;
+    static const uint8_t PACKET_HEADER_BYTE_LEN = 10;
+
+    struct meas_sample_t {
+      uint8_t quality;
+      uint16_t distance_mm_x4;
+    };
+    static const uint16_t MAX_DATA_BYTE_LEN = sizeof(meas_sample_t) * MAX_DATA_SAMPLES;
+
+    struct scan_packet_t {
+      uint8_t    start_byte; // 0xAA
+      uint16_t   packet_length;
+      uint8_t    protocol_version; // 0x01
+      uint8_t    packet_type; // 0x61
+      uint8_t    data_type; // 0xAE -> data_length -> scan_freq_x20 -> no data
+                            // 0xAD -> data_length -> scan_freq_x20 -> data
+      uint16_t   data_length; // n_samples = (data_length - 5)/3;
+      uint8_t    scan_freq_x20;
+      int16_t offset_angle_x100; // signed
+      uint16_t start_angle_x100; // unsigned?
+
+      meas_sample_t sample[MAX_DATA_SAMPLES];
+
+      uint16_t   checksum;
+    } __attribute__((packed));
+    // 15 scan steps per revolution
+    // float angle = start_angle + 22.5 * sample_idx / n_samples;
+    // float angle = start_angle + 24 * sample_idx / n_samples;
+
+    virtual void enableMotor(bool enable);
+    LDS::result_t processByte(uint8_t c);
+
+    float scan_rpm_setpoint;
+    bool motor_enabled;
+    uint8_t parser_state;
+    float pwm_val;
+    float pwm_last;
+    float scan_rpm;    
+    PID_v1 scanFreqPID;
+
+    scan_packet_t scan_packet;
+    uint16_t parser_idx;
+    uint16_t checksum;
+};