[EDIT LOG]
6-7-2022:
- add configurable WLED RTN (return to normal) setting
- expand upon max_speed/TDAT_refresh_rate relationship
- add configurable LED’s per meter as different strip options impact distance/LED_spacing
- completely revised WLED UDP Send
The YAML
esphome:
name: olimex-esp32-poe
on_boot:
priority: 700
then:
- wait_until:
api.connected:
- script.execute: get_all_params_script
- switch.template.publish:
id: detection_enabled
state: ON
- switch.template.publish:
id: enable_logging
state: OFF
includes:
- kld7.h
esp32:
board: esp32-poe-iso
framework:
type: arduino
substitutions:
device_name: olimex-esp32-poe
button:
- platform: restart
name: Restart $device_name
- platform: template
name: factory_reset_kld7 # RFSE
id: factory_reset_kld7
on_press:
- uart.write: [0x52, 0x46, 0x53, 0x45, 0x00, 0x00, 0x00, 0x00]
- delay: 2s
- uart.write: [0x49,0x4E,0x49,0x54,0x04,0x00,0x00,0x00,0x00,0x00,0x00,0x00] # perform kld7 INIT
- platform: template
name: get_all_params # GRPS
id: get_all_params
on_press:
- script.execute: get_all_params_script
script:
- id: get_all_params_script
then:
- uart.write: [0x47, 0x52, 0x50, 0x53, 0x00, 0x00, 0x00, 0x00]
web_server:
port: 80
version: 2
include_internal: true
ota: false
# Enable logging
logger:
# baud_rate: 0 # disable logging over uart
logs:
sensor: ERROR
# Enable Home Assistant API
api:
mdns:
disabled: false
ota:
password: "yadda"
ethernet:
type: LAN8720
mdc_pin: GPIO23
mdio_pin: GPIO18
clk_mode: GPIO17_OUT
phy_addr: 0
power_pin: GPIO12
switch:
- platform: safe_mode
name: use_safe_mode $device_name
- platform: template
name: enable_logging
id: enable_logging
optimistic: true
- platform: template
name: detection_enabled
id: detection_enabled
optimistic: true
restore_state: true
turn_on_action:
- uart.write: [0x49,0x4E,0x49,0x54,0x04,0x00,0x00,0x00,0x00,0x00,0x00,0x00] # perform kld7 INIT
- logger.log: "INIT sent"
turn_off_action:
- uart.write: [0x47, 0x42, 0x59, 0x45, 0x00, 0x00, 0x00, 0x00] # perform kld7 GBYE
number:
- platform: template
name: WLED_return_to_normal
id: WLED_return_to_normal
min_value: 1
max_value: 15
initial_value: 1
step: 1
restore_value: true
optimistic: true
- platform: template
name: LEDs_per_Meter
id: LEDs_per_Meter
min_value: 30
max_value: 144
initial_value: 60
step: 1
restore_value: true
optimistic: true
mode: box
- platform: template
name: distance_offset
id: distance_offset
min_value: -255
max_value: 255
initial_value: 0
step: 1
restore_value: true
unit_of_measurement: cm
optimistic: true
mode: box
- platform: template
name: max_speed # RSPI 0 = 12.5km/h, 1 = 25km/h, 2 = 50km/h, 3 = 100km/h
id: max_speed
min_value: 0
max_value: 3
initial_value: 1
step: 1
restore_value: true
unit_of_measurement: kph
set_action:
- uart.write: !lambda
return {0x52, 0x53, 0x50, 0x49, 0x04, 0x00, 0x00, 0x00, (unsigned char)x, 0x00, 0x00, 0x00};
- platform: template
name: max_range # RRAI 0 = 5m, 1 = 10m, 2 = 30m, 3 = 100m
id: max_range
min_value: 0
max_value: 3
initial_value: 1
step: 1
restore_value: true
set_action:
- uart.write: !lambda
return {0x52, 0x52, 0x41, 0x49, 0x04, 0x00, 0x00, 0x00, (unsigned char)x, 0x00, 0x00, 0x00};
- platform: template
name: threshold_offset # THOF 10-60 dB
id: threshold_offset
min_value: 10
max_value: 60
initial_value: 30
step: 1
restore_value: true
unit_of_measurement: dB
set_action:
- uart.write: !lambda
return {0x54, 0x48, 0x4F, 0x46, 0x04, 0x00, 0x00, 0x00, (unsigned char)x, 0x00, 0x00, 0x00};
- platform: template
name: tracking_filter # TRFT 0 = Standard, 1 = Fast detection, 2 = Long visibility
id: tracking_filter
min_value: 0
max_value: 2
initial_value: 0
step: 1
restore_value: true
set_action:
- uart.write: !lambda
return {0x54, 0x52, 0x46, 0x54, 0x04, 0x00, 0x00, 0x00, (unsigned char)x, 0x00, 0x00, 0x00};
- platform: template
name: vibration_suppression # VISU 0-16, 0 = No suppression, 16 = High suppression
id: vibration_suppression
min_value: 0
max_value: 16
initial_value: 2
step: 1
restore_value: true
set_action:
- uart.write: !lambda
return {0x56, 0x49, 0x53, 0x55, 0x04, 0x00, 0x00, 0x00, (unsigned char)x, 0x00, 0x00, 0x00};
- platform: template
name: min_detection_dist # MIRA
id: min_detection_dist
min_value: 0
max_value: 100
initial_value: 0
step: 1
restore_value: true
unit_of_measurement: "%"
mode: box
set_action:
- uart.write: !lambda
return {0x4d, 0x49, 0x52, 0x41, 0x04, 0x00, 0x00, 0x00, (unsigned char)x, 0x00, 0x00, 0x00};
- platform: template
name: max_detection_dist # MARA
id: max_detection_dist
min_value: 0
max_value: 100
initial_value: 50
step: 1
restore_value: true
unit_of_measurement: "%"
mode: box
set_action:
- uart.write: !lambda
return {0x4d, 0x41, 0x52, 0x41, 0x04, 0x00, 0x00, 0x00, (unsigned char)x, 0x00, 0x00, 0x00};
- platform: template
name: min_detection_angle # MIAN -90° to +90°
id: min_detection_angle
min_value: -90
max_value: 90
initial_value: -90
step: 1
restore_value: true
unit_of_measurement: "°"
mode: box
set_action:
- uart.write: !lambda
return {0x4d, 0x49, 0x41, 0x4e, 0x04, 0x00, 0x00, 0x00, (unsigned char)x, 0x00, 0x00, 0x00};
- platform: template
name: max_detection_angle # MAAN -90° to +90°
id: max_detection_angle
min_value: -90
max_value: 90
initial_value: 90
step: 1
restore_value: true
unit_of_measurement: "°"
mode: box
set_action:
- uart.write: !lambda
return {0x4d, 0x41, 0x41, 0x4e, 0x04, 0x00, 0x00, 0x00, (unsigned char)x, 0x00, 0x00, 0x00};
- platform: template
name: min_detection_speed # MISP 0 – 100% of speed setting
id: min_detection_speed
min_value: 0
max_value: 100
initial_value: 0
step: 1
restore_value: true
unit_of_measurement: "%"
mode: box
set_action:
- uart.write: !lambda
return {0x4d, 0x49, 0x53, 0x50, 0x04, 0x00, 0x00, 0x00, (unsigned char)x, 0x00, 0x00, 0x00};
- platform: template
name: max_detection_speed # MASP 0 – 100% of speed setting
id: max_detection_speed
min_value: 0
max_value: 100
initial_value: 100
step: 1
restore_value: true
unit_of_measurement: "%"
mode: box
set_action:
- uart.write: !lambda
return {0x4d, 0x41, 0x53, 0x50, 0x04, 0x00, 0x00, 0x00, (unsigned char)x, 0x00, 0x00, 0x00};
- platform: template
name: detection_direction # DEDI 0 = Receding, 1 = Approaching, 2 = Both
id: detection_direction
min_value: 0
max_value: 2
initial_value: 2
step: 1
restore_value: true
set_action:
- uart.write: !lambda
return {0x44, 0x45, 0x44, 0x49, 0x04, 0x00, 0x00, 0x00, (unsigned char)x, 0x00, 0x00, 0x00};
- platform: template
name: range_threshold # RATH 0 – 100% of range setting
id: range_threshold
min_value: 0
max_value: 100
initial_value: 10
step: 1
restore_value: true
unit_of_measurement: "%"
mode: box
set_action:
- uart.write: !lambda
return {0x52, 0x41, 0x54, 0x48, 0x04, 0x00, 0x00, 0x00, (unsigned char)x, 0x00, 0x00, 0x00};
- platform: template
name: angle_threshold # ANTH -90° to +90°
id: angle_threshold
min_value: -90
max_value: 90
initial_value: 0
step: 1
restore_value: true
unit_of_measurement: "°"
mode: box
set_action:
- uart.write: !lambda
return {0x41, 0x4e, 0x54, 0x48, 0x04, 0x00, 0x00, 0x00, (unsigned char)x, 0x00, 0x00, 0x00};
- platform: template
name: speed_threshold # SPTH 0 – 100% of speed setting
id: speed_threshold
min_value: 0
max_value: 100
initial_value: 50
step: 1
restore_value: true
unit_of_measurement: "%"
mode: box
set_action:
- uart.write: !lambda
return {0x53, 0x50, 0x54, 0x48, 0x04, 0x00, 0x00, 0x00, (unsigned char)x, 0x00, 0x00, 0x00};
- platform: template
name: hold # HOLD 0 – 7200s 484f4c44
id: hold
min_value: 1
max_value: 7200
initial_value: 1
unit_of_measurement: sec
step: 1
restore_value: true
mode: box
set_action:
- uart.write: !lambda
return {0x48, 0x4f, 0x4c, 0x44, 0x04, 0x00, 0x00, 0x00, (unsigned char)x, 0x00, 0x00, 0x00};
sensor:
- platform: custom
lambda: |-
auto s = new kld7(id(uart_bus0));
s->ip = "192.168.0.119";
s->port = 21324;
App.register_component(s);
return {s->distance_sensor, s->speed_sensor, s->angle_sensor, s->db_sensor};
sensors:
- name: distance_sensor
id: distance_sensor
internal: true
unit_of_measurement: cm
- name: speed_sensor
id: speed_sensor
internal: true
unit_of_measurement: kph
- name: angle_sensor
id: angle_sensor
internal: true
unit_of_measurement: deg
- name: db_sensor
id: db_sensor
internal: true
unit_of_measurement: dB
uart:
id: uart_bus0
rx_pin: GPIO36
tx_pin: GPIO4
baud_rate: 115200
parity: EVEN
binary_sensor:
- platform: gpio
name: kld7_detection
id: kld7_detection
pin:
number: GPIO05
mode: INPUT_PULLDOWN
on_state:
then:
while:
condition:
- binary_sensor.is_on: kld7_detection
- switch.is_on: detection_enabled
then:
- uart.write: [0x47,0x4E,0x46,0x44,0x04,0x00,0x00,0x00,0x08,0x00,0x00,0x00] # Get TDAT
- delay: !lambda
if (id(max_speed).state == 0) {
return 250;
} else if (id(max_speed).state == 1) {
return 125;
} else if (id(max_speed).state == 2) {
return 65;
} else if (id(max_speed).state == 3) {
return 35;
} else {
return 250;
}
kld7.h
#include "esphome.h"
#include "WiFiUdp.h"
#include <sstream>
class kld7 : public Component, public UARTDevice
{
public:
std::string ip;
uint16_t port;
kld7(UARTComponent *parent) : UARTDevice(parent) {}
Sensor *distance_sensor = new Sensor();
Sensor *speed_sensor = new Sensor();
Sensor *angle_sensor = new Sensor();
Sensor *db_sensor = new Sensor();
WiFiUDP UDP;
std::vector<uint8_t> bytes;
uint16_t oldLedOnIdx;
std::vector<uint16_t> oldLedsOn;
uint16_t newLedOnIdx;
std::vector<uint16_t> newLedsOn;
uint8_t onPacket[7] = {4, 1, 0, 0, 255, 5, 255};
uint8_t offPacket[7] = {4, 1};
bool doesHeaderMatch(std::vector<uint8_t> bytes, std::string header, int payloadLength)
{
if (bytes[0] != header[0] || bytes[1] != header[1] || bytes[2] != header[2] || bytes[3] != header[3])
{
return false;
}
return bytes[4] == payloadLength && bytes[5] == 0 && bytes[6] == 0 && bytes[7] == 0;
}
std::vector<uint16_t> fillVector(uint16_t firstIdx, uint16_t secondIdx)
{
uint16_t temp;
if (firstIdx > secondIdx)
{
temp = firstIdx;
firstIdx = secondIdx;
secondIdx = temp;
}
std::vector<uint16_t> result;
for (int i = firstIdx; i <= secondIdx; i++)
{
result.push_back(i);
}
return result; // always ascending
}
void setLedState(uint16_t index, bool state)
{
uint8_t *packet;
packet = state ? onPacket : offPacket;
packet[1] = (uint8_t)id(WLED_return_to_normal).state; //RTN
packet[2] = (uint8_t)(index >> 8);
packet[3] = (uint8_t)index;
IPAddress ipaddr;
ipaddr.fromString(ip.c_str());
UDP.beginPacket(ipaddr, port);
UDP.write(packet, 7);
UDP.endPacket();
}
std::vector<uint16_t> difference(std::vector<uint16_t> target, std::vector<uint16_t> reference, bool reverse)
{
std::vector<uint16_t> diff;
// make sure the vector is ascending. This only works because the vector is already sorted
if (target.size() > 1 && target[1] < target[0])
{
std::reverse(target.begin(), target.end());
}
if (reference.size() > 1 && reference[1] < reference[0])
{
std::reverse(reference.begin(), reference.end());
}
std::set_difference(reference.begin(), reference.end(), target.begin(), target.end(), std::back_inserter(diff));
if (reverse)
{
std::reverse(diff.begin(), diff.end());
}
return diff;
}
void loop() override
{
// ESP_LOGV("custom", "There is no byte: %i", bytes.size());
while (available() > 0)
{
bytes.push_back(read());
// ESP_LOGD("custom", "Show me the bytes: %i", bytes.size());
if (bytes.size() < 8)
{
continue;
}
// ESP_LOGD("custom", "Size of bytes: %i", bytes.size()); // whelp..
if (doesHeaderMatch(bytes, "TDAT", sizeof(TDAT)))
{
// ESP_LOGD("custom", "Size of TDAT: %i", sizeof(TDAT));
// ESP_LOGD("custom", "Size of TDAT bytes: %i", bytes.size());
if (bytes.size() < 8 + sizeof(TDAT))
{
continue;
}
TDATUnion tdatUnion;
std::copy(bytes.begin() + 8, bytes.end(), tdatUnion.bytes);
// ESP_LOGD("custom", "Sending TDAT");
distance_sensor->publish_state(tdatUnion.tdat.distance);
speed_sensor->publish_state(tdatUnion.tdat.speed / 100);
angle_sensor->publish_state(tdatUnion.tdat.angle / 100);
db_sensor->publish_state(tdatUnion.tdat.db / 100);
newLedOnIdx = (id(LEDs_per_Meter).state / 100) * (tdatUnion.tdat.distance - id(distance_offset).state);
//momentum
/*
uint16_t diffLength = newLedOnIdx - oldLedOnIdx;
if (diffLength < -3 || diffLength > 3)
{
int newMomentumIdx = newLedOnIdx + (int)(diffLength * 0.5);
if (newMomentumIdx < 0)
{
newLedOnIdx = 0;
}
else
{
newLedOnIdx = newMomentumIdx;
}
}
*/
newLedsOn = fillVector(oldLedOnIdx, newLedOnIdx);
// get direction of travel
bool movingRtl = newLedOnIdx < oldLedOnIdx;
std::vector<uint16_t> toTurnOff = difference(newLedsOn, oldLedsOn, movingRtl);
std::vector<uint16_t> toTurnOn = difference(oldLedsOn, newLedsOn, movingRtl);
for (auto offIdx : toTurnOff)
{
setLedState(offIdx, false);
}
for (auto onIdx : toTurnOn)
{
setLedState(onIdx, true);
}
oldLedOnIdx = newLedOnIdx;
oldLedsOn = newLedsOn;
bytes.clear();
}
else if (doesHeaderMatch(bytes, "RPST", sizeof(RPST)))
{
// ESP_LOGD("custom", "Size of RPST: %i", sizeof(RPST));
// ESP_LOGD("custom", "Size of RPST bytes: %i", bytes.size());
if (bytes.size() < 8 + sizeof(RPST))
{
continue;
}
RPSTUnion rpstUnion;
std::copy(bytes.begin() + 8, bytes.end(), rpstUnion.bytes);
// ESP_LOGD("custom", "Sending RPST");
id(max_speed).publish_state(rpstUnion.rpst.max_speed);
id(max_range).publish_state(rpstUnion.rpst.max_range);
id(threshold_offset).publish_state(rpstUnion.rpst.threshold_offset);
id(tracking_filter).publish_state(rpstUnion.rpst.tracking_filter);
id(vibration_suppression).publish_state(rpstUnion.rpst.vibration_suppression);
id(min_detection_dist).publish_state(rpstUnion.rpst.min_detection_dist);
id(max_detection_dist).publish_state(rpstUnion.rpst.max_detection_dist);
id(min_detection_angle).publish_state(rpstUnion.rpst.min_detection_angle);
id(max_detection_angle).publish_state(rpstUnion.rpst.max_detection_angle);
id(min_detection_speed).publish_state(rpstUnion.rpst.min_detection_speed);
id(max_detection_speed).publish_state(rpstUnion.rpst.max_detection_speed);
id(detection_direction).publish_state(rpstUnion.rpst.detection_direction);
id(range_threshold).publish_state(rpstUnion.rpst.range_threshold);
id(angle_threshold).publish_state(rpstUnion.rpst.angle_threshold);
id(speed_threshold).publish_state(rpstUnion.rpst.speed_threshold);
id(hold).publish_state(rpstUnion.rpst.hold);
bytes.clear();
}
else
{
bytes.erase(bytes.begin());
continue;
}
}
}
typedef struct
{
char version[19]; // Software Version STRING 19 Zero-terminated String K-LD7_APP-RFB-XXXX
uint8_t base_frequency; // Base frequency UINT8 1 0 = Low, 1 = Middle, 2 = High 1 = Middle
uint8_t max_speed; // Maximum speed UINT8 1 0 = 12.5km/h, 1 = 25km/h, 2 = 50km/h, 3 = 100km/h 1 = 25km/h
uint8_t max_range; // Maximum range UINT8 1 0 = 5m, 1 = 10m, 2 = 30m, 3 = 100m 1 = 10m
uint8_t threshold_offset; // Threshold offset UINT8 1 10-60 dB 30 dB
uint8_t tracking_filter; // Tracking filter type UINT8 1 0 = Standard, 1 = Fast detection, 2 = Long visibility 0 = Standard
uint8_t vibration_suppression; // Vibration suppression UINT8 1 0-16, 0 = No suppression, 16 = High suppression 2 = Low suppression
uint8_t min_detection_dist; // Minimum detection distance UINT8 1 0–100% of range setting 0%
uint8_t max_detection_dist; // Maximum detection distance UINT8 1 0–100% of range setting 50%
int8_t min_detection_angle; // Minimum detection angle INT8 1 -90° to +90° -90°
int8_t max_detection_angle; // Maximum detection angle INT8 1 -90° to +90° +90°
uint8_t min_detection_speed; // Minimum detection speed UINT8 1 0–100% of speed setting 0%
uint8_t max_detection_speed; // Maximum detection speed UINT8 1 0–100% of speed setting 100%
uint8_t detection_direction; // Detection direction UINT8 1 0 = Receding, 1 = Approaching, 2 = Both 2 = Both
uint8_t range_threshold; // Range threshold UINT8 1 0–100% of range setting 10%
int8_t angle_threshold; // Angle threshold INT8 1 -90° to +90° 0°
uint8_t speed_threshold; // Speed threshold UINT8 1 0–100% of speed setting 50%
uint8_t IO1; // Digital output 1 UINT8 1 0 =Direction, 1 = Angle, 2 = Range, 3 = Speed, 4 = Micro detection 0 = Direction
uint8_t IO2; // Digital output 2 UINT8 1 0 =Direction, 1 = Angle, 2 = Range, 3 = Speed, 4 = Micro detection 1 = Angle
uint8_t IO3; // Digital output 3 UINT8 1 0 =Direction, 1 = Angle, 2 = Range, 3 = Speed, 4 = Micro detection 2 = Range
uint16_t hold; // Hold time UINT16 2 1–7200s (1s–2h) 1s
uint8_t micro_retrigger; // Micro detection retrigger UINT8 1 0 = Off, 1 = Retrigger 0 = Off
uint8_t micro_sensitivity; // Micro detection sensitivity UINT8
} RPST;
typedef union
{
RPST rpst;
uint8_t bytes[sizeof(RPST)];
} RPSTUnion;
typedef struct
{
uint16_t distance;
int16_t speed;
int16_t angle;
uint16_t db;
} TDAT;
typedef union
{
TDAT tdat;
uint8_t bytes[sizeof(TDAT)];
} TDATUnion;
};