diff --git a/fw/can_if_up.sh b/fw/can_if_up.sh index 331eec7..1b3588b 100755 --- a/fw/can_if_up.sh +++ b/fw/can_if_up.sh @@ -1,7 +1,7 @@ #!/bin/bash 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__NODE__ID=127 diff --git a/fw/src/DRV8323S.hpp b/fw/src/DRV8323S.hpp index 14a8e37..64afc04 100644 --- a/fw/src/DRV8323S.hpp +++ b/fw/src/DRV8323S.hpp @@ -216,6 +216,7 @@ void drv8323s_init(struct drv8323s_foc_driver *dev, int phA, int phB, int phC, delay(1); drv8323s_lock_spi(dev, false); delay(1); + /* Set 3PWM Mode */ drv8323s_set_PWM_mode(dev, DRV8323S_PWM_MODE_3PWM); delay(1); 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->init(); - /* Set 3PWM Mode */ } 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; uint16_t result; drv8323s_write_spi(dev, reg, reg_value, &result); - - // TODO SPI call } #endif /* _DRV8323S_H */ diff --git a/fw/src/main.cpp b/fw/src/main.cpp index 8e74ff8..f4c9097 100644 --- a/fw/src/main.cpp +++ b/fw/src/main.cpp @@ -1,5 +1,7 @@ // #include +#include "esp32-hal.h" +#include #define NODE_NAME "N17BLDC" #define VERSION_MAJOR 0 @@ -8,49 +10,37 @@ #include "Arduino.h" -#define SSIF_USBSERIAL -#ifdef SSIF_USBSERIAL +#define USE_USBSERIAL +#ifdef USE_USBSERIAL #include "USB.h" USBCDC usbserial; -#define SSYNCIF usbserial +#ifdef Serial +#undef Serial +#endif #define Serial usbserial #else -#define SSYNCIF Serial1 +#define Serial Serial1 #endif -// #define SSYNCIF Serial +// #define Serial Serial #include "canard.c" #include "SPI.h" // #include "Wire.h" -// #include "Wire.h" #include -// #include #include "SimpleFOCDrivers.h" -// #include "comms/streams/BinaryIO.h" -// #include "encoders/as5600/MagneticSensorAS5600.h" #include "encoders/calibrated/CalibratedSensor.h" // #include "encoders/esp32hwencoder/ESP32HWEncoder.h" #include "DRV8323S.hpp" +#include "HybridStepperMotor.h" #include "esp32-hal-adc.h" #include "esp32-hal-gpio.h" -// #include "motors/HybridStepperMotor/HybridStepperMotor.h" -#include "HybridStepperMotor.h" -#include #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 #include "pin_def.h" @@ -63,7 +53,6 @@ USBCDC usbserial; (TEMP(analogRead(TERMISTOR_PCB) * 3.3 / 4096.0, 3435.0, 10000.0, 10000.0, \ 3.3)) - #define vdrive_read (analogRead(VSENSE_PIN) * 20.08f / 845) //* 20.8f/ 21033.75) @@ -81,28 +70,22 @@ USBCDC usbserial; #endif 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(ENC_A, ENC_B, 4096 / 4); // The Index pin can be omitted -Encoder encoder = Encoder(ENC_A, ENC_B, 4096 / 4); -CalibratedSensor encoder_calibrated = CalibratedSensor(encoder); +// ESP32HWEncoder(ENC_A, ENC_B, 4096 / 4); // The Index pin can be omitted +auto encoder_absolute = MagneticSensorSPI(AS5047_SPI, 18); +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); #ifdef MOTOR struct drv8323s_foc_driver drv8323s; // 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); -// DRV8316Driver3PWM driver = DRV8316Driver3PWM(INHA, INHB, INHC, SPI_DRV_SC); -// BLDCDriver3PWM driver = BLDCDriver3PWM(INHA,INHB,INHC); #endif -// BLDCDriver3PWM driver = BLDCDriver3PWM(pwmA, pwmB, pwmC, Enable(optional)); - void read_i2c(uint8_t addr, uint8_t reg, uint8_t *buf, uint8_t len) { 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; -struct sync_params {}; +struct UdralServoState servo_state = {0}; -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); - } +void mutexed_state_sync(UdralServoState *const state) { + xSemaphoreTake(mutex, portMAX_DELAY); + servo_state.target_force = state->target_force; + servo_state.target_acceleration = state->target_acceleration; + servo_state.target_velocity = state->target_velocity; + servo_state.target_position = state->target_position; + servo_state.arming = state->arming; + servo_state.pid = state->pid; + *state = servo_state; + xSemaphoreGive(mutex); } + TaskHandle_t taskCommHandle; -void comm_task(void *parameter) { - while (true) { - long now = millis(); +void comm_task(void *parameter) { udral_loop(mutexed_state_sync); } - vTaskDelay(1); - } -} - -void doA1(){encoder.handleA();} -void doB1(){encoder.handleB();} +void doA1() { encoder.handleA(); } +void doB1() { encoder.handleB(); } void setup() { pinMode(LED_PIN, OUTPUT); - + mutex = xSemaphoreCreateMutex(); +#ifdef USE_USBSERIAL 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); - digitalWrite(LED_PIN, 1); // enable - delay(1000); - } - mutex = xSemaphoreCreateMutex(); -#ifdef SSIF_USBSERIAL - SSYNCIF.begin(); - USB.begin(); - SSYNCIF.setDebugOutput( + Serial.setDebugOutput( true); // sends all log_e(), log_i() messages to USB HW CDC - SSYNCIF.setTxTimeoutMs(0); + Serial.setTxTimeoutMs(0); #else - SSYNCIF.begin(460800, SERIAL_8N1, 44, 43); + Serial.begin(460800, SERIAL_8N1, 44, 43); #endif - // prefs.begin("foc"); - // SSYNCIF.print(__cplusplus); + digitalWrite(LED_PIN, 0); // enable + delay(100); + digitalWrite(LED_PIN, 1); // disable + delay(1000); - // Wire.setTimeOut(10); - // Wire.begin(I2C_SDA, I2C_SCL, 1000000); + Serial.println("Start"); pinMode(SPI_DRV_SC, OUTPUT); @@ -222,17 +173,15 @@ void setup() { pinMode(CAL_PIN, OUTPUT); digitalWrite(CAL_PIN, 0); // enable - // write_i2c(0x36,0x07, 0b00000011); - // write_i2c(0x36, 0x09, 0xFF); - // encoder.init(&Wire); - + // initialise magnetic sensor hardware encoder.pullup = Pullup::USE_INTERN; encoder.init(); - encoder.enableInterrupts(doA1,doB1); + encoder.enableInterrupts(doA1, doB1); pinMode(SPI_MISO, INPUT_PULLUP); + SPI.begin(SPI_CLK, SPI_MISO, SPI_MOSI, SPI_DRV_SC); #ifdef MOTOR - SimpleFOCDebug::enable(&SSYNCIF); + SimpleFOCDebug::enable(&Serial); drv8323s_init(&drv8323s, INHA, INHB, INHC, SPI_DRV_SC, DRVEN, &SPI); digitalWrite(INLABC, 1); // enable // driver.init(&SPI); @@ -240,280 +189,122 @@ void setup() { motor.linkSensor(&encoder); motor.linkDriver(drv8323s.focdriver); // motor.foc_modulation = FOCModulationType::SinePWM; - motor.foc_modulation = FOCModulationType::SpaceVectorPWM; + motor.foc_modulation = FOCModulationType::SpaceVectorPWM; - motor.voltage_sensor_align = 3; - motor.voltage_limit = 5; + motor.voltage_sensor_align = 4; + motor.voltage_limit = 6; // motor.controller = MotionControlType::velocity_openloop; - motor.controller = MotionControlType::velocity; - // motor.controller = MotionControlType::torque; - // motor.torque_controller = TorqueControlType::voltage; - motor.useMonitoring(SSYNCIF); + // motor.controller = MotionControlType::velocity; + motor.controller = MotionControlType::torque; + motor.torque_controller = TorqueControlType::voltage; + motor.useMonitoring(Serial); drv8323s.focdriver->voltage_power_supply = vdrive_read; motor.init(); delay(1000); - // // set voltage to run calibration - encoder_calibrated.voltage_calibration = 3; - // // Running calibration - encoder_calibrated.calibrate(motor); + static Preferences pref; + pref.begin("simpleFOC"); - // //Serial.println("Calibrating Sensor Done."); - // // Linking sensor to motor object - // motor.linkSensor(&encoder_calibrated); - motor.initFOC(); + // set voltage to run calibration + encoder_calibrated.voltage_calibration = 3; + // Running calibration + 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); - // sensor.init(&SPI); - // motors[i].linkSensor(&sensors[i]); + digitalWrite(INLABC, 0); // enable #endif - // delay(5000); - // SSYNCIF.println("IMU.begin()"); - // int ret = IMU.begin(); - // if (ret != 0) { - // SSYNCIF.print("ICM42670 initialization failed: "); - // SSYNCIF.println(ret); - // while (1) { - // 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 - // ); + xTaskCreatePinnedToCore(&comm_task, // Function name of the task + "comm", // Name of the task (e.g. for debugging) + 65536, // Stack size (bytes) + NULL, // Parameter to pass + 1, // Task priority + &taskCommHandle, // 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 } int i = 0; void loop() { - mainloop(); - return; #ifdef MOTOR - // drv8323s.focdriver->voltage_power_supply = vdrive_read; - // digitalWrite(LED_PIN, 0); // enable - motor.loopFOC(); + bool led = false; - // motor.move(3); - // digitalWrite(LED_PIN, 1); // enable - motor.move(); + xSemaphoreGive(mutex); + led = servo_state.arming.armed; + 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 encoder.update(); // optional: Update manually if not using loopfoc() #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(); +} \ No newline at end of file diff --git a/fw/src/register.hpp b/fw/src/register.hpp index 1a55f85..4ca0c56 100644 --- a/fw/src/register.hpp +++ b/fw/src/register.hpp @@ -1,14 +1,21 @@ #pragma once +#include +#include + +#include "esp_err.h" +#include "esp_mac.h" + #ifdef RAM_PREFERENCES #include "hash_preferences.hpp" #else #include #endif + +#include #include #include -#include -#include + static Preferences preferences; static bool preferences_open = false; @@ -16,8 +23,7 @@ static bool preferences_open = false; #define REGISTER_LIST_KEY "__reglist" #define REGISTER_LIST_DELIM "|" -String -hash(const char *str) +String hash(const char *str) { unsigned long hash = 5381; int c; @@ -112,9 +118,35 @@ bool registerAssign(uavcan_register_Value_1_0 *const dst, const uavcan_register_ return false; } +#include + void registerDoFactoryReset(void) { - open_preferences_if_needed(); - preferences.clear(); - close_preferences_if_needed(); + nvs_flash_erase(); // erase the NVS partition and... + nvs_flash_init(); // initialize the NVS partition. + 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_); +} + diff --git a/fw/src/udral_servo.hpp b/fw/src/udral_servo.hpp index 1a3e5c1..4c79d94 100644 --- a/fw/src/udral_servo.hpp +++ b/fw/src/udral_servo.hpp @@ -13,8 +13,8 @@ /// Copyright (C) 2021 OpenCyphal /// Author: Pavel Kirienko -#include "esp32-hal.h" #define NUNAVUT_ASSERT(x) assert(x) +#include "esp32-hal.h" #include "pin_def.h" @@ -32,12 +32,14 @@ #include #include #include +#include #include #include #include #include -#include +#include "reg/udral/physics/dynamics/rotation/Planar_0_1.h" +#include #include #include @@ -47,7 +49,6 @@ #include #include "USB.h" - extern USBCDC usbserial; #define KILO 1000L @@ -57,68 +58,93 @@ extern USBCDC usbserial; /// For CAN FD the queue can be smaller. #define CAN_TX_QUEUE_CAPACITY 100 -/// We keep the state of the application here. Feel free to use static variables instead if desired. -typedef struct State -{ - CanardMicrosecond started_at; +/// We keep the state of the application here. Feel free to use static variables +/// instead if desired. - O1HeapInstance *heap; - struct CanardInstance canard; - struct CanardTxQueue canard_tx_queues[CAN_REDUNDANCY_FACTOR]; +struct UdralServoState { + /// Whether the servo is supposed to actuate the load or to stay idle (safe + /// low-power mode). + struct { + struct { + bool armed; + CanardMicrosecond last_update_at; + } arming; - /// The state of the business logic. - struct - { - /// Whether the servo is supposed to actuate the load or to stay idle (safe low-power mode). - struct - { - bool armed; - CanardMicrosecond last_update_at; - } arming; + float vcc_volts; + float current; - /// Setpoint & motion profile (unsupported constraints are to be ignored). - /// https://github.com/OpenCyphal/public_regulated_data_types/blob/master/reg/udral/service/actuator/servo/_.0.1.dsdl - /// As described in the linked documentation, there are two kinds of servos supported: linear and rotary. - /// Units per-kind are: LINEAR ROTARY - float position; ///< [meter] [radian] - float velocity; ///< [meter/second] [radian/second] - float acceleration; ///< [(meter/second)^2] [(radian/second)^2] - float force; ///< [newton] [netwon*meter] - } servo; + float curr_position; ///< [meter] [radian] + float curr_velocity; ///< [meter/second] [radian/second] + float curr_acceleration; ///< [(meter/second)^2] [(radian/second)^2] + float curr_force; ///< [newton] [netwon*meter] - /// 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: + float motor_temperature; + 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 - 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; + /// As described in the linked documentation, there are two kinds of servos + /// supported: linear and rotary. Units per-kind are: LINEAR ROTARY + float target_position; ///< [meter] [radian] + float target_velocity; ///< [meter/second] [radian/second] + float target_acceleration; ///< [(meter/second)^2] [(radian/second)^2] + float target_force; ///< [newton] [netwon*meter] - /// A transfer-ID is an integer that is incremented whenever a new message is published on a given subject. - /// It is used by the protocol for deduplication, message loss detection, and other critical things. - /// For CAN, each value can be of type uint8_t, but we use larger types for genericity and for statistical purposes, - /// as large values naturally contain the number of times each subject was published to. - struct - { - uint64_t uavcan_node_heartbeat; - uint64_t uavcan_node_port_list; - uint64_t uavcan_pnp_allocation; - // Messages published synchronously can share the same transfer-ID: - uint64_t servo_fast_loop; - uint64_t servo_1Hz_loop; - } next_transfer_id; -} State; + struct { + uavcan_primitive_scalar_Real32_1_0 p; + uavcan_primitive_scalar_Real32_1_0 i; + uavcan_primitive_scalar_Real32_1_0 d; + } pid; + }; +}; + +typedef void (*state_sync_f)(UdralServoState *const); + +struct UdralServoInternalState { + CanardMicrosecond started_at; + + 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. static volatile bool g_restart_required = false; @@ -134,26 +160,6 @@ static CanardMicrosecond getMonotonicMicroseconds() 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 { SUBJECT_ROLE_PUBLISHER, @@ -195,7 +201,7 @@ static CanardPortID getSubjectID(const SubjectRole role, const char *const port_ return result; } -static void send(State *const state, +static void send(struct UdralServoInternalState *const state, const CanardMicrosecond tx_deadline_usec, const CanardTransferMetadata *const metadata, 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 size_t payload_size, const void *const payload_data, @@ -226,17 +232,17 @@ static void sendResponse(State *const state, } /// 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. if (state->servo.arming.armed) { fprintf(stderr, "\rp=%.3f m v=%.3f m/s a=%.3f (m/s)^2 F=%.3f N \r", - (double)state->servo.position, - (double)state->servo.velocity, - (double)state->servo.acceleration, - (double)state->servo.force); + (double)state->servo.target_position, + (double)state->servo.target_velocity, + (double)state->servo.target_acceleration, + (double)state->servo.target_force); } 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. 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: msg.timestamp.microsecond = uavcan_time_SynchronizedTimestamp_1_0_UNKNOWN; - // A real application would source these values from the hardware; we republish the setpoint for demo purposes. - // TODO populate real values: - msg.value.kinematics.position.meter = state->servo.position; - msg.value.kinematics.velocity.meter_per_second = state->servo.velocity; - msg.value.kinematics.acceleration.meter_per_second_per_second = state->servo.acceleration; - msg.value.force.newton = state->servo.force; + + msg.value.kinematics.angular_position.radian = state->servo.curr_position; + msg.value.kinematics.angular_velocity.radian_per_second = state->servo.curr_velocity; + msg.value.kinematics.angular_acceleration.radian_per_second_per_second = state->servo.curr_acceleration; + msg.value._torque.newton_meter = state->servo.curr_force; // 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); const int8_t err = - reg_udral_physics_dynamics_translation_LinearTs_0_1_serialize_(&msg, &serialized[0], &serialized_size); - assert(err >= 0); + reg_udral_physics_dynamics_rotation_PlanarTs_0_1_serialize_(&msg, &serialized[0], &serialized_size); if (err >= 0) { 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: msg.timestamp.microsecond = uavcan_time_SynchronizedTimestamp_1_0_UNKNOWN; // TODO populate real values: - msg.value.current.ampere = 20.315F; - msg.value.voltage.volt = 51.3F; + msg.value.current.ampere = state->servo.current; + msg.value.voltage.volt = state->servo.vcc_volts; // 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); const int8_t err = reg_udral_physics_electricity_PowerTs_0_1_serialize_(&msg, &serialized[0], &serialized_size); - assert(err >= 0); if (err >= 0) { const CanardTransferMetadata transfer = { @@ -333,7 +336,7 @@ static void handleFastLoop(State *const state, const CanardMicrosecond now_usec) } /// 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; // 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. reg_udral_service_actuator_common_Status_0_1 msg = {0}; // 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_]; size_t serialized_size = sizeof(serialized); const int8_t err = @@ -479,7 +484,7 @@ static void fillServers(const struct CanardTreeNode *const tree, // NOLINT(misc- } /// 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. // 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 -static void processMessageServoSetpoint(State *const state, - const reg_udral_physics_dynamics_translation_Linear_0_1 *const msg) +static void processMessageServoSetpoint(struct UdralServoInternalState *const state, + const reg_udral_physics_dynamics_rotation_Planar_0_1 *const msg) { Serial.println("processMessageServoSetpoint"); - state->servo.position = msg->kinematics.position.meter; - state->servo.velocity = msg->kinematics.velocity.meter_per_second; - state->servo.acceleration = msg->kinematics.acceleration.meter_per_second_per_second; - state->servo.force = msg->force.newton; + state->servo.target_position = msg->kinematics.angular_position.radian; + state->servo.target_velocity = msg->kinematics.angular_velocity.radian_per_second; + state->servo.target_acceleration = msg->kinematics.angular_acceleration.radian_per_second_per_second; + 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 -static void processMessageServiceReadiness(State *const state, +static void processMessageServiceReadiness(struct UdralServoInternalState *const state, const reg_udral_service_common_Readiness_0_1 *const msg, const CanardMicrosecond monotonic_time) { @@ -557,7 +562,7 @@ static void processMessageServiceReadiness(State *const state, 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) { 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; } -static void processReceivedTransfer(State *const state, +static void processReceivedTransfer(struct UdralServoInternalState *const state, const struct CanardRxTransfer *const transfer, const CanardMicrosecond now_usec) { @@ -727,8 +732,8 @@ static void processReceivedTransfer(State *const state, size_t size = transfer->payload.size; if (transfer->metadata.port_id == state->port_id.sub.servo_setpoint) { - reg_udral_physics_dynamics_translation_Linear_0_1 msg = {0}; - if (reg_udral_physics_dynamics_translation_Linear_0_1_deserialize_(&msg, (uint8_t *)transfer->payload.data, &size) >= + reg_udral_physics_dynamics_rotation_Planar_0_1 msg = {0}; + if (reg_udral_physics_dynamics_rotation_Planar_0_1_deserialize_(&msg, (uint8_t *)transfer->payload.data, &size) >= 0) { processMessageServoSetpoint(state, &msg); @@ -836,7 +841,7 @@ static void processReceivedTransfer(State *const state, 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)); 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) { (void)amount; - O1HeapInstance *const heap = ((State *)user_reference)->heap; + O1HeapInstance *const heap = ((struct UdralServoInternalState *)user_reference)->heap; o1heapFree(heap, pointer); } -int mainloop() +int udral_loop(state_sync_f servo_state_sync_f) { - struct timespec ts; srand(micros()); - - State state = {0}; - - //registerDoFactoryReset(); + struct UdralServoInternalState state; // A simple application like a servo node typically does not require more than 20 KiB of heap and 4 KiB of stack. // For the background and related theory refer to the following resources: // - https://github.com/OpenCyphal/libcanard/blob/master/README.md @@ -943,12 +944,12 @@ int mainloop() state.port_id.pub.servo_dynamics = // Position/speed/acceleration/force feedback. getSubjectID(SUBJECT_ROLE_PUBLISHER, "servo.dynamics", - reg_udral_physics_dynamics_translation_LinearTs_0_1_FULL_NAME_AND_VERSION_); + reg_udral_physics_dynamics_rotation_PlanarTs_0_1_FULL_NAME_AND_VERSION_); // Subscriptions: state.port_id.sub.servo_setpoint = // This message actually commands the servo setpoint with the motion profile. getSubjectID(SUBJECT_ROLE_SUBSCRIBER, "servo.setpoint", - reg_udral_physics_dynamics_translation_Linear_0_1_FULL_NAME_AND_VERSION_); + 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. getSubjectID(SUBJECT_ROLE_SUBSCRIBER, "servo.readiness", @@ -979,7 +980,7 @@ int mainloop() canardRxSubscribe(&state.canard, CanardTransferKindMessage, 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, &rx); if (res < 0) @@ -1068,6 +1069,7 @@ int mainloop() CanardMicrosecond next_01_hz_iter_at = state.started_at + MEGA * 10; do { + servo_state_sync_f(&state.servo); // Run a trivial scheduler polling the loops that run the business logic. CanardMicrosecond now_usec = getMonotonicMicroseconds(); if (now_usec >= next_fast_iter_at)