cyphal wip
This commit is contained in:
parent
c4bd84cb2d
commit
64a00c5dde
5
fw-cyphal/README.md
Normal file
5
fw-cyphal/README.md
Normal file
@ -0,0 +1,5 @@
|
|||||||
|
## Develop
|
||||||
|
create `compile_commands.json`:
|
||||||
|
´´´
|
||||||
|
pio run -t compiledb
|
||||||
|
´´´
|
||||||
35
fw-cyphal/platformio.ini
Normal file
35
fw-cyphal/platformio.ini
Normal file
@ -0,0 +1,35 @@
|
|||||||
|
; 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
|
||||||
|
|
||||||
204
fw-cyphal/src/DRV8323S.hpp
Normal file
204
fw-cyphal/src/DRV8323S.hpp
Normal file
@ -0,0 +1,204 @@
|
|||||||
|
#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 */
|
||||||
143
fw-cyphal/src/esp32twaican.c
Normal file
143
fw-cyphal/src/esp32twaican.c
Normal file
@ -0,0 +1,143 @@
|
|||||||
|
/// This software is distributed under the terms of the MIT License.
|
||||||
|
/// Copyright (c) 2020 OpenCyphal
|
||||||
|
/// Authors: Pavel Kirienko <pavel.kirienko@zubax.com>, Tom De Rybel
|
||||||
|
/// <tom.derybel@robocow.be>
|
||||||
|
|
||||||
|
// This is needed to enable the necessary declarations in sys/
|
||||||
|
#include <string.h>
|
||||||
|
|
||||||
|
#include "esp32twaican.h"
|
||||||
|
|
||||||
|
#include "driver/twai.h"
|
||||||
|
|
||||||
|
int esp32twaicanOpen(const gpio_num_t pin_tx, const gpio_num_t pin_rx,
|
||||||
|
const 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;
|
||||||
|
twai_timing_config_t t_config = TWAI_TIMING_CONFIG_500KBITS();
|
||||||
|
twai_filter_config_t f_config = TWAI_FILTER_CONFIG_ACCEPT_ALL();
|
||||||
|
if (filter_config) {
|
||||||
|
f_config.acceptance_code = filter_config->extended_can_id;
|
||||||
|
f_config.acceptance_mask = filter_config->extended_mask;
|
||||||
|
f_config.single_filter = true;
|
||||||
|
}
|
||||||
|
int ret = twai_driver_install(&g_config, &t_config, &f_config);
|
||||||
|
if (ret != ESP_OK) {
|
||||||
|
// Serial.println("CAN1 Driver initialized");
|
||||||
|
return ret;
|
||||||
|
}
|
||||||
|
ret = twai_start();
|
||||||
|
if (ret != ESP_OK) {
|
||||||
|
return ret;
|
||||||
|
}
|
||||||
|
|
||||||
|
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;
|
||||||
|
ret = twai_reconfigure_alerts(alerts_to_enable, NULL);
|
||||||
|
if (ret != ESP_OK) {
|
||||||
|
// Serial.println("Failed to reconfigure alerts");
|
||||||
|
return ret;
|
||||||
|
}
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
int16_t esp32twaicanPush(const CanardFrame *const frame,
|
||||||
|
const CanardMicrosecond timeout_usec) {
|
||||||
|
if ((frame == NULL) || (frame->payload == NULL) ||
|
||||||
|
(frame->payload_size > UINT8_MAX)) {
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Queue message for transmission
|
||||||
|
twai_message_t message;
|
||||||
|
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)));
|
||||||
|
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;
|
||||||
|
}
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
|
||||||
|
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) {
|
||||||
|
if ((out_frame == NULL) || (payload_buffer == NULL)) {
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
|
||||||
|
uint32_t alerts_triggered;
|
||||||
|
twai_read_alerts(&alerts_triggered,
|
||||||
|
(timeout_usec * (1000000 / configTICK_RATE_HZ)));
|
||||||
|
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) {
|
||||||
|
// Non-blocking receive messages from the socket and validate.
|
||||||
|
const ssize_t read_size = message.data_length_code;
|
||||||
|
if (read_size < 0) {
|
||||||
|
return -1; // Error occurred -- return the negated error
|
||||||
|
// code.
|
||||||
|
}
|
||||||
|
if (read_size > payload_buffer_size) {
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
|
||||||
|
const bool valid = (message.extd) && // Extended frame
|
||||||
|
(message.rtr) && // Not RTR frame
|
||||||
|
(true /* error frame */); // Not error frame
|
||||||
|
if (!valid) {
|
||||||
|
return 0; // Not an extended data frame -- drop silently and return
|
||||||
|
// early.
|
||||||
|
}
|
||||||
|
|
||||||
|
// Handle the loopback frame logic.
|
||||||
|
const bool loopback_frame = false;
|
||||||
|
// ((uint32_t)msg.msg_flags & (uint32_t)MSG_CONFIRM) != 0;
|
||||||
|
if (loopback == NULL && loopback_frame) {
|
||||||
|
return 0; // The loopback pointer is NULL and this is a loopback frame
|
||||||
|
// -- drop silently and return early.
|
||||||
|
}
|
||||||
|
if (loopback != NULL) {
|
||||||
|
*loopback = loopback_frame;
|
||||||
|
}
|
||||||
|
// Obtain the CAN frame time stamp from the kernel.
|
||||||
|
if (NULL != out_timestamp_usec) {
|
||||||
|
// struct timeval tv;
|
||||||
|
// gettimeofday(&tv, NULL);
|
||||||
|
// (void)memset(out_frame, 0, sizeof(CanardFrame));
|
||||||
|
// *out_timestamp_usec = (CanardMicrosecond)(((uint64_t)tv.tv_sec *
|
||||||
|
// MEGA)
|
||||||
|
// + (uint64_t)tv.tv_usec);
|
||||||
|
*out_timestamp_usec =
|
||||||
|
(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;
|
||||||
|
(void)memcpy(payload_buffer, &message.data[0], message.data_length_code);
|
||||||
|
return 1;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
84
fw-cyphal/src/esp32twaican.h
Normal file
84
fw-cyphal/src/esp32twaican.h
Normal file
@ -0,0 +1,84 @@
|
|||||||
|
/// ____ ______ __ __
|
||||||
|
/// / __ `____ ___ ____ / ____/_ ______ / /_ ____
|
||||||
|
/// / /
|
||||||
|
/// / / / / __ `/ _ `/ __ `/ / / / / / __ `/ __ `/ __
|
||||||
|
/// `/ /
|
||||||
|
/// / /_/ / /_/ / __/ / / / /___/ /_/ / /_/ / / / / /_/
|
||||||
|
/// / /
|
||||||
|
/// `____/ .___/`___/_/ /_/`____/`__, / .___/_/
|
||||||
|
/// /_/`__,_/_/
|
||||||
|
/// /_/ /____/_/
|
||||||
|
///
|
||||||
|
/// 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
|
||||||
382
fw-cyphal/src/main.cpp
Normal file
382
fw-cyphal/src/main.cpp
Normal file
@ -0,0 +1,382 @@
|
|||||||
|
// #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();
|
||||||
9
fw-cyphal/src/test.h
Normal file
9
fw-cyphal/src/test.h
Normal file
@ -0,0 +1,9 @@
|
|||||||
|
#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); \
|
||||||
|
}
|
||||||
File diff suppressed because it is too large
Load Diff
@ -1,6 +1,6 @@
|
|||||||
{
|
{
|
||||||
"board": {
|
"board": {
|
||||||
"active_layer": 0,
|
"active_layer": 37,
|
||||||
"active_layer_preset": "",
|
"active_layer_preset": "",
|
||||||
"auto_track_width": false,
|
"auto_track_width": false,
|
||||||
"hidden_netclasses": [],
|
"hidden_netclasses": [],
|
||||||
|
|||||||
File diff suppressed because it is too large
Load Diff
Loading…
x
Reference in New Issue
Block a user