cyphal?
This commit is contained in:
parent
d26f6e3478
commit
67477dac47
6
.gitignore
vendored
6
.gitignore
vendored
@ -1,2 +1,8 @@
|
||||
*-backups
|
||||
fw/.pio
|
||||
fw-can/
|
||||
fw-cyphal/
|
||||
pcb/*-bak
|
||||
pcb/*.lck
|
||||
pyvenv.cfg
|
||||
compile_commands.json
|
||||
|
||||
3
.gitmodules
vendored
Normal file
3
.gitmodules
vendored
Normal file
@ -0,0 +1,3 @@
|
||||
[submodule "fw/lib/libcanard"]
|
||||
path = fw/lib/libcanard
|
||||
url = git@git.nilsschulte.de:nils/libcanard.git
|
||||
@ -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
|
||||
|
||||
@ -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();
|
||||
}
|
||||
@ -1,5 +0,0 @@
|
||||
## Develop
|
||||
create `compile_commands.json`:
|
||||
´´´
|
||||
pio run -t compiledb
|
||||
´´´
|
||||
@ -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
|
||||
|
||||
@ -1,204 +0,0 @@
|
||||
#ifndef _DRV8323S_H
|
||||
#define _DRV8323S_H
|
||||
|
||||
#include <Arduino.h>
|
||||
#include <SPI.h>
|
||||
#include <cstdint>
|
||||
#include <drivers/BLDCDriver3PWM.h>
|
||||
|
||||
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 */
|
||||
@ -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 <pavel.kirienko@zubax.com>
|
||||
|
||||
#ifndef ESP32TWAICAN_H_INCLUDED
|
||||
#define ESP32TWAICAN_H_INCLUDED
|
||||
|
||||
#include "canard.h"
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
#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
|
||||
@ -1,382 +0,0 @@
|
||||
// #include <Preferences.h>
|
||||
|
||||
#include "Arduino.h"
|
||||
#include "SPI.h"
|
||||
#include "USB.h"
|
||||
// #include "Wire.h"
|
||||
// #include "Wire.h"
|
||||
#include <SimpleFOC.h>
|
||||
// #include <INA3221.h>
|
||||
|
||||
#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 <cmath>
|
||||
|
||||
// #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 <esp_task_wdt.h>
|
||||
#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();
|
||||
@ -1,9 +0,0 @@
|
||||
#include <Arduino.h>
|
||||
|
||||
#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); \
|
||||
}
|
||||
7
fw/.gitignore
vendored
Normal file
7
fw/.gitignore
vendored
Normal file
@ -0,0 +1,7 @@
|
||||
.cache
|
||||
.pio/build
|
||||
.vscode
|
||||
*.bak
|
||||
include/nunavut/*
|
||||
include/reg/*
|
||||
include/uavcan/*
|
||||
10
fw/can_if_up.sh
Executable file
10
fw/can_if_up.sh
Executable file
@ -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
|
||||
1
fw/lib/libcanard
Submodule
1
fw/lib/libcanard
Submodule
@ -0,0 +1 @@
|
||||
Subproject commit 962e9c5ac9629d761c83d9c6f4f8179faad1e8b0
|
||||
98
fw/platformio-nunavut.py
Normal file
98
fw/platformio-nunavut.py
Normal file
@ -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)
|
||||
@ -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
|
||||
|
||||
@ -4,19 +4,28 @@
|
||||
/// <tom.derybel@robocow.be>
|
||||
|
||||
// This is needed to enable the necessary declarations in sys/
|
||||
#include "FreeRTOS.h"
|
||||
#include <string.h>
|
||||
|
||||
#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;
|
||||
}
|
||||
85
fw/src/esp32twaican.h
Normal file
85
fw/src/esp32twaican.h
Normal file
@ -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 <pavel.kirienko@zubax.com>
|
||||
|
||||
#ifndef ESP32TWAICAN_H_INCLUDED
|
||||
#define ESP32TWAICAN_H_INCLUDED
|
||||
|
||||
#include "canard.h"
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
#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
|
||||
@ -1,8 +1,27 @@
|
||||
// #include <Preferences.h>
|
||||
|
||||
#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 <SimpleFOC.h>
|
||||
@ -22,6 +41,8 @@
|
||||
#include "HybridStepperMotor.h"
|
||||
#include <cmath>
|
||||
|
||||
#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 <uavcan/pnp/NodeIDAllocationData_2_0.h>
|
||||
|
||||
#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;
|
||||
|
||||
32
fw/src/pin_def.h
Normal file
32
fw/src/pin_def.h
Normal file
@ -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
|
||||
137
fw/src/register.hpp
Normal file
137
fw/src/register.hpp
Normal file
@ -0,0 +1,137 @@
|
||||
#pragma once
|
||||
|
||||
#include <Preferences.h>
|
||||
#include <uavcan/_register/Value_1_0.h>
|
||||
#include <uavcan/_register/Name_1_0.h>
|
||||
#include <Arduino.h>
|
||||
#include <string.h>
|
||||
|
||||
#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 <nvs_flash.h>
|
||||
|
||||
void registerDoFactoryReset(void)
|
||||
{
|
||||
nvs_flash_erase(); // erase the NVS partition and...
|
||||
nvs_flash_init(); // initialize the NVS partition.
|
||||
}
|
||||
1173
fw/src/udral_servo.hpp
Normal file
1173
fw/src/udral_servo.hpp
Normal file
File diff suppressed because it is too large
Load Diff
@ -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
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user