diff --git a/src/LDS_DELTA_2A.cpp b/src/LDS_DELTA_2A.cpp new file mode 100644 index 0000000..92b9fff --- /dev/null +++ b/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"; } \ No newline at end of file diff --git a/src/LDS_DELTA_2A.h b/src/LDS_DELTA_2A.h new file mode 100644 index 0000000..35a8f9f --- /dev/null +++ b/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; +}; diff --git a/src/LDS_DELTA_2G.cpp b/src/LDS_DELTA_2G.cpp index ff6dbcc..33d119b 100644 --- a/src/LDS_DELTA_2G.cpp +++ b/src/LDS_DELTA_2G.cpp @@ -14,229 +14,4 @@ #include "LDS_DELTA_2G.h" -void LDS_DELTA_2G::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_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_freq_hz_setpoint; -} - -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_freq_hz; -} - -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) { - 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_2G::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_2G::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_2G::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_2G::getModelName() { return "3irobotics Delta-2G"; } \ No newline at end of file diff --git a/src/LDS_DELTA_2G.h b/src/LDS_DELTA_2G.h index be2b397..9adcd20 100644 --- a/src/LDS_DELTA_2G.h +++ b/src/LDS_DELTA_2G.h @@ -13,75 +13,9 @@ // limitations under the License. #pragma once -#include "LDS.h" -#include "PID_v1_0_0.h" +#include "LDS_DELTA_2A.h" -class LDS_DELTA_2G : public LDS { +class LDS_DELTA_2G : public LDS_DELTA_2A { 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; + const char* getModelName() override; }; diff --git a/src/LDS_LDS02RR.h b/src/LDS_LDS02RR.h index 3cbbd70..76add03 100644 --- a/src/LDS_LDS02RR.h +++ b/src/LDS_LDS02RR.h @@ -16,7 +16,6 @@ #pragma once #include "LDS_NEATO_XV11.h" -#include "PID_v1_0_0.h" class LDS_LDS02RR : public LDS_NEATO_XV11 { public: diff --git a/src/LDS_all_models.h b/src/LDS_all_models.h index b40ddc7..7058c04 100644 --- a/src/LDS_all_models.h +++ b/src/LDS_all_models.h @@ -21,5 +21,6 @@ #include #include #include +#include #include //#include \ No newline at end of file