Skip to content

Commit

Permalink
Delta-2A 230400 baud, Delta-2B before debug
Browse files Browse the repository at this point in the history
  • Loading branch information
makerspet committed Mar 8, 2024
1 parent d753515 commit dceb859
Show file tree
Hide file tree
Showing 9 changed files with 190 additions and 53 deletions.
102 changes: 65 additions & 37 deletions src/LDS_DELTA_2A.cpp → src/LDS_DELTA_2A_115200.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -12,60 +12,72 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#include "LDS_DELTA_2A.h"
#include "LDS_DELTA_2A_115200.h"

void LDS_DELTA_2A::init() {
void LDS_DELTA_2A_115200::init() {
motor_enabled = false;
pwm_val = 0.6;
scan_freq_hz = 0;
scan_freq_hz_setpoint = DEFAULT_SCAN_FREQ_HZ;
scan_freq_hz_setpoint = get_default_scan_freq_hz();
parser_state = 0;
checksum = 0;

uint16_t max_data_samples = get_max_data_sample_count();
uint16_t max_data_len_bytes = (uint16_t) (sizeof(meas_sample_t) * max_data_samples);
rx_buffer = new uint8_t[max_data_len_bytes];

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() {
uint16_t LDS_DELTA_2A_115200::get_max_data_sample_count() {
return 28;
}

float LDS_DELTA_2A_115200::get_default_scan_freq_hz() {
return 5.25f; // guesstimate
}

LDS::result_t LDS_DELTA_2A_115200::start() {
enableMotor(true);
postInfo(INFO_MODEL, getModelName());
return RESULT_OK;
}

uint32_t LDS_DELTA_2A::getSerialBaudRate() {
uint32_t LDS_DELTA_2A_115200::getSerialBaudRate() {
return 115200;
}

float LDS_DELTA_2A::getTargetScanFreqHz() {
float LDS_DELTA_2A_115200::getTargetScanFreqHz() {
return scan_freq_hz_setpoint;
}

int LDS_DELTA_2A::getSamplingRateHz() {
return 1890;
int LDS_DELTA_2A_115200::getSamplingRateHz() {
return 1890; // guesstimate
}

LDS::result_t LDS_DELTA_2A::setScanPIDCoeffs(float Kp, float Ki, float Kd) {
LDS::result_t LDS_DELTA_2A_115200::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) {
LDS::result_t LDS_DELTA_2A_115200::setScanPIDSamplePeriodMs(uint32_t sample_period_ms) {
scanFreqPID.SetSampleTime(sample_period_ms);
return RESULT_OK;
}

float LDS_DELTA_2A::getCurrentScanFreqHz() {
float LDS_DELTA_2A_115200::getCurrentScanFreqHz() {
return scan_freq_hz;
}

void LDS_DELTA_2A::stop() {
void LDS_DELTA_2A_115200::stop() {
enableMotor(false);
}

void LDS_DELTA_2A::enableMotor(bool enable) {
void LDS_DELTA_2A_115200::enableMotor(bool enable) {
motor_enabled = enable;

if (enable) {
Expand All @@ -77,24 +89,25 @@ void LDS_DELTA_2A::enableMotor(bool enable) {
}
}

bool LDS_DELTA_2A::isActive() {
bool LDS_DELTA_2A_115200::isActive() {
return motor_enabled;
}

LDS::result_t LDS_DELTA_2A::setScanTargetFreqHz(float freq) {
LDS::result_t LDS_DELTA_2A_115200::setScanTargetFreqHz(float freq) {
const float default_scan_freq_hz = get_default_scan_freq_hz();
if (freq <= 0) {
scan_freq_hz_setpoint = DEFAULT_SCAN_FREQ_HZ;
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)
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() {
void LDS_DELTA_2A_115200::loop() {
while (true) {
int c = readSerial();
if (c < 0)
Expand All @@ -115,13 +128,17 @@ void LDS_DELTA_2A::loop() {
}
}

LDS::result_t LDS_DELTA_2A::processByte(uint8_t c) {
LDS::result_t LDS_DELTA_2A_115200::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;
packet_header_t * packet_header = (packet_header_t *)rx_buffer;

if (parser_idx >= sizeof(scan_packet_t)) {
uint16_t max_data_samples = get_max_data_sample_count();
uint16_t max_data_len_bytes = (uint16_t) (sizeof(meas_sample_t) * max_data_samples);
uint16_t max_packet_len_bytes_less_crc = max_data_len_bytes + sizeof(packet_header_t);

if (parser_idx >= max_packet_len_bytes_less_crc) {
parser_idx = 0;
return RESULT_OK;//ERROR_INVALID_PACKET;
}
Expand All @@ -142,8 +159,8 @@ LDS::result_t LDS_DELTA_2A::processByte(uint8_t c) {
break;

case 3:
packet_length = decodeUInt16(scan_packet.packet_length);
if (packet_length > sizeof(scan_packet_t) - sizeof(scan_packet.checksum))
packet_length = decodeUInt16(packet_header->packet_length);
if (packet_length > max_packet_len_bytes_less_crc + sizeof(uint16_t))
result = ERROR_INVALID_PACKET;
break;

Expand All @@ -166,14 +183,14 @@ LDS::result_t LDS_DELTA_2A::processByte(uint8_t c) {
break;

case 8: // data length LSB
data_length = decodeUInt16(scan_packet.data_length);
if (data_length == 0 || data_length > MAX_DATA_BYTE_LEN)
data_length = decodeUInt16(packet_header->data_length);
if (data_length == 0 || data_length > max_data_len_bytes)
result = ERROR_INVALID_PACKET;
break;

default:
// Keep reading
packet_length = decodeUInt16(scan_packet.packet_length);
packet_length = decodeUInt16(packet_header->packet_length);
if (parser_idx != packet_length + 2)
break;

Expand All @@ -186,36 +203,41 @@ LDS::result_t LDS_DELTA_2A::processByte(uint8_t c) {
break;
}

scan_freq_hz = scan_packet.scan_freq_x20 * 0.05;
scan_freq_hz = packet_header->scan_freq_x20 * 0.05;
uint8_t packets_per_scan = get_packets_per_scan();

if (scan_packet.data_type == DATA_TYPE_RPM_AND_MEAS) {
uint16_t start_angle_x100 = decodeUInt16(scan_packet.start_angle_x100);
if (packet_header->data_type == DATA_TYPE_RPM_AND_MEAS) {
uint16_t start_angle_x100 = decodeUInt16(packet_header->start_angle_x100);
bool scan_completed = start_angle_x100 == 0;

postPacket(rx_buffer, parser_idx, scan_completed);

data_length = decodeUInt16(scan_packet.data_length);
data_length = decodeUInt16(packet_header->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) {
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);
//int16_t offset_angle_x100 = (int16_t)decodeUInt16((uint16_t)packet_header->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;
//DEG_PER_PACKET = 360.0f / (float)PACKETS_PER_SCAN; // 22.5 deg

float coeff = 360.0f / (packets_per_scan * sample_count);
// meas_sample_t * samples = (meas_sample_t *)&packet_header[sizeof(packet_header_t)];
meas_sample_t * samples = (meas_sample_t *)(packet_header + sizeof(packet_header_t));
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);
uint16_t distance_mm_x4 = decodeUInt16(samples[idx].distance_mm_x4);
float distance_mm = distance_mm_x4 * 0.25;
float quality = scan_packet.sample[idx].quality;
float quality = samples[idx].quality;
postScanPoint(angle_deg, distance_mm, quality, scan_completed);
scan_completed = false;
}
Expand All @@ -230,7 +252,7 @@ LDS::result_t LDS_DELTA_2A::processByte(uint8_t c) {
return result;
}

uint16_t LDS_DELTA_2A::decodeUInt16(const uint16_t value) const {
uint16_t LDS_DELTA_2A_115200::decodeUInt16(const uint16_t value) const {
union {
uint16_t i;
char c[2];
Expand All @@ -239,4 +261,10 @@ uint16_t LDS_DELTA_2A::decodeUInt16(const uint16_t value) const {
return bint.c[0] == 0x01 ? value : (value << 8) + (value >> 8);
}

const char* LDS_DELTA_2A::getModelName() { return "3irobotics Delta-2A"; }
const char* LDS_DELTA_2A_115200::getModelName() {
return "3irobotics Delta-2A 115200 baud";
}

uint8_t LDS_DELTA_2A_115200::get_packets_per_scan(){
return 16;
}
22 changes: 10 additions & 12 deletions src/LDS_DELTA_2A.h → src/LDS_DELTA_2A_115200.h
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,7 @@
#include "LDS.h"
#include "PID_v1_0_0.h"

class LDS_DELTA_2A : public LDS {
class LDS_DELTA_2A_115200 : public LDS {
public:
virtual void init() override;

Expand All @@ -36,23 +36,18 @@ class LDS_DELTA_2A : public LDS {
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 {
struct packet_header_t {
uint8_t start_byte; // 0xAA
uint16_t packet_length; // excludes checksum
uint8_t protocol_version; // 0x01
Expand All @@ -64,13 +59,16 @@ class LDS_DELTA_2A : public LDS {
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;
// 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);
virtual LDS::result_t processByte(uint8_t c);
virtual uint16_t get_max_data_sample_count();
virtual float get_default_scan_freq_hz();
virtual uint8_t get_packets_per_scan();

uint16_t decodeUInt16(const uint16_t value) const;

float scan_freq_hz_setpoint;
Expand All @@ -81,7 +79,7 @@ class LDS_DELTA_2A : public LDS {
float scan_freq_hz;
PID_v1 scanFreqPID;

scan_packet_t scan_packet;
uint16_t parser_idx;
uint16_t checksum;
uint8_t * rx_buffer;
};
31 changes: 31 additions & 0 deletions src/LDS_DELTA_2A_230400.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,31 @@
// 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_230400.h"

const char* LDS_DELTA_2A_230400::getModelName() {
return "3irobotics Delta-2A";
}

uint32_t LDS_DELTA_2A_230400::getSerialBaudRate() {
return 230400;
}

uint16_t LDS_DELTA_2A_230400::get_max_data_sample_count() {
return 61;
}

float LDS_DELTA_2A_230400::get_default_scan_freq_hz() {
return 6.0f;
}
25 changes: 25 additions & 0 deletions src/LDS_DELTA_2A_230400.h
Original file line number Diff line number Diff line change
@@ -0,0 +1,25 @@
// 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_DELTA_2A_115200.h"

class LDS_DELTA_2A_230400 : public LDS_DELTA_2A_115200 {
public:
const char* getModelName() override;
uint32_t getSerialBaudRate();
protected:
uint16_t get_max_data_sample_count();
float get_default_scan_freq_hz();
};
27 changes: 27 additions & 0 deletions src/LDS_DELTA_2B.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,27 @@
// 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_2B.h"

const char* LDS_DELTA_2B::getModelName() {
return "3irobotics Delta-2B";
}

uint32_t LDS_DELTA_2B::getSerialBaudRate() {
return 230400;
}

float LDS_DELTA_2B::get_default_scan_freq_hz() {
return 6.0f;
}
Loading

0 comments on commit dceb859

Please sign in to comment.