diff --git a/.gitignore b/.gitignore index 7ff6f19..8437e6d 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,8 @@ *-backups fw/.pio +fw-can/ +fw-cyphal/ +pcb/*-bak +pcb/*.lck +pyvenv.cfg +compile_commands.json diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000..b301240 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,3 @@ +[submodule "fw/lib/libcanard"] + path = fw/lib/libcanard + url = git@git.nilsschulte.de:nils/libcanard.git diff --git a/fw-can/platformio.ini b/fw-can/platformio.ini deleted file mode 100644 index 06afe7e..0000000 --- a/fw-can/platformio.ini +++ /dev/null @@ -1,35 +0,0 @@ -; PlatformIO Project Configuration File -; -; Build options: build flags, source filter -; Upload options: custom upload port, speed and extra flags -; Library options: dependencies, extra library storages -; Advanced options: extra scripting -; -; Please visit documentation for the other options and examples -; https://docs.platformio.org/page/projectconf.html - -[env:nilsdriverv1] -# platform = espressif32 -platform = https://github.com/cziter15/platform-espressif32.git -#platform = https://github.com/tasmota/platform-espressif32/releases/download/2023.10.10/platform-espressif32.zip -# board = adafruit_feather_esp32s3 -board = lolin_s3_mini -framework = arduino -upload_protocol = esptool -upload_port = /dev/ttyACM* -# board_build.mcu = esp32s3 -lib_archive = false -;platform_packages = espressif/toolchain-xtensa-esp32@8.4.0+2021r2-patch5 -monitor_speed = 115200 -lib_deps = -; https://github.com/simplefoc/Arduino-FOC.git#dev -; https://github.com/simplefoc/Arduino-FOC-drivers.git#dev -; https://github.com/handmade0octopus/ESP32-TWAI-CAN -; https://github.com/simplefoc/Arduino-FOC-dcmotor.git -; https://github.com/eric-wieser/packet-io.git -; https://git.nilsschulte.de/nils/simplesync.git -; https://github.com/Tinyu-Zhao/INA3221.git - -build_flags = -DRADIOLIB_EEPROM_UNSUPPORTED -;-DSIMPLEFOC_ESP32_USELEDC - diff --git a/fw-can/src/main.cpp b/fw-can/src/main.cpp deleted file mode 100644 index 1f27b85..0000000 --- a/fw-can/src/main.cpp +++ /dev/null @@ -1,181 +0,0 @@ -/* - Simple example of sending/receiving messages between the two CAN interfaces on - the ESP32-CAN-X2 dev board. https://wiki.autosportlabs.com/ESP32-CAN-X2 - - This example relies on the ESP32 supplied TWAI api for interfacing with CAN1, - and the Longan Labs mcp_canbus library for interfacing with the MCP2515 on - CAN2. -*/ - -#include "Arduino.h" -#include "SPI.h" -#include "USB.h" -#include "driver/twai.h" - -#define POLLING_RATE_MS 1 - -#define CAN1_ID 0xF6 -#define CAN2_ID 0xF7 - -#define CAN1_TX 45 -#define CAN1_RX 48 - -#define USBSERIAL -#ifdef USBSERIAL -USBCDC usbserial; -#define Serial usbserial -#endif - -static void sendCAN1() { - // Send message - // Configure message to transmit - twai_message_t message; - message.identifier = CAN1_ID; - message.extd = 0; - message.rtr = 0; - message.data_length_code = 4; - message.data[0] = (int8_t)'p'; - message.data[1] = (int8_t)'i'; - message.data[2] = (int8_t)'n'; - message.data[3] = (int8_t)'g'; - - // Queue message for transmission - if (twai_transmit(&message, pdMS_TO_TICKS(1000)) == ESP_OK) { - Serial.println("CAN1: Message queued for transmission"); - } else { - Serial.println("CAN1: Failed to queue message for transmission"); - } -} - -static void readCAN1() { - twai_message_t message; - while (twai_receive(&message, 0) == ESP_OK) { - - Serial.print("CAN1: Received "); - // Process received message - if (message.extd) { - Serial.print("extended "); - } else { - Serial.print("standard "); - } - - if (message.rtr) { - Serial.print("RTR "); - } - - Serial.printf("packet with id 0x%x", message.identifier); - - if (message.rtr) { - Serial.printf(" and requested length %d\n", message.data_length_code); - } else { - Serial.printf(" and length %d\n", message.data_length_code); - Serial.printf("CAN1: Data: %.*s\n", message.data_length_code, - message.data); - } - } -} - -void setup() { -#ifdef USBSERIAL - Serial.begin(); - USB.begin(); -#else - Serial.begin(460800, SERIAL_8N1, 44, 43); -#endif - delay(1000); - // pinMode(LED_BUILTIN, OUTPUT); - - Serial.println("Initializing builtin CAN peripheral"); - twai_general_config_t g_config = TWAI_GENERAL_CONFIG_DEFAULT( - (gpio_num_t)CAN1_TX, (gpio_num_t)CAN1_RX, TWAI_MODE_NORMAL); - g_config.rx_queue_len = 10; - g_config.tx_queue_len = 10; - twai_timing_config_t t_config = TWAI_TIMING_CONFIG_500KBITS(); - twai_filter_config_t f_config = TWAI_FILTER_CONFIG_ACCEPT_ALL(); - - if (twai_driver_install(&g_config, &t_config, &f_config) == ESP_OK) { - Serial.println("CAN1 Driver initialized"); - } else { - Serial.println("Failed to initialze CAN1 driver"); - return; - } - - if (twai_start() == ESP_OK) { - Serial.println("CAN1 interface started"); - } else { - Serial.println("Failed to start CAN1"); - return; - } - - uint32_t alerts_to_enable = TWAI_ALERT_TX_IDLE | TWAI_ALERT_TX_SUCCESS | - TWAI_ALERT_TX_FAILED | TWAI_ALERT_RX_QUEUE_FULL | - TWAI_ALERT_RX_DATA | TWAI_ALERT_ERR_PASS | - TWAI_ALERT_BUS_ERROR; - if (twai_reconfigure_alerts(alerts_to_enable, NULL) == ESP_OK) { - Serial.println("CAN1 Alerts reconfigured"); - } else { - Serial.println("Failed to reconfigure alerts"); - return; - } - - // if (CAN_OK == CAN.begin(CAN_500KBPS)) { - // Serial.println("CAN2 interface started"); - // } else { - // Serial.println("Failed to start CAN2"); - // while (1); - // } -} - -void loop() { - // Serial.println("MAIN:"); - // digitalWrite(LED_BUILTIN, HIGH); - // delay(1000); - - // Check if alert happened - uint32_t alerts_triggered; - twai_read_alerts(&alerts_triggered, pdMS_TO_TICKS(POLLING_RATE_MS)); - twai_status_info_t twaistatus; - twai_get_status_info(&twaistatus); - - // Handle alerts - if (alerts_triggered & TWAI_ALERT_ERR_PASS) { - Serial.println("CAN1: Alert: TWAI controller has become error passive."); - } - if (alerts_triggered & TWAI_ALERT_BUS_ERROR) { - Serial.println("CAN1: Alert: A (Bit, Stuff, CRC, Form, ACK) error has " - "occurred on the bus."); - Serial.printf("CAN1: Bus error count: %d\n", twaistatus.bus_error_count); - } - if (alerts_triggered & TWAI_ALERT_TX_FAILED) { - Serial.println("CAN1: Alert: The Transmission failed."); - Serial.printf("CAN1: TX buffered: %d\t", twaistatus.msgs_to_tx); - Serial.printf("CAN1: TX error: %d\t", twaistatus.tx_error_counter); - Serial.printf("CAN1: TX failed: %d\n", twaistatus.tx_failed_count); - } - if (alerts_triggered & TWAI_ALERT_RX_QUEUE_FULL) { - Serial.println("CAN1: Alert: The RX queue is full causing a received frame " - "to be lost."); - Serial.printf("CAN1: RX buffered: %d\t", twaistatus.msgs_to_rx); - Serial.printf("CAN1: RX missed: %d\t", twaistatus.rx_missed_count); - Serial.printf("CAN1: RX overrun %d\n", twaistatus.rx_overrun_count); - } - if (alerts_triggered & TWAI_ALERT_TX_SUCCESS) { - Serial.println("CAN1: Alert: The Transmission was successful."); - Serial.printf("CAN1: TX buffered: %d\n", twaistatus.msgs_to_tx); - } - // Check if message is received - if (alerts_triggered & TWAI_ALERT_RX_DATA) { - readCAN1(); - // twai_read_alerts(&alerts_triggered, pdMS_TO_TICKS(0)); - } - - // Send message - static int i = 0; - if (i++ % 1000 == 0) - sendCAN1(); - - // Serial.println("MAIN: Enable LED"); - // digitalWrite(LED_BUILTIN, LOW); - // delay(1000); - // readCAN2(); -} diff --git a/fw-cyphal/README.md b/fw-cyphal/README.md deleted file mode 100644 index 70c3e36..0000000 --- a/fw-cyphal/README.md +++ /dev/null @@ -1,5 +0,0 @@ -## Develop -create `compile_commands.json`: -´´´ -pio run -t compiledb -´´´ diff --git a/fw-cyphal/platformio.ini b/fw-cyphal/platformio.ini deleted file mode 100644 index 82604ea..0000000 --- a/fw-cyphal/platformio.ini +++ /dev/null @@ -1,35 +0,0 @@ -; PlatformIO Project Configuration File -; -; Build options: build flags, source filter -; Upload options: custom upload port, speed and extra flags -; Library options: dependencies, extra library storages -; Advanced options: extra scripting -; -; Please visit documentation for the other options and examples -; https://docs.platformio.org/page/projectconf.html - -[env:nilsdriverv1] -# platform = espressif32 -platform = https://github.com/cziter15/platform-espressif32.git#68ad40f6df654fe4b8d0a50982b810df5b49b677 -#platform = https://github.com/tasmota/platform-espressif32/releases/download/2023.10.10/platform-espressif32.zip -# board = adafruit_feather_esp32s3 -board = lolin_s3_mini -framework = arduino -upload_protocol = esptool -upload_port = /dev/ttyACM* -# board_build.mcu = esp32s3 -lib_archive = false -;platform_packages = espressif/toolchain-xtensa-esp32@8.4.0+2021r2-patch5 -monitor_speed = 115200 -lib_deps = - https://github.com/simplefoc/Arduino-FOC.git#dev - https://github.com/simplefoc/Arduino-FOC-drivers.git#dev - https://github.com/handmade0octopus/ESP32-TWAI-CAN -; https://github.com/simplefoc/Arduino-FOC-dcmotor.git -; https://github.com/eric-wieser/packet-io.git -; https://git.nilsschulte.de/nils/simplesync.git -; https://github.com/Tinyu-Zhao/INA3221.git - -build_flags = -DRADIOLIB_EEPROM_UNSUPPORTED -;-DSIMPLEFOC_ESP32_USELEDC - diff --git a/fw-cyphal/src/DRV8323S.hpp b/fw-cyphal/src/DRV8323S.hpp deleted file mode 100644 index cc9fe39..0000000 --- a/fw-cyphal/src/DRV8323S.hpp +++ /dev/null @@ -1,204 +0,0 @@ -#ifndef _DRV8323S_H -#define _DRV8323S_H - -#include -#include -#include -#include - -enum { - DRV8323S_FAULT_STATUS1_REG = 0x00, - DRV8323S_FAULT_STATUS2_REG = 0x01, - DRV8323S_DRIVER_CONTROL_REG = 0x02, - DRV8323S_GATE_DRIVE_HS_REG = 0x03, - DRV8323S_GATE_DRIVE_LS_REG = 0x04, - DRV8323S_OCP_CONTROL_REG = 0x05, - DRV8323S_CSA_CONTROL_REG = 0x06, -}; - -enum /* 0x00 */ { - DRV8323S_FAULT_STATUS1_FAULT_MASK = 1 << 10, /* logic or of all faults */ - DRV8323S_FAULT_STATUS1_VDS_OCP_MASK = 1 << 9, /* vds monitor overcurrent */ - DRV8323S_FAULT_STATUS1_GDF_MASK = 1 << 8, /* gate drive fault */ - DRV8323S_FAULT_STATUS1_UVLO_MASK = 1 << 7, /* undervoltage lockout fault */ - DRV8323S_FAULT_STATUS1_OTSD_MASK = 1 << 6, /* over temperature shutdown */ - DRV8323S_FAULT_STATUS1_VDS_HA_MASK = 1 << 5, /* overcurrent on FET */ - DRV8323S_FAULT_STATUS1_VDS_LA_MASK = 1 << 4, - DRV8323S_FAULT_STATUS1_VDS_HB_MASK = 1 << 3, - DRV8323S_FAULT_STATUS1_VDS_LB_MASK = 1 << 2, - DRV8323S_FAULT_STATUS1_VDS_HC_MASK = 1 << 1, - DRV8323S_FAULT_STATUS1_VDS_LC_MASK = 1 << 0, -}; - -enum /* 0x01 */ { - DRV8323S_FAULT_STATUS1_SA_OC_MASK = 1 << 10, /* Sense amp OC */ - DRV8323S_FAULT_STATUS1_SB_OC_MASK = 1 << 9, - DRV8323S_FAULT_STATUS1_SC_OC_MASK = 1 << 8, - DRV8323S_FAULT_STATUS1_OTW_MASK = 1 << 7, /* over temp warning */ - DRV8323S_FAULT_STATUS1_CPUV_MASK = 1 << 6, /* charge pump fault */ - DRV8323S_FAULT_STATUS1_VGS_HA_MASK = 1 << 5, /* FET gate drive failt */ - DRV8323S_FAULT_STATUS1_VGS_LA_MASK = 1 << 4, - DRV8323S_FAULT_STATUS1_VGS_HB_MASK = 1 << 3, - DRV8323S_FAULT_STATUS1_VGS_LB_MASK = 1 << 2, - DRV8323S_FAULT_STATUS1_VGS_HC_MASK = 1 << 1, - DRV8323S_FAULT_STATUS1_VGS_LC_MASK = 1 << 0, -}; - -enum /* 0x02 */ { - DRV8323S_DRIVER_CONTROL_DIS_CPUV_MASK = - 1 << 9, /* chrg pump uvlo fault disable */ - DRV8323S_DRIVER_CONTROL_DIS_GDF_MASK = 1 << 8, /* gate drive fault disable */ - DRV8323S_DRIVER_CONTROL_OTW_REP_MASK = 1 - << 7, /* over temp wariing reported */ - DRV8323S_DRIVER_CONTROL_PWM_MODE_MASK = 0b11 << 5, /* PWM mode */ - DRV8323S_DRIVER_CONTROL_1PWM_COM_MASK = 1 << 4, /* 1PWM mode diode freewheel*/ - DRV8323S_DRIVER_CONTROL_1PWM_DIR_MASK = 1 << 3, /* 1PWM mode direction */ - DRV8323S_DRIVER_CONTROL_COAST_MASK = 1 << 2, /* all FETs in Hi-Z */ - DRV8323S_DRIVER_CONTROL_BRAKE_MASK = 1 << 1, /* all low FETs on */ - DRV8323S_DRIVER_CONTROL_CLR_FLT_MASK = 1 << 1, /* clear latched fault bits */ -}; - -enum /* 0x03 */ { - DRV8323S_GATE_DRIVE_HS_LOCK_MASK = 0b111 << 8, /* lock register */ - DRV8323S_GATE_DRIVE_HS_IDRIVEP_HS_MASK = 0b1111 << 4, /* HS positiv current */ - DRV8323S_GATE_DRIVE_HS_IDRIVEN_HS_MASK = 0b1111 << 0, /* HS negativ current */ -}; - -enum /* 0x04 */ { - DRV8323S_GATE_DRIVE_LS_CBC_MASK = 1 << 10, /* cycle-by cycle operation */ - DRV8323S_GATE_DRIVE_LS_TDRIVE_MASK = 0b11 << 8, /* peak gate curr drive t*/ - DRV8323S_GATE_DRIVE_LS_IDRIVEP_LS_MASK = 0b1111 << 4, /* LS positiv current */ - DRV8323S_GATE_DRIVE_LS_IDRIVEN_LS_MASK = 0b1111 << 0, /* LS negativ current */ -}; - -enum /* 0x05 */ { - DRV8323S_OCP_CONTROL_TRETRY_MASK = 1 << 10, /* 0=4ms, 1=50us */ - DRV8323S_OCP_CONTROL_DEAD_TIME_MASK = 0b11 << 8, /* 50ns*2**x dead time */ - DRV8323S_OCP_CONTROL_OCP_MODE_MASK = 0b11 << 6, /* OCP Mode */ - DRV8323S_OCP_CONTROL_OCP_DEG_MASK = 0b11 << 4, /* OCP deglitch t=2ns*(1+x)*/ - DRV8323S_OCP_CONTROL_VDS_LVL_MASK = 0b1111 << 0, /* VDS LVL */ -}; - -enum /* 0x06 */ { - DRV8323S_CSA_CONTROL_CSA_FET_MASK = 1 << 10, /* 0:sense posi in = SPx 1:SHx */ - DRV8323S_CSA_CONTROL_VREF_DIV_MASK = 1 << 9, /* sense ref is Vref/2 */ - DRV8323S_CSA_CONTROL_LS_REF_MASK = 1 - << 8, /* 0:VDS_OCP over SHx-SPx 1:SHx-SNx*/ - DRV8323S_CSA_CONTROL_CSA_GAIN_MASK = 0b11 << 6, /* 2**x*5V/V */ - DRV8323S_CSA_CONTROL_DIS_SEN_MASK = 1 << 5, /* disable OC sense */ - DRV8323S_CSA_CONTROL_CSA_CAL_A_MASK = 1 << 4, /* cal sense offset A */ - DRV8323S_CSA_CONTROL_CSA_CAL_B_MASK = 1 << 3, /* cal sense offset B */ - DRV8323S_CSA_CONTROL_CSA_CAL_C_MASK = 1 << 2, /* cal sense offset C */ - DRV8323S_CSA_CONTROL_SEN_LVL_MASK = 0b11 << 6, /* Sense OCP = 0.25V*(1+x)*/ -}; - -enum drv8323s_pwm_mode_e { - DRV8323S_PWM_MODE_6PWM = 0b00 << 5, - DRV8323S_PWM_MODE_3PWM = 0b01 << 5, - DRV8323S_PWM_MODE_1PWM = 0b10 << 5, - DRV8323S_PWM_MODE_INDEPENDENT_PWM = 0b11 << 5, -}; - -enum drv8323s_lock_sequence_e { - DRV8323S_LOCK = 0b011 << 8, - DRV8323S_UNLOCK = 0b110 << 8, -}; - -enum drv8323s_idrive_shift_e { - DRV8323S_IDRIVEP = 4, - DRV8323S_IDRIVEN = 0, -}; - -unsigned int drv8323s_idrivep_mA_map[] = { - 10, 30, 60, 80, 120, 140, 170, 190, 260, 330, 370, 440, 570, 680, 820, 1000, -}; -unsigned int drv8323s_idriven_mA_map[] = { - 20, 60, 120, 160, 240, 280, 340, 380, - 520, 660, 740, 880, 1140, 1360, 1640, 2000, -}; - -struct drv8323s_foc_driver { - SPIClass *spi; - int CSn, en; - BLDCDriver3PWM *focdriver; -}; - -void drv8323s_init(struct drv8323s_foc_driver *dev, int phA, int phB, int phC, - int CSn, int en, SPIClass *spi) { - dev->CSn = CSn; - pinMode(CSn, OUTPUT); - dev->en = en; - pinMode(dev->en, OUTPUT); - digitalWrite(dev->en, 1); - // dev->focdriver = new BLDCDriver3PWM(phA, phB, phC, en); - dev->spi = spi; - - /* Set 3PWM Mode */ -} - -uint16_t drv8323s_read_spi(struct drv8323s_foc_driver *dev, uint8_t addr, - uint16_t *result) { - digitalWrite(dev->CSn, 0); - dev->spi->beginTransaction(SPISettings(500000, MSBFIRST, SPI_MODE0)); - uint16_t data =((uint16_t) (1 << 15)) | ((((uint16_t)addr) ) << 11); - *result = dev->spi->transfer16(data); - // *result = data; - dev->spi->endTransaction(); - digitalWrite(dev->CSn, 1); - // Serial.print("SPI Read Result: "); - // Serial.print(data, HEX); - // Serial.print(" -> "); - // Serial.println(result, HEX); - return 0; -} - -uint16_t drv8323s_write_spi(struct drv8323s_foc_driver *dev, uint8_t addr, - uint16_t value, uint16_t *result) { - digitalWrite(dev->CSn, 0); - dev->spi->beginTransaction(SPISettings(500000, MSBFIRST, SPI_MODE0)); - uint16_t data = ((addr & 0b1111) << 11) | (value & 0b1111111111); - *result = dev->spi->transfer16(data); - dev->spi->endTransaction(); - digitalWrite(dev->CSn, 1); - // Serial.print("SPI Write Result: "); - // Serial.print(data, HEX); - // Serial.print(" -> "); - // Serial.println(result, HEX); - return 0; -} - -void drv8323s_set_idrive(struct drv8323s_foc_driver *dev, int positive_i, - int negative_i, bool high_side_fet) { - uint16_t reg_value = 0x0; // TODO get reg - uint16_t reg = - high_side_fet ? DRV8323S_GATE_DRIVE_HS_REG : DRV8323S_GATE_DRIVE_LS_REG; - drv8323s_read_spi(dev, reg, ®_value); - if (high_side_fet) { - reg_value &= ~DRV8323S_GATE_DRIVE_HS_IDRIVEP_HS_MASK; - reg_value &= ~DRV8323S_GATE_DRIVE_HS_IDRIVEN_HS_MASK; - } else { - reg_value &= ~DRV8323S_GATE_DRIVE_LS_IDRIVEP_LS_MASK; - reg_value &= ~DRV8323S_GATE_DRIVE_LS_IDRIVEN_LS_MASK; - } - int i; - for (i = 0; - i < sizeof drv8323s_idrivep_mA_map / sizeof(drv8323s_idrivep_mA_map[0]); - i += 1) { - if (drv8323s_idrivep_mA_map[i] > positive_i) - break; - } - reg_value |= i << DRV8323S_IDRIVEP; - for (i = 0; - i < sizeof drv8323s_idriven_mA_map / sizeof(drv8323s_idriven_mA_map[0]); - i += 1) { - if (drv8323s_idriven_mA_map[i] > negative_i) - break; - } - reg_value |= i << DRV8323S_IDRIVEN; - uint16_t result; - drv8323s_write_spi(dev, reg, reg_value, &result); - - // TODO SPI call -} - -#endif /* _DRV8323S_H */ diff --git a/fw-cyphal/src/esp32twaican.h b/fw-cyphal/src/esp32twaican.h deleted file mode 100644 index ad281ea..0000000 --- a/fw-cyphal/src/esp32twaican.h +++ /dev/null @@ -1,84 +0,0 @@ -/// ____ ______ __ __ -/// / __ `____ ___ ____ / ____/_ ______ / /_ ____ -/// / / -/// / / / / __ `/ _ `/ __ `/ / / / / / __ `/ __ `/ __ -/// `/ / -/// / /_/ / /_/ / __/ / / / /___/ /_/ / /_/ / / / / /_/ -/// / / -/// `____/ .___/`___/_/ /_/`____/`__, / .___/_/ -/// /_/`__,_/_/ -/// /_/ /____/_/ -/// -/// This is a basic adapter library that bridges Libcanard with SocketCAN. -/// Read the API documentation for usage information. -/// -/// To integrate the library into your application, just copy-paste the c/h -/// files into your project tree. -/// -/// -------------------------------------------------------------------------------------------------------------------- -/// Changelog -/// -/// v3.0 - Update for compatibility with Libcanard v3. -/// -/// v2.0 - Added loop-back functionality. -/// API change in esp32twaicanPop(): loopback flag added. -/// - Changed to kernel-based time-stamping for received frames for -/// improved accuracy. -/// API change in esp32twaicanPop(): time stamp clock source is now -/// CLOCK_REALTIME, vs CLOCK_TAI before. -/// -/// v1.0 - Initial release -/// -------------------------------------------------------------------------------------------------------------------- -/// -/// This software is distributed under the terms of the MIT License. -/// Copyright (c) 2020 OpenCyphal -/// Author: Pavel Kirienko - -#ifndef ESP32TWAICAN_H_INCLUDED -#define ESP32TWAICAN_H_INCLUDED - -#include "canard.h" -#include -#include -#include - -#ifdef __cplusplus -extern "C" { -#endif - -int esp32twaicanOpen(const gpio_num_t pin_tx, const gpio_num_t pin_rx, - const CanardFilter *filter_config); - -/// Enqueue a new extended CAN data frame for transmission. -/// Block until the frame is enqueued or until the timeout is expired. -/// Zero timeout makes the operation non-blocking. -/// Returns 1 on success, 0 on timeout, negated errno on error. -int16_t esp32twaicanPush(const CanardFrame *const frame, - const CanardMillisecond timeout_msec); - -/// Fetch a new extended CAN data frame from the RX queue. -/// If the received frame is not an extended-ID data frame, it will be dropped -/// and the function will return early. The payload pointer of the returned -/// frame will point to the payload_buffer. It can be a stack-allocated array. -/// The payload_buffer_size shall be large enough (64 bytes is enough for CAN -/// FD), otherwise an error is returned. The received frame timestamp will be -/// set to CLOCK_REALTIME by the kernel, sampled near the moment of its arrival. -/// The loopback flag pointer is used to both indicate and control behavior when -/// a looped-back message is received. If the flag pointer is NULL, loopback -/// frames are silently dropped; if not NULL, they are accepted and indicated -/// using this flag. -/// The function will block until a frame is received or until the timeout is -/// expired. It may return early. Zero timeout makes the operation non-blocking. -/// Returns 1 on success, 0 on timeout, negated errno on error. -int16_t esp32twaicanPop(const SocketCANFD fd, CanardFrame *const out_frame, - CanardMicrosecond *const out_timestamp_usec, - const size_t payload_buffer_size, - void *const payload_buffer, - const CanardMicrosecond timeout_usec, - bool *const loopback); - -#ifdef __cplusplus -} -#endif - -#endif diff --git a/fw-cyphal/src/main.cpp b/fw-cyphal/src/main.cpp deleted file mode 100644 index 2d01b15..0000000 --- a/fw-cyphal/src/main.cpp +++ /dev/null @@ -1,382 +0,0 @@ -// #include - -#include "Arduino.h" -#include "SPI.h" -#include "USB.h" -// #include "Wire.h" -// #include "Wire.h" -#include -// #include - -#include "SimpleFOCDrivers.h" -#include "comms/streams/BinaryIO.h" -#include "encoders/esp32hwencoder/ESP32HWEncoder.h" - -#include "esp32-hal-adc.h" -#include "motors/HybridStepperMotor/HybridStepperMotor.h" -#include "DRV8323S.hpp" -#include - -// #include "encoders/sc60228/MagneticSensorSC60228.h" -// #include "esp32-hal-adc.h" -// #include "esp32-hal-gpio.h" -// #include "esp32-hal-uart.h" -// #include "esp32-hal.h" -// #include "simplesync.hpp" - -#define ENC_A 11 -#define ENC_B 8 - -#define I2C_SDA 18 -#define I2C_SCL 0 - -#define DRVEN 15 -#define TERMISTOR_PCB 3 -#define TERMISTOR_EXT 9 -#define TEMP(v0, b, rt, r1, vdd) ((1.0/((1.0/298.15)+((1.0/b)*log((v0*r1)/(rt*(vdd-v0))))))-273.15) -#define temp_pcb (TEMP(analogRead(TERMISTOR_PCB)*3.3/4096.0,3435.0,10000.0,10000.0,3.3)) - -#define SOA 1 -#define SOB 2 -#define SOC 10 - -#define INHA 14 -#define INHB 13 -#define INHC 12 -#define INLABC 46 - -#define CAN_TX 6 -#define CAN_RX 7 - -#define SPI_MISO 45 -#define SPI_MOSI 48 -#define SPI_CLK 47 -#define SPI_DRV_SC 21 - -#define LED_PIN 16 -#define VSENSE_PIN 5 -#define CAL_PIN 17 -#define vdrive_read (analogRead(VSENSE_PIN ) *20.08f / 845) -//* 20.8f/ 21033.75) - -// #define MOTOR -#define SSIF_USBSERIAL -#define FW_NO_WATCHDOG - -#ifndef FW_NO_WATCHDOG -#include -#define WDT_TIMEOUT_s 1 -// struct esp_task_wdt_config_t wd_config = { -// .timeout_ms = 100, -// .idle_core_mask = ~0x00000000, -// .trigger_panic = true -// }; -#endif - -const int voltage_lpf = 50; -// BLDC motor & driver instance -// BLDCMotor motor = BLDCMotor(pole pair number); -ESP32HWEncoder encoder = - ESP32HWEncoder(ENC_A, ENC_B, 2048 / 2); // The Index pin can be omitted - -#ifdef MOTOR -struct drv8323s_foc_driver drv8323s; -BLDCMotor motor = BLDCMotor(100); // 24N22P -// HybridStepperMotor motor = HybridStepperMotor(50, 3.7, 10, 4.5 / 1000); -// DRV8316Driver3PWM driver = DRV8316Driver3PWM(INHA, INHB, INHC, SPI_DRV_SC); -// BLDCDriver3PWM driver = BLDCDriver3PWM(INHA,INHB,INHC); -#endif -// BLDCDriver3PWM driver = BLDCDriver3PWM(pwmA, pwmB, pwmC, Enable(optional)); - -#ifdef SSIF_USBSERIAL -USBCDC usbserial; -#define SSYNCIF usbserial -#else -#define SSYNCIF Serial1 -#endif -// #define SSYNCIF Serial - -void read_i2c(uint8_t addr, uint8_t reg, uint8_t *buf, uint8_t len) { - Wire.beginTransmission(addr); - Wire.write(reg); // MAN:0x2E - int error = Wire.endTransmission(); - if (error != 0) { - return; - } - delayMicroseconds(50); - int ret = Wire.requestFrom(addr, len); - Wire.readBytes(buf, ret); - return; -} - -void write_i2c(uint8_t addr, uint8_t reg_lsb, uint8_t val) { - Wire.beginTransmission(addr); - uint8_t data[] = {reg_lsb, val}; - Wire.write(data, sizeof data); - int error = Wire.endTransmission(); - if (error != 0) { - return; - } -} - -void write_dual_i2c(uint8_t addr, uint8_t reg_lsb, uint8_t val1, uint8_t val2) { - Wire.beginTransmission(addr); - uint8_t data[] = {reg_lsb, val1, val2}; - Wire.write(data, sizeof data); - int error = Wire.endTransmission(); - if (error != 0) { - return; - } -} - -SemaphoreHandle_t mutex; -struct sync_params {}; - -TaskHandle_t taskFocHandle; -void foc_task(void *parameter) { - long last_comm = 0; - while (true) { -#ifdef MOTOR - // drivers[0].voltage_power_supply = s_foc.vcc_voltage_mV / 1000.0f; - // for (int i = 1; i < NUM_MOTORS; i += 1) - // drivers[i].voltage_power_supply = drivers[0].voltage_power_supply; -#endif - -#ifdef MOTOR - // motors.target = s_foc.vt[i] / anglefactor; - // motors.loopFOC(); - // motors.move(); -#else - // sensors.update(); -#endif - vTaskDelay(1); - } -} -TaskHandle_t taskCommHandle; -void comm_task(void *parameter) { - while (true) { - long now = millis(); - - vTaskDelay(1); - } -} - -void setup() { - mutex = xSemaphoreCreateMutex(); -#ifdef SSIF_USBSERIAL - SSYNCIF.begin(); - USB.begin(); -#else - SSYNCIF.begin(460800, SERIAL_8N1, 44, 43); -#endif - // prefs.begin("foc"); - // SSYNCIF.print(__cplusplus); - - // Wire.setTimeOut(10); - Wire.begin(I2C_SDA, I2C_SCL, 400000); - - - pinMode(LED_PIN, OUTPUT); - pinMode(SPI_DRV_SC, OUTPUT); - digitalWrite(LED_PIN, 1); // enable - - pinMode(INHA, OUTPUT); - digitalWrite(INHA, 0); // enable - pinMode(INHB, OUTPUT); - digitalWrite(INHB, 0); // enable - pinMode(INHC, OUTPUT); - digitalWrite(INHC, 0); // enable - pinMode(INLABC, OUTPUT); - digitalWrite(INLABC, 0); // enable - pinMode(CAL_PIN, OUTPUT); - digitalWrite(CAL_PIN, 0); // enable - - // write_i2c(0x36,0x07, 0b00000011); - write_i2c(0x36, 0x09, 0xFF); - - encoder.init(); -#ifdef MOTOR - - // driver.voltage_power_supply = 15; - SPI.begin(SPI_CLK, SPI_MISO, SPI_MOSI, SPI_DRV_SC); - drv8323s_init(&drv8323s,INHA, INHB, INHC, SPI_DRV_SC, SPI_DRV_SC/*DRVEN*/, &SPI); - // driver.init(&SPI); - // status(); - // motor.linkDriver(&driver); - // motor.linkSensor(&encoder); - motor.foc_modulation = FOCModulationType::SpaceVectorPWM; - motor.voltage_sensor_align = 5; - motor.voltage_limit = 6; - motor.controller = MotionControlType::velocity_openloop; - // motor.init(); - // motor.initFOC(); - - // sensor.init(&SPI); - // motors[i].linkSensor(&sensors[i]); -#endif - - // ina.getCurrent(INA3221_CH1) * 1000; ina.getVoltage(INA3221_CH1); - // ina.getCurrent(INA3221_CH2) * 1000; ina.getVoltage(INA3221_CH2); - // ina.getCurrent(INA3221_CH3) * 1000; ina.getVoltage(INA3221_CH3); - - // initialise magnetic sensor hardware - -#ifdef MOTOR - // pinMode(nRESET, OUTPUT); - // digitalWrite(nRESET, 1); -#endif - // power supply voltage [V] - // s_exchange.vcc_voltage_mV = GET_VCC; - // delay(100); - // s_exchange.vcc_voltage_mV = GET_VCC; - // drivers[0].voltage_power_supply = s_exchange.vcc_voltage_mV / 1000.0f; - // for (int i = 1; i < NUM_MOTORS; i += 1) - // drivers[i].voltage_power_supply = drivers[0].voltage_power_supply; - // s_foc = s_exchange; - - // for (int i = 0; i < NUM_MOTORS; i += 1) { -#ifdef MOTOR - // drivers[i].init(); - // link driver - // motors[i].linkDriver(&drivers[i]); - // motors[i].voltage_sensor_align = 5; - - // set control loop type to be used - // motors[i].torque_controller = TorqueControlType::voltage; - // motors[i].controller = MotionControlType::velocity; - - // contoller configuration based on the control type - // motors[i].PID_velocity.P = 0.35; - // motors[i].PID_velocity.I = 8; - // motors[i].PID_velocity.D = 0; - - // default voltage_power_supply - // motors[i].voltage_limit = 3.4 * 3; - - // velocity low pass filtering time constant - // motors[i].LPF_velocity.Tf = 0.01; -#endif - // } -#ifdef MOTOR - // drivers[2].init(); - // drivers[2].voltage_limit = 3.4 * 3; - // drivers[2].setPwm(s_exchange.vcc_voltage_mV,s_exchange.vcc_voltage_mV,s_exchange.vcc_voltage_mV); - - // pinMode(4, OUTPUT); - // digitalWrite(4, 1); - // pinMode(5, OUTPUT); - // digitalWrite(5, 1); - // pinMode(6, OUTPUT); - // digitalWrite(6, 1); -#endif - // angle loop controller - // motor.P_angle.P = 20; - // angle loop velocity limit - // motor.velocity_limit = 150; - - // initialise motor - // align encoder and start FOC -#ifdef MOTOR - // for (int i = 0; i < NUM_MOTORS; i += 1) { - // motors[i].init(); - // // std::string pref_zero_key = "m" + std::to_string(i) + ".zero"; - // // std::string pref_dir_key = "m" + std::to_string(i) + ".dir"; - // // if (prefs.isKey(pref_zero_key.c_str()) && - // // prefs.isKey(pref_dir_key.c_str())) { - // // motors[i].zero_electric_angle = - // // prefs.getFloat(pref_zero_key.c_str()); motors[i].sensor_direction - // = - // // (Direction)prefs.getInt(pref_dir_key.c_str()); - // // } - // motors[i].initFOC(); - // // if (!prefs.isKey(pref_zero_key.c_str()) || - // // !prefs.isKey(pref_dir_key.c_str())) { - // // prefs.clear(); - // // prefs.putFloat(pref_zero_key.c_str(), - // motors[i].zero_electric_angle); - // // prefs.putInt(pref_dir_key.c_str(), - // (int)motors[i].sensor_direction); - // // } - // } -#endif - // prefs.end(); - // set the inital target value -#ifdef MOTOR - // for (int i = 0; i < NUM_MOTORS; i += 1) - // motors[i].target = 0; -#endif - - // xTaskCreatePinnedToCore(&comm_task, // Function name of the task - // "Comm", // Name of the task (e.g. for - // debugging) 4096, // Stack size (bytes) NULL, - // // Parameter to pass 1, // Task priority - // &taskCommHandle, // Assign task handle - // 1 // Run on the non-Arduino (1) core - // ); - // xTaskCreatePinnedToCore(&foc_task, // Function name of the task - // "foc", // Name of the task (e.g. for debugging) - // 2048, // Stack size (bytes) - // NULL, // Parameter to pass - // 1, // Task priority - // &taskFocHandle, // Assign task handle - // 0 // Run on the non-Arduino (1) core - // ); - // esp_task_wdt_init(WDT_TIMEOUT_s, true); // enable panic so ESP32 restarts - // esp_task_wdt_add(NULL); // add current thread to WDT watch -} - -void loop() { -#ifdef MOTOR - // motor.loopFOC(); - - // motor.move(3); -#endif - encoder.update(); // optional: Update manually if not using loopfoc() - static float angle = 0; - static float old_angle = 0; - // SSYNCIF.print(angle = encoder.getAngle()); -#ifdef MOTOR - // SSYNCIF.print(" "); - uint16_t result=0; - drv8323s_read_spi(&drv8323s,0x03, &result); - SSYNCIF.print(result,HEX); - SSYNCIF.print(" "); - result=0; - delay(1); - drv8323s_read_spi(&drv8323s,0x04, &result); - SSYNCIF.print(result,HEX); - SSYNCIF.print(" "); - result=0; - delay(1); - drv8323s_read_spi(&drv8323s,0x05, &result); - SSYNCIF.print(result,HEX); -#endif - // SSYNCIF.print(" "); - // SSYNCIF.print(analogRead(TERMISTOR_PCB)); - SSYNCIF.print(" "); - static float t = temp_pcb; - t= t*0.95 + 0.05*temp_pcb; - SSYNCIF.print(t); - SSYNCIF.print(" "); - SSYNCIF.println(vdrive_read); - if (abs(angle - old_angle) > 0.01) { - digitalWrite(LED_PIN, 0); - old_angle = angle; - } - delay(100); - digitalWrite(LED_PIN, 1); - // static int i = 0; - // if (i++ % 100 == 0) - // status(); -} - -// int i = 0; -// void loop() { - -// //encoder.update(); // optional: Update manually if not using loopfoc() -// // -// // Access the sensor value -// SSYNCIF.println(encoder.getAngle()); -// delay(10); -// } -// esp_task_wdt_reset(); diff --git a/fw-cyphal/src/test.h b/fw-cyphal/src/test.h deleted file mode 100644 index 410e462..0000000 --- a/fw-cyphal/src/test.h +++ /dev/null @@ -1,9 +0,0 @@ -#include - -#define TEST(x) \ - if (!(x)) { \ - Serial.printf("\033[31;1mFAILED:\033[22;39m %s:%d %s\n", __FILE__, __LINE__, #x); \ - status = 1; \ - } else { \ - Serial.printf("\033[32;1mOK:\033[22;39m %s\n", #x); \ - } diff --git a/fw/.gitignore b/fw/.gitignore new file mode 100644 index 0000000..2705729 --- /dev/null +++ b/fw/.gitignore @@ -0,0 +1,7 @@ +.cache +.pio/build +.vscode +*.bak +include/nunavut/* +include/reg/* +include/uavcan/* diff --git a/fw/can_if_up.sh b/fw/can_if_up.sh new file mode 100755 index 0000000..5808652 --- /dev/null +++ b/fw/can_if_up.sh @@ -0,0 +1,10 @@ +#!/bin/bash +sudo ip link set can0 type can bitrate 500000 && sudo ip link set can0 up + +rm ~/allocation_table.db + +export UAVCAN__CAN__IFACE="socketcan:can0" +export UAVCAN__NODE__ID=127 +export UAVCAN__CAN__MTU=8 + +# y mon --plug-and-play ~/allocation_table.db diff --git a/fw/lib/libcanard b/fw/lib/libcanard new file mode 160000 index 0000000..962e9c5 --- /dev/null +++ b/fw/lib/libcanard @@ -0,0 +1 @@ +Subproject commit 962e9c5ac9629d761c83d9c6f4f8179faad1e8b0 diff --git a/fw/platformio-nunavut.py b/fw/platformio-nunavut.py new file mode 100644 index 0000000..96a2d31 --- /dev/null +++ b/fw/platformio-nunavut.py @@ -0,0 +1,98 @@ +import os +import hashlib +import pathlib +import shlex +import subprocess + + +import SCons.Action +from platformio import fs + +from pathlib import Path + +Import("env") + +# We don't use `env.Execute` because it does not handle spaces in path +# See https://github.com/nanopb/nanopb/pull/834 +# So, we resolve the path to the executable and then use `subprocess.run` +python_exe = env.subst("$PYTHONEXE") + +try: + import nunavut +except ImportError: + print("[nunavut] Installing") + subprocess.run([python_exe, '-m', 'pip', 'install', "nunavut"]) + import nunavut + +import pydsdl +import subprocess + +def run_nnvg(asset, lookup, generated_src_dir): + + # Define the command as a list of arguments + cmd = [ + "nnvg", + "--target-language", "c", + "--enable-serialization-asserts", + f"{asset}", # The input DSDL namespace directory + "--outdir", f"{generated_src_dir}", + ] + if len(lookup) > 0: + cmd.extend(["--lookup-dir", f"{lookup}"]) + result = subprocess.run(cmd, text=True, capture_output=True) + print("running", " ".join(cmd)) + err = str(result.stderr) + if len(err.strip()) > 0: + print("nnvg ERROR:\n", result.stderr) + +project_dir = env.subst("$PROJECT_DIR") + +build_dir = env.subst("$BUILD_DIR") + +generated_src_dir = os.path.join(build_dir, 'nunavut', 'generated-src') +generated_src_dir = os.path.join(project_dir, 'include') +print("generated_src_dir",generated_src_dir) +generated_build_dir = os.path.join(build_dir, 'nunavut', 'generated-build') +md5_dir = os.path.join(build_dir, 'nunavut', 'md5') + +dsdl_folders = [x for x in env.subst(env.GetProjectOption("custom_dsdl_folders", "")).split("\n") if x != ""] +print(dsdl_folders) +from pathlib import Path +def generate_dsdl_files(*args, **kwargs): + if not dsdl_folders: + print("[nunavut] No generation needed.") + else: + parent_folder = Path(project_dir) + print("parent_folder", parent_folder) + uavcan_dir = None + for path in parent_folder.rglob("*"): + #if path.is_dir() and path.name in dsdl_folders: + if path.is_dir() and path.name in ["uavcan"]: + uavcan_dir = (str(path.absolute())) + #uavcan_dir = (str(path.relative_to(project_dir))) + print("found uavcan dir:", uavcan_dir) + + for path in parent_folder.rglob("*"): + #if path.is_dir() and path.name in dsdl_folders: + if path.is_dir() and any([str(path.absolute()).endswith(d) for d in dsdl_folders]) : + run_nnvg(path.absolute(), uavcan_dir, generated_src_dir) + # + # Add generated includes and sources to build environment + # + run_nnvg(uavcan_dir, "", generated_src_dir) + env.Append(CPPPATH=[generated_src_dir]) + env.Append(CXXFLAGS=[f"-I{generated_src_dir}", "flag2"]) + #env.BuildSources(generated_build_dir, generated_src_dir) + + # # Fix for ESP32 ESP-IDF https://github.com/nanopb/nanopb/issues/734#issuecomment-1001544447 + # global_env = DefaultEnvironment() + # already_called_env_name = "_PROTOBUF_GENERATOR_ALREADY_CALLED_" + env['PIOENV'].replace("-", "_") + # if not global_env.get(already_called_env_name, False): + # env.BuildSources(generated_build_dir, generated_src_dir) + # global_env[already_called_env_name] = True + + + +#env.AddCustomTarget("gen_dsdl", None, generate_dsdl_files) +generate_dsdl_files() +#env.AddPreAction("buildprog", generate_dsdl_files) \ No newline at end of file diff --git a/fw/platformio.ini b/fw/platformio.ini index ffb7456..f77bb8c 100644 --- a/fw/platformio.ini +++ b/fw/platformio.ini @@ -10,18 +10,32 @@ [env:nilsdriverv1] platform = espressif32 +framework = arduino # platform = https://github.com/cziter15/platform-espressif32.git#68ad40f6df654fe4b8d0a50982b810df5b49b677 # platform = https://github.com/cziter15/platform-espressif32.git #platform = https://github.com/tasmota/platform-espressif32/releases/download/2023.10.10/platform-espressif32.zip -# board = adafruit_feather_esp32s3 -board = lolin_s3_mini -framework = arduino + +; Flash: 4MB QD, PSRAM: 8MB OT +board = esp32-s3-devkitc-1 +board_build.arduino.memory_type = qio_opi +board_build.flash_mode = qio +board_build.psram_type = opi +board_upload.flash_size = 4MB +board_upload.maximum_size = 4194304 +board_build.partitions = default.csv +board_build.extra_flags = + -DBOARD_HAS_PSRAM + upload_protocol = esptool upload_port = /dev/ttyACM* # board_build.mcu = esp32s3 lib_archive = false # ;platform_packages = espressif/toolchain-xtensa-esp32@8.4.0+2021r2-patch5 monitor_speed = 115200 + +build_type = debug +monitor_filters = esp32_exception_decoder + lib_deps = https://github.com/schnilz/Arduino-FOC.git#dev https://github.com/schnilz/Arduino-FOC-drivers.git#dev @@ -31,8 +45,22 @@ lib_deps = ; https://github.com/eric-wieser/packet-io.git ; https://git.nilsschulte.de/nils/simplesync.git ; https://github.com/Tinyu-Zhao/INA3221.git + https://github.com/OpenCyphal/public_regulated_data_types#f9f6790 + ;https://github.com/OpenCyphal/nunavut.git#11785de + https://github.com/pavel-kirienko/o1heap.git#bd93277 +lib_deps_external = + https://github.com/OpenCyphal/libcanard.git#551af7f + +build_flags = + -DRADIOLIB_EEPROM_UNSUPPORTED + -I".pio/build/nilsdriverv1/nunavut/generated-src" + -I"lib/libcanard/libcanard" + -DARDUINO_LOOP_STACK_SIZE=65536 -build_flags = -DRADIOLIB_EEPROM_UNSUPPORTED ; -DARDUINO_USB_MODE ;-DSIMPLEFOC_ESP32_USELEDC +extra_scripts = platformio-nunavut.py +; nnvg --target-language c --enable-serialization-asserts --lookup-dir uavcan +custom_dsdl_folders = + public_regulated_data_types/reg diff --git a/fw-cyphal/src/esp32twaican.c b/fw/src/esp32twaican.cpp similarity index 73% rename from fw-cyphal/src/esp32twaican.c rename to fw/src/esp32twaican.cpp index 4797a30..3137fa5 100644 --- a/fw-cyphal/src/esp32twaican.c +++ b/fw/src/esp32twaican.cpp @@ -4,19 +4,28 @@ /// // This is needed to enable the necessary declarations in sys/ +#include "FreeRTOS.h" #include +#include "canard.h" #include "esp32twaican.h" +#include "task.h" #include "driver/twai.h" -int esp32twaicanOpen(const gpio_num_t pin_tx, const gpio_num_t pin_rx, - const CanardFilter *filter_config) { +#include "Arduino.h" + +#include "USB.h" +extern USBCDC usbserial; +#define Serial usbserial + +int esp32twaicanOpen(const int pin_tx, const int pin_rx, + const struct CanardFilter *filter_config) { twai_general_config_t g_config = TWAI_GENERAL_CONFIG_DEFAULT( (gpio_num_t)pin_tx, (gpio_num_t)pin_rx, TWAI_MODE_NORMAL); - g_config.rx_queue_len = 10; - g_config.tx_queue_len = 10; + g_config.rx_queue_len = 30; + g_config.tx_queue_len = 30; twai_timing_config_t t_config = TWAI_TIMING_CONFIG_500KBITS(); twai_filter_config_t f_config = TWAI_FILTER_CONFIG_ACCEPT_ALL(); if (filter_config) { @@ -27,10 +36,12 @@ int esp32twaicanOpen(const gpio_num_t pin_tx, const gpio_num_t pin_rx, int ret = twai_driver_install(&g_config, &t_config, &f_config); if (ret != ESP_OK) { // Serial.println("CAN1 Driver initialized"); + assert(false); return ret; } ret = twai_start(); if (ret != ESP_OK) { + assert(false); return ret; } @@ -40,16 +51,19 @@ int esp32twaicanOpen(const gpio_num_t pin_tx, const gpio_num_t pin_rx, TWAI_ALERT_BUS_ERROR; ret = twai_reconfigure_alerts(alerts_to_enable, NULL); if (ret != ESP_OK) { + assert(false); // Serial.println("Failed to reconfigure alerts"); return ret; } + digitalWrite(38, 0); /*enable*/delay(1000); digitalWrite(38, 1); /*disable*/delay(1000); + return 0; } -int16_t esp32twaicanPush(const CanardFrame *const frame, +int16_t esp32twaicanPush(const struct CanardFrame *const frame, const CanardMicrosecond timeout_usec) { - if ((frame == NULL) || (frame->payload == NULL) || - (frame->payload_size > UINT8_MAX)) { + if ((frame == NULL) || (frame->payload.size == 0) || + (frame->payload.size > UINT8_MAX)) { return -1; } @@ -58,22 +72,25 @@ int16_t esp32twaicanPush(const CanardFrame *const frame, message.extd = 1; // extended can id message.identifier = frame->extended_can_id; message.rtr = 0; - message.data_length_code = frame->payload_size; - memcpy(&message.data[0], &frame->payload[0], (int)frame->payload_size); - - int ret = - twai_transmit(&message, (timeout_usec * (1000000 / configTICK_RATE_HZ))); + message.data_length_code = frame->payload.size; + //printf(stderr, "size %d data %d", frame->payload.size, (int)frame->payload.data); + memcpy(&message.data[0], ((const uint8_t *)frame->payload.data), (int)frame->payload.size); + + int ret = twai_transmit(&message, (timeout_usec * (1000000 / configTICK_RATE_HZ))); + if (ret == ESP_OK) { return 1; // Serial.println("CAN1: Message queued for transmission"); } else if (ret == ESP_ERR_TIMEOUT) { /* Queue full and timed out waiting for empty slot */ return 0; + } else if( ret == ESP_ERR_INVALID_STATE) { + assert(false); } return -1; } -int16_t esp32twaicanPop(const SocketCANFD fd, CanardFrame *const out_frame, +int16_t esp32twaicanPop(struct CanardFrame *const out_frame, CanardMicrosecond *const out_timestamp_usec, const size_t payload_buffer_size, void *const payload_buffer, @@ -89,9 +106,9 @@ int16_t esp32twaicanPop(const SocketCANFD fd, CanardFrame *const out_frame, twai_status_info_t twaistatus; twai_get_status_info(&twaistatus); if (alerts_triggered & TWAI_ALERT_RX_DATA) { - twai_message_t message; if (twai_receive(&message, 0) == ESP_OK) { + //Serial.println("TWAI_ALERT_RX_DATA ESP_OK"); // Non-blocking receive messages from the socket and validate. const ssize_t read_size = message.data_length_code; if (read_size < 0) { @@ -103,9 +120,11 @@ int16_t esp32twaicanPop(const SocketCANFD fd, CanardFrame *const out_frame, } const bool valid = (message.extd) && // Extended frame - (message.rtr) && // Not RTR frame + (!message.rtr) && // Not RTR frame (true /* error frame */); // Not error frame if (!valid) { + + //Serial.println("invalid"); return 0; // Not an extended data frame -- drop silently and return // early. } @@ -120,6 +139,7 @@ int16_t esp32twaicanPop(const SocketCANFD fd, CanardFrame *const out_frame, if (loopback != NULL) { *loopback = loopback_frame; } + //Serial.println("ok"); // Obtain the CAN frame time stamp from the kernel. if (NULL != out_timestamp_usec) { // struct timeval tv; @@ -132,9 +152,12 @@ int16_t esp32twaicanPop(const SocketCANFD fd, CanardFrame *const out_frame, (CanardMicrosecond)(xTaskGetTickCount() * (1000000 / configTICK_RATE_HZ)); } - out_frame->extended_can_id = sockcan_frame.can_id & CAN_EFF_MASK; - out_frame->payload_size = sockcan_frame.len; - out_frame->payload = payload_buffer; + + //Serial.print("can id "); + //Serial.println(message.identifier); + out_frame->extended_can_id = message.identifier; + out_frame->payload.size = message.data_length_code; + out_frame->payload.data = payload_buffer; (void)memcpy(payload_buffer, &message.data[0], message.data_length_code); return 1; } diff --git a/fw/src/esp32twaican.h b/fw/src/esp32twaican.h new file mode 100644 index 0000000..c8bde02 --- /dev/null +++ b/fw/src/esp32twaican.h @@ -0,0 +1,85 @@ +/// ____ ______ __ __ +/// / __ `____ ___ ____ / ____/_ ______ / /_ ____ +/// / / +/// / / / / __ `/ _ `/ __ `/ / / / / / __ `/ __ `/ __ +/// `/ / +/// / /_/ / /_/ / __/ / / / /___/ /_/ / /_/ / / / / /_/ +/// / / +/// `____/ .___/`___/_/ /_/`____/`__, / .___/_/ +/// /_/`__,_/_/ +/// /_/ /____/_/ +/// +/// This is a basic adapter library that bridges Libcanard with SocketCAN. +/// Read the API documentation for usage information. +/// +/// To integrate the library into your application, just copy-paste the c/h +/// files into your project tree. +/// +/// -------------------------------------------------------------------------------------------------------------------- +/// Changelog +/// +/// v3.0 - Update for compatibility with Libcanard v3. +/// +/// v2.0 - Added loop-back functionality. +/// API change in esp32twaicanPop(): loopback flag added. +/// - Changed to kernel-based time-stamping for received frames for +/// improved accuracy. +/// API change in esp32twaicanPop(): time stamp clock source is now +/// CLOCK_REALTIME, vs CLOCK_TAI before. +/// +/// v1.0 - Initial release +/// -------------------------------------------------------------------------------------------------------------------- +/// +/// This software is distributed under the terms of the MIT License. +/// Copyright (c) 2020 OpenCyphal +/// Author: Pavel Kirienko + +#ifndef ESP32TWAICAN_H_INCLUDED +#define ESP32TWAICAN_H_INCLUDED + +#include "canard.h" +#include +#include +#include + +#ifdef __cplusplus +extern "C" +{ +#endif + + int esp32twaicanOpen(const int pin_tx, const int pin_rx, + const struct CanardFilter *filter_config); + + /// Enqueue a new extended CAN data frame for transmission. + /// Block until the frame is enqueued or until the timeout is expired. + /// Zero timeout makes the operation non-blocking. + /// Returns 1 on success, 0 on timeout, negated errno on error. + int16_t esp32twaicanPush(const struct CanardFrame *const frame, + const CanardMicrosecond timeout_usec); + + /// Fetch a new extended CAN data frame from the RX queue. + /// If the received frame is not an extended-ID data frame, it will be dropped + /// and the function will return early. The payload pointer of the returned + /// frame will point to the payload_buffer. It can be a stack-allocated array. + /// The payload_buffer_size shall be large enough (64 bytes is enough for CAN + /// FD), otherwise an error is returned. The received frame timestamp will be + /// set to CLOCK_REALTIME by the kernel, sampled near the moment of its arrival. + /// The loopback flag pointer is used to both indicate and control behavior when + /// a looped-back message is received. If the flag pointer is NULL, loopback + /// frames are silently dropped; if not NULL, they are accepted and indicated + /// using this flag. + /// The function will block until a frame is received or until the timeout is + /// expired. It may return early. Zero timeout makes the operation non-blocking. + /// Returns 1 on success, 0 on timeout, negated errno on error. + int16_t esp32twaicanPop(struct CanardFrame *const out_frame, + CanardMicrosecond *const out_timestamp_usec, + const size_t payload_buffer_size, + void *const payload_buffer, + const CanardMicrosecond timeout_usec, + bool *const loopback); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/fw/src/main.cpp b/fw/src/main.cpp index 7130542..7d19e60 100644 --- a/fw/src/main.cpp +++ b/fw/src/main.cpp @@ -1,8 +1,27 @@ // #include +#define NODE_NAME "N17BLDC" + +#define VERSION_MAJOR 0 +#define VERSION_MINOR 1 +#define VCS_REVISION_ID 0 + #include "Arduino.h" -#include "SPI.h" + +#define SSIF_USBSERIAL +#ifdef SSIF_USBSERIAL #include "USB.h" +USBCDC usbserial; +#define SSYNCIF usbserial +#define Serial usbserial +#else +#define SSYNCIF Serial1 +#endif +// #define SSYNCIF Serial + +#include "canard.c" + +#include "SPI.h" // #include "Wire.h" // #include "Wire.h" #include @@ -22,6 +41,8 @@ #include "HybridStepperMotor.h" #include +#include "udral_servo.hpp" + // #include "encoders/sc60228/MagneticSensorSC60228.h" // #include "esp32-hal-adc.h" // #include "esp32-hal-gpio.h" @@ -29,17 +50,11 @@ // #include "esp32-hal.h" // #include "simplesync.hpp" -#define ENC_A 11 -#define ENC_B 8 -// #define I2C_SDA 18 -// #define I2C_SCL 0 +#include + +#include "pin_def.h" -#define SPI_ACC_nCS 0 -#define DRV_nFAULT 15 -#define DRVEN 16 -#define TERMISTOR_PCB 3 -#define TERMISTOR_EXT 9 #define TEMP(v0, b, rt, r1, vdd) \ ((1.0 / \ ((1.0 / 298.15) + ((1.0 / b) * log((v0 * r1) / (rt * (vdd - v0)))))) - \ @@ -48,31 +63,11 @@ (TEMP(analogRead(TERMISTOR_PCB) * 3.3 / 4096.0, 3435.0, 10000.0, 10000.0, \ 3.3)) -#define SOA 1 -#define SOB 2 -#define SOC 10 -#define INHA 14 -#define INHB 12 -#define INHC 13 -#define INLABC 46 - -#define CAN_TX 6 -#define CAN_RX 7 - -#define SPI_MISO 45 -#define SPI_MOSI 48 -#define SPI_CLK 47 -#define SPI_DRV_SC 21 - -#define LED_PIN 38 -#define VSENSE_PIN 5 -#define CAL_PIN 17 #define vdrive_read (analogRead(VSENSE_PIN) * 20.08f / 845) //* 20.8f/ 21033.75) #define MOTOR -#define SSIF_USBSERIAL #define FW_NO_WATCHDOG #ifndef FW_NO_WATCHDOG @@ -108,13 +103,6 @@ HybridStepperMotor motor = HybridStepperMotor(50); //, 3.7, 10, 4.5 / 1000); #endif // BLDCDriver3PWM driver = BLDCDriver3PWM(pwmA, pwmB, pwmC, Enable(optional)); -#ifdef SSIF_USBSERIAL -USBCDC usbserial; -#define SSYNCIF usbserial -#else -#define SSYNCIF Serial1 -#endif -// #define SSYNCIF Serial void read_i2c(uint8_t addr, uint8_t reg, uint8_t *buf, uint8_t len) { Wire.beginTransmission(addr); @@ -186,6 +174,19 @@ void doB1(){encoder.handleB();} void setup() { pinMode(LED_PIN, OUTPUT); + + Serial.begin(); + USB.begin(); + delay(1000); + digitalWrite(LED_PIN, 0); // enable + delay(100); + digitalWrite(LED_PIN, 1); // enable + delay(2000); + Serial.println("Start"); + + //digitalWrite(38, 0); /*enable*/delay(1000); digitalWrite(38, 1); /*disable*/delay(1000); + return; + for (int i = 0; i < 1; i++) { digitalWrite(LED_PIN, 0); // enable delay(1000); @@ -398,6 +399,7 @@ void setup() { int i = 0; void loop() { + mainloop(); #ifdef MOTOR // drv8323s.focdriver->voltage_power_supply = vdrive_read; // digitalWrite(LED_PIN, 0); // enable @@ -421,6 +423,7 @@ if(i++ % 100 == 0) { SSYNCIF.print("\tcal: "); SSYNCIF.print(encoder_calibrated.getAngle() * 180.0 / 3.1415); SSYNCIF.println(); + } // inv_imu_sensor_event_t imu_event; diff --git a/fw/src/pin_def.h b/fw/src/pin_def.h new file mode 100644 index 0000000..b5ccf00 --- /dev/null +++ b/fw/src/pin_def.h @@ -0,0 +1,32 @@ +#pragma once +#define ENC_A 11 +#define ENC_B 8 + +// #define I2C_SDA 18 +// #define I2C_SCL 0 + +#define SPI_ACC_nCS 0 +#define DRV_nFAULT 15 +#define DRVEN 16 +#define TERMISTOR_PCB 3 +#define TERMISTOR_EXT 9 +#define SOA 1 +#define SOB 2 +#define SOC 10 + +#define INHA 14 +#define INHB 12 +#define INHC 13 +#define INLABC 46 + +#define CAN_TX 6 +#define CAN_RX 7 + +#define SPI_MISO 45 +#define SPI_MOSI 48 +#define SPI_CLK 47 +#define SPI_DRV_SC 21 + +#define LED_PIN 38 +#define VSENSE_PIN 5 +#define CAL_PIN 17 \ No newline at end of file diff --git a/fw/src/register.hpp b/fw/src/register.hpp new file mode 100644 index 0000000..b568a1b --- /dev/null +++ b/fw/src/register.hpp @@ -0,0 +1,137 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include "USB.h" +extern USBCDC usbserial; +#define Serial usbserial + +Preferences preferences; + +#define REGISTER_LIST_KEY "__reglist" +#define REGISTER_LIST_DELIM "|" + +String +hash(const char *str) +{ + unsigned long hash = 5381; + int c; + + while (c = *(str)++) + hash = ((hash << 5) + hash) + c; /* hash * 33 + c */ + + return String(hash, HEX); +} + +void registerWrite(const char *const register_name, const uavcan_register_Value_1_0 *const value) +{ + preferences.begin("uavcan"); + int bytes_read = preferences.putBytes(hash(register_name).c_str(), (uint8_t *)value, sizeof(*value)); + + Serial.printf("Register %s written (free %d)\n", register_name, preferences.freeEntries()); + String reglist = preferences.getString(REGISTER_LIST_KEY, ""); + String search = String(register_name) + REGISTER_LIST_DELIM; + if (reglist.indexOf(search) == -1) + { + reglist += search; + preferences.putString(REGISTER_LIST_KEY, reglist); + Serial.printf("Register list written: %s\n", reglist.c_str()); + } + + preferences.end(); +} + +void registerRead(const char *const register_name, uavcan_register_Value_1_0 *const value) +{ + preferences.begin("uavcan", true); + if (preferences.isKey(hash(register_name).c_str())) + { + preferences.getBytes(hash(register_name).c_str(), (uint8_t *)value, sizeof(*value)); + preferences.end(); + } + else + { + preferences.end(); + Serial.printf("Register %s not found\n", register_name); + registerWrite(register_name, value); + } + +} + +uavcan_register_Name_1_0 registerGetNameByIndex(const uint16_t index) +{ + uavcan_register_Name_1_0 name; + name.name.elements[0] = '\0'; + name.name.count = 0; + + preferences.begin("uavcan", true); + String reglist = preferences.getString(REGISTER_LIST_KEY, ""); + //Serial.printf("Register list read: %s\n", reglist.c_str()); + preferences.end(); + /* + Register list written: + reg.udral.service.actuator.servo| + uavcan.pub.servo.feedback.type| + uavcan.pub.servo.status.type| + uavcan.pub.servo.power.type| + uavcan.pub.servo.dynamics.type| + uavcan.sub.servo.setpoint.type| + uavcan.sub.servo.readiness.type + */ + + /* + Register list read: + reg.udral.service.actuator.servo| + uavcan.pub.servo.feedback.type| + uavcan.pub.servo.status.type| + uavcan.pub.servo.power.type| + uavcan.pub.servo.dynamics.type| + uavcan.sub.servo.setpoint.type| + uavcan.sub.servo.readiness.type| + uavcan.node.id| + */ + int start = 0; + int found = 0; + while (found <= index) + { + int end = reglist.indexOf(REGISTER_LIST_DELIM, start); + if (end == -1) + break; + + if (found == index) + { + String reg = reglist.substring(start, end); + strcpy((char *)name.name.elements, reg.c_str()); + name.name.count = reg.length(); + break; + } + + start = end + 1; + found++; + } + + Serial.printf("Register '%s' at idx %d %s found\n", (char *)&name.name, index, (name.name.count ? "" : "NOT")); + return name; // Empty if not found +} + +bool registerAssign(uavcan_register_Value_1_0 *const dst, const uavcan_register_Value_1_0 *const src) +{ + if (dst->_tag_ == src->_tag_ || uavcan_register_Value_1_0_is_empty_(dst)) + { + *dst = *src; + return true; + } + return false; +} + +#include + +void registerDoFactoryReset(void) +{ + nvs_flash_erase(); // erase the NVS partition and... + nvs_flash_init(); // initialize the NVS partition. +} diff --git a/fw/src/udral_servo.hpp b/fw/src/udral_servo.hpp new file mode 100644 index 0000000..1c0ea5d --- /dev/null +++ b/fw/src/udral_servo.hpp @@ -0,0 +1,1173 @@ +/// ____ ______ __ __ +/// / __ `____ ___ ____ / ____/_ ______ / /_ ____ / / +/// / / / / __ `/ _ `/ __ `/ / / / / / __ `/ __ `/ __ `/ / +/// / /_/ / /_/ / __/ / / / /___/ /_/ / /_/ / / / / /_/ / / +/// `____/ .___/`___/_/ /_/`____/`__, / .___/_/ /_/`__,_/_/ +/// /_/ /____/_/ +/// +/// A demo application showcasing the implementation of a UDRAL servo network service. +/// This application is intended to run on GNU/Linux but it is trivially adaptable to baremetal environments. +/// Please refer to the enclosed README for details. +/// +/// This software is distributed under the terms of the MIT License. +/// Copyright (C) 2021 OpenCyphal +/// Author: Pavel Kirienko + +#define NUNAVUT_ASSERT(x) assert(x) + +#include "pin_def.h" + +#include "canard.h" + +// #include "socketcan.h" +#include "esp32twaican.h" +#include "register.hpp" +#include + +#include +#include +#include +#include +#include +#include +#include "NodeIDAllocationData_1_0.h" +// #include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "USB.h" + +extern USBCDC usbserial; + +#define KILO 1000L +#define MEGA ((int64_t)KILO * KILO) + +#define CAN_REDUNDANCY_FACTOR 1 +/// For CAN FD the queue can be smaller. +#define CAN_TX_QUEUE_CAPACITY 100 + +/// We keep the state of the application here. Feel free to use static variables instead if desired. +typedef struct State +{ + CanardMicrosecond started_at; + + O1HeapInstance *heap; + struct CanardInstance canard; + struct CanardTxQueue canard_tx_queues[CAN_REDUNDANCY_FACTOR]; + + /// The state of the business logic. + struct + { + /// Whether the servo is supposed to actuate the load or to stay idle (safe low-power mode). + struct + { + bool armed; + CanardMicrosecond last_update_at; + } arming; + + /// Setpoint & motion profile (unsupported constraints are to be ignored). + /// https://github.com/OpenCyphal/public_regulated_data_types/blob/master/reg/udral/service/actuator/servo/_.0.1.dsdl + /// As described in the linked documentation, there are two kinds of servos supported: linear and rotary. + /// Units per-kind are: LINEAR ROTARY + float position; ///< [meter] [radian] + float velocity; ///< [meter/second] [radian/second] + float acceleration; ///< [(meter/second)^2] [(radian/second)^2] + float force; ///< [newton] [netwon*meter] + } servo; + + /// These values are read from the registers at startup. You can also implement hot reloading if desired. + /// The subjects of the servo network service are defined in the UDRAL data type definitions here: + /// https://github.com/OpenCyphal/public_regulated_data_types/blob/master/reg/udral/service/actuator/servo/_.0.1.dsdl + struct + { + struct + { + CanardPortID servo_feedback; //< reg.udral.service.actuator.common.Feedback + CanardPortID servo_status; //< reg.udral.service.actuator.common.Status + CanardPortID servo_power; //< reg.udral.physics.electricity.PowerTs + CanardPortID servo_dynamics; //< (timestamped dynamics) + } pub; + struct + { + CanardPortID servo_setpoint; //< (non-timestamped dynamics) + CanardPortID servo_readiness; //< reg.udral.service.common.Readiness + } sub; + } port_id; + + /// A transfer-ID is an integer that is incremented whenever a new message is published on a given subject. + /// It is used by the protocol for deduplication, message loss detection, and other critical things. + /// For CAN, each value can be of type uint8_t, but we use larger types for genericity and for statistical purposes, + /// as large values naturally contain the number of times each subject was published to. + struct + { + uint64_t uavcan_node_heartbeat; + uint64_t uavcan_node_port_list; + uint64_t uavcan_pnp_allocation; + // Messages published synchronously can share the same transfer-ID: + uint64_t servo_fast_loop; + uint64_t servo_1Hz_loop; + } next_transfer_id; +} State; + +/// This flag is raised when the node is requested to restart. +static volatile bool g_restart_required = false; + +/// A deeply embedded system should sample a microsecond-resolution non-overflowing 64-bit timer. +/// Here is a simple non-blocking implementation as an example: +/// https://github.com/PX4/sapog/blob/601f4580b71c3c4da65cc52237e62a/firmware/src/motor/realtime/motor_timer.c#L233-L274 +/// Mind the difference between monotonic time and wall time. Monotonic time never changes rate or makes leaps, +/// it is therefore impossible to synchronize with an external reference. Wall time can be synchronized and therefore +/// it may change rate or make leap adjustments. The two kinds of time serve completely different purposes. +static CanardMicrosecond getMonotonicMicroseconds() +{ + struct timespec ts; + if (clock_gettime(CLOCK_MONOTONIC, &ts) != 0) + { + abort(); + } + return (uint64_t)(ts.tv_sec * 1000000 + ts.tv_nsec / 1000); +} + +// Returns the 128-bit unique-ID of the local node. This value is used in uavcan.node.GetInfo.Response and during the +// plug-and-play node-ID allocation by uavcan.pnp.NodeIDAllocationData. The function is infallible. +static void getUniqueID(uint8_t out[uavcan_node_GetInfo_Response_1_0_unique_id_ARRAY_CAPACITY_]) +{ + // A real hardware node would read its unique-ID from some hardware-specific source (typically stored in ROM). + // This example is a software-only node, so we store the unique-ID in a (read-only) register instead. + uavcan_register_Value_1_0 value = {0}; + uavcan_register_Value_1_0_select_unstructured_(&value); + // Populate the default; it is only used at the first run if there is no such register. + for (uint8_t i = 0; i < uavcan_node_GetInfo_Response_1_0_unique_id_ARRAY_CAPACITY_; i++) + { + value.unstructured.value.elements[value.unstructured.value.count++] = (uint8_t)rand(); // NOLINT + } + registerRead("uavcan.node.unique_id", &value); + assert(uavcan_register_Value_1_0_is_unstructured_(&value) && + value.unstructured.value.count == uavcan_node_GetInfo_Response_1_0_unique_id_ARRAY_CAPACITY_); + memcpy(&out[0], &value.unstructured.value, uavcan_node_GetInfo_Response_1_0_unique_id_ARRAY_CAPACITY_); + memset(out, 0xAB, uavcan_node_GetInfo_Response_1_0_unique_id_ARRAY_CAPACITY_); +} + +typedef enum SubjectRole +{ + SUBJECT_ROLE_PUBLISHER, + SUBJECT_ROLE_SUBSCRIBER, +} SubjectRole; + +/// Reads the port-ID from the corresponding standard register. The standard register schema is documented in +/// the Cyphal Specification, section for the standard service uavcan.register.Access. You can also find it here: +/// https://github.com/OpenCyphal/public_regulated_data_types/blob/master/uavcan/register/384.Access.1.0.dsdl +/// A very hands-on demo is available in Python: https://pycyphal.readthedocs.io/en/stable/pages/demo.html +static CanardPortID getSubjectID(const SubjectRole role, const char *const port_name, const char *const type_name) +{ + // Deduce the register name from port name. + const char *const role_name = (role == SUBJECT_ROLE_PUBLISHER) ? "pub" : "sub"; + char register_name[uavcan_register_Name_1_0_name_ARRAY_CAPACITY_] = {0}; + snprintf(®ister_name[0], sizeof(register_name), "uavcan.%s.%s.id", role_name, port_name); + + // Set up the default value. It will be used to populate the register if it doesn't exist. + uavcan_register_Value_1_0 val = {0}; + uavcan_register_Value_1_0_select_natural16_(&val); + val.natural16.value.count = 1; + val.natural16.value.elements[0] = UINT16_MAX; // This means "undefined", per Specification, which is the default. + + // Read the register with defensive self-checks. + registerRead(®ister_name[0], &val); + assert(uavcan_register_Value_1_0_is_natural16_(&val) && (val.natural16.value.count == 1)); + const uint16_t result = val.natural16.value.elements[0]; + + // This part is NOT required but recommended by the Specification for enhanced introspection capabilities. It is + // very cheap to implement so all implementations should do so. This register simply contains the name of the + // type exposed at this port. It should be immutable, but it is not strictly required so in this implementation + // we take shortcuts by making it mutable since it's behaviorally simpler in this specific case. + snprintf(®ister_name[0], sizeof(register_name), "uavcan.%s.%s.type", role_name, port_name); + uavcan_register_Value_1_0_select_string_(&val); + val._string.value.count = nunavutChooseMin(strlen(type_name), uavcan_primitive_String_1_0_value_ARRAY_CAPACITY_); + memcpy(&val._string.value.elements[0], type_name, val._string.value.count); + registerWrite(®ister_name[0], &val); // Unconditionally overwrite existing value because it's read-only. + + return result; +} + +static void send(State *const state, + const CanardMicrosecond tx_deadline_usec, + const CanardTransferMetadata *const metadata, + const size_t payload_size, + const void *const payload_data, + const CanardMicrosecond now_usec) +{ + for (uint8_t ifidx = 0; ifidx < CAN_REDUNDANCY_FACTOR; ifidx++) + { + const struct CanardPayload payload = {.size = payload_size, .data = payload_data}; + (void)canardTxPush(&state->canard_tx_queues[ifidx], + &state->canard, + tx_deadline_usec, + metadata, + payload, + now_usec, NULL); + } +} + +static void sendResponse(State *const state, + const struct CanardRxTransfer *const original_request_transfer, + const size_t payload_size, + const void *const payload_data, + const CanardMicrosecond now_usec) +{ + CanardTransferMetadata meta = original_request_transfer->metadata; + meta.transfer_kind = CanardTransferKindResponse; + send(state, original_request_transfer->timestamp_usec + MEGA, &meta, payload_size, payload_data, now_usec); +} + +/// Invoked at the rate of the fastest loop. +static void handleFastLoop(State *const state, const CanardMicrosecond now_usec) +{ + // Apply control inputs if armed. + if (state->servo.arming.armed) + { + fprintf(stderr, + "\rp=%.3f m v=%.3f m/s a=%.3f (m/s)^2 F=%.3f N \r", + (double)state->servo.position, + (double)state->servo.velocity, + (double)state->servo.acceleration, + (double)state->servo.force); + } + else + { + // fprintf(stderr, "\rDISARMED \r"); + } + + const bool anonymous = state->canard.node_id > CANARD_NODE_ID_MAX; + const uint64_t servo_transfer_id = state->next_transfer_id.servo_fast_loop++; + + // Publish feedback if the subject is enabled and the node is non-anonymous. + if (!anonymous && (state->port_id.pub.servo_feedback <= CANARD_SUBJECT_ID_MAX)) + { + reg_udral_service_actuator_common_Feedback_0_1 msg = {0}; + msg.heartbeat.readiness.value = state->servo.arming.armed ? reg_udral_service_common_Readiness_0_1_ENGAGED + : reg_udral_service_common_Readiness_0_1_STANDBY; + // If there are any hardware or configuration issues, report them here: + msg.heartbeat.health.value = uavcan_node_Health_1_0_NOMINAL; + // Serialize and publish the message: + uint8_t serialized[reg_udral_service_actuator_common_Feedback_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_]; + size_t serialized_size = sizeof(serialized); + const int8_t err = + reg_udral_service_actuator_common_Feedback_0_1_serialize_(&msg, &serialized[0], &serialized_size); + assert(err >= 0); + if (err >= 0) + { + const CanardTransferMetadata transfer = { + .priority = CanardPriorityHigh, + .transfer_kind = CanardTransferKindMessage, + .port_id = state->port_id.pub.servo_feedback, + .remote_node_id = CANARD_NODE_ID_UNSET, + .transfer_id = (CanardTransferID)servo_transfer_id, + }; + send(state, now_usec + 10 * KILO, &transfer, serialized_size, &serialized[0], now_usec); + } + } + + // Publish dynamics if the subject is enabled and the node is non-anonymous. + if (!anonymous && (state->port_id.pub.servo_dynamics <= CANARD_SUBJECT_ID_MAX)) + { + reg_udral_physics_dynamics_translation_LinearTs_0_1 msg = {0}; + // Our node does not synchronize its clock with the network, so we cannot timestamp our publications: + msg.timestamp.microsecond = uavcan_time_SynchronizedTimestamp_1_0_UNKNOWN; + // A real application would source these values from the hardware; we republish the setpoint for demo purposes. + // TODO populate real values: + msg.value.kinematics.position.meter = state->servo.position; + msg.value.kinematics.velocity.meter_per_second = state->servo.velocity; + msg.value.kinematics.acceleration.meter_per_second_per_second = state->servo.acceleration; + msg.value.force.newton = state->servo.force; + // Serialize and publish the message: + uint8_t serialized[reg_udral_physics_dynamics_translation_LinearTs_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_]; + size_t serialized_size = sizeof(serialized); + const int8_t err = + reg_udral_physics_dynamics_translation_LinearTs_0_1_serialize_(&msg, &serialized[0], &serialized_size); + assert(err >= 0); + if (err >= 0) + { + const CanardTransferMetadata transfer = { + .priority = CanardPriorityHigh, + .transfer_kind = CanardTransferKindMessage, + .port_id = state->port_id.pub.servo_dynamics, + .remote_node_id = CANARD_NODE_ID_UNSET, + .transfer_id = (CanardTransferID)servo_transfer_id, + }; + send(state, now_usec + 10 * KILO, &transfer, serialized_size, &serialized[0], now_usec); + } + } + + // Publish power if the subject is enabled and the node is non-anonymous. + if (!anonymous && (state->port_id.pub.servo_power <= CANARD_SUBJECT_ID_MAX)) + { + reg_udral_physics_electricity_PowerTs_0_1 msg = {0}; + // Our node does not synchronize its clock with the network, so we cannot timestamp our publications: + msg.timestamp.microsecond = uavcan_time_SynchronizedTimestamp_1_0_UNKNOWN; + // TODO populate real values: + msg.value.current.ampere = 20.315F; + msg.value.voltage.volt = 51.3F; + // Serialize and publish the message: + uint8_t serialized[reg_udral_physics_dynamics_translation_LinearTs_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_]; + size_t serialized_size = sizeof(serialized); + const int8_t err = reg_udral_physics_electricity_PowerTs_0_1_serialize_(&msg, &serialized[0], &serialized_size); + assert(err >= 0); + if (err >= 0) + { + const CanardTransferMetadata transfer = { + .priority = CanardPriorityHigh, + .transfer_kind = CanardTransferKindMessage, + .port_id = state->port_id.pub.servo_power, + .remote_node_id = CANARD_NODE_ID_UNSET, + .transfer_id = (CanardTransferID)servo_transfer_id, + }; + send(state, now_usec + 10 * KILO, &transfer, serialized_size, &serialized[0], now_usec); + } + } +} + +/// Invoked every second. +static void handle1HzLoop(State *const state, const CanardMicrosecond now_usec) +{ + const bool anonymous = state->canard.node_id > CANARD_NODE_ID_MAX; + // Publish heartbeat every second unless the local node is anonymous. Anonymous nodes shall not publish heartbeat. + if (!anonymous) + { + Serial.println("Heartbeat"); + uavcan_node_Heartbeat_1_0 heartbeat = {0}; + heartbeat.uptime = (uint32_t)((now_usec - state->started_at) / MEGA); + heartbeat.mode.value = uavcan_node_Mode_1_0_OPERATIONAL; + const O1HeapDiagnostics heap_diag = o1heapGetDiagnostics(state->heap); + if (heap_diag.oom_count > 0) + { + heartbeat.health.value = uavcan_node_Health_1_0_CAUTION; + } + else + { + heartbeat.health.value = uavcan_node_Health_1_0_NOMINAL; + } + + uint8_t serialized[uavcan_node_Heartbeat_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_]; + size_t serialized_size = sizeof(serialized); + const int8_t err = uavcan_node_Heartbeat_1_0_serialize_(&heartbeat, &serialized[0], &serialized_size); + assert(err >= 0); + if (err >= 0) + { + const CanardTransferMetadata transfer = { + .priority = CanardPriorityNominal, + .transfer_kind = CanardTransferKindMessage, + .port_id = uavcan_node_Heartbeat_1_0_FIXED_PORT_ID_, + .remote_node_id = CANARD_NODE_ID_UNSET, + .transfer_id = (CanardTransferID)(state->next_transfer_id.uavcan_node_heartbeat++), + }; + send(state, + now_usec + MEGA, // Set transmission deadline 1 second, optimal for heartbeat. + &transfer, + serialized_size, + &serialized[0], + now_usec); + } + } + else // If we don't have a node-ID, obtain one by publishing allocation request messages until we get a response. + { + // The Specification says that the allocation request publication interval shall be randomized. + // We implement randomization by calling rand() at fixed intervals and comparing it against some threshold. + // There are other ways to do it, of course. See the docs in the Specification or in the DSDL definition here: + // https://github.com/OpenCyphal/public_regulated_data_types/blob/master/uavcan/pnp/8165.NodeIDAllocationData.2.0.dsdl + // Note that a high-integrity/safety-certified application is unlikely to be able to rely on this feature. + if (rand() > RAND_MAX / 2) // NOLINT + { + // Note that this will only work over CAN FD. If you need to run PnP over Classic CAN, use message v1.0. + uavcan_pnp_NodeIDAllocationData_1_0 msg = {0}; + msg.allocated_node_id.elements[0].value = UINT16_MAX; + msg.allocated_node_id.count = 0; + uint8_t id[128 / 8]; + getUniqueID(id); + memcpy((uint8_t *)&msg.unique_id_hash, id, sizeof(msg.unique_id_hash)); + Serial.print("hash: "); + Serial.println(msg.unique_id_hash, HEX); + uint8_t serialized[uavcan_pnp_NodeIDAllocationData_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_]; + size_t serialized_size = sizeof(serialized); + const int8_t err = uavcan_pnp_NodeIDAllocationData_1_0_serialize_(&msg, &serialized[0], &serialized_size); + assert(err >= 0); + if (err >= 0) + { + const CanardTransferMetadata transfer = { + .priority = CanardPrioritySlow, + .transfer_kind = CanardTransferKindMessage, + .port_id = uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_, + .remote_node_id = CANARD_NODE_ID_UNSET, + .transfer_id = (CanardTransferID)(state->next_transfer_id.uavcan_pnp_allocation++), + }; + send(state, // The response will arrive asynchronously eventually. + now_usec + MEGA, + &transfer, + serialized_size, + &serialized[0], + now_usec); + } + } + } + + const uint64_t servo_transfer_id = state->next_transfer_id.servo_1Hz_loop++; + + if (!anonymous) + { + // Publish the servo status -- this is a low-rate message with low-severity diagnostics. + reg_udral_service_actuator_common_Status_0_1 msg = {0}; + // TODO: POPULATE THE MESSAGE: temperature, errors, etc. + uint8_t serialized[reg_udral_service_actuator_common_Status_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_]; + size_t serialized_size = sizeof(serialized); + const int8_t err = + reg_udral_service_actuator_common_Status_0_1_serialize_(&msg, &serialized[0], &serialized_size); + assert(err >= 0); + if (err >= 0) + { + const CanardTransferMetadata transfer = { + .priority = CanardPriorityNominal, + .transfer_kind = CanardTransferKindMessage, + .port_id = state->port_id.pub.servo_status, + .remote_node_id = CANARD_NODE_ID_UNSET, + .transfer_id = (CanardTransferID)servo_transfer_id, + }; + send(state, now_usec + MEGA, &transfer, serialized_size, &serialized[0], now_usec); + } + } + + // Disarm automatically if the arming subject has not been updated in a while. + if (state->servo.arming.armed && ((now_usec - state->servo.arming.last_update_at) > + (uint64_t)(reg_udral_service_actuator_common___0_1_CONTROL_TIMEOUT * MEGA))) + { + state->servo.arming.armed = false; + puts("Disarmed by timeout "); + } +} + +/// This is needed only for constructing uavcan_node_port_List_0_1. +static void fillSubscriptions(const struct CanardTreeNode *const tree, // NOLINT(misc-no-recursion) + uavcan_node_port_SubjectIDList_0_1 *const obj) +{ + if (NULL != tree) + { + fillSubscriptions(tree->lr[0], obj); + const struct CanardRxSubscription *crs = (const struct CanardRxSubscription *)tree; + assert(crs->port_id <= CANARD_SUBJECT_ID_MAX); + assert(obj->sparse_list.count < uavcan_node_port_SubjectIDList_0_1_sparse_list_ARRAY_CAPACITY_); + obj->sparse_list.elements[obj->sparse_list.count++].value = crs->port_id; + fillSubscriptions(tree->lr[1], obj); + } +} + +/// This is needed only for constructing uavcan_node_port_List_0_1. +static void fillServers(const struct CanardTreeNode *const tree, // NOLINT(misc-no-recursion) + uavcan_node_port_ServiceIDList_0_1 *const obj) +{ + if (NULL != tree) + { + fillServers(tree->lr[0], obj); + const struct CanardRxSubscription *crs = (const struct CanardRxSubscription *)tree; + assert(crs->port_id <= CANARD_SERVICE_ID_MAX); + (void)nunavutSetBit(&obj->mask_bitpacked_[0], sizeof(obj->mask_bitpacked_), crs->port_id, true); + fillServers(tree->lr[1], obj); + } +} + +/// Invoked every 10 seconds. +static void handle01HzLoop(State *const state, const CanardMicrosecond now_usec) +{ + // Publish the recommended (not required) port introspection message. No point publishing it if we're anonymous. + // The message is a bit heavy on the stack (about 2 KiB) but this is not a problem for a modern MCU. + if (state->canard.node_id <= CANARD_NODE_ID_MAX) + { + uavcan_node_port_List_0_1 m = {0}; + uavcan_node_port_List_0_1_initialize_(&m); + uavcan_node_port_SubjectIDList_0_1_select_sparse_list_(&m.publishers); + uavcan_node_port_SubjectIDList_0_1_select_sparse_list_(&m.subscribers); + + // Indicate which subjects we publish to. Don't forget to keep this updated if you add new publications! + { + size_t *const cnt = &m.publishers.sparse_list.count; + m.publishers.sparse_list.elements[(*cnt)++].value = uavcan_node_Heartbeat_1_0_FIXED_PORT_ID_; + m.publishers.sparse_list.elements[(*cnt)++].value = uavcan_node_port_List_0_1_FIXED_PORT_ID_; + if (state->port_id.pub.servo_feedback <= CANARD_SUBJECT_ID_MAX) + { + m.publishers.sparse_list.elements[(*cnt)++].value = state->port_id.pub.servo_feedback; + } + if (state->port_id.pub.servo_status <= CANARD_SUBJECT_ID_MAX) + { + m.publishers.sparse_list.elements[(*cnt)++].value = state->port_id.pub.servo_status; + } + if (state->port_id.pub.servo_power <= CANARD_SUBJECT_ID_MAX) + { + m.publishers.sparse_list.elements[(*cnt)++].value = state->port_id.pub.servo_power; + } + if (state->port_id.pub.servo_dynamics <= CANARD_SUBJECT_ID_MAX) + { + m.publishers.sparse_list.elements[(*cnt)++].value = state->port_id.pub.servo_dynamics; + } + } + + // Indicate which servers and subscribers we implement. + // We could construct the list manually but it's easier and more robust to just query libcanard for that. + fillSubscriptions(state->canard.rx_subscriptions[CanardTransferKindMessage], &m.subscribers); + fillServers(state->canard.rx_subscriptions[CanardTransferKindRequest], &m.servers); + fillServers(state->canard.rx_subscriptions[CanardTransferKindResponse], &m.clients); // For regularity. + + // Serialize and publish the message. Use a smaller buffer if you know that message is always small. + uint8_t serialized[uavcan_node_port_List_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_]; + size_t serialized_size = uavcan_node_port_List_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_; + if (uavcan_node_port_List_0_1_serialize_(&m, &serialized[0], &serialized_size) >= 0) + { + const CanardTransferMetadata transfer = { + .priority = CanardPriorityOptional, // Mind the priority. + .transfer_kind = CanardTransferKindMessage, + .port_id = uavcan_node_port_List_0_1_FIXED_PORT_ID_, + .remote_node_id = CANARD_NODE_ID_UNSET, + .transfer_id = (CanardTransferID)(state->next_transfer_id.uavcan_node_port_list++), + }; + send(state, now_usec + MEGA, &transfer, serialized_size, &serialized[0], now_usec); + } + } +} + +/// https://github.com/OpenCyphal/public_regulated_data_types/blob/master/reg/udral/service/actuator/servo/_.0.1.dsdl +static void processMessageServoSetpoint(State *const state, + const reg_udral_physics_dynamics_translation_Linear_0_1 *const msg) +{ + Serial.println("processMessageServoSetpoint"); + state->servo.position = msg->kinematics.position.meter; + state->servo.velocity = msg->kinematics.velocity.meter_per_second; + state->servo.acceleration = msg->kinematics.acceleration.meter_per_second_per_second; + state->servo.force = msg->force.newton; +} + +/// https://github.com/OpenCyphal/public_regulated_data_types/blob/master/reg/udral/service/common/Readiness.0.1.dsdl +static void processMessageServiceReadiness(State *const state, + const reg_udral_service_common_Readiness_0_1 *const msg, + const CanardMicrosecond monotonic_time) +{ + Serial.println("processMessageServiceReadiness"); + state->servo.arming.armed = msg->value >= reg_udral_service_common_Readiness_0_1_ENGAGED; + state->servo.arming.last_update_at = monotonic_time; +} + +static void processMessagePlugAndPlayNodeIDAllocation(State *const state, + const uavcan_pnp_NodeIDAllocationData_1_0 *const msg) +{ + uint8_t uid[uavcan_node_GetInfo_Response_1_0_unique_id_ARRAY_CAPACITY_] = {0}; + getUniqueID(uid); + if ((msg->allocated_node_id.elements[0].value <= CANARD_NODE_ID_MAX) && (msg->allocated_node_id.count > 0) && (memcmp(uid, (uint8_t *)&msg->unique_id_hash, 6) == 0)) + { + Serial.print("Got processMessagePlugAndPlayNodeIDAllocation response "); + Serial.println(msg->allocated_node_id.elements[0].value); + // printf("Got PnP node-ID allocation: %d\n", msg->allocated_node_id.elements[0].value); + state->canard.node_id = (CanardNodeID)msg->allocated_node_id.elements[0].value; + // Store the value into the non-volatile storage. + uavcan_register_Value_1_0 reg = {0}; + uavcan_register_Value_1_0_select_natural16_(®); + reg.natural16.value.elements[0] = msg->allocated_node_id.elements[0].value; + reg.natural16.value.count = 1; + registerWrite("uavcan.node.id", ®); + // We no longer need the subscriber, drop it to free up the resources (both memory and CPU time). + (void)canardRxUnsubscribe(&state->canard, + CanardTransferKindMessage, + uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_); + } + else + { + Serial.println("Ignoring processMessagePlugAndPlayNodeIDAllocation response"); + } + // Otherwise, ignore it: either it is a request from another node or it is a response to another node. +} + +static uavcan_node_ExecuteCommand_Response_1_1 processRequestExecuteCommand( + const uavcan_node_ExecuteCommand_Request_1_1 *req) +{ + uavcan_node_ExecuteCommand_Response_1_1 resp = {0}; + switch (req->command) + { + case uavcan_node_ExecuteCommand_Request_1_1_COMMAND_BEGIN_SOFTWARE_UPDATE: + { + Serial.println("uavcan_node_ExecuteCommand_Request_1_1_COMMAND_BEGIN_SOFTWARE_UPDATE"); + char file_name[uavcan_node_ExecuteCommand_Request_1_1_parameter_ARRAY_CAPACITY_ + 1] = {0}; + memcpy(file_name, req->parameter.elements, req->parameter.count); + file_name[req->parameter.count] = '\0'; + // TODO: invoke the bootloader with the specified file name. See https://github.com/Zubax/kocherga/ + // printf("Firmware update request; filename: '%s' \n", &file_name[0]); + resp.status = uavcan_node_ExecuteCommand_Response_1_1_STATUS_BAD_STATE; // This is a stub. + break; + } + case uavcan_node_ExecuteCommand_Request_1_1_COMMAND_FACTORY_RESET: + { + Serial.println("uavcan_node_ExecuteCommand_Request_1_1_COMMAND_FACTORY_RESET"); + registerDoFactoryReset(); + resp.status = uavcan_node_ExecuteCommand_Response_1_1_STATUS_SUCCESS; + break; + } + case uavcan_node_ExecuteCommand_Request_1_1_COMMAND_RESTART: + { + Serial.println("uavcan_node_ExecuteCommand_Request_1_1_COMMAND_RESTART"); + g_restart_required = true; + resp.status = uavcan_node_ExecuteCommand_Response_1_1_STATUS_SUCCESS; + break; + } + case uavcan_node_ExecuteCommand_Request_1_1_COMMAND_STORE_PERSISTENT_STATES: + { + Serial.println("uavcan_node_ExecuteCommand_Request_1_1_COMMAND_STORE_PERSISTENT_STATES"); + // If your registers are not automatically synchronized with the non-volatile storage, use this command + // to commit them to the storage explicitly. Otherwise, it is safe to remove it. + // In this demo, the registers are stored in files, so there is nothing to do. + resp.status = uavcan_node_ExecuteCommand_Response_1_1_STATUS_SUCCESS; + break; + } + // You can add vendor-specific commands here as well. + default: + { + Serial.println("uavcan_node_ExecuteCommand_Response_1_1_STATUS_BAD_COMMAND"); + resp.status = uavcan_node_ExecuteCommand_Response_1_1_STATUS_BAD_COMMAND; + break; + } + } + return resp; +} + +/// Performance notice: the register storage may be slow to access depending on its implementation (e.g., if it is +/// backed by an uncached filesystem). If your register storage implementation is slow, this may disrupt real-time +/// activities of the device. To avoid this, you can employ either measure: +/// +/// - Load registers to memory at startup, synchronize with the storage at reboot/power-down. +/// To implement fast register access you can use https://github.com/pavel-kirienko/cavl. +/// See also uavcan.node.ExecuteCommand.COMMAND_STORE_PERSISTENT_STATES. +/// +/// - If an RTOS is used (not a baremetal system), you can run a separate Cyphal processing task for +/// soft-real-time blocking operations (this approach is used in PX4). +/// +/// - Document an operational limitation that the register interface should not be accessed while ENGAGED (armed). +/// Cyphal networks usually have no service traffic while the vehicle is operational. +/// +static uavcan_register_Access_Response_1_0 processRequestRegisterAccess(const uavcan_register_Access_Request_1_0 *req) +{ + char name[uavcan_register_Name_1_0_name_ARRAY_CAPACITY_ + 1] = {0}; + assert(req->name.name.count < sizeof(name)); + memcpy(&name[0], req->name.name.elements, req->name.name.count); + name[req->name.name.count] = '\0'; + + uavcan_register_Access_Response_1_0 resp = {0}; + Serial.println("processRequestRegisterAccess"); + Serial.print("name: "); + Serial.println((char *)&req->name.name); + + // If we're asked to write a new value, do it now: + if (!uavcan_register_Value_1_0_is_empty_(&req->value)) + { + uavcan_register_Value_1_0_select_empty_(&resp.value); + registerRead(&name[0], &resp.value); + // If such register exists and it can be assigned from the request value: + if (!uavcan_register_Value_1_0_is_empty_(&resp.value) && registerAssign(&resp.value, &req->value)) + { + registerWrite(&name[0], &resp.value); + } + } + + // Regardless of whether we've just wrote a value or not, we need to read the current one and return it. + // The client will determine if the write was successful or not by comparing the request value with response. + uavcan_register_Value_1_0_select_empty_(&resp.value); + registerRead(&name[0], &resp.value); + + // Currently, all registers we implement are mutable and persistent. This is an acceptable simplification, + // but more advanced implementations will need to differentiate between them to support advanced features like + // exposing internal states via registers, perfcounters, etc. + resp._mutable = true; + resp.persistent = true; + + // Our node does not synchronize its time with the network so we can't populate the timestamp. + resp.timestamp.microsecond = uavcan_time_SynchronizedTimestamp_1_0_UNKNOWN; + + return resp; +} + +/// Constructs a response to uavcan.node.GetInfo which contains the basic information about this node. +static uavcan_node_GetInfo_Response_1_0 processRequestNodeGetInfo() +{ + + Serial.println("processRequestNodeGetInfo"); + uavcan_node_GetInfo_Response_1_0 resp = {0}; + resp.protocol_version.major = CANARD_CYPHAL_SPECIFICATION_VERSION_MAJOR; + resp.protocol_version.minor = CANARD_CYPHAL_SPECIFICATION_VERSION_MINOR; + + // The hardware version is not populated in this demo because it runs on no specific hardware. + // An embedded node like a servo would usually determine the version by querying the hardware. + + resp.software_version.major = VERSION_MAJOR; + resp.software_version.minor = VERSION_MINOR; + resp.software_vcs_revision_id = VCS_REVISION_ID; + + getUniqueID(resp.unique_id); + + // The node name is the name of the product like a reversed Internet domain name (or like a Java package). + resp.name.count = strlen(NODE_NAME); + memcpy(&resp.name.elements, NODE_NAME, resp.name.count); + + // The software image CRC and the Certificate of Authenticity are optional so not populated in this demo. + return resp; +} + +static void processReceivedTransfer(State *const state, + const struct CanardRxTransfer *const transfer, + const CanardMicrosecond now_usec) +{ + if (transfer->metadata.transfer_kind == CanardTransferKindMessage) + { + size_t size = transfer->payload.size; + if (transfer->metadata.port_id == state->port_id.sub.servo_setpoint) + { + reg_udral_physics_dynamics_translation_Linear_0_1 msg = {0}; + if (reg_udral_physics_dynamics_translation_Linear_0_1_deserialize_(&msg, (uint8_t *)transfer->payload.data, &size) >= + 0) + { + processMessageServoSetpoint(state, &msg); + } + } + else if (transfer->metadata.port_id == state->port_id.sub.servo_readiness) + { + reg_udral_service_common_Readiness_0_1 msg = {0}; + if (reg_udral_service_common_Readiness_0_1_deserialize_(&msg, (uint8_t *)transfer->payload.data, &size) >= 0) + { + processMessageServiceReadiness(state, &msg, transfer->timestamp_usec); + } + } + else if (transfer->metadata.port_id == uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_) + { + uavcan_pnp_NodeIDAllocationData_1_0 msg = {0}; + if (uavcan_pnp_NodeIDAllocationData_1_0_deserialize_(&msg, (uint8_t *)transfer->payload.data, &size) >= 0) + { + processMessagePlugAndPlayNodeIDAllocation(state, &msg); + } + } + else + { + assert(false); // Seems like we have set up a port subscription without a handler -- bad implementation. + } + } + else if (transfer->metadata.transfer_kind == CanardTransferKindRequest) + { + if (transfer->metadata.port_id == uavcan_node_GetInfo_1_0_FIXED_PORT_ID_) + { + // The request object is empty so we don't bother deserializing it. Just send the response. + const uavcan_node_GetInfo_Response_1_0 resp = processRequestNodeGetInfo(); + uint8_t serialized[uavcan_node_GetInfo_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_]; + size_t serialized_size = sizeof(serialized); + const int8_t res = uavcan_node_GetInfo_Response_1_0_serialize_(&resp, &serialized[0], &serialized_size); + if (res >= 0) + { + sendResponse(state, transfer, serialized_size, &serialized[0], now_usec); + } + else + { + assert(false); + } + } + else if (transfer->metadata.port_id == uavcan_register_Access_1_0_FIXED_PORT_ID_) + { + uavcan_register_Access_Request_1_0 req = {0}; + size_t size = transfer->payload.size; + if (uavcan_register_Access_Request_1_0_deserialize_(&req, (uint8_t *)transfer->payload.data, &size) >= 0) + { + const uavcan_register_Access_Response_1_0 resp = processRequestRegisterAccess(&req); + uint8_t serialized[uavcan_register_Access_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_]; + size_t serialized_size = sizeof(serialized); + if (uavcan_register_Access_Response_1_0_serialize_(&resp, &serialized[0], &serialized_size) >= 0) + { + sendResponse(state, transfer, serialized_size, &serialized[0], now_usec); + } + } + } + else if (transfer->metadata.port_id == uavcan_register_List_1_0_FIXED_PORT_ID_) + { + + uavcan_register_List_Request_1_0 req = {0}; + size_t size = transfer->payload.size; + + Serial.println("uavcan_register_List_1_0_FIXED_PORT_ID_"); + if (uavcan_register_List_Request_1_0_deserialize_(&req, (uint8_t *)transfer->payload.data, &size) >= 0) + { + const uavcan_register_List_Response_1_0 resp = {.name = registerGetNameByIndex(req.index)}; + uint8_t serialized[uavcan_register_List_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_]; + size_t serialized_size = sizeof(serialized); + if (uavcan_register_List_Response_1_0_serialize_(&resp, &serialized[0], &serialized_size) >= 0) + { + sendResponse(state, transfer, serialized_size, &serialized[0], now_usec); + } + } + } + else if (transfer->metadata.port_id == uavcan_node_ExecuteCommand_1_1_FIXED_PORT_ID_) + { + uavcan_node_ExecuteCommand_Request_1_1 req = {0}; + size_t size = transfer->payload.size; + Serial.println("uavcan_node_ExecuteCommand_1_1_FIXED_PORT_ID_"); + if (uavcan_node_ExecuteCommand_Request_1_1_deserialize_(&req, (uint8_t *)transfer->payload.data, &size) >= 0) + { + const uavcan_node_ExecuteCommand_Response_1_1 resp = processRequestExecuteCommand(&req); + uint8_t serialized[uavcan_node_ExecuteCommand_Response_1_1_SERIALIZATION_BUFFER_SIZE_BYTES_]; + size_t serialized_size = sizeof(serialized); + if (uavcan_node_ExecuteCommand_Response_1_1_serialize_(&resp, &serialized[0], &serialized_size) >= 0) + { + sendResponse(state, transfer, serialized_size, &serialized[0], now_usec); + } + } + } + else + { + assert(false); // Seems like we have set up a port subscription without a handler -- bad implementation. + } + } + else + { + assert(false); // Bad implementation -- check your subscriptions. + } +} + +static void *canardAllocate(void *const user_reference, const size_t amount) +{ + O1HeapInstance *const heap = ((State *)user_reference)->heap; + assert(o1heapDoInvariantsHold(heap)); + return o1heapAllocate(heap, amount); +} + +static void canardDeallocate(void *const user_reference, const size_t amount, void *const pointer) +{ + (void)amount; + O1HeapInstance *const heap = ((State *)user_reference)->heap; + o1heapFree(heap, pointer); +} + +int mainloop() +{ + struct timespec ts; + (void)clock_gettime(CLOCK_REALTIME, &ts); + srand((unsigned)ts.tv_nsec); + + State state = {0}; + + registerDoFactoryReset(); + // A simple application like a servo node typically does not require more than 20 KiB of heap and 4 KiB of stack. + // For the background and related theory refer to the following resources: + // - https://github.com/OpenCyphal/libcanard/blob/master/README.md + // - https://github.com/pavel-kirienko/o1heap/blob/master/README.md + // - https://forum.opencyphal.org/t/uavcanv1-libcanard-nunavut-templates-memory-usage-concerns/1118/4 + _Alignas(O1HEAP_ALIGNMENT) static uint8_t heap_arena[1024 * 20] = {0}; + state.heap = o1heapInit(heap_arena, sizeof(heap_arena), NULL, NULL); + assert(NULL != state.heap); + struct CanardMemoryResource canard_memory = { + .user_reference = &state, + .deallocate = canardDeallocate, + .allocate = canardAllocate, + }; + + // The libcanard instance requires the allocator for managing protocol states. + state.canard = canardInit(canard_memory); + state.canard.user_reference = &state; // Make the state reachable from the canard instance. + + // Restore the node-ID from the corresponding standard register. Default to anonymous. + uavcan_register_Value_1_0 val = {0}; + uavcan_register_Value_1_0_select_natural16_(&val); + val.natural16.value.count = 1; + val.natural16.value.elements[0] = UINT16_MAX; // This means undefined (anonymous), per Specification/libcanard. + registerRead("uavcan.node.id", &val); // The names of the standard registers are regulated by the Specification. + assert(uavcan_register_Value_1_0_is_natural16_(&val) && (val.natural16.value.count == 1)); + state.canard.node_id = (val.natural16.value.elements[0] > CANARD_NODE_ID_MAX) + ? CANARD_NODE_ID_UNSET + : (CanardNodeID)val.natural16.value.elements[0]; + + // The description register is optional but recommended because it helps constructing/maintaining large networks. + // It simply keeps a human-readable description of the node that should be empty by default. + uavcan_register_Value_1_0_select_string_(&val); + val._string.value.count = 0; + registerRead("uavcan.node.description", &val); // We don't need the value, we just need to ensure it exists. + + // The UDRAL cookie is used to mark nodes that are auto-configured by a specific auto-configuration authority. + // We don't use this value, it is managed by remote nodes; our only responsibility is to persist it across reboots. + // This register is entirely optional though; if not provided, the node will have to be configured manually. + uavcan_register_Value_1_0_select_string_(&val); + val._string.value.count = 0; // The value should be empty by default, meaning that the node is not configured. + registerRead("udral.pnp.cookie", &val); + + // Announce which UDRAL network services we support by populating appropriate registers. They are supposed to be + // immutable (read-only), but in this simplified demo we don't support that, so we make them mutable (do fix this). + uavcan_register_Value_1_0_select_string_(&val); + strcpy((char *)val._string.value.elements, "servo"); // The prefix in port names like "servo.feedback", etc. + val._string.value.count = strlen((const char *)val._string.value.elements); + registerWrite("reg.udral.service.actuator.servo", &val); + + // Configure the transport by reading the appropriate standard registers. + uavcan_register_Value_1_0_select_natural16_(&val); + val.natural16.value.count = 1; + val.natural16.value.elements[0] = CANARD_MTU_CAN_CLASSIC; + registerRead("uavcan.can.mtu", &val); + assert(uavcan_register_Value_1_0_is_natural16_(&val) && (val.natural16.value.count == 1)); + // We also need the bitrate configuration register. In this demo we can't really use it but an embedded application + // shall define "uavcan.can.bitrate" of type natural32[2]; the second value is zero/ignored if CAN FD not supported. + const int sock[CAN_REDUNDANCY_FACTOR] = { + // socketcanOpen("vcan0", val.natural16.value.elements[0]) // + esp32twaicanOpen(CAN_TX, CAN_RX, NULL)}; + + for (uint8_t ifidx = 0; ifidx < CAN_REDUNDANCY_FACTOR; ifidx++) + { + state.canard_tx_queues[ifidx] = + canardTxInit(CAN_TX_QUEUE_CAPACITY, val.natural16.value.elements[0], canard_memory); + } + + // Load the port-IDs from the registers. You can implement hot-reloading at runtime if desired. Specification here: + // https://github.com/OpenCyphal/public_regulated_data_types/blob/master/reg/udral/service/actuator/servo/_.0.1.dsdl + // https://github.com/OpenCyphal/public_regulated_data_types/blob/master/reg/udral/README.md + // As follows from the Specification, the register group name prefix can be arbitrary; here we just use "servo". + // Publications: + state.port_id.pub.servo_feedback = // High-rate status information: all good or not, engaged or sleeping. + getSubjectID(SUBJECT_ROLE_PUBLISHER, + "servo.feedback", + reg_udral_service_actuator_common_Feedback_0_1_FULL_NAME_AND_VERSION_); + state.port_id.pub.servo_status = // A low-rate high-level status overview: temperatures, fault flags, errors. + getSubjectID(SUBJECT_ROLE_PUBLISHER, + "servo.status", + reg_udral_service_actuator_common_Status_0_1_FULL_NAME_AND_VERSION_); + state.port_id.pub.servo_power = // Electric power input measurements (voltage and current). + getSubjectID(SUBJECT_ROLE_PUBLISHER, + "servo.power", + reg_udral_physics_electricity_PowerTs_0_1_FULL_NAME_AND_VERSION_); + state.port_id.pub.servo_dynamics = // Position/speed/acceleration/force feedback. + getSubjectID(SUBJECT_ROLE_PUBLISHER, + "servo.dynamics", + reg_udral_physics_dynamics_translation_LinearTs_0_1_FULL_NAME_AND_VERSION_); + // Subscriptions: + state.port_id.sub.servo_setpoint = // This message actually commands the servo setpoint with the motion profile. + getSubjectID(SUBJECT_ROLE_SUBSCRIBER, + "servo.setpoint", + reg_udral_physics_dynamics_translation_Linear_0_1_FULL_NAME_AND_VERSION_); + state.port_id.sub.servo_readiness = // Arming subject: whether to act upon the setpoint or to stay idle. + getSubjectID(SUBJECT_ROLE_SUBSCRIBER, + "servo.readiness", + reg_udral_service_common_Readiness_0_1_FULL_NAME_AND_VERSION_); + + // Set up subject subscriptions and RPC-service servers. + // Message subscriptions: + static const CanardMicrosecond servo_transfer_id_timeout = 100 * KILO; + if (state.canard.node_id > CANARD_NODE_ID_MAX) + { + static struct CanardRxSubscription rx; + const int8_t res = // + canardRxSubscribe(&state.canard, + CanardTransferKindMessage, + uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_, + uavcan_pnp_NodeIDAllocationData_1_0_EXTENT_BYTES_, + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, + &rx); + if (res < 0) + { + return -res; + } + } + if (state.port_id.sub.servo_setpoint <= CANARD_SUBJECT_ID_MAX) // Do not subscribe if not configured. + { + static struct CanardRxSubscription rx; + const int8_t res = // + canardRxSubscribe(&state.canard, + CanardTransferKindMessage, + state.port_id.sub.servo_setpoint, + reg_udral_physics_dynamics_translation_Linear_0_1_EXTENT_BYTES_, + servo_transfer_id_timeout, + &rx); + if (res < 0) + { + return -res; + } + } + if (state.port_id.sub.servo_readiness <= CANARD_SUBJECT_ID_MAX) // Do not subscribe if not configured. + { + static struct CanardRxSubscription rx; + const int8_t res = // + canardRxSubscribe(&state.canard, + CanardTransferKindMessage, + state.port_id.sub.servo_readiness, + reg_udral_service_common_Readiness_0_1_EXTENT_BYTES_, + servo_transfer_id_timeout, + &rx); + if (res < 0) + { + return -res; + } + } + // Service servers: + { + static struct CanardRxSubscription rx; + const int8_t res = // + canardRxSubscribe(&state.canard, + CanardTransferKindRequest, + uavcan_node_GetInfo_1_0_FIXED_PORT_ID_, + uavcan_node_GetInfo_Request_1_0_EXTENT_BYTES_, + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, + &rx); + if (res < 0) + { + return -res; + } + } + { + static struct CanardRxSubscription rx; + const int8_t res = // + canardRxSubscribe(&state.canard, + CanardTransferKindRequest, + uavcan_node_ExecuteCommand_1_1_FIXED_PORT_ID_, + uavcan_node_ExecuteCommand_Request_1_1_EXTENT_BYTES_, + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, + &rx); + if (res < 0) + { + return -res; + } + } + { + static struct CanardRxSubscription rx; + const int8_t res = // + canardRxSubscribe(&state.canard, + CanardTransferKindRequest, + uavcan_register_Access_1_0_FIXED_PORT_ID_, + uavcan_register_Access_Request_1_0_EXTENT_BYTES_, + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, + &rx); + if (res < 0) + { + return -res; + } + } + { + static struct CanardRxSubscription rx; + const int8_t res = // + canardRxSubscribe(&state.canard, + CanardTransferKindRequest, + uavcan_register_List_1_0_FIXED_PORT_ID_, + uavcan_register_List_Request_1_0_EXTENT_BYTES_, + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, + &rx); + if (res < 0) + { + return -res; + } + } + + // Now the node is initialized and we're ready to roll. + state.started_at = getMonotonicMicroseconds(); + const CanardMicrosecond fast_loop_period = MEGA / 50; + CanardMicrosecond next_fast_iter_at = state.started_at + fast_loop_period; + CanardMicrosecond next_1_hz_iter_at = state.started_at + MEGA; + CanardMicrosecond next_01_hz_iter_at = state.started_at + MEGA * 10; + do + { + // Run a trivial scheduler polling the loops that run the business logic. + CanardMicrosecond now_usec = getMonotonicMicroseconds(); + if (now_usec >= next_fast_iter_at) + { + next_fast_iter_at += fast_loop_period; + handleFastLoop(&state, now_usec); + } + if (now_usec >= next_1_hz_iter_at) + { + next_1_hz_iter_at += MEGA; + handle1HzLoop(&state, now_usec); + } + if (now_usec >= next_01_hz_iter_at) + { + next_01_hz_iter_at += MEGA * 10; + handle01HzLoop(&state, now_usec); + } + + // Manage CAN RX/TX per redundant interface. + for (uint8_t ifidx = 0; ifidx < CAN_REDUNDANCY_FACTOR; ifidx++) + { + + // Transmit pending frames from the prioritized TX queues managed by libcanard. + struct CanardTxQueue *const que = &state.canard_tx_queues[ifidx]; + struct CanardTxQueueItem *tqi = canardTxPeek(que); // Find the highest-priority frame. + while (tqi != NULL) + { + // Attempt transmission only if the frame is not yet timed out while waiting in the TX queue. + // Otherwise just drop it and move on to the next one. + if ((tqi->tx_deadline_usec == 0) || (tqi->tx_deadline_usec > now_usec)) + { + const struct CanardFrame canard_frame = {.extended_can_id = tqi->frame.extended_can_id, + .payload = {.size = tqi->frame.payload.size, + .data = tqi->frame.payload.data}}; + // const int16_t result = socketcanPush(sock[ifidx], &canard_frame, 0); // Non-blocking write attempt. + const int16_t result = esp32twaicanPush(&canard_frame, 0); // Non-blocking write attempt. + // digitalWrite(38, 0); /*enable*/delay(1000); digitalWrite(38, 1); /*disable*/delay(1000); + + // const int16_t result = 0; + if (result == 0) + { + break; // The queue is full, we will try again on the next iteration. + } + if (result < 0) + { + return -result; // SocketCAN interface failure (link down?) + } + } + + struct CanardTxQueueItem *const mut_tqi = canardTxPop(que, tqi); + canardTxFree(que, &state.canard, mut_tqi); + + tqi = canardTxPeek(que); + } + + // Process received frames by feeding them from SocketCAN to libcanard. + // The order in which we handle the redundant interfaces doesn't matter -- libcanard can accept incoming + // frames from any of the redundant interface in an arbitrary order. + // The internal state machine will sort them out and remove duplicates automatically. + struct CanardFrame frame = {0}; + uint8_t buf[CANARD_MTU_CAN_FD] = {0}; + // const int16_t socketcan_result = socketcanPop(sock[ifidx], &frame, NULL, sizeof(buf), buf, 0, NULL); + const int16_t recieve_result = esp32twaicanPop(&frame, NULL, sizeof(buf), buf, 0, NULL); + + if (recieve_result == 0) // The read operation has timed out with no frames, nothing to do here. + { + break; + } + if (recieve_result < 0) // The read operation has failed. This is not a normal condition. + { + return -recieve_result; + } + // The SocketCAN adapter uses the wall clock for timestamping, but we need monotonic. + // Wall clock can only be used for time synchronization. + const CanardMicrosecond timestamp_usec = getMonotonicMicroseconds(); + struct CanardRxTransfer transfer; + memset(&transfer, 0, sizeof transfer); + const int8_t canard_result = canardRxAccept(&state.canard, timestamp_usec, &frame, ifidx, &transfer, NULL); + if (canard_result > 0) + { + processReceivedTransfer(&state, &transfer, now_usec); + state.canard.memory.deallocate(state.canard.memory.user_reference, + transfer.payload.allocated_size, + transfer.payload.data); + } + else if ((canard_result == 0) || (canard_result == -CANARD_ERROR_OUT_OF_MEMORY)) + { + (void)0; // The frame did not complete a transfer so there is nothing to do. + // OOM should never occur if the heap is sized correctly. You can track OOM errors via heap API. + } + else + { + assert(false); // No other error can possibly occur at runtime. + } + } + } while (!g_restart_required); + return 0; +} \ No newline at end of file diff --git a/pcb/stepper-closed-loop.kicad_sch b/pcb/stepper-closed-loop.kicad_sch index 32d9b6e..cb7fd23 100644 --- a/pcb/stepper-closed-loop.kicad_sch +++ b/pcb/stepper-closed-loop.kicad_sch @@ -14791,7 +14791,7 @@ (justify left) ) ) - (property "Footprint" "TestPoint:TestPoint_Pad_2.0x2.0mm" + (property "Footprint" "TestPoint:TestPoint_Pad_1.5x1.5mm" (at 22.86 124.46 0) (effects (font