Delta2A; circle of dots

src/LDS_DELTA_2A.cpp

@@ -0,0 +1,242 @@
+// 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_2A.h"
+
+void LDS_DELTA_2A::init() {
+  motor_enabled = false;
+  pwm_val = 0.6;
+  scan_freq_hz = 0;
+  scan_freq_hz_setpoint = DEFAULT_SCAN_FREQ_HZ;
+  parser_state = 0;
+  checksum = 0;
+
+  scanFreqPID.init(&scan_freq_hz, &pwm_val, &scan_freq_hz_setpoint, 3.0e-1, 1.0e-1, 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_2A::start() {
+  enableMotor(true);
+  postInfo(INFO_MODEL, getModelName());
+  return RESULT_OK;
+}
+
+uint32_t LDS_DELTA_2A::getSerialBaudRate() {
+  return 115200;
+}
+
+float LDS_DELTA_2A::getTargetScanFreqHz() {
+  return scan_freq_hz_setpoint;
+}
+
+int LDS_DELTA_2A::getSamplingRateHz() {
+  return 1890;
+}
+
+LDS::result_t LDS_DELTA_2A::setScanPIDCoeffs(float Kp, float Ki, float Kd) {
+  scanFreqPID.SetTunings(Kp, Ki, Kd);
+  return RESULT_OK;
+}
+
+LDS::result_t LDS_DELTA_2A::setScanPIDSamplePeriodMs(uint32_t sample_period_ms) {
+  scanFreqPID.SetSampleTime(sample_period_ms);
+  return RESULT_OK;
+}
+
+float LDS_DELTA_2A::getCurrentScanFreqHz() {
+  return scan_freq_hz;
+}
+
+void LDS_DELTA_2A::stop() {
+  enableMotor(false);
+}
+
+void LDS_DELTA_2A::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_2A::isActive() {
+  return motor_enabled;
+}
+
+LDS::result_t LDS_DELTA_2A::setScanTargetFreqHz(float freq) {
+  if (freq <= 0) {
+    scan_freq_hz_setpoint = DEFAULT_SCAN_FREQ_HZ;
+    return RESULT_OK;
+  }
+
+  if (freq <= DEFAULT_SCAN_FREQ_HZ*0.9f || freq >= DEFAULT_SCAN_FREQ_HZ*1.1f)
+    return ERROR_INVALID_VALUE;
+
+  scan_freq_hz_setpoint = freq;
+  return RESULT_OK;
+}
+
+void LDS_DELTA_2A::loop() {
+  while (true) {
+    int c = readSerial();
+    if (c < 0)
+      break;
+
+    result_t result = processByte((uint8_t)c);
+    if (result < 0)
+      postError(result, "");
+  }
+
+  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_2A::processByte(uint8_t c) {
+  uint16_t packet_length = 0;
+  uint16_t data_length = 0;
+  LDS::result_t result = RESULT_OK;
+  uint8_t * rx_buffer = (uint8_t *)&scan_packet;
+
+  if (parser_idx >= sizeof(scan_packet_t)) {
+    parser_idx = 0;
+    return RESULT_OK;//ERROR_INVALID_PACKET;
+  }
+
+  rx_buffer[parser_idx++] = c;
+  checksum += c;
+
+  switch (parser_idx) {
+  case 1:
+    if (c != START_BYTE) {
+      result = RESULT_OK; //ERROR_INVALID_VALUE;
+      parser_idx = 0;
+    } else
+      checksum = c;
+    break;
+
+  case 2:
+    break;
+
+  case 3:
+    packet_length = decodeUInt16(scan_packet.packet_length);
+    if (packet_length > sizeof(scan_packet_t) - sizeof(scan_packet.checksum))
+      result = ERROR_INVALID_PACKET;
+    break;
+
+  case 4:
+    if (c != PROTOCOL_VERSION)
+      result = ERROR_INVALID_VALUE;
+    break;
+
+  case 5:
+    if (c != PACKET_TYPE)
+      result = ERROR_INVALID_VALUE;
+    break;
+
+  case 6:
+    if (c != DATA_TYPE_RPM_AND_MEAS && c != DATA_TYPE_RPM_ONLY)
+      result = ERROR_INVALID_VALUE;
+    break;
+
+  case 7: // data length MSB
+    break;
+
+  case 8: // data length LSB
+    data_length = decodeUInt16(scan_packet.data_length);
+    if (data_length == 0 || data_length > MAX_DATA_BYTE_LEN)
+      result = ERROR_INVALID_PACKET;
+    break;
+
+  default:
+    // Keep reading
+    packet_length = decodeUInt16(scan_packet.packet_length);
+    if (parser_idx != packet_length + 2)
+      break;
+
+    uint16_t pkt_checksum = (rx_buffer[parser_idx-2] << 8) + rx_buffer[parser_idx-1];
+
+    pkt_checksum += rx_buffer[parser_idx-2];
+    pkt_checksum += rx_buffer[parser_idx-1];
+    if (checksum != pkt_checksum) {
+      result = ERROR_CHECKSUM;
+      break;
+    }
+
+    scan_freq_hz = scan_packet.scan_freq_x20 * 0.05;
+
+    if (scan_packet.data_type == DATA_TYPE_RPM_AND_MEAS) {
+      uint16_t start_angle_x100 = decodeUInt16(scan_packet.start_angle_x100);
+      bool scan_completed = start_angle_x100 == 0;
+
+      postPacket(rx_buffer, parser_idx, scan_completed);
+
+      data_length = decodeUInt16(scan_packet.data_length);
+      if (data_length < 8) {
+        result = ERROR_INVALID_PACKET;
+        break;
+      }
+
+      uint16_t sample_count = (data_length - 5) / 3;
+      if (sample_count > MAX_DATA_SAMPLES) {
+        result = ERROR_INVALID_PACKET;
+        break;
+      }
+
+      //int16_t offset_angle_x100 = (int16_t)decodeUInt16((uint16_t)scan_packet.offset_angle_x100);
+      float start_angle = start_angle_x100 * 0.01;
+      //start_angle += offset_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;
+
+        uint16_t distance_mm_x4 = decodeUInt16(scan_packet.sample[idx].distance_mm_x4);
+        float distance_mm = distance_mm_x4 * 0.25;
+        float quality = scan_packet.sample[idx].quality;
+        postScanPoint(angle_deg, distance_mm, quality, scan_completed);
+        scan_completed = false;
+      }
+    }
+    parser_idx = 0;
+    break;
+  }
+
+  if (result < RESULT_OK)
+    parser_idx = 0;
+
+  return result;
+}
+
+uint16_t LDS_DELTA_2A::decodeUInt16(const uint16_t value) const {
+  union {
+    uint16_t i;
+    char c[2];
+  } bint = {0x0102};
+
+  return bint.c[0] == 0x01 ? value : (value << 8) + (value >> 8);
+}
+
+const char* LDS_DELTA_2A::getModelName() { return "3irobotics Delta-2A"; }

src/LDS_DELTA_2A.h

@@ -0,0 +1,87 @@
+// 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_2A : 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_FREQ_HZ = 5.25f;
+    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 = 0xAD;
+    static const uint8_t DATA_TYPE_RPM_ONLY = 0xAE;
+    static const uint8_t PACKETS_PER_SCAN = 16;
+    static constexpr float DEG_PER_PACKET = 360.0f / (float)PACKETS_PER_SCAN; // 22.5 deg
+    static const uint8_t MAX_DATA_SAMPLES = 28;
+
+    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; // excludes checksum
+      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]; // 3*28=84
+
+      uint16_t   checksum;
+    } __attribute__((packed)); // 8 + 5 + 84 + 2 = 97
+
+    virtual void enableMotor(bool enable);
+    LDS::result_t processByte(uint8_t c);
+    uint16_t decodeUInt16(const uint16_t value) const;
+
+    float scan_freq_hz_setpoint;
+    bool motor_enabled;
+    uint8_t parser_state;
+    float pwm_val;
+    float pwm_last;
+    float scan_freq_hz;
+    PID_v1 scanFreqPID;
+
+    scan_packet_t scan_packet;
+    uint16_t parser_idx;
+    uint16_t checksum;
+};