Skip to content

Commit

Permalink
Rework arduino/esp32 serail (not tested)
Browse files Browse the repository at this point in the history
  • Loading branch information
@sashacmc
sashacmc committed Dec 9, 2024
1 parent addd19e commit 7add668
Show file tree
Hide file tree
Showing 2 changed files with 38 additions and 108 deletions.
6 changes: 5 additions & 1 deletion include/zenoh-pico/system/platform/arduino/esp32.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,7 +47,11 @@ typedef struct {
BluetoothSerial *_bts; // As pointer to cross the boundary between C and C++
#endif
#if Z_FEATURE_LINK_SERIAL == 1
HardwareSerial *_serial; // As pointer to cross the boundary between C and C++
struct {
HardwareSerial *_serial; // As pointer to cross the boundary between C and C++
uint8_t *tmp_buf;
uint8_t *raw_buf;
};
#endif
};
} _z_sys_net_socket_t;
Expand Down
140 changes: 33 additions & 107 deletions src/system/arduino/esp32/network.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,8 @@ extern "C" {
#include "zenoh-pico/collections/slice.h"
#include "zenoh-pico/collections/string.h"
#include "zenoh-pico/config.h"
#include "zenoh-pico/protocol/codec/serial.h"
#include "zenoh-pico/system/common/serial.h"
#include "zenoh-pico/system/link/bt.h"
#include "zenoh-pico/system/link/serial.h"
#include "zenoh-pico/system/platform.h"
Expand Down Expand Up @@ -687,8 +689,6 @@ z_result_t _z_open_serial_from_pins(_z_sys_net_socket_t *sock, uint32_t txpin, u
}

z_result_t _z_open_serial_from_dev(_z_sys_net_socket_t *sock, char *dev, uint32_t baudrate) {
z_result_t ret = _Z_RES_OK;

uint8_t uart = 255;
uint32_t rxpin = 0;
uint32_t txpin = 0;
Expand All @@ -705,28 +705,26 @@ z_result_t _z_open_serial_from_dev(_z_sys_net_socket_t *sock, char *dev, uint32_
rxpin = 16;
txpin = 17;
} else {
ret = _Z_ERR_GENERIC;
return _Z_ERR_GENERIC;
}

if (ret == _Z_RES_OK) {
// WARNING: Glitch on the very 1st byte is common when the serial input line is allowed to float.
// To minimize this issue the RxPin is set to INPUT_PULLUP, and set the TxPin is driven to HIGH. This
// will keep the pins high and upon initialization of the serial port using serial.begin() the line
// does not float (drops to low).
pinMode(rxpin, INPUT_PULLUP);
pinMode(txpin, OUTPUT);
digitalWrite(txpin, HIGH);

sock->_serial = new HardwareSerial(uart);
if (sock->_serial != NULL) {
sock->_serial->begin(baudrate);
sock->_serial->flush();
} else {
ret = _Z_ERR_GENERIC;
}
// WARNING: Glitch on the very 1st byte is common when the serial input line is allowed to float.
// To minimize this issue the RxPin is set to INPUT_PULLUP, and set the TxPin is driven to HIGH. This
// will keep the pins high and upon initialization of the serial port using serial.begin() the line
// does not float (drops to low).
pinMode(rxpin, INPUT_PULLUP);
pinMode(txpin, OUTPUT);
digitalWrite(txpin, HIGH);

sock->_serial = new HardwareSerial(uart);
if (sock->_serial != NULL) {
sock->_serial->begin(baudrate);
sock->_serial->flush();
} else {
return _Z_ERR_GENERIC;
}

return ret;
return _z_connect_serial(*sock);
}

z_result_t _z_listen_serial_from_pins(_z_sys_net_socket_t *sock, uint32_t txpin, uint32_t rxpin, uint32_t baudrate) {
Expand Down Expand Up @@ -757,112 +755,40 @@ z_result_t _z_listen_serial_from_dev(_z_sys_net_socket_t *sock, char *dev, uint3
void _z_close_serial(_z_sys_net_socket_t *sock) {
sock->_serial->end();
delete sock->_serial;
z_free(sock->tmp_buf);
z_free(sock->raw_buf);
}

size_t _z_read_serial(const _z_sys_net_socket_t sock, uint8_t *ptr, size_t len) {
z_result_t ret = _Z_RES_OK;

uint8_t *before_cobs = new uint8_t[_Z_SERIAL_MAX_COBS_BUF_SIZE]();
size_t _z_read_serial_internal(const _z_sys_net_socket_t sock, uint8_t *header, uint8_t *ptr, size_t len) {
size_t rb = 0;
for (size_t i = 0; i < _Z_SERIAL_MAX_COBS_BUF_SIZE; i++) {
while (sock._serial->available() < 1) {
z_sleep_ms(1); // FIXME: Yield by sleeping.
}
before_cobs[i] = sock._serial->read();
sock.raw_buf[i] = sock._serial->read();
rb = rb + (size_t)1;
if (before_cobs[i] == (uint8_t)0x00) {
if (sock.raw_buf[i] == (uint8_t)0x00) {
break;
}
}

uint8_t *after_cobs = new uint8_t[_Z_SERIAL_MFS_SIZE]();
size_t trb = _z_cobs_decode(before_cobs, rb, after_cobs);

size_t i = 0;
uint16_t payload_len = 0;
for (i = 0; i < sizeof(payload_len); i++) {
payload_len |= (after_cobs[i] << ((uint8_t)i * (uint8_t)8));
}

if (trb == (size_t)(payload_len + (uint16_t)6)) {
(void)memcpy(ptr, &after_cobs[i], payload_len);
i = i + (size_t)payload_len;

uint32_t crc = 0;
for (uint8_t j = 0; j < sizeof(crc); j++) {
crc |= (uint32_t)(after_cobs[i] << (j * (uint8_t)8));
i = i + (size_t)1;
}

uint32_t c_crc = _z_crc32(ptr, payload_len);
if (c_crc != crc) {
ret = _Z_ERR_GENERIC;
}
} else {
ret = _Z_ERR_GENERIC;
}

delete before_cobs;
delete after_cobs;

rb = payload_len;
if (ret != _Z_RES_OK) {
rb = SIZE_MAX;
}

return rb;
return _z_serial_msg_deserialize(sock.raw_buf, rb, ptr, len, header, sock.tmp_buf, _Z_SERIAL_MFS_SIZE);
}

size_t _z_read_exact_serial(const _z_sys_net_socket_t sock, uint8_t *ptr, size_t len) {
size_t n = len;
size_t rb = 0;

do {
rb = _z_read_serial(sock, ptr, n);
if (rb == SIZE_MAX) return rb;

n -= rb;
ptr = ptr + (len - n);
} while (n > 0);
size_t _z_send_serial_internal(const _z_sys_net_socket_t sock, uint8_t header, const uint8_t *ptr, size_t len) {
size_t ret = _z_serial_msg_serialize(sock.raw_buf, _Z_SERIAL_MAX_COBS_BUF_SIZE, ptr, len, header, sock.tmp_buf,
_Z_SERIAL_MFS_SIZE);

return len;
}

size_t _z_send_serial(const _z_sys_net_socket_t sock, const uint8_t *ptr, size_t len) {
z_result_t ret = _Z_RES_OK;

uint8_t *before_cobs = (uint8_t *)z_malloc(_Z_SERIAL_MFS_SIZE);
size_t i = 0;
for (i = 0; i < sizeof(uint16_t); ++i) {
before_cobs[i] = (len >> (i * (size_t)8)) & (size_t)0XFF;
}

(void)memcpy(&before_cobs[i], ptr, len);
i = i + len;

uint32_t crc = _z_crc32(ptr, len);
for (uint8_t j = 0; j < sizeof(crc); j++) {
before_cobs[i] = (crc >> (j * (uint8_t)8)) & (uint32_t)0XFF;
i = i + (size_t)1;
if (ret == SIZE_MAX) {
return ret;
}

uint8_t *after_cobs = (uint8_t *)z_malloc(_Z_SERIAL_MAX_COBS_BUF_SIZE);
ssize_t twb = _z_cobs_encode(before_cobs, i, after_cobs);
after_cobs[twb] = 0x00; // Manually add the COBS delimiter
ssize_t wb = sock._serial->write(after_cobs, twb + (ssize_t)1);
if (wb != (twb + (ssize_t)1)) {
ret = _Z_ERR_GENERIC;
ssize_t wb = sock._serial->write(sock.raw_buf, ret);
if (wb != (ssize_t)ret) {
ret = SIZE_MAX;
}

z_free(before_cobs);
z_free(after_cobs);

size_t sb = len;
if (ret != _Z_RES_OK) {
sb = SIZE_MAX;
}

return sb;
return len;
}
#endif

Expand Down

0 comments on commit 7add668

Please sign in to comment.