nils/N17BLDC
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

save lut?

Browse Source
This commit is contained in:
Nils Schulte 2025-06-16 18:33:00 +02:00
parent dc902aa9b3
commit 7dfe4041fb
5 changed files with 310 additions and 487 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
fw/can_if_up.sh
View File

@ -1,7 +1,7 @@
#!/bin/bash #!/bin/bash
sudo ip link set can0 type can bitrate 500000 && sudo ip link set can0 up sudo ip link set can0 type can bitrate 500000 && sudo ip link set can0 up
rm ~/allocation_table.db # rm ~/allocation_table.db
export UAVCAN__CAN__IFACE="socketcan:can0" export UAVCAN__CAN__IFACE="socketcan:can0"
export UAVCAN__NODE__ID=127 export UAVCAN__NODE__ID=127

4
fw/src/DRV8323S.hpp
View File

@ -216,6 +216,7 @@ void drv8323s_init(struct drv8323s_foc_driver *dev, int phA, int phB, int phC,
delay(1); delay(1);
drv8323s_lock_spi(dev, false); drv8323s_lock_spi(dev, false);
delay(1); delay(1);
/* Set 3PWM Mode */
drv8323s_set_PWM_mode(dev, DRV8323S_PWM_MODE_3PWM); drv8323s_set_PWM_mode(dev, DRV8323S_PWM_MODE_3PWM);
delay(1); delay(1);
drv8323s_set_OCP_mode(dev, DRV8323S_OCP_MODE_LATCHED); drv8323s_set_OCP_mode(dev, DRV8323S_OCP_MODE_LATCHED);
@ -227,7 +228,6 @@ void drv8323s_init(struct drv8323s_foc_driver *dev, int phA, int phB, int phC,
dev->focdriver = new BLDCDriver3PWM(phA, phB, phC); dev->focdriver = new BLDCDriver3PWM(phA, phB, phC);
dev->focdriver->init(); dev->focdriver->init();
/* Set 3PWM Mode */
} }
void drv8323s_set_idrive(struct drv8323s_foc_driver *dev, int positive_i, void drv8323s_set_idrive(struct drv8323s_foc_driver *dev, int positive_i,
@ -260,8 +260,6 @@ void drv8323s_set_idrive(struct drv8323s_foc_driver *dev, int positive_i,
reg_value |= i << DRV8323S_IDRIVEN; reg_value |= i << DRV8323S_IDRIVEN;
uint16_t result; uint16_t result;
drv8323s_write_spi(dev, reg, reg_value, &result); drv8323s_write_spi(dev, reg, reg_value, &result);
// TODO SPI call
} }
#endif /* _DRV8323S_H */ #endif /* _DRV8323S_H */

495
fw/src/main.cpp
View File

@ -1,5 +1,7 @@
// #include <Preferences.h> // #include <Preferences.h>
#include "esp32-hal.h"
#include <cmath>
#define NODE_NAME "N17BLDC" #define NODE_NAME "N17BLDC"
#define VERSION_MAJOR 0 #define VERSION_MAJOR 0
@ -8,49 +10,37 @@
#include "Arduino.h" #include "Arduino.h"
#define SSIF_USBSERIAL #define USE_USBSERIAL
#ifdef SSIF_USBSERIAL #ifdef USE_USBSERIAL
#include "USB.h" #include "USB.h"
USBCDC usbserial; USBCDC usbserial;
#define SSYNCIF usbserial #ifdef Serial
#undef Serial
#endif
#define Serial usbserial #define Serial usbserial
#else #else
#define SSYNCIF Serial1 #define Serial Serial1
#endif #endif
// #define SSYNCIF Serial // #define Serial Serial
#include "canard.c" #include "canard.c"
#include "SPI.h" #include "SPI.h"
// #include "Wire.h" // #include "Wire.h"
// #include "Wire.h"
#include <SimpleFOC.h> #include <SimpleFOC.h>
// #include <INA3221.h>
#include "SimpleFOCDrivers.h" #include "SimpleFOCDrivers.h"
// #include "comms/streams/BinaryIO.h"
// #include "encoders/as5600/MagneticSensorAS5600.h"
#include "encoders/calibrated/CalibratedSensor.h" #include "encoders/calibrated/CalibratedSensor.h"
// #include "encoders/esp32hwencoder/ESP32HWEncoder.h" // #include "encoders/esp32hwencoder/ESP32HWEncoder.h"
#include "DRV8323S.hpp" #include "DRV8323S.hpp"
#include "HybridStepperMotor.h"
#include "esp32-hal-adc.h" #include "esp32-hal-adc.h"
#include "esp32-hal-gpio.h" #include "esp32-hal-gpio.h"
// #include "motors/HybridStepperMotor/HybridStepperMotor.h"
#include "HybridStepperMotor.h"
#include <cmath>
#include "udral_servo.hpp" #include "udral_servo.hpp"
// #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"
#include <uavcan/pnp/NodeIDAllocationData_2_0.h> #include <uavcan/pnp/NodeIDAllocationData_2_0.h>
#include "pin_def.h" #include "pin_def.h"
@ -63,7 +53,6 @@ USBCDC usbserial;
(TEMP(analogRead(TERMISTOR_PCB) * 3.3 / 4096.0, 3435.0, 10000.0, 10000.0, \ (TEMP(analogRead(TERMISTOR_PCB) * 3.3 / 4096.0, 3435.0, 10000.0, 10000.0, \
3.3)) 3.3))
#define vdrive_read (analogRead(VSENSE_PIN) * 20.08f / 845) #define vdrive_read (analogRead(VSENSE_PIN) * 20.08f / 845)
//* 20.8f/ 21033.75) //* 20.8f/ 21033.75)
@ -81,28 +70,22 @@ USBCDC usbserial;
#endif #endif
const int voltage_lpf = 50; const int voltage_lpf = 50;
// BLDC motor & driver instance
// BLDCMotor motor = BLDCMotor(pole pair number);
// #include "ICM42670P.h"
// ICM42670 IMU(SPI, 0, 100000);
// ESP32HWEncoder encoder = // ESP32HWEncoder encoder =
// ESP32HWEncoder(ENC_A, ENC_B, 4096 / 4); // The Index pin can be omitted // ESP32HWEncoder(ENC_A, ENC_B, 4096 / 4); // The Index pin can be omitted
Encoder encoder = Encoder(ENC_A, ENC_B, 4096 / 4); auto encoder_absolute = MagneticSensorSPI(AS5047_SPI, 18);
CalibratedSensor encoder_calibrated = CalibratedSensor(encoder); Encoder encoder = Encoder(ENC_A, ENC_B, 4096 / 4, 0, &encoder_absolute);
float encoder_calibration_lut[90];
CalibratedSensor encoder_calibrated =
CalibratedSensor(encoder, sizeof(encoder_calibration_lut) /
sizeof(encoder_calibration_lut[0]));
// MagneticSensorAS5600 encoder = MagneticSensorAS5600(0x36); // MagneticSensorAS5600 encoder = MagneticSensorAS5600(0x36);
#ifdef MOTOR #ifdef MOTOR
struct drv8323s_foc_driver drv8323s; struct drv8323s_foc_driver drv8323s;
// BLDCMotor motor = BLDCMotor(100); // 24N22P // BLDCMotor motor = BLDCMotor(100); // 24N22P
HybridStepperMotor motor = HybridStepperMotor(50); //, 3.7, 10, 4.5 / 1000); HybridStepperMotor motor = HybridStepperMotor(50); //, 3.7, 10, 4.5 / 1000);
// 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 #endif
// BLDCDriver3PWM driver = BLDCDriver3PWM(pwmA, pwmB, pwmC, Enable(optional));
void read_i2c(uint8_t addr, uint8_t reg, uint8_t *buf, uint8_t len) { void read_i2c(uint8_t addr, uint8_t reg, uint8_t *buf, uint8_t len) {
Wire.beginTransmission(addr); Wire.beginTransmission(addr);
@ -138,76 +121,44 @@ void write_dual_i2c(uint8_t addr, uint8_t reg_lsb, uint8_t val1, uint8_t val2) {
} }
SemaphoreHandle_t mutex; SemaphoreHandle_t mutex;
struct sync_params {}; struct UdralServoState servo_state = {0};
TaskHandle_t taskFocHandle; void mutexed_state_sync(UdralServoState *const state) {
void foc_task(void *parameter) { xSemaphoreTake(mutex, portMAX_DELAY);
long last_comm = 0; servo_state.target_force = state->target_force;
while (true) { servo_state.target_acceleration = state->target_acceleration;
#ifdef MOTOR servo_state.target_velocity = state->target_velocity;
// drivers[0].voltage_power_supply = s_foc.vcc_voltage_mV / 1000.0f; servo_state.target_position = state->target_position;
// for (int i = 1; i < NUM_MOTORS; i += 1) servo_state.arming = state->arming;
// drivers[i].voltage_power_supply = drivers[0].voltage_power_supply; servo_state.pid = state->pid;
#endif *state = servo_state;
xSemaphoreGive(mutex);
#ifdef MOTOR
// motors.target = s_foc.vt[i] / anglefactor;
// motors.loopFOC();
// motors.move();
#else
// sensors.update();
#endif
vTaskDelay(1);
}
} }
TaskHandle_t taskCommHandle; TaskHandle_t taskCommHandle;
void comm_task(void *parameter) { void comm_task(void *parameter) { udral_loop(mutexed_state_sync); }
while (true) {
long now = millis();
vTaskDelay(1); void doA1() { encoder.handleA(); }
} void doB1() { encoder.handleB(); }
}
void doA1(){encoder.handleA();}
void doB1(){encoder.handleB();}
void setup() { void setup() {
pinMode(LED_PIN, OUTPUT); pinMode(LED_PIN, OUTPUT);
mutex = xSemaphoreCreateMutex();
#ifdef USE_USBSERIAL
Serial.begin(); Serial.begin();
USB.begin(); USB.begin();
delay(1000); Serial.setDebugOutput(
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);
digitalWrite(LED_PIN, 1); // enable
delay(1000);
}
mutex = xSemaphoreCreateMutex();
#ifdef SSIF_USBSERIAL
SSYNCIF.begin();
USB.begin();
SSYNCIF.setDebugOutput(
true); // sends all log_e(), log_i() messages to USB HW CDC true); // sends all log_e(), log_i() messages to USB HW CDC
SSYNCIF.setTxTimeoutMs(0); Serial.setTxTimeoutMs(0);
#else #else
SSYNCIF.begin(460800, SERIAL_8N1, 44, 43); Serial.begin(460800, SERIAL_8N1, 44, 43);
#endif #endif
// prefs.begin("foc"); digitalWrite(LED_PIN, 0); // enable
// SSYNCIF.print(__cplusplus); delay(100);
digitalWrite(LED_PIN, 1); // disable
delay(1000);
// Wire.setTimeOut(10); Serial.println("Start");
// Wire.begin(I2C_SDA, I2C_SCL, 1000000);
pinMode(SPI_DRV_SC, OUTPUT); pinMode(SPI_DRV_SC, OUTPUT);
@ -222,17 +173,15 @@ void setup() {
pinMode(CAL_PIN, OUTPUT); pinMode(CAL_PIN, OUTPUT);
digitalWrite(CAL_PIN, 0); // enable digitalWrite(CAL_PIN, 0); // enable
// write_i2c(0x36,0x07, 0b00000011); // initialise magnetic sensor hardware
// write_i2c(0x36, 0x09, 0xFF);
// encoder.init(&Wire);
encoder.pullup = Pullup::USE_INTERN; encoder.pullup = Pullup::USE_INTERN;
encoder.init(); encoder.init();
encoder.enableInterrupts(doA1,doB1); encoder.enableInterrupts(doA1, doB1);
pinMode(SPI_MISO, INPUT_PULLUP); pinMode(SPI_MISO, INPUT_PULLUP);
SPI.begin(SPI_CLK, SPI_MISO, SPI_MOSI, SPI_DRV_SC); SPI.begin(SPI_CLK, SPI_MISO, SPI_MOSI, SPI_DRV_SC);
#ifdef MOTOR #ifdef MOTOR
SimpleFOCDebug::enable(&SSYNCIF); SimpleFOCDebug::enable(&Serial);
drv8323s_init(&drv8323s, INHA, INHB, INHC, SPI_DRV_SC, DRVEN, &SPI); drv8323s_init(&drv8323s, INHA, INHB, INHC, SPI_DRV_SC, DRVEN, &SPI);
digitalWrite(INLABC, 1); // enable digitalWrite(INLABC, 1); // enable
// driver.init(&SPI); // driver.init(&SPI);
@ -240,280 +189,122 @@ void setup() {
motor.linkSensor(&encoder); motor.linkSensor(&encoder);
motor.linkDriver(drv8323s.focdriver); motor.linkDriver(drv8323s.focdriver);
// motor.foc_modulation = FOCModulationType::SinePWM; // motor.foc_modulation = FOCModulationType::SinePWM;
motor.foc_modulation = FOCModulationType::SpaceVectorPWM; motor.foc_modulation = FOCModulationType::SpaceVectorPWM;
motor.voltage_sensor_align = 3; motor.voltage_sensor_align = 4;
motor.voltage_limit = 5; motor.voltage_limit = 6;
// motor.controller = MotionControlType::velocity_openloop; // motor.controller = MotionControlType::velocity_openloop;
motor.controller = MotionControlType::velocity; // motor.controller = MotionControlType::velocity;
// motor.controller = MotionControlType::torque; motor.controller = MotionControlType::torque;
// motor.torque_controller = TorqueControlType::voltage; motor.torque_controller = TorqueControlType::voltage;
motor.useMonitoring(SSYNCIF); motor.useMonitoring(Serial);
drv8323s.focdriver->voltage_power_supply = vdrive_read; drv8323s.focdriver->voltage_power_supply = vdrive_read;
motor.init(); motor.init();
delay(1000); delay(1000);
// // set voltage to run calibration static Preferences pref;
encoder_calibrated.voltage_calibration = 3; pref.begin("simpleFOC");
// // Running calibration
encoder_calibrated.calibrate(motor);
// //Serial.println("Calibrating Sensor Done."); // set voltage to run calibration
// // Linking sensor to motor object encoder_calibrated.voltage_calibration = 3;
// motor.linkSensor(&encoder_calibrated); // Running calibration
motor.initFOC(); const char *encoder_calibration_lut_str = "enc_cal_lut";
const char *zero_electric_angle_str = "zero_el_angle";
const char *sensor_direction_str = "enc_dir_str";
const int settle_time_ms = 150;
if (!pref.isKey(encoder_calibration_lut_str) ||
pref.getBytesLength(encoder_calibration_lut_str) !=
sizeof(encoder_calibration_lut)) {
encoder_calibrated.calibrate(motor, NULL, 0.0, Direction::UNKNOWN,
settle_time_ms);
pref.putBytes(encoder_calibration_lut_str,
&encoder_calibrated.calibrationLut[0],
sizeof(encoder_calibration_lut));
pref.putFloat(zero_electric_angle_str, motor.zero_electric_angle);
pref.putInt(sensor_direction_str, (int)motor.sensor_direction);
} else {
pref.getBytes(encoder_calibration_lut_str, &encoder_calibration_lut[0],
sizeof(encoder_calibration_lut));
encoder_calibrated.calibrate(motor, &encoder_calibration_lut[0],
motor.zero_electric_angle, Direction::CW,
settle_time_ms);
}
if (!pref.isKey(zero_electric_angle_str)) {
motor.initFOC();
pref.putFloat(zero_electric_angle_str, motor.zero_electric_angle);
pref.putInt(sensor_direction_str, (int)motor.sensor_direction);
} else {
motor.zero_electric_angle = pref.getFloat(zero_electric_angle_str);
motor.sensor_direction = (Direction)pref.getInt(sensor_direction_str);
motor.initFOC();
}
pref.end();
motor.move(0); motor.move(0);
// sensor.init(&SPI); digitalWrite(INLABC, 0); // enable
// motors[i].linkSensor(&sensors[i]);
#endif #endif
// delay(5000); xTaskCreatePinnedToCore(&comm_task, // Function name of the task
// SSYNCIF.println("IMU.begin()"); "comm", // Name of the task (e.g. for debugging)
// int ret = IMU.begin(); 65536, // Stack size (bytes)
// if (ret != 0) { NULL, // Parameter to pass
// SSYNCIF.print("ICM42670 initialization failed: "); 1, // Task priority
// SSYNCIF.println(ret); &taskCommHandle, // Assign task handle
// while (1) { 0 // Run on the non-Arduino (1) core
// SSYNCIF.print("ICM42670 initialization failed: "); );
// SSYNCIF.println(ret);
// delay(100);
// }
// }
// Accel ODR = 100 Hz and Full Scale Range = 16G
// IMU.startAccel(100, 16);
// Gyro ODR = 100 Hz and Full Scale Range = 2000 dps
// IMU.startGyro(100, 2000);
// Wait IMU to start
// delay(100);
// 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.get(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(),
// 7/
// 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_init(WDT_TIMEOUT_s, true); // enable panic so ESP32 restarts
// esp_task_wdt_add(NULL); // add current thread to WDT watch // esp_task_wdt_add(NULL); // add current thread to WDT watch
} }
int i = 0; int i = 0;
void loop() { void loop() {
mainloop();
return;
#ifdef MOTOR #ifdef MOTOR
// drv8323s.focdriver->voltage_power_supply = vdrive_read; bool led = false;
// digitalWrite(LED_PIN, 0); // enable
motor.loopFOC();
// motor.move(3); xSemaphoreGive(mutex);
// digitalWrite(LED_PIN, 1); // enable led = servo_state.arming.armed;
motor.move(); if (!servo_state.arming.armed) {
if (motor.enabled) {
motor.move(0);
digitalWrite(INLABC, 0);
motor.disable();
}
} else {
if (!motor.enabled) {
digitalWrite(INLABC, 1);
motor.enable();
}
motor.move(servo_state.target_force);
}
servo_state.controller_temperature = temp_pcb + 273.15f; /* Kelvin */
servo_state.motor_temperature = NAN;
float curr_angle = encoder_calibrated.getAngle();
float curr_vel = encoder_calibrated.getVelocity();
unsigned long now = millis();
static unsigned long last_update = now;
unsigned long tdelta = now - last_update;
last_update = now;
float curr_accel =
tdelta ? (curr_vel - servo_state.curr_velocity) / tdelta : 0;
servo_state.curr_acceleration = curr_accel;
servo_state.curr_velocity = curr_vel;
servo_state.curr_position = curr_angle;
servo_state.curr_force = servo_state.target_force;
servo_state.vcc_volts = drv8323s.focdriver->voltage_power_supply;
servo_state.current = NAN;
xSemaphoreTake(mutex, portMAX_DELAY);
digitalWrite(LED_PIN, led ? LOW : HIGH);
drv8323s.focdriver->voltage_power_supply = vdrive_read;
motor.loopFOC();
#else #else
encoder.update(); // optional: Update manually if not using loopfoc() encoder.update(); // optional: Update manually if not using loopfoc()
#endif #endif
if(i++ % 100 == 0) { }
static float angle = 0;
static float old_angle = 0;
angle = encoder.getAngle();
SSYNCIF.print("enc: ");
SSYNCIF.print(angle * 180.0 / 3.1415);
SSYNCIF.print("mec: ");
SSYNCIF.print(encoder.getMechanicalAngle() * 180.0 / 3.1415);
SSYNCIF.print("\tcal: ");
SSYNCIF.print(encoder_calibrated.getAngle() * 180.0 / 3.1415);
SSYNCIF.println();
}
// inv_imu_sensor_event_t imu_event;
// // Get last event
// IMU.getDataFromRegisters(imu_event);
// // Format data for Serial Plotter
// SSYNCIF.print(imu_event.accel[0]);
// SSYNCIF.print(", ");
// SSYNCIF.print(imu_event.accel[1]);
// SSYNCIF.print(", ");
// SSYNCIF.print(imu_event.accel[2]);
// SSYNCIF.print(", ");
// SSYNCIF.print(imu_event.gyro[0]);
// SSYNCIF.print(", ");
// SSYNCIF.print(imu_event.gyro[1]);
// SSYNCIF.print(", ");
// SSYNCIF.print(imu_event.gyro[2]);
// SSYNCIF.print(", ");
// SSYNCIF.print(imu_event.temperature / 128 + 25);
// SSYNCIF.println();
// // Run @ ODR 100Hz
// delay(100);
// // SSYNCIF.print("V: ");
// // SSYNCIF.println(vdrive_read);
#ifdef MOTOR
// SSYNCIF.print(" ");
// uint16_t result = 0;
// SSYNCIF.print(" ");
// result = 0;
// delay(1);
// drv8323s_read_spi(&drv8323s, 0x01, &result);
// SSYNCIF.print(result, HEX);
// SSYNCIF.print(" ");
// result = 0;
// delay(1);
// drv8323s_read_spi(&drv8323s, 0x02, &result);
// SSYNCIF.print(result, HEX);
// result = 0;
// SSYNCIF.print(" ");
// 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);
// SSYNCIF.print(" ");
// result = 0;
// delay(1);
// drv8323s_read_spi(&drv8323s, 0x06, &result);
// SSYNCIF.print(result, HEX);
#endif
// SSYNCIF.print(" ");
// SSYNCIF.print(analogRead(TERMISTOR_PCB));
// SSYNCIF.print(" ");
// SSYNCIF.print(analogRead(SOA));
// SSYNCIF.print(" ");
// SSYNCIF.print(analogRead(SOB));
// SSYNCIF.print(" ");
// SSYNCIF.print(analogRead(SOC));
// SSYNCIF.print(" ");
// static float t = temp_pcb;
// t = t * 0.95 + 0.05 * temp_pcb;
// SSYNCIF.print(t);
// SSYNCIF.print(" ");
// SSYNCIF.println(vdrive_read);
// analogWrite(INHA, 255/3);
// analogWrite(INHB, 255/2);
// analogWrite(INHC, 255*2/3);
// 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();

46
fw/src/register.hpp
View File

@ -1,14 +1,21 @@
#pragma once #pragma once
#include <Arduino.h>
#include <string.h>
#include "esp_err.h"
#include "esp_mac.h"
#ifdef RAM_PREFERENCES #ifdef RAM_PREFERENCES
#include "hash_preferences.hpp" #include "hash_preferences.hpp"
#else #else
#include <Preferences.h> #include <Preferences.h>
#endif #endif
#include <uavcan/node/GetInfo_1_0.h>
#include <uavcan/_register/Value_1_0.h> #include <uavcan/_register/Value_1_0.h>
#include <uavcan/_register/Name_1_0.h> #include <uavcan/_register/Name_1_0.h>
#include <Arduino.h>
#include <string.h>
static Preferences preferences; static Preferences preferences;
static bool preferences_open = false; static bool preferences_open = false;
@ -16,8 +23,7 @@ static bool preferences_open = false;
#define REGISTER_LIST_KEY "__reglist" #define REGISTER_LIST_KEY "__reglist"
#define REGISTER_LIST_DELIM "|" #define REGISTER_LIST_DELIM "|"
String String hash(const char *str)
hash(const char *str)
{ {
unsigned long hash = 5381; unsigned long hash = 5381;
int c; int c;
@ -112,9 +118,35 @@ bool registerAssign(uavcan_register_Value_1_0 *const dst, const uavcan_register_
return false; return false;
} }
#include <nvs_flash.h>
void registerDoFactoryReset(void) void registerDoFactoryReset(void)
{ {
open_preferences_if_needed(); nvs_flash_erase(); // erase the NVS partition and...
preferences.clear(); nvs_flash_init(); // initialize the NVS partition.
close_preferences_if_needed(); esp_restart();
} }
// Returns the 128-bit unique-ID of the local node. This value is used in uavcan.node.GetInfo.Response and during the
// plug-and-play node-ID allocation by uavcan.pnp.NodeIDAllocationData. The function is infallible.
void getUniqueID(uint8_t out[uavcan_node_GetInfo_Response_1_0_unique_id_ARRAY_CAPACITY_])
{
// A real hardware node would read its unique-ID from some hardware-specific source (typically stored in ROM).
// This example is a software-only node, so we store the unique-ID in a (read-only) register instead.
uavcan_register_Value_1_0 value = {0};
uavcan_register_Value_1_0_select_unstructured_(&value);
if(esp_base_mac_addr_get(&value.unstructured.value.elements[0]) == ESP_OK) {
value.unstructured.value.count = 8;
}
for (uint8_t i = value.unstructured.value.count; i < uavcan_node_GetInfo_Response_1_0_unique_id_ARRAY_CAPACITY_; i += 1)
{
value.unstructured.value.elements[value.unstructured.value.count++] = (uint8_t)rand(); // NOLINT
}
// Populate the default; it is only used at the first run if there is no such register.
registerRead("uavcan.node.unique_id", &value);
assert(uavcan_register_Value_1_0_is_unstructured_(&value) &&
value.unstructured.value.count == uavcan_node_GetInfo_Response_1_0_unique_id_ARRAY_CAPACITY_);
memcpy(&out[0], &value.unstructured.value, uavcan_node_GetInfo_Response_1_0_unique_id_ARRAY_CAPACITY_);
}

250
fw/src/udral_servo.hpp
View File

@ -13,8 +13,8 @@
/// Copyright (C) 2021 OpenCyphal <maintainers@opencyphal.org> /// Copyright (C) 2021 OpenCyphal <maintainers@opencyphal.org>
/// Author: Pavel Kirienko <pavel@opencyphal.org> /// Author: Pavel Kirienko <pavel@opencyphal.org>
#include "esp32-hal.h"
#define NUNAVUT_ASSERT(x) assert(x) #define NUNAVUT_ASSERT(x) assert(x)
#include "esp32-hal.h"
#include "pin_def.h" #include "pin_def.h"
@ -32,12 +32,14 @@
#include <uavcan/_register/Access_1_0.h> #include <uavcan/_register/Access_1_0.h>
#include <uavcan/_register/List_1_0.h> #include <uavcan/_register/List_1_0.h>
#include <uavcan/pnp/NodeIDAllocationData_1_0.h> #include <uavcan/pnp/NodeIDAllocationData_1_0.h>
#include <uavcan/primitive/scalar/Real32_1_0.h>
#include <reg/udral/service/common/Readiness_0_1.h> #include <reg/udral/service/common/Readiness_0_1.h>
#include <reg/udral/service/actuator/common/_0_1.h> #include <reg/udral/service/actuator/common/_0_1.h>
#include <reg/udral/service/actuator/common/Feedback_0_1.h> #include <reg/udral/service/actuator/common/Feedback_0_1.h>
#include <reg/udral/service/actuator/common/Status_0_1.h> #include <reg/udral/service/actuator/common/Status_0_1.h>
#include <reg/udral/physics/dynamics/translation/LinearTs_0_1.h> #include "reg/udral/physics/dynamics/rotation/Planar_0_1.h"
#include <reg/udral/physics/dynamics/rotation/PlanarTs_0_1.h>
#include <reg/udral/physics/electricity/PowerTs_0_1.h> #include <reg/udral/physics/electricity/PowerTs_0_1.h>
#include <stdio.h> #include <stdio.h>
@ -47,7 +49,6 @@
#include <unistd.h> #include <unistd.h>
#include "USB.h" #include "USB.h"
extern USBCDC usbserial; extern USBCDC usbserial;
#define KILO 1000L #define KILO 1000L
@ -57,68 +58,93 @@ extern USBCDC usbserial;
/// For CAN FD the queue can be smaller. /// For CAN FD the queue can be smaller.
#define CAN_TX_QUEUE_CAPACITY 100 #define CAN_TX_QUEUE_CAPACITY 100
/// We keep the state of the application here. Feel free to use static variables instead if desired. /// We keep the state of the application here. Feel free to use static variables
typedef struct State /// instead if desired.
{
CanardMicrosecond started_at;
O1HeapInstance *heap; struct UdralServoState {
struct CanardInstance canard; /// Whether the servo is supposed to actuate the load or to stay idle (safe
struct CanardTxQueue canard_tx_queues[CAN_REDUNDANCY_FACTOR]; /// low-power mode).
struct {
struct {
bool armed;
CanardMicrosecond last_update_at;
} arming;
/// The state of the business logic. float vcc_volts;
struct float current;
{
/// Whether the servo is supposed to actuate the load or to stay idle (safe low-power mode).
struct
{
bool armed;
CanardMicrosecond last_update_at;
} arming;
/// Setpoint & motion profile (unsupported constraints are to be ignored). float curr_position; ///< [meter] [radian]
/// https://github.com/OpenCyphal/public_regulated_data_types/blob/master/reg/udral/service/actuator/servo/_.0.1.dsdl float curr_velocity; ///< [meter/second] [radian/second]
/// As described in the linked documentation, there are two kinds of servos supported: linear and rotary. float curr_acceleration; ///< [(meter/second)^2] [(radian/second)^2]
/// Units per-kind are: LINEAR ROTARY float curr_force; ///< [newton] [netwon*meter]
float position; ///< [meter] [radian]
float velocity; ///< [meter/second] [radian/second]
float acceleration; ///< [(meter/second)^2] [(radian/second)^2]
float force; ///< [newton] [netwon*meter]
} servo;
/// These values are read from the registers at startup. You can also implement hot reloading if desired. float motor_temperature;
/// The subjects of the servo network service are defined in the UDRAL data type definitions here: float controller_temperature;
};
struct {
/// Setpoint & motion profile (unsupported constraints are to be ignored).
/// https://github.com/OpenCyphal/public_regulated_data_types/blob/master/reg/udral/service/actuator/servo/_.0.1.dsdl /// https://github.com/OpenCyphal/public_regulated_data_types/blob/master/reg/udral/service/actuator/servo/_.0.1.dsdl
struct /// As described in the linked documentation, there are two kinds of servos
{ /// supported: linear and rotary. Units per-kind are: LINEAR ROTARY
struct float target_position; ///< [meter] [radian]
{ float target_velocity; ///< [meter/second] [radian/second]
CanardPortID servo_feedback; //< reg.udral.service.actuator.common.Feedback float target_acceleration; ///< [(meter/second)^2] [(radian/second)^2]
CanardPortID servo_status; //< reg.udral.service.actuator.common.Status float target_force; ///< [newton] [netwon*meter]
CanardPortID servo_power; //< reg.udral.physics.electricity.PowerTs
CanardPortID servo_dynamics; //< (timestamped dynamics)
} pub;
struct
{
CanardPortID servo_setpoint; //< (non-timestamped dynamics)
CanardPortID servo_readiness; //< reg.udral.service.common.Readiness
} sub;
} port_id;
/// A transfer-ID is an integer that is incremented whenever a new message is published on a given subject. struct {
/// It is used by the protocol for deduplication, message loss detection, and other critical things. uavcan_primitive_scalar_Real32_1_0 p;
/// For CAN, each value can be of type uint8_t, but we use larger types for genericity and for statistical purposes, uavcan_primitive_scalar_Real32_1_0 i;
/// as large values naturally contain the number of times each subject was published to. uavcan_primitive_scalar_Real32_1_0 d;
struct } pid;
{ };
uint64_t uavcan_node_heartbeat; };
uint64_t uavcan_node_port_list;
uint64_t uavcan_pnp_allocation; typedef void (*state_sync_f)(UdralServoState *const);
// Messages published synchronously can share the same transfer-ID:
uint64_t servo_fast_loop; struct UdralServoInternalState {
uint64_t servo_1Hz_loop; CanardMicrosecond started_at;
} next_transfer_id;
} State; O1HeapInstance *heap;
struct CanardInstance canard;
struct CanardTxQueue canard_tx_queues[CAN_REDUNDANCY_FACTOR];
/// The state of the business logic.
UdralServoState servo;
/// These values are read from the registers at startup. You can also
/// implement hot reloading if desired. The subjects of the servo network
/// service are defined in the UDRAL data type definitions here:
/// https://github.com/OpenCyphal/public_regulated_data_types/blob/master/reg/udral/service/actuator/servo/_.0.1.dsdl
struct {
struct {
CanardPortID
servo_feedback; //< reg.udral.service.actuator.common.Feedback
CanardPortID servo_status; //< reg.udral.service.actuator.common.Status
CanardPortID servo_power; //< reg.udral.physics.electricity.PowerTs
CanardPortID servo_dynamics; //< (timestamped dynamics)
} pub;
struct {
CanardPortID servo_setpoint; //< (non-timestamped dynamics)
CanardPortID servo_readiness; //< reg.udral.service.common.Readiness
} sub;
} port_id;
/// A transfer-ID is an integer that is incremented whenever a new message is
/// published on a given subject. It is used by the protocol for
/// deduplication, message loss detection, and other critical things. For CAN,
/// each value can be of type uint8_t, but we use larger types for genericity
/// and for statistical purposes, as large values naturally contain the number
/// of times each subject was published to.
struct {
uint64_t uavcan_node_heartbeat;
uint64_t uavcan_node_port_list;
uint64_t uavcan_pnp_allocation;
// Messages published synchronously can share the same transfer-ID:
uint64_t servo_fast_loop;
uint64_t servo_1Hz_loop;
} next_transfer_id;
};
/// This flag is raised when the node is requested to restart. /// This flag is raised when the node is requested to restart.
static volatile bool g_restart_required = false; static volatile bool g_restart_required = false;
@ -134,26 +160,6 @@ static CanardMicrosecond getMonotonicMicroseconds()
return micros(); return micros();
} }
// Returns the 128-bit unique-ID of the local node. This value is used in uavcan.node.GetInfo.Response and during the
// plug-and-play node-ID allocation by uavcan.pnp.NodeIDAllocationData. The function is infallible.
static void getUniqueID(uint8_t out[uavcan_node_GetInfo_Response_1_0_unique_id_ARRAY_CAPACITY_])
{
// A real hardware node would read its unique-ID from some hardware-specific source (typically stored in ROM).
// This example is a software-only node, so we store the unique-ID in a (read-only) register instead.
uavcan_register_Value_1_0 value = {0};
uavcan_register_Value_1_0_select_unstructured_(&value);
// Populate the default; it is only used at the first run if there is no such register.
for (uint8_t i = 0; i < uavcan_node_GetInfo_Response_1_0_unique_id_ARRAY_CAPACITY_; i++)
{
value.unstructured.value.elements[value.unstructured.value.count++] = (uint8_t)rand(); // NOLINT
}
registerRead("uavcan.node.unique_id", &value);
assert(uavcan_register_Value_1_0_is_unstructured_(&value) &&
value.unstructured.value.count == uavcan_node_GetInfo_Response_1_0_unique_id_ARRAY_CAPACITY_);
memcpy(&out[0], &value.unstructured.value, uavcan_node_GetInfo_Response_1_0_unique_id_ARRAY_CAPACITY_);
memset(out, 0xAB, uavcan_node_GetInfo_Response_1_0_unique_id_ARRAY_CAPACITY_);
}
typedef enum SubjectRole typedef enum SubjectRole
{ {
SUBJECT_ROLE_PUBLISHER, SUBJECT_ROLE_PUBLISHER,
@ -195,7 +201,7 @@ static CanardPortID getSubjectID(const SubjectRole role, const char *const port_
return result; return result;
} }
static void send(State *const state, static void send(struct UdralServoInternalState *const state,
const CanardMicrosecond tx_deadline_usec, const CanardMicrosecond tx_deadline_usec,
const CanardTransferMetadata *const metadata, const CanardTransferMetadata *const metadata,
const size_t payload_size, const size_t payload_size,
@ -214,7 +220,7 @@ static void send(State *const state,
} }
} }
static void sendResponse(State *const state, static void sendResponse(struct UdralServoInternalState *const state,
const struct CanardRxTransfer *const original_request_transfer, const struct CanardRxTransfer *const original_request_transfer,
const size_t payload_size, const size_t payload_size,
const void *const payload_data, const void *const payload_data,
@ -226,17 +232,17 @@ static void sendResponse(State *const state,
} }
/// Invoked at the rate of the fastest loop. /// Invoked at the rate of the fastest loop.
static void handleFastLoop(State *const state, const CanardMicrosecond now_usec) static void handleFastLoop(struct UdralServoInternalState *const state, const CanardMicrosecond now_usec)
{ {
// Apply control inputs if armed. // Apply control inputs if armed.
if (state->servo.arming.armed) if (state->servo.arming.armed)
{ {
fprintf(stderr, fprintf(stderr,
"\rp=%.3f m v=%.3f m/s a=%.3f (m/s)^2 F=%.3f N \r", "\rp=%.3f m v=%.3f m/s a=%.3f (m/s)^2 F=%.3f N \r",
(double)state->servo.position, (double)state->servo.target_position,
(double)state->servo.velocity, (double)state->servo.target_velocity,
(double)state->servo.acceleration, (double)state->servo.target_acceleration,
(double)state->servo.force); (double)state->servo.target_force);
} }
else else
{ {
@ -276,21 +282,19 @@ static void handleFastLoop(State *const state, const CanardMicrosecond now_usec)
// Publish dynamics if the subject is enabled and the node is non-anonymous. // Publish dynamics if the subject is enabled and the node is non-anonymous.
if (!anonymous && (state->port_id.pub.servo_dynamics <= CANARD_SUBJECT_ID_MAX)) if (!anonymous && (state->port_id.pub.servo_dynamics <= CANARD_SUBJECT_ID_MAX))
{ {
reg_udral_physics_dynamics_translation_LinearTs_0_1 msg = {0}; reg_udral_physics_dynamics_rotation_PlanarTs_0_1 msg = {0};
// Our node does not synchronize its clock with the network, so we cannot timestamp our publications: // Our node does not synchronize its clock with the network, so we cannot timestamp our publications:
msg.timestamp.microsecond = uavcan_time_SynchronizedTimestamp_1_0_UNKNOWN; msg.timestamp.microsecond = uavcan_time_SynchronizedTimestamp_1_0_UNKNOWN;
// A real application would source these values from the hardware; we republish the setpoint for demo purposes.
// TODO populate real values: msg.value.kinematics.angular_position.radian = state->servo.curr_position;
msg.value.kinematics.position.meter = state->servo.position; msg.value.kinematics.angular_velocity.radian_per_second = state->servo.curr_velocity;
msg.value.kinematics.velocity.meter_per_second = state->servo.velocity; msg.value.kinematics.angular_acceleration.radian_per_second_per_second = state->servo.curr_acceleration;
msg.value.kinematics.acceleration.meter_per_second_per_second = state->servo.acceleration; msg.value._torque.newton_meter = state->servo.curr_force;
msg.value.force.newton = state->servo.force;
// Serialize and publish the message: // Serialize and publish the message:
uint8_t serialized[reg_udral_physics_dynamics_translation_LinearTs_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_]; uint8_t serialized[reg_udral_physics_dynamics_rotation_PlanarTs_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_];
size_t serialized_size = sizeof(serialized); size_t serialized_size = sizeof(serialized);
const int8_t err = const int8_t err =
reg_udral_physics_dynamics_translation_LinearTs_0_1_serialize_(&msg, &serialized[0], &serialized_size); reg_udral_physics_dynamics_rotation_PlanarTs_0_1_serialize_(&msg, &serialized[0], &serialized_size);
assert(err >= 0);
if (err >= 0) if (err >= 0)
{ {
const CanardTransferMetadata transfer = { const CanardTransferMetadata transfer = {
@ -311,13 +315,12 @@ static void handleFastLoop(State *const state, const CanardMicrosecond now_usec)
// Our node does not synchronize its clock with the network, so we cannot timestamp our publications: // Our node does not synchronize its clock with the network, so we cannot timestamp our publications:
msg.timestamp.microsecond = uavcan_time_SynchronizedTimestamp_1_0_UNKNOWN; msg.timestamp.microsecond = uavcan_time_SynchronizedTimestamp_1_0_UNKNOWN;
// TODO populate real values: // TODO populate real values:
msg.value.current.ampere = 20.315F; msg.value.current.ampere = state->servo.current;
msg.value.voltage.volt = 51.3F; msg.value.voltage.volt = state->servo.vcc_volts;
// Serialize and publish the message: // Serialize and publish the message:
uint8_t serialized[reg_udral_physics_dynamics_translation_LinearTs_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_]; uint8_t serialized[reg_udral_physics_electricity_PowerTs_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_];
size_t serialized_size = sizeof(serialized); size_t serialized_size = sizeof(serialized);
const int8_t err = reg_udral_physics_electricity_PowerTs_0_1_serialize_(&msg, &serialized[0], &serialized_size); const int8_t err = reg_udral_physics_electricity_PowerTs_0_1_serialize_(&msg, &serialized[0], &serialized_size);
assert(err >= 0);
if (err >= 0) if (err >= 0)
{ {
const CanardTransferMetadata transfer = { const CanardTransferMetadata transfer = {
@ -333,7 +336,7 @@ static void handleFastLoop(State *const state, const CanardMicrosecond now_usec)
} }
/// Invoked every second. /// Invoked every second.
static void handle1HzLoop(State *const state, const CanardMicrosecond now_usec) static void handle1HzLoop(struct UdralServoInternalState *const state, const CanardMicrosecond now_usec)
{ {
const bool anonymous = state->canard.node_id > CANARD_NODE_ID_MAX; const bool anonymous = state->canard.node_id > CANARD_NODE_ID_MAX;
// Publish heartbeat every second unless the local node is anonymous. Anonymous nodes shall not publish heartbeat. // Publish heartbeat every second unless the local node is anonymous. Anonymous nodes shall not publish heartbeat.
@ -422,6 +425,8 @@ static void handle1HzLoop(State *const state, const CanardMicrosecond now_usec)
// Publish the servo status -- this is a low-rate message with low-severity diagnostics. // Publish the servo status -- this is a low-rate message with low-severity diagnostics.
reg_udral_service_actuator_common_Status_0_1 msg = {0}; reg_udral_service_actuator_common_Status_0_1 msg = {0};
// TODO: POPULATE THE MESSAGE: temperature, errors, etc. // TODO: POPULATE THE MESSAGE: temperature, errors, etc.
msg.motor_temperature.kelvin = state->servo.motor_temperature;
msg.controller_temperature.kelvin = state->servo.controller_temperature;
uint8_t serialized[reg_udral_service_actuator_common_Status_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_]; uint8_t serialized[reg_udral_service_actuator_common_Status_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_];
size_t serialized_size = sizeof(serialized); size_t serialized_size = sizeof(serialized);
const int8_t err = const int8_t err =
@ -479,7 +484,7 @@ static void fillServers(const struct CanardTreeNode *const tree, // NOLINT(misc-
} }
/// Invoked every 10 seconds. /// Invoked every 10 seconds.
static void handle01HzLoop(State *const state, const CanardMicrosecond now_usec) static void handle01HzLoop(struct UdralServoInternalState *const state, const CanardMicrosecond now_usec)
{ {
// Publish the recommended (not required) port introspection message. No point publishing it if we're anonymous. // Publish the recommended (not required) port introspection message. No point publishing it if we're anonymous.
// The message is a bit heavy on the stack (about 2 KiB) but this is not a problem for a modern MCU. // The message is a bit heavy on the stack (about 2 KiB) but this is not a problem for a modern MCU.
@ -537,18 +542,18 @@ static void handle01HzLoop(State *const state, const CanardMicrosecond now_usec)
} }
/// https://github.com/OpenCyphal/public_regulated_data_types/blob/master/reg/udral/service/actuator/servo/_.0.1.dsdl /// https://github.com/OpenCyphal/public_regulated_data_types/blob/master/reg/udral/service/actuator/servo/_.0.1.dsdl
static void processMessageServoSetpoint(State *const state, static void processMessageServoSetpoint(struct UdralServoInternalState *const state,
const reg_udral_physics_dynamics_translation_Linear_0_1 *const msg) const reg_udral_physics_dynamics_rotation_Planar_0_1 *const msg)
{ {
Serial.println("processMessageServoSetpoint"); Serial.println("processMessageServoSetpoint");
state->servo.position = msg->kinematics.position.meter; state->servo.target_position = msg->kinematics.angular_position.radian;
state->servo.velocity = msg->kinematics.velocity.meter_per_second; state->servo.target_velocity = msg->kinematics.angular_velocity.radian_per_second;
state->servo.acceleration = msg->kinematics.acceleration.meter_per_second_per_second; state->servo.target_acceleration = msg->kinematics.angular_acceleration.radian_per_second_per_second;
state->servo.force = msg->force.newton; state->servo.target_force = msg->_torque.newton_meter;
} }
/// https://github.com/OpenCyphal/public_regulated_data_types/blob/master/reg/udral/service/common/Readiness.0.1.dsdl /// https://github.com/OpenCyphal/public_regulated_data_types/blob/master/reg/udral/service/common/Readiness.0.1.dsdl
static void processMessageServiceReadiness(State *const state, static void processMessageServiceReadiness(struct UdralServoInternalState *const state,
const reg_udral_service_common_Readiness_0_1 *const msg, const reg_udral_service_common_Readiness_0_1 *const msg,
const CanardMicrosecond monotonic_time) const CanardMicrosecond monotonic_time)
{ {
@ -557,7 +562,7 @@ static void processMessageServiceReadiness(State *const state,
state->servo.arming.last_update_at = monotonic_time; state->servo.arming.last_update_at = monotonic_time;
} }
static void processMessagePlugAndPlayNodeIDAllocation(State *const state, static void processMessagePlugAndPlayNodeIDAllocation(struct UdralServoInternalState *const state,
const uavcan_pnp_NodeIDAllocationData_1_0 *const msg) const uavcan_pnp_NodeIDAllocationData_1_0 *const msg)
{ {
uint8_t uid[uavcan_node_GetInfo_Response_1_0_unique_id_ARRAY_CAPACITY_] = {0}; uint8_t uid[uavcan_node_GetInfo_Response_1_0_unique_id_ARRAY_CAPACITY_] = {0};
@ -718,7 +723,7 @@ static uavcan_node_GetInfo_Response_1_0 processRequestNodeGetInfo()
return resp; return resp;
} }
static void processReceivedTransfer(State *const state, static void processReceivedTransfer(struct UdralServoInternalState *const state,
const struct CanardRxTransfer *const transfer, const struct CanardRxTransfer *const transfer,
const CanardMicrosecond now_usec) const CanardMicrosecond now_usec)
{ {
@ -727,8 +732,8 @@ static void processReceivedTransfer(State *const state,
size_t size = transfer->payload.size; size_t size = transfer->payload.size;
if (transfer->metadata.port_id == state->port_id.sub.servo_setpoint) if (transfer->metadata.port_id == state->port_id.sub.servo_setpoint)
{ {
reg_udral_physics_dynamics_translation_Linear_0_1 msg = {0}; reg_udral_physics_dynamics_rotation_Planar_0_1 msg = {0};
if (reg_udral_physics_dynamics_translation_Linear_0_1_deserialize_(&msg, (uint8_t *)transfer->payload.data, &size) >= if (reg_udral_physics_dynamics_rotation_Planar_0_1_deserialize_(&msg, (uint8_t *)transfer->payload.data, &size) >=
0) 0)
{ {
processMessageServoSetpoint(state, &msg); processMessageServoSetpoint(state, &msg);
@ -836,7 +841,7 @@ static void processReceivedTransfer(State *const state,
static void *canardAllocate(void *const user_reference, const size_t amount) static void *canardAllocate(void *const user_reference, const size_t amount)
{ {
O1HeapInstance *const heap = ((State *)user_reference)->heap; O1HeapInstance *const heap = ((struct UdralServoInternalState *)user_reference)->heap;
assert(o1heapDoInvariantsHold(heap)); assert(o1heapDoInvariantsHold(heap));
return o1heapAllocate(heap, amount); return o1heapAllocate(heap, amount);
} }
@ -844,18 +849,14 @@ static void *canardAllocate(void *const user_reference, const size_t amount)
static void canardDeallocate(void *const user_reference, const size_t amount, void *const pointer) static void canardDeallocate(void *const user_reference, const size_t amount, void *const pointer)
{ {
(void)amount; (void)amount;
O1HeapInstance *const heap = ((State *)user_reference)->heap; O1HeapInstance *const heap = ((struct UdralServoInternalState *)user_reference)->heap;
o1heapFree(heap, pointer); o1heapFree(heap, pointer);
} }
int mainloop() int udral_loop(state_sync_f servo_state_sync_f)
{ {
struct timespec ts;
srand(micros()); srand(micros());
struct UdralServoInternalState state;
State state = {0};
//registerDoFactoryReset();
// A simple application like a servo node typically does not require more than 20 KiB of heap and 4 KiB of stack. // A simple application like a servo node typically does not require more than 20 KiB of heap and 4 KiB of stack.
// For the background and related theory refer to the following resources: // For the background and related theory refer to the following resources:
// - https://github.com/OpenCyphal/libcanard/blob/master/README.md // - https://github.com/OpenCyphal/libcanard/blob/master/README.md
@ -943,12 +944,12 @@ int mainloop()
state.port_id.pub.servo_dynamics = // Position/speed/acceleration/force feedback. state.port_id.pub.servo_dynamics = // Position/speed/acceleration/force feedback.
getSubjectID(SUBJECT_ROLE_PUBLISHER, getSubjectID(SUBJECT_ROLE_PUBLISHER,
"servo.dynamics", "servo.dynamics",
reg_udral_physics_dynamics_translation_LinearTs_0_1_FULL_NAME_AND_VERSION_); reg_udral_physics_dynamics_rotation_PlanarTs_0_1_FULL_NAME_AND_VERSION_);
// Subscriptions: // Subscriptions:
state.port_id.sub.servo_setpoint = // This message actually commands the servo setpoint with the motion profile. state.port_id.sub.servo_setpoint = // This message actually commands the servo setpoint with the motion profile.
getSubjectID(SUBJECT_ROLE_SUBSCRIBER, getSubjectID(SUBJECT_ROLE_SUBSCRIBER,
"servo.setpoint", "servo.setpoint",
reg_udral_physics_dynamics_translation_Linear_0_1_FULL_NAME_AND_VERSION_); reg_udral_physics_dynamics_rotation_Planar_0_1_FULL_NAME_AND_VERSION_);
state.port_id.sub.servo_readiness = // Arming subject: whether to act upon the setpoint or to stay idle. state.port_id.sub.servo_readiness = // Arming subject: whether to act upon the setpoint or to stay idle.
getSubjectID(SUBJECT_ROLE_SUBSCRIBER, getSubjectID(SUBJECT_ROLE_SUBSCRIBER,
"servo.readiness", "servo.readiness",
@ -979,7 +980,7 @@ int mainloop()
canardRxSubscribe(&state.canard, canardRxSubscribe(&state.canard,
CanardTransferKindMessage, CanardTransferKindMessage,
state.port_id.sub.servo_setpoint, state.port_id.sub.servo_setpoint,
reg_udral_physics_dynamics_translation_Linear_0_1_EXTENT_BYTES_, reg_udral_physics_dynamics_rotation_Planar_0_1_EXTENT_BYTES_,
servo_transfer_id_timeout, servo_transfer_id_timeout,
&rx); &rx);
if (res < 0) if (res < 0)
@ -1068,6 +1069,7 @@ int mainloop()
CanardMicrosecond next_01_hz_iter_at = state.started_at + MEGA * 10; CanardMicrosecond next_01_hz_iter_at = state.started_at + MEGA * 10;
do do
{ {
servo_state_sync_f(&state.servo);
// Run a trivial scheduler polling the loops that run the business logic. // Run a trivial scheduler polling the loops that run the business logic.
CanardMicrosecond now_usec = getMonotonicMicroseconds(); CanardMicrosecond now_usec = getMonotonicMicroseconds();
if (now_usec >= next_fast_iter_at) if (now_usec >= next_fast_iter_at)