From 1d82f266654e8a70de06d65e9ed7139c48a1f6a8 Mon Sep 17 00:00:00 2001 From: qiaoxu Date: Thu, 15 Oct 2026 22:20:03 +0800 Subject: [PATCH 1/4] =?UTF-8?q?=E4=BF=AE=E6=94=B9=E4=B8=BA=E5=8F=AF?= =?UTF-8?q?=E7=94=A8=E4=BA=8E=E4=BA=A7=E5=93=81=E6=9D=BF=E7=9A=84=E5=8F=8C?= =?UTF-8?q?=E7=94=B5=E6=9C=BA=E6=8E=A7=E5=88=B6=E5=9F=BA=E7=A1=80=E7=A8=8B?= =?UTF-8?q?=E5=BA=8F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Firmware/Board/v3/Inc/board.h | 8 ++- Firmware/Board/v3/Inc/main.h | 4 +- Firmware/Board/v3/STM32F405RGTx_FLASH.ld | 3 +- Firmware/Board/v3/startup_stm32f405xx.s | 3 +- Firmware/Makefile | 17 ++++--- Firmware/MotorControl/controller.hpp | 12 ++--- Firmware/MotorControl/encoder.hpp | 51 +++++++++++++++---- Firmware/MotorControl/main.cpp | 23 +++++++++ Firmware/MotorControl/motor.hpp | 17 ++++--- Firmware/MotorControl/odrive_main.h | 6 +-- .../Device/ST/STM32F4xx/Include/stm32f405xx.h | 2 + Firmware/communication/can/can_simple.cpp | 44 +++++++++++++--- Firmware/communication/can/odrive_can.hpp | 2 +- ODrive_Workspace.code-workspace | 18 ++----- 14 files changed, 150 insertions(+), 60 deletions(-) diff --git a/Firmware/Board/v3/Inc/board.h b/Firmware/Board/v3/Inc/board.h index db6adcf72..708dc8532 100644 --- a/Firmware/Board/v3/Inc/board.h +++ b/Firmware/Board/v3/Inc/board.h @@ -25,9 +25,15 @@ #if HW_VERSION_MINOR <= 3 #define SHUNT_RESISTANCE (675e-6f) #else -#define SHUNT_RESISTANCE (500e-6f) +//#define SHUNT_RESISTANCE (500e-6f) +#define SHUNT_RESISTANCE (2e-3f) //产品板 #endif +//qiaoxu add +//#define ENCODER_USE_SPI_ABS_AMS +#define ENCODER_USE_HALL + + #define AXIS_COUNT (2) // Total count of GPIOs, including encoder pins, CAN pins and a dummy GPIO0. diff --git a/Firmware/Board/v3/Inc/main.h b/Firmware/Board/v3/Inc/main.h index 26d75a060..9f762048e 100644 --- a/Firmware/Board/v3/Inc/main.h +++ b/Firmware/Board/v3/Inc/main.h @@ -67,7 +67,9 @@ /* Private define ------------------------------------------------------------*/ #define TIM_1_8_CLOCK_HZ 168000000 -#define TIM_1_8_PERIOD_CLOCKS 3500 +//#define TIM_1_8_PERIOD_CLOCKS 3500//24khz 原程序24khz跑两个电机的中断时间不够,会报错:MOTOR_ERROR_CONTROL_DEADLINE_MISSED,磁编电机还会报ENCODER_ERROR_ABS_SPI_COM_FAIL +#define TIM_1_8_PERIOD_CLOCKS 7000 //qiaoxu modify 12khz +//#define TIM_1_8_PERIOD_CLOCKS 5250 //qiaoxu modify 16khz #define TIM_1_8_DEADTIME_CLOCKS 20 #define TIM_APB1_CLOCK_HZ 84000000 #define TIM_APB1_PERIOD_CLOCKS 4096 diff --git a/Firmware/Board/v3/STM32F405RGTx_FLASH.ld b/Firmware/Board/v3/STM32F405RGTx_FLASH.ld index 79e374413..4619ed276 100644 --- a/Firmware/Board/v3/STM32F405RGTx_FLASH.ld +++ b/Firmware/Board/v3/STM32F405RGTx_FLASH.ld @@ -44,7 +44,8 @@ MEMORY { RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 128K CCMRAM (rw) : ORIGIN = 0x10000000, LENGTH = 64K -FLASH (rx) : ORIGIN = 0x8000000, LENGTH = 768K +FLASH (rx) : ORIGIN = 0x8000000, LENGTH = 768K +/* FLASH (rx) : ORIGIN = 0x800C000, LENGTH = 720K */ NVM (r) : ORIGIN = 0x80C0000, LENGTH = 256K } diff --git a/Firmware/Board/v3/startup_stm32f405xx.s b/Firmware/Board/v3/startup_stm32f405xx.s index 34c01d1d8..1b1a8afe4 100644 --- a/Firmware/Board/v3/startup_stm32f405xx.s +++ b/Firmware/Board/v3/startup_stm32f405xx.s @@ -107,7 +107,8 @@ LoopFillZerobss: /* Call the clock system intitialization function.*/ bl SystemInit - bl early_start_checks + /* qiaoxu delete */ +/* bl early_start_checks */ /* Call static constructors */ bl __libc_init_array diff --git a/Firmware/Makefile b/Firmware/Makefile index 4dc1ff903..d7c67032b 100644 --- a/Firmware/Makefile +++ b/Firmware/Makefile @@ -27,21 +27,26 @@ endif $(info board version: $(CONFIG_BOARD_VERSION)) -all: +# Build firmware only (skip Python script generation) +firmware: @mkdir -p autogen - @$(PY_CMD) ../tools/odrive/version.py --output autogen/version.c @tup --quiet -no-environ-check - @$(PY_CMD) interface_generator_stub.py --definitions odrive-interface.yaml --template ../tools/enums_template.j2 --output ../tools/odrive/enums.py - @$(PY_CMD) interface_generator_stub.py --definitions odrive-interface.yaml --template ../tools/arduino_enums_template.j2 --output ../Arduino/ODriveArduino/ODriveEnums.h - @cd ../tools/ && $(PY_CMD) create_can_dbc.py # Copy libfibre files to odrivetool if they were built +libfibre-copy: @ ! test -f "fibre-cpp/build/libfibre-linux-amd64.so" || cp fibre-cpp/build/libfibre-linux-amd64.so ../tools/odrive/pyfibre/fibre/ @ ! test -f "fibre-cpp/build/libfibre-linux-armhf.so" || cp fibre-cpp/build/libfibre-linux-armhf.so ../tools/odrive/pyfibre/fibre/ @ ! test -f "fibre-cpp/build/libfibre-linux-aarch64.so" || cp fibre-cpp/build/libfibre-linux-aarch64.so ../tools/odrive/pyfibre/fibre/ @ ! test -f "fibre-cpp/build/libfibre-macos-x86.dylib" || cp fibre-cpp/build/libfibre-macos-x86.dylib ../tools/odrive/pyfibre/fibre/ @ ! test -f "fibre-cpp/build/libfibre-windows-amd64.dll" || cp fibre-cpp/build/libfibre-windows-amd64.dll ../tools/odrive/pyfibre/fibre/ +flash: firmware + $(OPENOCD) \ + -c 'reset halt' \ + -c 'flash write_image erase $(FIRMWARE)' \ + -c 'reset run' \ + -c exit + libfibre-linux-armhf: docker run -it -v "`pwd`/fibre-cpp":/build -v /tmp/fibre-linux-armhf-build:/build/build -w /build fibre-compiler configs/linux-armhf.config cp /tmp/fibre-linux-armhf-build/libfibre-*.so ../tools/odrive/pyfibre/fibre/ @@ -106,4 +111,4 @@ unlock: unlock-stlink2 .PHONY: stlink2-config flash-stlink2 gdb-stlink2 erase-stlink2 unlock-stlink2 .PHONY: flash-bmp gdb-bmp -.PHONY: all clean flash gdb erase unlock dfu fibre +.PHONY: all clean flash gdb erase unlock dfu fibre firmware libfibre-copy diff --git a/Firmware/MotorControl/controller.hpp b/Firmware/MotorControl/controller.hpp index 2b78d6c58..b7ca90703 100644 --- a/Firmware/MotorControl/controller.hpp +++ b/Firmware/MotorControl/controller.hpp @@ -22,16 +22,16 @@ class Controller : public ODriveIntf::ControllerIntf { }; struct Config_t { - ControlMode control_mode = CONTROL_MODE_POSITION_CONTROL; //see: ControlMode_t - InputMode input_mode = INPUT_MODE_PASSTHROUGH; //see: InputMode_t + ControlMode control_mode = CONTROL_MODE_VELOCITY_CONTROL; //see: ControlMode_t + InputMode input_mode = INPUT_MODE_VEL_RAMP; //see: InputMode_t float pos_gain = 20.0f; // [(turn/s) / turn] - float vel_gain = 1.0f / 6.0f; // [Nm/(turn/s)] + float vel_gain = 0.05f; // [Nm/(turn/s)] // float vel_gain = 0.2f / 200.0f, // [Nm/(rad/s)] - float vel_integrator_gain = 2.0f / 6.0f; // [Nm/(turn/s * s)] - float vel_limit = 2.0f; // [turn/s] Infinity to disable. + float vel_integrator_gain = 0.04f;//2.0f / 6.0f; // [Nm/(turn/s * s)] + float vel_limit = 200.0f; // [turn/s] Infinity to disable. float vel_limit_tolerance = 1.2f; // ratio to vel_lim. Infinity to disable. float vel_integrator_limit = INFINITY; // Vel. integrator clamping value. Infinity to disable. - float vel_ramp_rate = 1.0f; // [(turn/s) / s] + float vel_ramp_rate = 20.0f; // [(turn/s) / s] float torque_ramp_rate = 0.01f; // Nm / sec bool circular_setpoints = false; float circular_setpoint_range = 1.0f; // Circular range when circular_setpoints is true. [turn] diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp index d764582f2..b605e3fde 100644 --- a/Firmware/MotorControl/encoder.hpp +++ b/Firmware/MotorControl/encoder.hpp @@ -17,33 +17,62 @@ class Encoder : public ODriveIntf::EncoderIntf { {0.0f, 1.0f, 2.0f, 3.0f, 4.0f, 5.0f}; struct Config_t { - Mode mode = MODE_INCREMENTAL; +#ifdef ENCODER_USE_HALL + Mode mode = MODE_HALL; float calib_range = 0.02f; // Accuracy required to pass encoder cpr check float calib_scan_distance = 16.0f * M_PI; // rad electrical float calib_scan_omega = 4.0f * M_PI; // rad/s electrical - float bandwidth = 1000.0f; + float bandwidth = 100.0f;//1000.0f; + int32_t phase_offset = 46; // Offset between encoder count and rotor electrical phase + float phase_offset_float = 0.5068125128746033f; // Sub-count phase alignment offset + int32_t cpr = 24;//(2048 * 4); // Default resolution of CUI-AMT102 encoder, + float index_offset = 0.0f; + bool use_index = false; + bool pre_calibrated = true; // If true, this means the offset stored in + // configuration is valid and does not need + // be determined by run_offset_calibration. + // In this case the encoder will enter ready + // state as soon as the index is found. + int32_t direction = 1; // direction with respect to motor + bool use_index_offset = true; + bool enable_phase_interpolation = true; // Use velocity to interpolate inside the count state + bool find_idx_on_lockin_only = false; // Only be sensitive during lockin scan constant vel state + bool ignore_illegal_hall_state = false; // dont error on bad states like 000 or 111 + uint8_t hall_polarity = 0; + bool hall_polarity_calibrated = true; + std::array hall_edge_phcnt = hall_edge_defaults; + uint16_t abs_spi_cs_gpio_pin = 7; + uint16_t sincos_gpio_pin_sin = 1; + uint16_t sincos_gpio_pin_cos = 2; +#endif +#ifdef ENCODER_USE_SPI_ABS_AMS + Mode mode = MODE_SPI_ABS_AMS; + float calib_range = 0.02f; // Accuracy required to pass encoder cpr check + float calib_scan_distance = 16.0f * M_PI; // rad electrical + float calib_scan_omega = 4.0f * M_PI; // rad/s electrical + float bandwidth = 500.0f;//1000.0f; int32_t phase_offset = 0; // Offset between encoder count and rotor electrical phase - float phase_offset_float = 0.0f; // Sub-count phase alignment offset - int32_t cpr = (2048 * 4); // Default resolution of CUI-AMT102 encoder, + float phase_offset_float = 0.5f; // Sub-count phase alignment offset + int32_t cpr = 16384;//(2048 * 4); // Default resolution of CUI-AMT102 encoder, float index_offset = 0.0f; bool use_index = false; - bool pre_calibrated = false; // If true, this means the offset stored in + bool pre_calibrated = true; // If true, this means the offset stored in // configuration is valid and does not need // be determined by run_offset_calibration. // In this case the encoder will enter ready // state as soon as the index is found. - int32_t direction = 0; // direction with respect to motor + int32_t direction = -1; // direction with respect to motor bool use_index_offset = true; bool enable_phase_interpolation = true; // Use velocity to interpolate inside the count state bool find_idx_on_lockin_only = false; // Only be sensitive during lockin scan constant vel state bool ignore_illegal_hall_state = false; // dont error on bad states like 000 or 111 uint8_t hall_polarity = 0; - bool hall_polarity_calibrated = false; + bool hall_polarity_calibrated = true; std::array hall_edge_phcnt = hall_edge_defaults; - uint16_t abs_spi_cs_gpio_pin = 1; - uint16_t sincos_gpio_pin_sin = 3; - uint16_t sincos_gpio_pin_cos = 4; - + uint16_t abs_spi_cs_gpio_pin = 7; + uint16_t sincos_gpio_pin_sin = 1; + uint16_t sincos_gpio_pin_cos = 2; +#endif // custom setters Encoder* parent = nullptr; diff --git a/Firmware/MotorControl/main.cpp b/Firmware/MotorControl/main.cpp index 959a62bf9..6b437e6a2 100644 --- a/Firmware/MotorControl/main.cpp +++ b/Firmware/MotorControl/main.cpp @@ -818,6 +818,29 @@ extern "C" int main(void) { HAL_GPIO_Init(get_gpio(i).port_, &GPIO_InitStruct); } + //qiaoxu 抱闸控制 axis0-gpio8 axis1-gpio5 + get_gpio(8).config(GPIO_MODE_OUTPUT_PP, GPIO_NOPULL); + get_gpio(5).config(GPIO_MODE_OUTPUT_PP, GPIO_NOPULL); + get_gpio(8).write(true);//开闸 + get_gpio(5).write(true); + // get_gpio(8).write(false);//抱闸 + // get_gpio(5).write(false); + //qiaoxu 手动抱闸检测 axis0-gpio3 axis1-gpio4 + get_gpio(3).config(GPIO_MODE_INPUT, GPIO_PULLUP); + get_gpio(4).config(GPIO_MODE_INPUT, GPIO_PULLUP); + + //qiaoxu +#ifdef ENCODER_USE_SPI_ABS_AMS + // 磁编电机,每次进闭环都会被覆盖。 + axes[0].encoder_.config_.abs_spi_cs_gpio_pin = 7; + axes[0].encoder_.abs_spi_cs_pin_init(); + axes[0].encoder_.config_.direction = -1; + axes[1].encoder_.config_.abs_spi_cs_gpio_pin = 6; + axes[1].encoder_.abs_spi_cs_pin_init(); + axes[1].encoder_.config_.direction = 1; +#endif + //axes[1].encoder_.config_.mode = Encoder::MODE_HALL; + /////////////////////////////////////////// // Init usb irq binary semaphore, and start with no tokens by removing the starting one. osSemaphoreDef(sem_usb_irq); sem_usb_irq = osSemaphoreCreate(osSemaphore(sem_usb_irq), 1); diff --git a/Firmware/MotorControl/motor.hpp b/Firmware/MotorControl/motor.hpp index 7091e2538..962c73f26 100644 --- a/Firmware/MotorControl/motor.hpp +++ b/Firmware/MotorControl/motor.hpp @@ -15,22 +15,23 @@ class Motor : public ODriveIntf::MotorIntf { // example: vel_gain is [V/(turn/s)] instead of [Nm/(turn/s)] // example: current_lim and calibration_current will instead determine the maximum voltage applied to the motor. struct Config_t { - bool pre_calibrated = false; // can be set to true to indicate that all values here are valid - int32_t pole_pairs = 7; + bool pre_calibrated = true; // can be set to true to indicate that all values here are valid + int32_t pole_pairs = 4; + //int32_t pole_pairs = 26; float calibration_current = 10.0f; // [A] - float resistance_calib_max_voltage = 2.0f; // [V] - You may need to increase this if this voltage isn't sufficient to drive calibration_current through the motor. - float phase_inductance = 0.0f; // to be set by measure_phase_inductance - float phase_resistance = 0.0f; // to be set by measure_phase_resistance + float resistance_calib_max_voltage = 4.0f; // [V] - You may need to increase this if this voltage isn't sufficient to drive calibration_current through the motor. + float phase_inductance = 2.1256068066577427e-05;//0.0f; // to be set by measure_phase_inductance + float phase_resistance = 0.04318242892622948;//0.0f; // to be set by measure_phase_resistance float torque_constant = 0.04f; // [Nm/A] for PM motors, [Nm/A^2] for induction motors. Equal to 8.27/Kv of the motor MotorType motor_type = MOTOR_TYPE_HIGH_CURRENT; // Read out max_allowed_current to see max supported value for current_lim. // float current_lim = 70.0f; //[A] - float current_lim = 10.0f; //[A] - float current_lim_margin = 8.0f; // Maximum violation of current_lim + float current_lim = 46.0f; //[A] + float current_lim_margin = 20.0f; // Maximum violation of current_lim float torque_lim = std::numeric_limits::infinity(); //[Nm]. // Value used to compute shunt amplifier gains float requested_current_range = 60.0f; // [A] - float current_control_bandwidth = 1000.0f; // [rad/s] + float current_control_bandwidth = 300.0f; // [rad/s] float inverter_temp_limit_lower = 100; float inverter_temp_limit_upper = 120; diff --git a/Firmware/MotorControl/odrive_main.h b/Firmware/MotorControl/odrive_main.h index 4ddba244b..83129cd2b 100644 --- a/Firmware/MotorControl/odrive_main.h +++ b/Firmware/MotorControl/odrive_main.h @@ -80,8 +80,8 @@ struct BoardConfig_t { float max_regen_current = 0.0f; float brake_resistance = DEFAULT_BRAKE_RESISTANCE; bool enable_brake_resistor = false; - float dc_bus_undervoltage_trip_level = DEFAULT_MIN_DC_VOLTAGE; //(msg, 0, 32, true); + //给轴0设置速度输入 + //axis.controller_.input_vel_ = vel; + axes[0].controller_.input_vel_ = vel; } + + // if((msg.id == 0x219)) + // { + // float vel = can_getSignal(msg, 0, 32, true); + // if(vel != 0.0f) + // { + // if(axes[0].current_state_ != ODriveIntf::AxisIntf::AXIS_STATE_CLOSED_LOOP_CONTROL) + // { + // axes[0].requested_state_ = ODriveIntf::AxisIntf::AXIS_STATE_CLOSED_LOOP_CONTROL; + // } + // else + // { + // //给轴0设置速度输入 + // axes[0].controller_.input_vel_ = vel; + // //canbus_->send_message(txmsg); + // } + // } + // else + // { + // axes[0].requested_state_ = ODriveIntf::AxisIntf::AXIS_STATE_IDLE; + // axes[0].controller_.input_vel_ = 0.0f; + // } + // } + + // for (auto& axis : axes) { + // if ((axis.config_.can.node_id == nodeID) && (axis.config_.can.is_extended == msg.isExt)) { + // do_command(axis, msg); + // return; + // } + // } } void CANSimple::do_command(Axis& axis, const can_Message_t& msg) { diff --git a/Firmware/communication/can/odrive_can.hpp b/Firmware/communication/can/odrive_can.hpp index 5910c730d..a5227eed8 100644 --- a/Firmware/communication/can/odrive_can.hpp +++ b/Firmware/communication/can/odrive_can.hpp @@ -22,7 +22,7 @@ enum { class ODriveCAN : public CanBusBase, public ODriveIntf::CanIntf { public: struct Config_t { - uint32_t baud_rate = CAN_BAUD_250K; + uint32_t baud_rate = CAN_BAUD_500K; Protocol protocol = PROTOCOL_SIMPLE; ODriveCAN* parent = nullptr; // set in apply_config() diff --git a/ODrive_Workspace.code-workspace b/ODrive_Workspace.code-workspace index 27f8552d6..54dc5cbd9 100644 --- a/ODrive_Workspace.code-workspace +++ b/ODrive_Workspace.code-workspace @@ -2,21 +2,6 @@ "folders": [ { "path": "Firmware" - }, - { - "path": "tools" - }, - { - "path": "docs" - }, - { - "path": "analysis" - }, - { - "path": "Arduino" - }, - { - "path": "GUI" } ], "settings": { @@ -109,5 +94,8 @@ "image": "cpp", "imageutils": "cpp" } + }, + "trae": { + "workspaceId": "4811426dac073260154bca5b8ce49c82" } } From 821005e960adfe025e084d9bf6cbc86fb66c08c7 Mon Sep 17 00:00:00 2001 From: qiaoxu Date: Tue, 20 Oct 2026 17:38:34 +0800 Subject: [PATCH 2/4] =?UTF-8?q?=E5=B7=B2=E5=AE=8C=E6=88=90=E7=A7=BB?= =?UTF-8?q?=E6=A4=8DCAN=E9=80=9A=E8=AE=AF=EF=BC=8C=E4=BD=86=E4=B8=8A?= =?UTF-8?q?=E7=94=B5=E4=BC=9A=E6=8A=A5M1=E7=9A=84SPI=E6=95=85=E9=9A=9C?= =?UTF-8?q?=EF=BC=8CDR8301=E6=95=85=E9=9A=9C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Firmware/Board/v3/Inc/board.h | 31 +- Firmware/Board/v3/Inc/main.h | 4 +- Firmware/Board/v3/STM32F405RGTx_FLASH.ld | 13 +- Firmware/Board/v3/Src/i2c.c | 11 + Firmware/Board/v3/Src/stm32f4xx_it.c | 6 + Firmware/Board/v3/Src/usart.c | 24 +- Firmware/Board/v3/board.cpp | 16 + Firmware/Board/v3/startup_stm32f405xx.s | 2 +- Firmware/Makefile | 16 +- Firmware/MotorControl/axis.hpp | 15 +- Firmware/MotorControl/encoder.cpp | 1 + Firmware/MotorControl/main.cpp | 51 +- .../Device/ST/STM32F4xx/Include/stm32f405xx.h | 4 +- Firmware/communication/ascii_protocol.cpp | 5 + Firmware/communication/can/can_simple.cpp | 1378 ++++++++++++----- Firmware/communication/can/can_simple.hpp | 171 +- Firmware/communication/communication.cpp | 4 + Firmware/communication/interface_uart.cpp | 12 +- Firmware/communication/interface_usb.cpp | 9 +- 19 files changed, 1280 insertions(+), 493 deletions(-) diff --git a/Firmware/Board/v3/Inc/board.h b/Firmware/Board/v3/Inc/board.h index 708dc8532..af4d18b15 100644 --- a/Firmware/Board/v3/Inc/board.h +++ b/Firmware/Board/v3/Inc/board.h @@ -33,6 +33,24 @@ //#define ENCODER_USE_SPI_ABS_AMS #define ENCODER_USE_HALL +//qiaoxu add: 通信外设裁剪开关 (不用的外设注释掉, 对应代码不参与编译, 可减小固件体积) +// USE_UART_A : UART4 (GPIO1/GPIO2, ODrive 的 uart0) +// USE_UART_B : USART2 (GPIO3/GPIO4, ODrive 的 uart1) +// USE_UART_C : 预留, 本板未接 +// USE_I2C_A : I2C1 (GPIO13/GPIO14, 与 CAN_R/CAN_D 复用) +//#define USE_UART_A +//#define USE_UART_B +//#define USE_UART_C +//#define USE_I2C_A + +//qiaoxu add: 通信协议裁剪开关 +// USE_ASCII_PROTOCOL : USB CDC / UART 上的 ASCII 文本协议 ("p 0 1.0"、"r vbus_voltage" 等) +// 本机走 CAN + fibre(odrivetool), 不用 ASCII 文本协议, 关闭可省 ~29KB +//#define USE_ASCII_PROTOCOL + +#if defined(USE_UART_A) || defined(USE_UART_B) || defined(USE_UART_C) +#define USE_UART_ANY +#endif #define AXIS_COUNT (2) @@ -52,10 +70,19 @@ #define DEFAULT_ERROR_PIN 0 #define DEFAULT_MIN_DC_VOLTAGE 8.0f +//qiaoxu add: 裁剪掉 UART_A 时 GPIO1/GPIO2 不再默认复用为 UART +#ifdef USE_UART_A +#define DEFAULT_GPIO_MODE_1 ODriveIntf::GPIO_MODE_UART_A +#define DEFAULT_GPIO_MODE_2 ODriveIntf::GPIO_MODE_UART_A +#else +#define DEFAULT_GPIO_MODE_1 ODriveIntf::GPIO_MODE_DIGITAL +#define DEFAULT_GPIO_MODE_2 ODriveIntf::GPIO_MODE_DIGITAL +#endif + #define DEFAULT_GPIO_MODES \ ODriveIntf::GPIO_MODE_DIGITAL, \ - ODriveIntf::GPIO_MODE_UART_A, \ - ODriveIntf::GPIO_MODE_UART_A, \ + DEFAULT_GPIO_MODE_1, \ + DEFAULT_GPIO_MODE_2, \ ODriveIntf::GPIO_MODE_ANALOG_IN, \ ODriveIntf::GPIO_MODE_ANALOG_IN, \ ODriveIntf::GPIO_MODE_ANALOG_IN, \ diff --git a/Firmware/Board/v3/Inc/main.h b/Firmware/Board/v3/Inc/main.h index 9f762048e..2583d329d 100644 --- a/Firmware/Board/v3/Inc/main.h +++ b/Firmware/Board/v3/Inc/main.h @@ -67,8 +67,8 @@ /* Private define ------------------------------------------------------------*/ #define TIM_1_8_CLOCK_HZ 168000000 -//#define TIM_1_8_PERIOD_CLOCKS 3500//24khz 原程序24khz跑两个电机的中断时间不够,会报错:MOTOR_ERROR_CONTROL_DEADLINE_MISSED,磁编电机还会报ENCODER_ERROR_ABS_SPI_COM_FAIL -#define TIM_1_8_PERIOD_CLOCKS 7000 //qiaoxu modify 12khz +#define TIM_1_8_PERIOD_CLOCKS 3500//24khz 原程序24khz跑两个电机的中断时间不够,会报错:MOTOR_ERROR_CONTROL_DEADLINE_MISSED,磁编电机还会报ENCODER_ERROR_ABS_SPI_COM_FAIL +//#define TIM_1_8_PERIOD_CLOCKS 7000 //qiaoxu modify 12khz //#define TIM_1_8_PERIOD_CLOCKS 5250 //qiaoxu modify 16khz #define TIM_1_8_DEADTIME_CLOCKS 20 #define TIM_APB1_CLOCK_HZ 84000000 diff --git a/Firmware/Board/v3/STM32F405RGTx_FLASH.ld b/Firmware/Board/v3/STM32F405RGTx_FLASH.ld index 4619ed276..221a64ad4 100644 --- a/Firmware/Board/v3/STM32F405RGTx_FLASH.ld +++ b/Firmware/Board/v3/STM32F405RGTx_FLASH.ld @@ -44,8 +44,8 @@ MEMORY { RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 128K CCMRAM (rw) : ORIGIN = 0x10000000, LENGTH = 64K -FLASH (rx) : ORIGIN = 0x8000000, LENGTH = 768K -/* FLASH (rx) : ORIGIN = 0x800C000, LENGTH = 720K */ +/* FLASH (rx) : ORIGIN = 0x8000000, LENGTH = 768K */ +FLASH (rx) : ORIGIN = 0x800C000, LENGTH = 336k /*720K*/ NVM (r) : ORIGIN = 0x80C0000, LENGTH = 256K } @@ -60,6 +60,15 @@ SECTIONS . = ALIGN(4); } >FLASH + /* 添加版本信息段 */ + .version_info : + { + . = ALIGN(4); + /* 版本信息位于0x0800C188*/ + KEEP(*(.version_info)) + . = ALIGN(4); + } >FLASH + /* The program code and other data goes into FLASH */ .text : { diff --git a/Firmware/Board/v3/Src/i2c.c b/Firmware/Board/v3/Src/i2c.c index c4fca9423..2674de515 100644 --- a/Firmware/Board/v3/Src/i2c.c +++ b/Firmware/Board/v3/Src/i2c.c @@ -53,6 +53,9 @@ #include "gpio.h" #include "dma.h" +//qiaoxu add: 引入 USE_I2C_A 外设裁剪开关 +#include "board.h" + /* USER CODE BEGIN 0 */ /* USER CODE END 0 */ @@ -61,6 +64,7 @@ I2C_HandleTypeDef hi2c1; DMA_HandleTypeDef hdma_i2c1_rx; DMA_HandleTypeDef hdma_i2c1_tx; +#ifdef USE_I2C_A /* I2C1 init function */ void MX_I2C1_Init(uint8_t addr) { @@ -80,10 +84,13 @@ void MX_I2C1_Init(uint8_t addr) } } +#endif /* USE_I2C_A */ void HAL_I2C_MspInit(I2C_HandleTypeDef* i2cHandle) { + (void)i2cHandle; +#ifdef USE_I2C_A if(i2cHandle->Instance==I2C1) { /* USER CODE BEGIN I2C1_MspInit 0 */ @@ -139,11 +146,14 @@ void HAL_I2C_MspInit(I2C_HandleTypeDef* i2cHandle) /* USER CODE END I2C1_MspInit 1 */ } +#endif /* USE_I2C_A */ } void HAL_I2C_MspDeInit(I2C_HandleTypeDef* i2cHandle) { + (void)i2cHandle; +#ifdef USE_I2C_A if(i2cHandle->Instance==I2C1) { /* USER CODE BEGIN I2C1_MspDeInit 0 */ @@ -169,6 +179,7 @@ void HAL_I2C_MspDeInit(I2C_HandleTypeDef* i2cHandle) /* USER CODE END I2C1_MspDeInit 1 */ } +#endif /* USE_I2C_A */ } /* USER CODE BEGIN 1 */ diff --git a/Firmware/Board/v3/Src/stm32f4xx_it.c b/Firmware/Board/v3/Src/stm32f4xx_it.c index 59ab950bd..e16393fa8 100644 --- a/Firmware/Board/v3/Src/stm32f4xx_it.c +++ b/Firmware/Board/v3/Src/stm32f4xx_it.c @@ -39,6 +39,8 @@ /* USER CODE BEGIN 0 */ #include +//qiaoxu add: 引入 USE_UART_A / USE_UART_B 外设裁剪开关 +#include "board.h" /* USER CODE END 0 */ /* External variables --------------------------------------------------------*/ @@ -353,6 +355,7 @@ void CAN1_SCE_IRQHandler(void) /* USER CODE END CAN1_SCE_IRQn 1 */ } +#ifdef USE_UART_B /** * @brief This function handles USART2 global interrupt. */ @@ -366,6 +369,7 @@ void USART2_IRQHandler(void) /* USER CODE END USART2_IRQn 1 */ } +#endif /* USE_UART_B */ /** * @brief This function handles TIM8 trigger and commutation interrupts and TIM14 global interrupt. @@ -396,6 +400,7 @@ void SPI3_IRQHandler(void) /* USER CODE END SPI3_IRQn 1 */ } +#ifdef USE_UART_A /** * @brief This function handles UART4 global interrupt. */ @@ -409,6 +414,7 @@ void UART4_IRQHandler(void) /* USER CODE END UART4_IRQn 1 */ } +#endif /* USE_UART_A */ /* USER CODE BEGIN 1 */ diff --git a/Firmware/Board/v3/Src/usart.c b/Firmware/Board/v3/Src/usart.c index cddfd4e05..3228b25f3 100644 --- a/Firmware/Board/v3/Src/usart.c +++ b/Firmware/Board/v3/Src/usart.c @@ -53,6 +53,9 @@ #include "gpio.h" #include "dma.h" +//qiaoxu add: 引入 USE_UART_A / USE_UART_B 外设裁剪开关 +#include "board.h" + /* USER CODE BEGIN 0 */ /* USER CODE END 0 */ @@ -64,6 +67,7 @@ DMA_HandleTypeDef hdma_uart4_tx; DMA_HandleTypeDef hdma_usart2_rx; DMA_HandleTypeDef hdma_usart2_tx; +#ifdef USE_UART_A /* UART4 init function */ void MX_UART4_Init(void) { @@ -82,6 +86,9 @@ void MX_UART4_Init(void) } } +#endif /* USE_UART_A */ + +#ifdef USE_UART_B /* USART2 init function */ void MX_USART2_UART_Init(void) @@ -101,10 +108,13 @@ void MX_USART2_UART_Init(void) } } +#endif /* USE_UART_B */ void HAL_UART_MspInit(UART_HandleTypeDef* uartHandle) { + (void)uartHandle; +#ifdef USE_UART_A if(uartHandle->Instance==UART4) { /* USER CODE BEGIN UART4_MspInit 0 */ @@ -157,7 +167,10 @@ void HAL_UART_MspInit(UART_HandleTypeDef* uartHandle) /* USER CODE END UART4_MspInit 1 */ } - else if(uartHandle->Instance==USART2) +#endif /* USE_UART_A */ + +#ifdef USE_UART_B + if(uartHandle->Instance==USART2) { /* USER CODE BEGIN USART2_MspInit 0 */ @@ -209,11 +222,14 @@ void HAL_UART_MspInit(UART_HandleTypeDef* uartHandle) /* USER CODE END USART2_MspInit 1 */ } +#endif /* USE_UART_B */ } void HAL_UART_MspDeInit(UART_HandleTypeDef* uartHandle) { + (void)uartHandle; +#ifdef USE_UART_A if(uartHandle->Instance==UART4) { /* USER CODE BEGIN UART4_MspDeInit 0 */ @@ -232,7 +248,10 @@ void HAL_UART_MspDeInit(UART_HandleTypeDef* uartHandle) /* USER CODE END UART4_MspDeInit 1 */ } - else if(uartHandle->Instance==USART2) +#endif /* USE_UART_A */ + +#ifdef USE_UART_B + if(uartHandle->Instance==USART2) { /* USER CODE BEGIN USART2_MspDeInit 0 */ @@ -250,6 +269,7 @@ void HAL_UART_MspDeInit(UART_HandleTypeDef* uartHandle) /* USER CODE END USART2_MspDeInit 1 */ } +#endif /* USE_UART_B */ } /* USER CODE BEGIN 1 */ diff --git a/Firmware/Board/v3/board.cpp b/Firmware/Board/v3/board.cpp index 8ca399c96..7ad4eaebc 100644 --- a/Firmware/Board/v3/board.cpp +++ b/Firmware/Board/v3/board.cpp @@ -32,8 +32,16 @@ uint8_t __attribute__((section(".testdata"))) fake_otp[FLASH_OTP_END + 1 - FLASH Stm32SpiArbiter spi3_arbiter{&hspi3}; Stm32SpiArbiter& ext_spi_arbiter = spi3_arbiter; +#ifdef USE_UART_A UART_HandleTypeDef* uart_a = &huart4; +#else +UART_HandleTypeDef* uart_a = nullptr; +#endif +#ifdef USE_UART_B UART_HandleTypeDef* uart_b = &huart2; // TODO: this could be supported in ODrive v3.6 (or similar) using STM32's USART2 +#else +UART_HandleTypeDef* uart_b = nullptr; +#endif UART_HandleTypeDef* uart_c = nullptr; Drv8301 m0_gate_driver{ @@ -327,16 +335,21 @@ bool board_init() { HAL_NVIC_SetPriority(TIM8_UP_TIM13_IRQn, 0, 0); HAL_NVIC_EnableIRQ(TIM8_UP_TIM13_IRQn); +#ifdef USE_UART_A if (odrv.config_.enable_uart_a) { uart_a->Init.BaudRate = odrv.config_.uart_a_baudrate; MX_UART4_Init(); } +#endif +#ifdef USE_UART_B if (odrv.config_.enable_uart_b) { uart_b->Init.BaudRate = odrv.config_.uart_b_baudrate; MX_USART2_UART_Init(); } +#endif +#ifdef USE_I2C_A if (odrv.config_.enable_i2c_a) { // Set up the direction GPIO as input get_gpio(3).config(GPIO_MODE_INPUT, GPIO_PULLUP); @@ -350,6 +363,7 @@ bool board_init() { i2c_stats_.addr |= get_gpio(5).read() ? 0x4 : 0; MX_I2C1_Init(i2c_stats_.addr); } +#endif if (odrv.config_.enable_can_a) { // The CAN initialization will (and must) init its own GPIOs before the @@ -574,6 +588,7 @@ void ControlLoop_IRQHandler(void) { TaskTimer::enabled = false; } +#ifdef USE_I2C_A void I2C1_EV_IRQHandler(void) { COUNT_IRQ(I2C1_EV_IRQn); HAL_I2C_EV_IRQHandler(&hi2c1); @@ -583,6 +598,7 @@ void I2C1_ER_IRQHandler(void) { COUNT_IRQ(I2C1_ER_IRQn); HAL_I2C_ER_IRQHandler(&hi2c1); } +#endif extern PCD_HandleTypeDef hpcd_USB_OTG_FS; // defined in usbd_conf.c void OTG_FS_IRQHandler(void) { diff --git a/Firmware/Board/v3/startup_stm32f405xx.s b/Firmware/Board/v3/startup_stm32f405xx.s index 1b1a8afe4..35f0c2318 100644 --- a/Firmware/Board/v3/startup_stm32f405xx.s +++ b/Firmware/Board/v3/startup_stm32f405xx.s @@ -108,7 +108,7 @@ LoopFillZerobss: /* Call the clock system intitialization function.*/ bl SystemInit /* qiaoxu delete */ -/* bl early_start_checks */ +/* bl early_start_checks qiaoxu*/ /* Call static constructors */ bl __libc_init_array diff --git a/Firmware/Makefile b/Firmware/Makefile index d7c67032b..72e053fc7 100644 --- a/Firmware/Makefile +++ b/Firmware/Makefile @@ -32,6 +32,9 @@ firmware: @mkdir -p autogen @tup --quiet -no-environ-check +# qiaoxu fix: 原来没有 all 规则, 导致 flash 不会重新编译 +all: firmware + # Copy libfibre files to odrivetool if they were built libfibre-copy: @ ! test -f "fibre-cpp/build/libfibre-linux-amd64.so" || cp fibre-cpp/build/libfibre-linux-amd64.so ../tools/odrive/pyfibre/fibre/ @@ -40,13 +43,6 @@ libfibre-copy: @ ! test -f "fibre-cpp/build/libfibre-macos-x86.dylib" || cp fibre-cpp/build/libfibre-macos-x86.dylib ../tools/odrive/pyfibre/fibre/ @ ! test -f "fibre-cpp/build/libfibre-windows-amd64.dll" || cp fibre-cpp/build/libfibre-windows-amd64.dll ../tools/odrive/pyfibre/fibre/ -flash: firmware - $(OPENOCD) \ - -c 'reset halt' \ - -c 'flash write_image erase $(FIRMWARE)' \ - -c 'reset run' \ - -c exit - libfibre-linux-armhf: docker run -it -v "`pwd`/fibre-cpp":/build -v /tmp/fibre-linux-armhf-build:/build/build -w /build fibre-compiler configs/linux-armhf.config cp /tmp/fibre-linux-armhf-build/libfibre-*.so ../tools/odrive/pyfibre/fibre/ @@ -68,10 +64,12 @@ libfibre-all: clean: -rm -fR .dep $(BUILD_DIR) -flash-stlink2: all +# qiaoxu fix: 必须刷 HEX(只含 0x0800C000 起的真实数据), +# 刷 ELF 会把 ELF 头/填充写到 0x08000000..0x0800BFFF, 冲掉 boot 和 0x08008000 的 app1_ok 标志 +flash-stlink2: firmware $(OPENOCD) \ -c 'reset halt' \ - -c 'flash write_image erase $(FIRMWARE)' \ + -c 'flash write_image erase $(FIRMWARE_HEX)' \ -c 'reset run' \ -c exit diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index 1bfd58d98..2288adadc 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -52,8 +52,8 @@ class Axis : public ODriveIntf::AxisIntf { static LockinConfig_t default_lockin(); struct CANConfig_t { - uint32_t node_id = 0; - bool is_extended = false; + uint32_t node_id = 2;//qiaoxu modify + bool is_extended = true;//qiaoxu modify uint32_t heartbeat_rate_ms = 100; uint32_t encoder_rate_ms = 10; uint32_t motor_error_rate_ms = 0; @@ -107,13 +107,14 @@ class Axis : public ODriveIntf::AxisIntf { }; struct CAN_t { - uint32_t last_heartbeat = 0; - uint32_t last_encoder = 0; - uint32_t last_motor_error = 0; - uint32_t last_encoder_error = 0; + uint32_t last_heartbeat = 0; // 0x274 + uint32_t last_encoder = 0; // 0x21C 速度反馈 + uint32_t last_motor_error = 0; // 0x272/0x273 + uint32_t last_encoder_error = 0; // 0x275/0x276 uint32_t last_controller_error = 0; uint32_t last_sensorless_error = 0; - uint32_t last_encoder_count = 0; + uint32_t last_encoder_count = 0; // 0x21A/0x21E + uint32_t last_axis_error = 0; // 0x270/0x271 uint32_t last_iq = 0; uint32_t last_sensorless = 0; uint32_t last_bus_vi = 0; diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index 73771c340..183506064 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -47,6 +47,7 @@ void Encoder::setup() { .CLKPhase = SPI_PHASE_2EDGE, .NSS = SPI_NSS_SOFT, .BaudRatePrescaler = SPI_BAUDRATEPRESCALER_16, +// .BaudRatePrescaler = SPI_BAUDRATEPRESCALER_32, //qiaoxu modify V0.5.1 .FirstBit = SPI_FIRSTBIT_MSB, .TIMode = SPI_TIMODE_DISABLE, .CRCCalculation = SPI_CRCCALCULATION_DISABLE, diff --git a/Firmware/MotorControl/main.cpp b/Firmware/MotorControl/main.cpp index 6b437e6a2..fa842f35f 100644 --- a/Firmware/MotorControl/main.cpp +++ b/Firmware/MotorControl/main.cpp @@ -267,21 +267,33 @@ void vApplicationIdleHook(void) { std::transform(axes.begin(), axes.end(), std::begin(min_stack_space), [](auto& axis) { return uxTaskGetStackHighWaterMark(axis.thread_id_) * sizeof(StackType_t); }); odrv.system_stats_.max_stack_usage_axis = axes[0].stack_size_ - *std::min_element(std::begin(min_stack_space), std::end(min_stack_space)); odrv.system_stats_.max_stack_usage_usb = stack_size_usb_thread - uxTaskGetStackHighWaterMark(usb_thread) * sizeof(StackType_t); +#ifdef USE_UART_ANY odrv.system_stats_.max_stack_usage_uart = stack_size_uart_thread - uxTaskGetStackHighWaterMark(uart_thread) * sizeof(StackType_t); +#else + odrv.system_stats_.max_stack_usage_uart = 0; +#endif odrv.system_stats_.max_stack_usage_startup = stack_size_default_task - uxTaskGetStackHighWaterMark(defaultTaskHandle) * sizeof(StackType_t); odrv.system_stats_.max_stack_usage_can = odrv.can_.stack_size_ - uxTaskGetStackHighWaterMark(odrv.can_.thread_id_) * sizeof(StackType_t); odrv.system_stats_.max_stack_usage_analog = stack_size_analog_thread - uxTaskGetStackHighWaterMark(analog_thread) * sizeof(StackType_t); odrv.system_stats_.stack_size_axis = axes[0].stack_size_; odrv.system_stats_.stack_size_usb = stack_size_usb_thread; +#ifdef USE_UART_ANY odrv.system_stats_.stack_size_uart = stack_size_uart_thread; +#else + odrv.system_stats_.stack_size_uart = 0; +#endif odrv.system_stats_.stack_size_startup = stack_size_default_task; odrv.system_stats_.stack_size_can = odrv.can_.stack_size_; odrv.system_stats_.stack_size_analog = stack_size_analog_thread; odrv.system_stats_.prio_axis = osThreadGetPriority(axes[0].thread_id_); odrv.system_stats_.prio_usb = osThreadGetPriority(usb_thread); +#ifdef USE_UART_ANY odrv.system_stats_.prio_uart = osThreadGetPriority(uart_thread); +#else + odrv.system_stats_.prio_uart = 0; +#endif odrv.system_stats_.prio_startup = osThreadGetPriority(defaultTaskHandle); odrv.system_stats_.prio_can = osThreadGetPriority(odrv.can_.thread_id_); odrv.system_stats_.prio_analog = osThreadGetPriority(analog_thread); @@ -396,7 +408,9 @@ void ODrive::control_loop_cb(uint32_t timestamp) { axis.sensorless_estimator_.vel_estimate_.reset(); } +#ifdef USE_UART_ANY uart_poll(); +#endif odrv.oscilloscope_.update(); } @@ -675,6 +689,39 @@ extern "C" int main(void) { config_apply_all(); } + //qiaoxu add: 编译期被裁剪掉的通信外设强制关闭, 并把对应 GPIO 模式回退为普通数字口, + //避免设备里残留的旧 NVM 配置(如 enable_uart_a=true / gpio_modes=UART_A)导致 misconfigured +#ifndef USE_UART_A + odrv.config_.enable_uart_a = false; + for (auto& mode : odrv.config_.gpio_modes) { + if (mode == ODriveIntf::GPIO_MODE_UART_A) mode = ODriveIntf::GPIO_MODE_DIGITAL; + } +#endif +#ifndef USE_UART_B + odrv.config_.enable_uart_b = false; + for (auto& mode : odrv.config_.gpio_modes) { + if (mode == ODriveIntf::GPIO_MODE_UART_B) mode = ODriveIntf::GPIO_MODE_DIGITAL; + } +#endif +#ifndef USE_UART_C + odrv.config_.enable_uart_c = false; + for (auto& mode : odrv.config_.gpio_modes) { + if (mode == ODriveIntf::GPIO_MODE_UART_C) mode = ODriveIntf::GPIO_MODE_DIGITAL; + } +#endif +#ifndef USE_I2C_A + odrv.config_.enable_i2c_a = false; + for (auto& mode : odrv.config_.gpio_modes) { + if (mode == ODriveIntf::GPIO_MODE_I2C_A) mode = ODriveIntf::GPIO_MODE_DIGITAL; + } +#endif +#ifndef USE_ASCII_PROTOCOL + //qiaoxu add: 没有编译 ASCII 协议时, 把 CDC 串口降级为纯 stdout(printf), 否则协议字段会指向不存在的解析器 + if (odrv.config_.usb_cdc_protocol == ODriveIntf::STREAM_PROTOCOL_TYPE_ASCII || + odrv.config_.usb_cdc_protocol == ODriveIntf::STREAM_PROTOCOL_TYPE_ASCII_AND_STDOUT) { + odrv.config_.usb_cdc_protocol = ODriveIntf::STREAM_PROTOCOL_TYPE_STDOUT; + } +#endif odrv.misconfigured_ = odrv.misconfigured_ || (odrv.config_.enable_uart_a && !uart_a) || (odrv.config_.enable_uart_b && !uart_b) @@ -821,8 +868,8 @@ extern "C" int main(void) { //qiaoxu 抱闸控制 axis0-gpio8 axis1-gpio5 get_gpio(8).config(GPIO_MODE_OUTPUT_PP, GPIO_NOPULL); get_gpio(5).config(GPIO_MODE_OUTPUT_PP, GPIO_NOPULL); - get_gpio(8).write(true);//开闸 - get_gpio(5).write(true); + // get_gpio(8).write(true);//开闸 + // get_gpio(5).write(true); // get_gpio(8).write(false);//抱闸 // get_gpio(5).write(false); //qiaoxu 手动抱闸检测 axis0-gpio3 axis1-gpio4 diff --git a/Firmware/ThirdParty/CMSIS/Device/ST/STM32F4xx/Include/stm32f405xx.h b/Firmware/ThirdParty/CMSIS/Device/ST/STM32F4xx/Include/stm32f405xx.h index c2897bbd7..148ade251 100644 --- a/Firmware/ThirdParty/CMSIS/Device/ST/STM32F4xx/Include/stm32f405xx.h +++ b/Firmware/ThirdParty/CMSIS/Device/ST/STM32F4xx/Include/stm32f405xx.h @@ -825,8 +825,8 @@ typedef struct * @{ */ //qiaoxu modify -#define FLASH_BASE 0x08000000U /*!< FLASH(up to 1 MB) base address in the alias region */ -//#define FLASH_BASE 0x0800C000U // APP start address +//#define FLASH_BASE 0x08000000U /*!< FLASH(up to 1 MB) base address in the alias region */ +#define FLASH_BASE 0x0800C000U // APP start address #define CCMDATARAM_BASE 0x10000000U /*!< CCM(core coupled memory) data RAM(64 KB) base address in the alias region */ #define SRAM1_BASE 0x20000000U /*!< SRAM1(112 KB) base address in the alias region */ #define SRAM2_BASE 0x2001C000U /*!< SRAM2(16 KB) base address in the alias region */ diff --git a/Firmware/communication/ascii_protocol.cpp b/Firmware/communication/ascii_protocol.cpp index 5d7bdab78..aadb9b0c5 100644 --- a/Firmware/communication/ascii_protocol.cpp +++ b/Firmware/communication/ascii_protocol.cpp @@ -16,6 +16,9 @@ #include "autogen/type_info.hpp" #include "communication/interface_can.hpp" +//qiaoxu add: 不用 ASCII 文本协议时, 整个文件不参与编译 +#ifdef USE_ASCII_PROTOCOL + using namespace fibre; /* Private macros ------------------------------------------------------------*/ @@ -452,3 +455,5 @@ void AsciiProtocol::start() { TransferHandle dummy; rx_channel_->start_read(rx_buf_, &dummy, MEMBER_CB(this, on_read_finished)); } + +#endif // USE_ASCII_PROTOCOL diff --git a/Firmware/communication/can/can_simple.cpp b/Firmware/communication/can/can_simple.cpp index f50d88223..28aaf5b45 100644 --- a/Firmware/communication/can/can_simple.cpp +++ b/Firmware/communication/can/can_simple.cpp @@ -1,499 +1,1133 @@ - #include "can_simple.hpp" #include -#include -bool CANSimple::init() { - for (size_t i = 0; i < AXIS_COUNT; ++i) { - if (!renew_subscription(i)) { - return false; - } +#include +#include +#include + +// 版本信息读取 (定义见文件末尾, 数据由 .version_info 段提供) +uint16_t get_hardware_version(void); +uint32_t get_software_version(void); +uint32_t get_publish_data(void); + +/* ------------------------------------------------------------------------- */ +/* 本文件移植自 odrive-vlago 工程的 communication/can_simple.cpp */ +/* 接收侧改为 "2 个硬件 bank + 一个 switch (msg.id) 分发"; */ +/* 原来 ODrive 标准的 do_command() (nodeID|cmdID) 解析已停用。 */ +/* ------------------------------------------------------------------------- */ + +// 周期参数 (与参考工程一致) +#define VEL_ESTIMATE_PERIOD_MS (20) // 0x21C 速度反馈 +#define AXIS_ERROR_PERIOD_MS (1000) // 0x270/0x271 +#define MOTOR_ERROR_PERIOD_MS (1005) // 0x272/0x273 +#define ENCODER_ERROR_PERIOD_MS (1010) // 0x275/0x276 +#define ENCODER_COUNT_PERIOD_MS (50) // 0x21A/0x21E +#define HEARTBEAT_PERIOD_MS (100) // 0x274, 内部每 5 次真正发一帧 (500ms) + +#define MAX_MSG_LOST_COUNT (3) // 指令丢失判定 (100ms * 4 = 400ms) +#define BRAKE_FILTER_COUNT (6) // 手动抱闸 IO 滤波次数 + +// 抱闸动作阈值 (单位: turn/s) +#define BREAK_SPEED_MT (2.0f) +#define MIN_ZERO_IDLE_CNT (12) // 抱闸后再延时 12 个指令周期切 IDLE + +#ifdef ENCODER_USE_SPI_ABS_AMS +#define MIN_ZERO_SPEED_CNT (750) // 磁编: 速度小于阈值后 15s 抱闸 +#elif defined(ENCODER_USE_HALL) +#define MIN_ZERO_SPEED_CNT (50) // 霍尔 +#else +#define MIN_ZERO_SPEED_CNT (750) +#endif + +// PID 参数 (可通过 0x208 修改) +float PID_P = 0.1f; +float PID_I = 0.05f; +float PID_P_ENCODER_ABS = 0.15f; +float PID_I_ENCODER_ABS = 0.1f; + +uint8_t g_identify_mode = 0; // 进入识别模式 +uint8_t g_level = 1; // 摇杆档位 (预留) +uint8_t g_drive_mode = 1; // 驾驶模式: 1-舒适 2-运动 +uint8_t g_operate_mode = 4; // 运行模式: 3-推行, 4-标准, 7-狭窄空间 + +uint32_t CANSimple::msg_lost_count_ = 0; +bool CANSimple::brake_sts_ = false; +bool CANSimple::back_push_sts_ = false; +bool CANSimple::brake_released_[AXIS_COUNT] = {false, false}; + +/* ========================================================================= */ +/* 摇杆(ADC)差速控制 —— 移植自 odrive-vlago can_simple.cpp (生效版本: */ +/* "优化低速转弯顿挫+三挡转弯降速") */ +/* 输入: 0x279 的 ADC 值; 输出: 左右轮目标转速 (turn/s) */ +/* 注意: JOY_ADC_* / 档位表是实车标定参数, 换摇杆或换车需要重新标定 */ +/* ========================================================================= */ + +#define SPORT_MODE 1 // 1-运动模式 0-舒适模式 + +// 摇杆 ADC 标定 (白车摇杆) +#define WHEELBASE_M 0.62f // 轮距 [m] +#define JOY_ADC_X_MIN (0x0175 + 50) // X 轴(左右)最小值 +#define JOY_ADC_X_MAX (0x0E05 - 50) // X 轴(左右)最大值 +#define JOY_ADC_X_CENTER 0x082E // X 轴中位值 +#define JOY_ADC_Y_MIN (0x01A1 + 50) // Y 轴(前后)最小值 +#define JOY_ADC_Y_MAX (0x0E22 - 50) // Y 轴(前后)最大值 +#define JOY_ADC_Y_CENTER 0x081B // Y 轴中位值 + +// 控制参数 +#define DEADZONE_X_AD 150 // 中位死区(ADC 值) +#define LOWPASS_ALPHA_X_SPORT 0.8f // 运动模式 X 轴低通系数 +#define LOWPASS_ALPHA_X_COMFORT 0.2f // 舒适模式 X 轴低通系数 +#define RATE_ALPHA_X 1.0f // X 轴变化率限制 (1 = 不限制) +#define DEADZONE_Y_AD 150 // 中位死区(ADC 值) +#define LOWPASS_ALPHA_Y 0.05f // Y 轴低通系数 +#define RATE_ALPHA_Y_SPORT 0.01f // 运动模式 Y 轴变化率 +#define RATE_ALPHA_Y_SPORT_GEAR1 0.03f // 运动模式 1 档 Y 轴变化率 +#define RATE_ALPHA_Y_COMFORT 0.005f // 舒适模式 Y 轴变化率 + +// 档位参数 (1~4 档) +float MAX_MOTOR_RPS = 12.0f; // 直行最大速度 [turn/s] +float MAX_MOTOR_RPS_REVERSE = 7.0f; // 倒车最大速度 +float MAX_MOTOR_RPS_SPIN_TURN = 5.0f;// 原地转圈最大速度 +float TURN_SENSITIVITY = 0.8f; // 转向灵敏度 +float TURN_MARGIN = 3.0f; // 转弯时外轮允许超出的限幅余量 + +static const float max_rps_table_comfort[] = {9.0f, 18.0f, 27.0f, 36.0f}; +static const float max_rps_table_sport[] = {12.0f, 24.0f, 36.0f, 48.0f}; +static const float max_rps_reverse_table[] = {7.0f, 9.0f, 11.0f, 13.0f}; +static const float max_rps_spin_turn_table_comfort[] = {4.0f, 5.0f, 6.0f, 7.0f}; +static const float max_rps_spin_turn_table_sport[] = {5.0f, 6.0f, 7.0f, 8.0f}; +static const float turn_sensitivity_table[] = {0.8f, 0.55f, 0.45f, 0.4f}; +static const float turn_margin_table[] = {20.0f, 25.0f, 35.0f, 50.0f}; + +#define MAX_LEVEL 4 // 档位数 +static_assert((int)(sizeof(max_rps_table_sport) / sizeof(max_rps_table_sport[0])) == MAX_LEVEL, + "档位参数表长度与 MAX_LEVEL 不一致"); +#define GEAR_TRANSITION_ALPHA 0.03f // 换档平滑系数 +#define SPIN_WEIGHT_ALPHA 0.9f // 原地转圈权重平滑系数 + +typedef struct { + // 输入 + uint16_t adc_x_raw; // 摇杆 X 轴原始 ADC 值 + uint16_t adc_y_raw; // 摇杆 Y 轴原始 ADC 值 + + // 内部状态 + float joy_x_filtered; // 滤波后的 X 轴指令 (-1.0 ~ +1.0) + float joy_y_filtered; // 滤波后的 Y 轴指令 (-1.0 ~ +1.0) + float left_speed_cmd; // 计算出的左轮速度命令 [turn/s] + float right_speed_cmd; // 计算出的右轮速度命令 [turn/s] + + // 换档平滑参数 (仅前进和后退) + float max_motor_rps_smooth; + float max_motor_rps_reverse_smooth; + + // 输出 + float left_speed_out; + float right_speed_out; +} JoystickController_t; + +static JoystickController_t joystick_ctrl; + +static void JoystickCtrl_Init(JoystickController_t *ctrl) { + if (ctrl == NULL) return; + + ctrl->adc_x_raw = JOY_ADC_X_CENTER; + ctrl->adc_y_raw = JOY_ADC_Y_CENTER; + ctrl->joy_x_filtered = 0.0f; + ctrl->joy_y_filtered = 0.0f; + ctrl->left_speed_cmd = 0.0f; + ctrl->right_speed_cmd = 0.0f; + ctrl->left_speed_out = 0.0f; + ctrl->right_speed_out = 0.0f; + ctrl->max_motor_rps_smooth = max_rps_table_sport[0]; + ctrl->max_motor_rps_reverse_smooth = max_rps_reverse_table[0]; +} + +// ADC 值映射到 [-1, 1], 中位死区内输出 0 +static float _normalize_with_deadzone_optimized(uint16_t raw, + uint16_t min_val, + uint16_t max_val, + uint16_t center_val, + uint16_t deadzone_adc) { + uint16_t deadzone_min = (center_val > deadzone_adc) ? center_val - deadzone_adc : min_val; + uint16_t deadzone_max = (center_val + deadzone_adc < max_val) ? center_val + deadzone_adc : max_val; + + if (raw >= deadzone_min && raw <= deadzone_max) { + return 0.0f; + } + + if (raw < deadzone_min) { + if (raw <= min_val) return -1.0f; + uint16_t left_range = deadzone_min - min_val; + if (left_range == 0) return -1.0f; + return -1.0f * (float)(deadzone_min - raw) / (float)left_range; + } + + if (raw >= max_val) return 1.0f; + uint16_t right_range = max_val - deadzone_max; + if (right_range == 0) return 1.0f; + return (float)(raw - deadzone_max) / (float)right_range; +} + +// 一阶低通滤波 +static float _lowpass_filter(float new_value, float old_value, float alpha) { + float value = alpha * new_value + (1.0f - alpha) * old_value; + if (fabsf(value) < 0.001f) value = 0.0f; // 小值置零, 避免漂移 + return value; +} + +// 变化率限制, 防突变 +static float _rate_limit(float new_cmd, float old_cmd, float max_accel_delta, float max_reduce_delta) { + if (fabsf(new_cmd) < 0.001f) return 0.0f; + + float delta = new_cmd - old_cmd; + float abs_new = fabsf(new_cmd); + float abs_old = fabsf(old_cmd); + float limit; + + if ((new_cmd > 0 && old_cmd < 0) || (new_cmd < 0 && old_cmd > 0)) { + limit = max_reduce_delta; // 方向改变, 采用更严格的限制 + } else if (abs_new > abs_old) { + limit = max_accel_delta; + } else { + limit = max_reduce_delta; } - return true; + if (fabsf(delta) > limit) { + return (delta > 0) ? (old_cmd + limit) : (old_cmd - limit); + } + return new_cmd; } -bool CANSimple::renew_subscription(size_t i) { - Axis& axis = axes[i]; +// 摇杆 ADC -> 左右轮目标转速 +static void JoystickCtrl_Update(JoystickController_t *ctrl, uint16_t adc_x, uint16_t adc_y, + uint8_t gear_num, uint8_t drive_mode) { + if (ctrl == NULL) return; + ctrl->adc_x_raw = adc_x; + ctrl->adc_y_raw = adc_y; + + // 标定到 [-1,1] 并应用死区 + float joy_x_normalized = _normalize_with_deadzone_optimized(adc_x, JOY_ADC_X_MIN, JOY_ADC_X_MAX, + JOY_ADC_X_CENTER, DEADZONE_X_AD); + float joy_y_normalized = _normalize_with_deadzone_optimized(adc_y, JOY_ADC_Y_MIN, JOY_ADC_Y_MAX, + JOY_ADC_Y_CENTER, DEADZONE_Y_AD); + joy_x_normalized = -1.0f * joy_x_normalized; // X 轴方向修正 + + // --- 指令滤波 --- + static float last_joy_x = 0.0f, last_joy_y = 0.0f; + if (drive_mode == 0) { // 舒适模式 + ctrl->joy_x_filtered = _lowpass_filter(joy_x_normalized, ctrl->joy_x_filtered, LOWPASS_ALPHA_X_COMFORT); + ctrl->joy_y_filtered = _lowpass_filter(joy_y_normalized, ctrl->joy_y_filtered, LOWPASS_ALPHA_Y); + if (joy_y_normalized != 0) { + ctrl->joy_y_filtered = _rate_limit(ctrl->joy_y_filtered, last_joy_y, RATE_ALPHA_Y_COMFORT, 1); + } + } else { // 运动模式 + ctrl->joy_x_filtered = _lowpass_filter(joy_x_normalized, ctrl->joy_x_filtered, LOWPASS_ALPHA_X_SPORT); + ctrl->joy_y_filtered = _lowpass_filter(joy_y_normalized, ctrl->joy_y_filtered, LOWPASS_ALPHA_Y); + + if (fabsf(joy_y_normalized) < 0.1f) { + ctrl->joy_x_filtered = _rate_limit(ctrl->joy_x_filtered, last_joy_x, RATE_ALPHA_X, 0.03f); + } + if ((joy_y_normalized != 0) && (gear_num != 1)) { + ctrl->joy_y_filtered = _rate_limit(ctrl->joy_y_filtered, last_joy_y, RATE_ALPHA_Y_SPORT, 1); + } else if ((joy_y_normalized != 0) && (gear_num == 1)) { + ctrl->joy_y_filtered = _rate_limit(ctrl->joy_y_filtered, last_joy_y, RATE_ALPHA_Y_SPORT_GEAR1, 1); + } + } + last_joy_x = ctrl->joy_x_filtered; + last_joy_y = ctrl->joy_y_filtered; + + // --- 档位参数 --- + uint8_t gear_index = (gear_num > 0 && gear_num <= MAX_LEVEL) ? (uint8_t)(gear_num - 1) : 0; - // TODO: remove these two lines (see comment in header) - node_ids_[i] = axis.config_.can.node_id; - extended_node_ids_[i] = axis.config_.can.is_extended; + float target_max_motor_rps; + float target_max_motor_rps_reverse = max_rps_reverse_table[gear_index]; + if (drive_mode == 0) { + target_max_motor_rps = max_rps_table_comfort[gear_index]; + } else { + target_max_motor_rps = max_rps_table_sport[gear_index]; + } + + // 只对前进/后退最高速做换档平滑 + ctrl->max_motor_rps_smooth = _lowpass_filter(target_max_motor_rps, ctrl->max_motor_rps_smooth, GEAR_TRANSITION_ALPHA); + ctrl->max_motor_rps_reverse_smooth = _lowpass_filter(target_max_motor_rps_reverse, ctrl->max_motor_rps_reverse_smooth, GEAR_TRANSITION_ALPHA); + + MAX_MOTOR_RPS = ctrl->max_motor_rps_smooth; + MAX_MOTOR_RPS_REVERSE = ctrl->max_motor_rps_reverse_smooth; - MsgIdFilterSpecs filter = { - .id = {}, - .mask = (uint32_t)(0xffffffff << NUM_CMD_ID_BITS)}; - if (axis.config_.can.is_extended) { - filter.id = (uint32_t)(axis.config_.can.node_id << NUM_CMD_ID_BITS); + TURN_SENSITIVITY = turn_sensitivity_table[gear_index]; + TURN_MARGIN = turn_margin_table[gear_index]; + if (drive_mode == 0) { + MAX_MOTOR_RPS_SPIN_TURN = max_rps_spin_turn_table_comfort[gear_index]; } else { - filter.id = (uint16_t)(axis.config_.can.node_id << NUM_CMD_ID_BITS); + MAX_MOTOR_RPS_SPIN_TURN = max_rps_spin_turn_table_sport[gear_index]; } - if (subscription_handles_[i]) { - canbus_->unsubscribe(subscription_handles_[i]); + // --- 原地转圈模式 --- + float spin_speed = ctrl->joy_x_filtered * MAX_MOTOR_RPS_SPIN_TURN; + float spin_left = -spin_speed; // 左轮反向 + float spin_right = spin_speed; // 右轮正向 + + // --- 转向半径模型 --- + float joy_x_abs = fabsf(ctrl->joy_x_filtered); + float joy_y_abs = fabsf(ctrl->joy_y_filtered); + float base_speed = (ctrl->joy_y_filtered >= 0.0f) ? + (ctrl->joy_y_filtered * MAX_MOTOR_RPS) : + (ctrl->joy_y_filtered * MAX_MOTOR_RPS_REVERSE); + float turn_gain = ctrl->joy_x_filtered * TURN_SENSITIVITY; + if (ctrl->joy_y_filtered < 0.0f) { + turn_gain = -turn_gain; // 倒车时转向反向 + } + float radius_left = base_speed * (1.0f - turn_gain); + float radius_right = base_speed * (1.0f + turn_gain); + + // 动态限幅: 外轮饱和时整体缩放, 保持转向 + float max_allowed = max_rps_table_sport[MAX_LEVEL - 1]; + float abs_max = fabsf(max_allowed); + if (fabsf(radius_left) > abs_max + 0.1f || fabsf(radius_right) > abs_max + 0.1f) { + float max_speed = fmaxf(fabsf(radius_left), fabsf(radius_right)); + float scale = abs_max / max_speed; + radius_left *= scale; + radius_right *= scale; + } + + // --- 两种模型平滑切换 --- + float spin_weight_raw = (1.0f - joy_y_abs) * joy_x_abs; + if (spin_weight_raw < 0.0f) spin_weight_raw = 0.0f; + if (spin_weight_raw > 1.0f) spin_weight_raw = 1.0f; + + static float spin_weight_filtered = 0.0f; + if (joy_y_abs >= 0.2f) { + float alpha = 0.1f * joy_y_abs * SPIN_WEIGHT_ALPHA; // y 较大: 慢平滑 + spin_weight_filtered = _lowpass_filter(spin_weight_raw, spin_weight_filtered, alpha); + } else if (joy_y_abs >= 0.001f) { + float alpha = 0.5f * (1.0f - joy_y_abs / 0.2f); // y 较小: 快平滑 + spin_weight_filtered = _lowpass_filter(spin_weight_raw, spin_weight_filtered, alpha); + } else { + spin_weight_filtered = spin_weight_raw; // y=0: 直接使用 } - return canbus_->subscribe( - filter, [](void* ctx, const can_Message_t& msg) { - ((CANSimple*)ctx)->handle_can_message(msg); - }, - this, &subscription_handles_[i]); + float spin_weight = spin_weight_filtered; + float radius_weight = 1.0f - spin_weight; + + ctrl->left_speed_cmd = radius_left * radius_weight + spin_left * spin_weight; + ctrl->right_speed_cmd = radius_right * radius_weight + spin_right * spin_weight; + + ctrl->left_speed_out = ctrl->left_speed_cmd; + ctrl->right_speed_out = ctrl->right_speed_cmd; } -void CANSimple::handle_can_message(const can_Message_t& msg) { - // Frame - // nodeID | CMD - // 6 bits | 5 bits - uint32_t nodeID = get_node_id(msg.id); - Axis& axis = axes[0]; - - if(msg.id == 0x219) - { - float vel = can_getSignal(msg, 0, 32, true); - //给轴0设置速度输入 - //axis.controller_.input_vel_ = vel; - axes[0].controller_.input_vel_ = vel; - } - - // if((msg.id == 0x219)) - // { - // float vel = can_getSignal(msg, 0, 32, true); - // if(vel != 0.0f) - // { - // if(axes[0].current_state_ != ODriveIntf::AxisIntf::AXIS_STATE_CLOSED_LOOP_CONTROL) - // { - // axes[0].requested_state_ = ODriveIntf::AxisIntf::AXIS_STATE_CLOSED_LOOP_CONTROL; - // } - // else - // { - // //给轴0设置速度输入 - // axes[0].controller_.input_vel_ = vel; - // //canbus_->send_message(txmsg); - // } - // } - // else - // { - // axes[0].requested_state_ = ODriveIntf::AxisIntf::AXIS_STATE_IDLE; - // axes[0].controller_.input_vel_ = 0.0f; - // } - // } - - // for (auto& axis : axes) { - // if ((axis.config_.can.node_id == nodeID) && (axis.config_.can.is_extended == msg.isExt)) { - // do_command(axis, msg); - // return; - // } - // } +bool CANSimple::init() { + // bank0: 扩展帧 0x200~0x2FF, 一条掩码覆盖 + // 0x208 / 0x217 / 0x219 / 0x21B / 0x21D / 0x277 + // 以及上控/摇杆相关: 0x220 / 0x221 / 0x222 / 0x229 / 0x279 + // 掩码 0x700 只比较 bit8~bit10, 因此只有 0x2xx 的扩展帧能进来; + // 具体是哪条命令由 handle_can_message() 的 switch (msg.id) 精确分发。 + MsgIdFilterSpecs cmd_filter = { + .id = (uint32_t)0x200, + .mask = (uint32_t)0x700}; + if (!canbus_->subscribe(cmd_filter, &CANSimple::can_message_callback, this, &subscription_handle_)) { + return false; + } + + // bank1: 扩展帧 0x701 精确匹配 (OTA/复位, 与 bootloader 一致, 29 位全掩码) + MsgIdFilterSpecs ota_filter = { + .id = (uint32_t)MSG_OTA_RESET, + .mask = (uint32_t)0x1FFFFFFF}; + return canbus_->subscribe(ota_filter, &CANSimple::can_message_callback, this, &ota_subscription_handle_); } -void CANSimple::do_command(Axis& axis, const can_Message_t& msg) { - const uint32_t cmd = get_cmd_id(msg.id); - axis.watchdog_feed(); - switch (cmd) { - case MSG_CO_NMT_CTRL: - break; - case MSG_CO_HEARTBEAT_CMD: - break; - case MSG_ODRIVE_HEARTBEAT: - // We don't currently do anything to respond to ODrive heartbeat messages - break; - case MSG_ODRIVE_ESTOP: - estop_callback(axis, msg); - break; - case MSG_GET_MOTOR_ERROR: - if (msg.rtr || msg.len == 0) - get_motor_error_callback(axis); - break; - case MSG_GET_ENCODER_ERROR: - if (msg.rtr || msg.len == 0) - get_encoder_error_callback(axis); - break; - case MSG_GET_SENSORLESS_ERROR: - if (msg.rtr || msg.len == 0) - get_sensorless_error_callback(axis); - break; - case MSG_SET_AXIS_NODE_ID: - set_axis_nodeid_callback(axis, msg); - break; - case MSG_SET_AXIS_REQUESTED_STATE: - set_axis_requested_state_callback(axis, msg); - break; - case MSG_SET_AXIS_STARTUP_CONFIG: - set_axis_startup_config_callback(axis, msg); - break; - case MSG_GET_ENCODER_ESTIMATES: - if (msg.rtr || msg.len == 0) - get_encoder_estimates_callback(axis); - break; - case MSG_GET_ENCODER_COUNT: - if (msg.rtr || msg.len == 0) - get_encoder_count_callback(axis); - break; - case MSG_SET_INPUT_POS: - set_input_pos_callback(axis, msg); - break; - case MSG_SET_INPUT_VEL: - set_input_vel_callback(axis, msg); - break; - case MSG_SET_INPUT_TORQUE: - set_input_torque_callback(axis, msg); - break; - case MSG_SET_CONTROLLER_MODES: - set_controller_modes_callback(axis, msg); - break; - case MSG_SET_LIMITS: - set_limits_callback(axis, msg); - break; - case MSG_START_ANTICOGGING: - start_anticogging_callback(axis, msg); - break; - case MSG_SET_TRAJ_INERTIA: - set_traj_inertia_callback(axis, msg); - break; - case MSG_SET_TRAJ_ACCEL_LIMITS: - set_traj_accel_limits_callback(axis, msg); - break; - case MSG_SET_TRAJ_VEL_LIMIT: - set_traj_vel_limit_callback(axis, msg); - break; - case MSG_GET_IQ: - if (msg.rtr || msg.len == 0) - get_iq_callback(axis); +void CANSimple::can_message_callback(void* ctx, const can_Message_t& msg) { + static_cast(ctx)->handle_can_message(msg); +} + +void CANSimple::handle_can_message(const can_Message_t& msg) { + if (!msg.isExt) { + return; + } + + // 识别/校准过程中只接受 0x277 本身 (OTA 复位始终允许) + if ((g_identify_mode != 0) && (msg.id != MSG_IDENTIFY) && (msg.id != MSG_OTA_RESET)) { + return; + } + + // 单一入口, 按固定 CAN ID 分发 (do_command 已停用) + switch (msg.id) { + case MSG_SET_AXIS_CONFIG_PID: // 0x208 设置 PID + set_axis_pid_config_callback(msg); break; - case MSG_GET_SENSORLESS_ESTIMATES: - if (msg.rtr || msg.len == 0) - get_sensorless_estimates_callback(axis); + + case MSG_GET_VBUS_VOLTAGE: // 0x217 获取母线电压/电流 + get_vbus_voltage_callback(msg); break; - case MSG_RESET_ODRIVE: - NVIC_SystemReset(); + + case MSG_SET_VLAGO_INPUT_VEL: // 0x219 上控速度指令 + case MSG_SET_UP_CONTROLL_VEL: // 0x220 上控速度指令 + case MSG_SET_VLAGO_ADC: // 0x279 摇杆 ADC (本地摇杆控车) + set_vlago_input_vel_callback(msg); break; - case MSG_GET_BUS_VOLTAGE_CURRENT: - if (msg.rtr || msg.len == 0) - get_bus_voltage_current_callback(axis); + + case MSG_SET_VLAGO_BRAKE_VEL: // 0x21B 抱闸控制 + set_vlago_brake_vel_callback(msg); break; - case MSG_CLEAR_ERRORS: - clear_errors_callback(axis, msg); + + case MSG_SET_OPERATE_MODE: // 0x221 运行模式 + set_operate_mode_callback(msg); break; - case MSG_SET_LINEAR_COUNT: - set_linear_count_callback(axis, msg); + + case MSG_SET_LEVEL: // 0x222 档位 + set_level_callback(msg); break; - case MSG_SET_POS_GAIN: - set_pos_gain_callback(axis, msg); + + case MSG_SET_DRIVE_MODE: // 0x229 驾驶模式 + set_drive_mode_callback(msg); break; - case MSG_SET_VEL_GAINS: - set_vel_gains_callback(axis, msg); + + case MSG_GET_VLAGO_VERSION: // 0x21D 获取版本信息 + get_vlago_version_callback(msg); break; - case MSG_GET_ADC_VOLTAGE: - get_adc_voltage_callback(axis, msg); + + case MSG_IDENTIFY: // 0x277 识别/校准 + identify_callback(msg); break; - case MSG_GET_CONTROLLER_ERROR: - get_controller_error_callback(axis); + + case MSG_OTA_RESET: // 0x701 OTA 复位 + handle_ota_message(msg); break; + default: break; } -} -void CANSimple::nmt_callback(const Axis& axis, const can_Message_t& msg) { - // Not implemented + // NOTE: 原来的 per-axis 匹配 + do_command() 已停用, 说明见文件末尾注释 } -void CANSimple::estop_callback(Axis& axis, const can_Message_t& msg) { - axis.error_ |= Axis::ERROR_ESTOP_REQUESTED; -} +/* ------------------------------------------------------------------------- */ +/* 接收命令处理 */ +/* ------------------------------------------------------------------------- */ + +// 0x208: 设置 PID, 并把实际生效的编码器 PID 回读给上位机 +void CANSimple::set_axis_pid_config_callback(const can_Message_t& msg) { + PID_P = can_getSignal(msg, 0, 32, true); + PID_I = can_getSignal(msg, 32, 32, true); + PID_P_ENCODER_ABS = PID_P; + PID_I_ENCODER_ABS = PID_I; -bool CANSimple::get_motor_error_callback(const Axis& axis) { can_Message_t txmsg; - txmsg.id = axis.config_.can.node_id << NUM_CMD_ID_BITS; - txmsg.id += MSG_GET_MOTOR_ERROR; // heartbeat ID - txmsg.isExt = axis.config_.can.is_extended; + txmsg.id = MSG_SET_AXIS_CONFIG_PID; + txmsg.isExt = true; txmsg.len = 8; - - can_setSignal(txmsg, axis.motor_.error_, 0, 64, true); - - return canbus_->send_message(txmsg); + can_setSignal(txmsg, PID_P_ENCODER_ABS, 0, 32, true); + can_setSignal(txmsg, PID_I_ENCODER_ABS, 32, 32, true); + canbus_->send_message(txmsg); } -bool CANSimple::get_encoder_error_callback(const Axis& axis) { +// 0x217: 母线电压/电流回读 +void CANSimple::get_vbus_voltage_callback(const can_Message_t& msg) { + if (!(msg.rtr || msg.len == 0)) { + return; + } + can_Message_t txmsg; - txmsg.id = axis.config_.can.node_id << NUM_CMD_ID_BITS; - txmsg.id += MSG_GET_ENCODER_ERROR; // heartbeat ID - txmsg.isExt = axis.config_.can.is_extended; + txmsg.id = MSG_GET_VBUS_VOLTAGE; + txmsg.isExt = true; txmsg.len = 8; + can_setSignal(txmsg, vbus_voltage, 0, 32, true); + can_setSignal(txmsg, ibus_, 32, 32, true); + canbus_->send_message(txmsg); +} - can_setSignal(txmsg, axis.encoder_.error_, 0, 32, true); +// 0x219: 速度指令 (左右轮各一个 float), 含抱闸/状态机/增益调度逻辑 +void CANSimple::set_vlago_input_vel_callback(const can_Message_t& msg) { + msg_lost_count_ = 0; - return canbus_->send_message(txmsg); -} + // 推行/手动抱闸/错误状态 -> 直接停机 + if (back_push_sts_ || brake_sts_) { + stop_axes(); + return; + } + for (auto& axis : axes) { + if (axis.error_ != Axis::ERROR_NONE) { + stop_axes(); + return; + } + } -bool CANSimple::get_sensorless_error_callback(const Axis& axis) { - can_Message_t txmsg; - txmsg.id = axis.config_.can.node_id << NUM_CMD_ID_BITS; - txmsg.id += MSG_GET_SENSORLESS_ERROR; // heartbeat ID - txmsg.isExt = axis.config_.can.is_extended; - txmsg.len = 8; + float input_vel[AXIS_COUNT] = {0.0f, 0.0f}; - can_setSignal(txmsg, axis.sensorless_estimator_.error_, 0, 32, true); + // 摇杆控制器只需初始化一次 (必须在第一次 JoystickCtrl_Update 之前) + static bool joystick_inited = false; + if (!joystick_inited) { + joystick_inited = true; + JoystickCtrl_Init(&joystick_ctrl); + } - return canbus_->send_message(txmsg); -} + if (msg.id == MSG_SET_VLAGO_ADC) { // 0x279 摇杆 AD 值 -> 差速控制 + if (msg.len < 6) { // 必须带 X/Y 两个 16bit 值, 否则丢弃 + return; + } + const uint16_t adc_x = (uint16_t)(msg.buf[4] | (msg.buf[5] << 8)); + const uint16_t adc_y = (uint16_t)(msg.buf[0] | (msg.buf[1] << 8)); +#if SPORT_MODE + JoystickCtrl_Update(&joystick_ctrl, adc_x, adc_y, g_level, 1); // 运动模式 +#else + JoystickCtrl_Update(&joystick_ctrl, adc_x, adc_y, g_level, 0); // 舒适模式 +#endif +#ifdef ENCODER_USE_SPI_ABS_AMS + input_vel[0] = -1.0f * joystick_ctrl.left_speed_out; // 磁编: 轴0 方向取反 +#else + input_vel[0] = joystick_ctrl.left_speed_out; +#endif + input_vel[1] = joystick_ctrl.right_speed_out; + } else { // 0x219 / 0x220 上控速度值 (两个 float) + if (msg.len < 8) { // 帧长不足, 丢弃 + return; + } +#ifdef ENCODER_USE_SPI_ABS_AMS + // 磁编: 轴0 方向取反 (与参考工程一致) + input_vel[0] = -1.0f * can_getSignal(msg, 0, 32, true); +#else + input_vel[0] = can_getSignal(msg, 0, 32, true); +#endif + input_vel[1] = can_getSignal(msg, 32, 32, true); + } -bool CANSimple::get_controller_error_callback(const Axis& axis) { - can_Message_t txmsg; - txmsg.id = axis.config_.can.node_id << NUM_CMD_ID_BITS; - txmsg.id += MSG_GET_CONTROLLER_ERROR; // heartbeat ID - txmsg.isExt = axis.config_.can.is_extended; - txmsg.len = 8; + // 上电仅设置一次控制模式 (速度控制 + 速度斜坡) + static bool mode_configured = false; + if (!mode_configured) { + mode_configured = true; + for (auto& axis : axes) { + if (axis.controller_.config_.control_mode != Controller::CONTROL_MODE_VELOCITY_CONTROL) { + axis.controller_.config_.control_mode = Controller::CONTROL_MODE_VELOCITY_CONTROL; + axis.controller_.control_mode_updated(); + } + if (axis.controller_.config_.input_mode != Controller::INPUT_MODE_VEL_RAMP) { + axis.controller_.config_.input_mode = Controller::INPUT_MODE_VEL_RAMP; + } + } + } - can_setSignal(txmsg, axis.controller_.error_, 0, 32, true); + static uint16_t axes_wait[AXIS_COUNT] = {}; + static uint16_t axes_zero[AXIS_COUNT] = {}; + static uint16_t idle_status[AXIS_COUNT] = {}; + static uint16_t idle_cnt[AXIS_COUNT] = {}; - return canbus_->send_message(txmsg); -} + for (size_t i = 0; i < AXIS_COUNT; ++i) { + Axis& axis = axes[i]; + const float vel_est = axis.encoder_.vel_estimate_.any().value_or(0.0f); + + // 1---速度给定为 0 且速度反馈足够小: 延时抱闸, 再延时切 IDLE + if ((input_vel[i] == 0.0f) && (std::fabs(vel_est) <= BREAK_SPEED_MT)) { + if (brake_released_[i]) { + axes_zero[i]++; + if (axes_zero[i] >= MIN_ZERO_SPEED_CNT) { + axes_zero[i] = 0; + if (axis.encoder_.config_.pre_calibrated) { + set_brake(i, false); // 抱闸 + idle_status[i] = 1; + } + } + } else { + axes_zero[i] = 0; + } + } -void CANSimple::set_axis_nodeid_callback(Axis& axis, const can_Message_t& msg) { - axis.config_.can.node_id = can_getSignal(msg, 0, 32, true); -} + // 抱闸后延时 120ms 再切 IDLE (参考工程把这段嵌套在上面的 if 里, + // 抱闸后条件不再成立, 实际永远切不到 IDLE, 这里按注释意图修正) + if (idle_status[i]) { + idle_cnt[i]++; + if (idle_cnt[i] >= MIN_ZERO_IDLE_CNT) { + axis.requested_state_ = Axis::AXIS_STATE_IDLE; + idle_cnt[i] = 0; + idle_status[i] = 0; + } + } -void CANSimple::set_axis_requested_state_callback(Axis& axis, const can_Message_t& msg) { - axis.requested_state_ = static_cast(can_getSignal(msg, 0, 32, true)); -} + // 2---速度给定不为 0: 开闸, 延时 120ms 再给速度, 避免冲击机械抱闸 + if (input_vel[i] != 0.0f) { + idle_status[i] = 0; + idle_cnt[i] = 0; + if (axis.encoder_.config_.pre_calibrated) { + set_brake(i, true); // 开闸 + if (axis.current_state_ != Axis::AXIS_STATE_CLOSED_LOOP_CONTROL) { + axis.requested_state_ = Axis::AXIS_STATE_CLOSED_LOOP_CONTROL; + } + } + if (axes_wait[i] < MIN_ZERO_IDLE_CNT / 2) { + axes_wait[i]++; + input_vel[i] = 0.0f; + } + } else { + axes_wait[i] = 0; + } -void CANSimple::set_axis_startup_config_callback(Axis& axis, const can_Message_t& msg) { - // Not Implemented + axis.controller_.input_vel_ = input_vel[i]; + axis.watchdog_feed(); // 收到有效速度指令即喂狗 + +#ifdef ENCODER_USE_HALL + // 霍尔带宽随转速切换 + axis.encoder_.config_.bandwidth = (std::fabs(vel_est) < 5.0f) ? 20.0f : 60.0f; +#endif + + // 3---驻坡: 零速附近且电流较大时提高增益, 稳定后恢复 + static uint8_t zhupo_cnt[AXIS_COUNT] = {}; + static uint8_t zhupo_flag[AXIS_COUNT] = {}; + if ((std::fabs(input_vel[i]) < 1.0f) && (std::fabs(vel_est) <= 5.0f)) { + if (zhupo_flag[i] == 0) { + zhupo_cnt[i]++; + if (zhupo_cnt[i] >= 250) { + zhupo_flag[i] = 1; + zhupo_cnt[i] = 0; +#ifdef ENCODER_USE_SPI_ABS_AMS + axis.controller_.config_.vel_gain = PID_P_ENCODER_ABS; + axis.controller_.config_.vel_integrator_gain = PID_I_ENCODER_ABS; +#else + axis.controller_.config_.vel_gain = PID_P; + axis.controller_.config_.vel_integrator_gain = PID_I; +#endif + } else { + if (std::fabs(axis.motor_.current_control_.Iq_measured_) > 3.0f) { +#ifdef ENCODER_USE_SPI_ABS_AMS + axis.controller_.config_.vel_gain = 0.3f; + axis.controller_.config_.vel_integrator_gain = 0.4f; +#else + axis.controller_.config_.vel_gain = 0.2f; + axis.controller_.config_.vel_integrator_gain = 0.3f; +#endif + } + } + } + } else { + zhupo_flag[i] = 0; + zhupo_cnt[i] = 0; +#ifdef ENCODER_USE_SPI_ABS_AMS + axis.controller_.config_.vel_gain = PID_P_ENCODER_ABS; + axis.controller_.config_.vel_integrator_gain = PID_I_ENCODER_ABS; +#else + axis.controller_.config_.vel_gain = PID_P; + axis.controller_.config_.vel_integrator_gain = PID_I; +#endif + } + } } -bool CANSimple::get_encoder_estimates_callback(const Axis& axis) { - can_Message_t txmsg; - txmsg.id = axis.config_.can.node_id << NUM_CMD_ID_BITS; - txmsg.id += MSG_GET_ENCODER_ESTIMATES; // heartbeat ID - txmsg.isExt = axis.config_.can.is_extended; - txmsg.len = 8; +// 0x21B: 抱闸控制 (两位, 1-解除抱闸 0-抱闸), 并回读抱闸状态 +void CANSimple::set_vlago_brake_vel_callback(const can_Message_t& msg) { + const int16_t brake_0_sts = can_getSignal(msg, 0, 16, true); + const int16_t brake_1_sts = can_getSignal(msg, 16, 16, true); - can_setSignal(txmsg, axis.controller_.pos_estimate_linear_src_.any().value_or(0.0f), 0, 32, true); - can_setSignal(txmsg, axis.controller_.vel_estimate_src_.any().value_or(0.0f), 32, 32, true); + set_brake(0, brake_0_sts != 0); + set_brake(1, brake_1_sts != 0); - return canbus_->send_message(txmsg); -} + if (brake_0_sts && brake_1_sts) { + // 两轴都被要求抱闸 -> 按推行处理, 速度指令作废 + back_push_sts_ = true; + for (auto& axis : axes) { + axis.requested_state_ = Axis::AXIS_STATE_IDLE; + } + } else if ((0 == brake_0_sts) && (0 == brake_1_sts)) { + back_push_sts_ = false; + } -bool CANSimple::get_sensorless_estimates_callback(const Axis& axis) { can_Message_t txmsg; - txmsg.id = axis.config_.can.node_id << NUM_CMD_ID_BITS; - txmsg.id += MSG_GET_SENSORLESS_ESTIMATES; // heartbeat ID - txmsg.isExt = axis.config_.can.is_extended; + txmsg.id = MSG_SET_VLAGO_BRAKE_VEL; + txmsg.isExt = true; txmsg.len = 8; + txmsg.buf[0] = brake_released_[0] ? 1 : 0; + txmsg.buf[1] = 0; + txmsg.buf[2] = brake_released_[1] ? 1 : 0; + txmsg.buf[3] = 0; + txmsg.buf[4] = 1; + txmsg.buf[5] = 2; + txmsg.buf[6] = 3; + txmsg.buf[7] = 4; + canbus_->send_message(txmsg); +} - static_assert(sizeof(float) == sizeof(axis.sensorless_estimator_.pll_pos_)); +// 0x221: 运行模式, 模式 7 (狭窄空间) 时切换到另一组 PID +void CANSimple::set_operate_mode_callback(const can_Message_t& msg) { + if (msg.len < 2) { + return; + } + g_operate_mode = msg.buf[1]; + if (g_operate_mode == 7) { + PID_P = 0.3f; + PID_I = 0.5f; + PID_P_ENCODER_ABS = 0.3f; + PID_I_ENCODER_ABS = 0.5f; + } else { + PID_P = 0.1f; + PID_I = 0.05f; + PID_P_ENCODER_ABS = 0.15f; + PID_I_ENCODER_ABS = 0.1f; + } +} - can_setSignal(txmsg, axis.sensorless_estimator_.pll_pos_, 0, 32, true); - can_setSignal(txmsg, axis.sensorless_estimator_.vel_estimate_.any().value_or(0.0f), 32, 32, true); +// 0x222: 档位 (1~4) +void CANSimple::set_level_callback(const can_Message_t& msg) { + if (msg.len < 1) { + return; + } + if ((msg.buf[0] >= 1) && (msg.buf[0] <= MAX_LEVEL)) { + g_level = msg.buf[0]; + } +} - return canbus_->send_message(txmsg); +// 0x229: 驾驶模式 1-舒适 2-运动 +void CANSimple::set_drive_mode_callback(const can_Message_t& msg) { + if (msg.len < 1) { + return; + } + if ((msg.buf[0] == 0x01) || (msg.buf[0] == 0x02)) { + g_drive_mode = msg.buf[0]; + } } -bool CANSimple::get_encoder_count_callback(const Axis& axis) { +// 0x21D: 版本信息 (硬件版本 / 软件版本 / 发布日期) +void CANSimple::get_vlago_version_callback(const can_Message_t& msg) { + (void)msg; + const uint16_t hardware_version = get_hardware_version(); + const uint16_t software_version = (uint16_t)get_software_version(); + const uint32_t publish_data = get_publish_data(); + can_Message_t txmsg; - txmsg.id = axis.config_.can.node_id << NUM_CMD_ID_BITS; - txmsg.id += MSG_GET_ENCODER_COUNT; - txmsg.isExt = axis.config_.can.is_extended; + txmsg.id = MSG_GET_VLAGO_VERSION; + txmsg.isExt = true; txmsg.len = 8; - - can_setSignal(txmsg, axis.encoder_.shadow_count_, 0, 32, true); - can_setSignal(txmsg, axis.encoder_.count_in_cpr_, 32, 32, true); - return canbus_->send_message(txmsg); + txmsg.buf[0] = hardware_version >> 8; + txmsg.buf[1] = hardware_version & 0xFF; + txmsg.buf[2] = software_version >> 8; + txmsg.buf[3] = software_version & 0xFF; + txmsg.buf[4] = publish_data >> 24; + txmsg.buf[5] = publish_data >> 16; + txmsg.buf[6] = publish_data >> 8; + txmsg.buf[7] = publish_data & 0xFF; + canbus_->send_message(txmsg); } -void CANSimple::set_input_pos_callback(Axis& axis, const can_Message_t& msg) { - axis.controller_.set_input_pos_and_steps(can_getSignal(msg, 0, 32, true)); - axis.controller_.input_vel_ = can_getSignal(msg, 32, 16, true, 0.001f, 0); - axis.controller_.input_torque_ = can_getSignal(msg, 48, 16, true, 0.001f, 0); - axis.controller_.input_pos_updated(); -} +// 0x277: 识别/校准命令, buf[1..7] 必须为 0x11..0x77 +void CANSimple::identify_callback(const can_Message_t& msg) { + if (msg.len < 8) { + return; + } + if (!((msg.buf[1] == 0x11) && (msg.buf[2] == 0x22) && (msg.buf[3] == 0x33) && + (msg.buf[4] == 0x44) && (msg.buf[5] == 0x55) && (msg.buf[6] == 0x66) && + (msg.buf[7] == 0x77))) { + return; + } -void CANSimple::set_input_vel_callback(Axis& axis, const can_Message_t& msg) { - axis.controller_.input_vel_ = can_getSignal(msg, 0, 32, true); - axis.controller_.input_torque_ = can_getSignal(msg, 32, 32, true); + g_identify_mode = msg.buf[0]; + switch (g_identify_mode) { + case 1: + set_brake(0, true); // 开闸便于校准 + set_brake(1, true); + axes[0].requested_state_ = Axis::AXIS_STATE_MOTOR_CALIBRATION; + break; + case 2: + axes[1].requested_state_ = Axis::AXIS_STATE_MOTOR_CALIBRATION; + break; + case 3: + axes[0].requested_state_ = Axis::AXIS_STATE_ENCODER_OFFSET_CALIBRATION; + break; + case 4: + axes[1].requested_state_ = Axis::AXIS_STATE_ENCODER_OFFSET_CALIBRATION; + break; + case 5: + for (auto& axis : axes) { + axis.motor_.config_.pre_calibrated = true; + axis.encoder_.config_.pre_calibrated = true; + axis.requested_state_ = Axis::AXIS_STATE_IDLE; + } + if ((axes[0].error_ == Axis::ERROR_NONE) || (axes[1].error_ == Axis::ERROR_NONE)) { + odrv.save_configuration(); + odrv.reboot(); + } + break; + default: + break; + } } -void CANSimple::set_input_torque_callback(Axis& axis, const can_Message_t& msg) { - axis.controller_.input_torque_ = can_getSignal(msg, 0, 32, true); +// 0x701: OTA 复位, 扩展帧 0x701 + 数据 04 31 01 02 03 -> 复位由 bootloader 接管升级 +void CANSimple::handle_ota_message(const can_Message_t& msg) { + if (!msg.isExt || (msg.len < 5)) { + return; + } + if (!((msg.buf[0] == 0x04) && (msg.buf[1] == 0x31) && (msg.buf[2] == 0x01) && + (msg.buf[3] == 0x02) && (msg.buf[4] == 0x03))) { + return; + } + NVIC_SystemReset(); } -void CANSimple::set_controller_modes_callback(Axis& axis, const can_Message_t& msg) { - Controller::ControlMode const mode = static_cast(can_getSignal(msg, 0, 32, true)); - axis.controller_.config_.control_mode = static_cast(mode); - axis.controller_.config_.input_mode = static_cast(can_getSignal(msg, 32, 32, true)); - axis.controller_.control_mode_updated(); -} +/* ------------------------------------------------------------------------- */ +/* 辅助 */ +/* ------------------------------------------------------------------------- */ -void CANSimple::set_limits_callback(Axis& axis, const can_Message_t& msg) { - axis.controller_.config_.vel_limit = can_getSignal(msg, 0, 32, true); - axis.motor_.config_.current_lim = can_getSignal(msg, 32, 32, true); +// 抱闸操作沿用 main.cpp 里已有的定义 (见 main.cpp: 抱闸控制 axis0-gpio8 axis1-gpio5): +// 轴0 = GPIO8, 轴1 = GPIO5, 高电平开闸 / 低电平抱闸 +// 手动抱闸检测: 轴0 = GPIO3, 轴1 = GPIO4 (输入上拉, 高电平表示手动抱闸未解除) +void CANSimple::set_brake(size_t i, bool release) { + if (i >= AXIS_COUNT) { + return; + } + brake_released_[i] = release; + get_gpio(i == 0 ? 8 : 5).write(release); } -void CANSimple::start_anticogging_callback(const Axis& axis, const can_Message_t& msg) { - axis.controller_.start_anticogging_calibration(); +void CANSimple::stop_axes() { + for (auto& axis : axes) { + axis.controller_.input_vel_ = 0.0f; + axis.requested_state_ = Axis::AXIS_STATE_IDLE; + } } -void CANSimple::set_traj_vel_limit_callback(Axis& axis, const can_Message_t& msg) { - axis.trap_traj_.config_.vel_limit = can_getSignal(msg, 0, 32, true); -} +/* ------------------------------------------------------------------------- */ +/* 发送 */ +/* ------------------------------------------------------------------------- */ -void CANSimple::set_traj_accel_limits_callback(Axis& axis, const can_Message_t& msg) { - axis.trap_traj_.config_.accel_limit = can_getSignal(msg, 0, 32, true); - axis.trap_traj_.config_.decel_limit = can_getSignal(msg, 32, 32, true); -} +// 0x270/0x271 轴错误 +bool CANSimple::send_axis_error(Axis& axis, uint32_t msg_id) { + can_Message_t txmsg; + txmsg.id = (&axis == &axes[0]) ? msg_id : (msg_id + 1); + txmsg.isExt = true; + txmsg.len = 8; -void CANSimple::set_traj_inertia_callback(Axis& axis, const can_Message_t& msg) { - axis.controller_.config_.inertia = can_getSignal(msg, 0, 32, true); -} + txmsg.buf[0] = 0x20; // MM 模块编号 (20-驱动板) + txmsg.buf[1] = (&axis == &axes[0]) ? 0x07 : 0x08; // 子模块: 轴0/轴1 + txmsg.buf[2] = 0x09; + txmsg.buf[3] = (axis.error_ != Axis::ERROR_NONE) ? 0x01 : 0x02; // 发生/解除 + const uint32_t error = static_cast(axis.error_); + txmsg.buf[4] = error >> 24; + txmsg.buf[5] = error >> 16; + txmsg.buf[6] = error >> 8; + txmsg.buf[7] = error; -void CANSimple::set_linear_count_callback(Axis& axis, const can_Message_t& msg) { - axis.encoder_.set_linear_count(can_getSignal(msg, 0, 32, true)); + return canbus_->send_message(txmsg); } -void CANSimple::set_pos_gain_callback(Axis& axis, const can_Message_t& msg) { - axis.controller_.config_.pos_gain = can_getSignal(msg, 0, 32, true); -} +// 0x272/0x273 电机错误 +bool CANSimple::send_axis_motor_error(Axis& axis, uint32_t msg_id) { + can_Message_t txmsg; + txmsg.id = (&axis == &axes[0]) ? msg_id : (msg_id + 1); + txmsg.isExt = true; + txmsg.len = 8; -void CANSimple::set_vel_gains_callback(Axis& axis, const can_Message_t& msg) { - axis.controller_.config_.vel_gain = can_getSignal(msg, 0, 32, true); - axis.controller_.config_.vel_integrator_gain = can_getSignal(msg, 32, 32, true); + txmsg.buf[0] = 0x20; + txmsg.buf[1] = (&axis == &axes[0]) ? 0x09 : 0x0A; // 电机0/电机1 + txmsg.buf[2] = 0x09; + txmsg.buf[3] = (axis.motor_.error_ != Motor::ERROR_NONE) ? 0x01 : 0x02; + const uint32_t error = static_cast(axis.motor_.error_); + txmsg.buf[4] = error >> 24; + txmsg.buf[5] = error >> 16; + txmsg.buf[6] = error >> 8; + txmsg.buf[7] = error; + + return canbus_->send_message(txmsg); } -bool CANSimple::get_iq_callback(const Axis& axis) { +// 0x275/0x276 编码器错误 +bool CANSimple::send_encoder_error(Axis& axis, uint32_t msg_id) { can_Message_t txmsg; - txmsg.id = axis.config_.can.node_id << NUM_CMD_ID_BITS; - txmsg.id += MSG_GET_IQ; - txmsg.isExt = axis.config_.can.is_extended; + txmsg.id = (&axis == &axes[0]) ? msg_id : (msg_id + 1); + txmsg.isExt = true; txmsg.len = 8; - std::optional Idq_setpoint = axis.motor_.current_control_.Idq_setpoint_; - if (!Idq_setpoint.has_value()) { - Idq_setpoint = {0.0f, 0.0f}; - } - - static_assert(sizeof(float) == sizeof(Idq_setpoint->second)); - static_assert(sizeof(float) == sizeof(axis.motor_.current_control_.Iq_measured_)); - can_setSignal(txmsg, Idq_setpoint->second, 0, 32, true); - can_setSignal(txmsg, axis.motor_.current_control_.Iq_measured_, 32, 32, true); + txmsg.buf[0] = 0x20; + txmsg.buf[1] = (&axis == &axes[0]) ? 0x0B : 0x0C; // 编码器0/编码器1 + txmsg.buf[2] = 0x09; + txmsg.buf[3] = (axis.encoder_.error_ != Encoder::ERROR_NONE) ? 0x01 : 0x02; + const uint32_t error = static_cast(axis.encoder_.error_); + txmsg.buf[4] = error >> 24; + txmsg.buf[5] = error >> 16; + txmsg.buf[6] = error >> 8; + txmsg.buf[7] = error; return canbus_->send_message(txmsg); } -bool CANSimple::get_bus_voltage_current_callback(const Axis& axis) { +// 0x21A/0x21E 累积编码器计数 (64 位, 大端), 用于上控里程统计 +bool CANSimple::send_encoder_count(Axis& axis, uint32_t msg_id) { + static int64_t accumulated_count[AXIS_COUNT] = {}; + static int32_t last_shadow_count[AXIS_COUNT] = {}; + static bool first_time[AXIS_COUNT] = {true, true}; + + for (size_t i = 0; i < AXIS_COUNT; ++i) { + int32_t current = axes[i].encoder_.shadow_count_; + if (first_time[i]) { + last_shadow_count[i] = current; + first_time[i] = false; + continue; + } + // 用无符号差值自动处理 int32 溢出 + int32_t delta = static_cast(static_cast(current) - + static_cast(last_shadow_count[i])); + accumulated_count[i] += delta; + last_shadow_count[i] = current; + } + + const bool is_axis0 = (&axis == &axes[0]); + int64_t count = accumulated_count[is_axis0 ? 0 : 1]; +#ifdef ENCODER_USE_SPI_ABS_AMS + // 磁编: 轴0 方向与上控约定相反 + if (is_axis0) { + count = -count; + } +#endif + can_Message_t txmsg; + txmsg.id = msg_id; + txmsg.isExt = true; + txmsg.len = 8; + txmsg.buf[0] = count >> 56; + txmsg.buf[1] = count >> 48; + txmsg.buf[2] = count >> 40; + txmsg.buf[3] = count >> 32; + txmsg.buf[4] = count >> 24; + txmsg.buf[5] = count >> 16; + txmsg.buf[6] = count >> 8; + txmsg.buf[7] = count; - txmsg.id = axis.config_.can.node_id << NUM_CMD_ID_BITS; - txmsg.id += MSG_GET_BUS_VOLTAGE_CURRENT; - txmsg.isExt = axis.config_.can.is_extended; + return canbus_->send_message(txmsg); +} + +// 0x21C 左右轮速度反馈 (turn/s, 两个 float) +bool CANSimple::send_vel_estimate() { + can_Message_t txmsg; + txmsg.id = MSG_TX_VEL_ESTIMATE; + txmsg.isExt = true; txmsg.len = 8; - static_assert(sizeof(float) == sizeof(vbus_voltage)); - static_assert(sizeof(float) == sizeof(ibus_)); - can_setSignal(txmsg, vbus_voltage, 0, 32, true); - can_setSignal(txmsg, ibus_, 32, 32, true); + for (size_t i = 0; i < AXIS_COUNT; ++i) { + float vel = axes[i].encoder_.vel_estimate_.any().value_or(0.0f); + if (std::fabs(vel) < 0.1f) { + vel = 0.0f; + } else { +#ifdef ENCODER_USE_SPI_ABS_AMS + if (i == 0) { + vel = -vel; + } +#endif + } + can_setSignal(txmsg, vel, (uint8_t)(32 * i), 32, true); + } return canbus_->send_message(txmsg); } -bool CANSimple::get_adc_voltage_callback(const Axis& axis, const can_Message_t& msg) { - can_Message_t txmsg; +// 0x274 心跳/抱闸状态 (每 500ms 发一帧), 同时承担指令超时保护 +bool CANSimple::send_heartbeat() { + // 未完成识别前打开抱闸, 避免校准被机械闸卡住 + if (!(axes[0].encoder_.config_.pre_calibrated && axes[1].encoder_.config_.pre_calibrated)) { + set_brake(0, true); + set_brake(1, true); + } else { + msg_lost_count_++; - txmsg.id = axis.config_.can.node_id << NUM_CMD_ID_BITS; - txmsg.id += MSG_GET_ADC_VOLTAGE; - txmsg.isExt = axis.config_.can.is_extended; - txmsg.len = 8; + // 指令超时: 速度清零 + if (msg_lost_count_ > MAX_MSG_LOST_COUNT) { + for (auto& a : axes) { + if (a.controller_.input_vel_ != 0.0f) { + a.controller_.input_vel_ = 0.0f; + } + } + } - auto gpio_num = can_getSignal(msg, 0, 8, true); - if (gpio_num < GPIO_COUNT) { - auto voltage = get_adc_voltage(get_gpio(gpio_num)); - can_setSignal(txmsg, voltage, 0, 32, true); - return canbus_->send_message(txmsg); + // 指令超时且已停稳: 抱闸并切 IDLE + if ((msg_lost_count_ > MAX_MSG_LOST_COUNT) && !back_push_sts_ && (g_identify_mode == 0)) { + for (size_t i = 0; i < AXIS_COUNT; ++i) { + Axis& a = axes[i]; + const float vel_est = a.encoder_.vel_estimate_.any().value_or(0.0f); + if ((std::fabs(vel_est) <= 0.01f) && (a.controller_.input_vel_ == 0.0f)) { + if (a.requested_state_ == Axis::AXIS_STATE_CLOSED_LOOP_CONTROL) { + set_brake(i, false); // 抱闸 + a.requested_state_ = Axis::AXIS_STATE_IDLE; + } + } + } + } + } + + // 任一轴有错误时抱闸 (推行模式除外) + for (auto& a : axes) { + if (a.error_ != Axis::ERROR_NONE) { + const bool release = (g_operate_mode == 3); + set_brake(0, release); + set_brake(1, release); + break; + } + } + + // 手动抱闸检测 (GPIO3=轴0, GPIO4=轴1, 输入上拉), 带滤波防抖 + static uint16_t cnt_locked = 0; + static uint16_t cnt_released = 0; + const bool axis0_brake_sts = get_gpio(3).read(); // 1-手动抱闸未解除 + const bool axis1_brake_sts = get_gpio(4).read(); + if (axis0_brake_sts && axis1_brake_sts) { + if (cnt_locked < BRAKE_FILTER_COUNT + 1) cnt_locked++; + if (cnt_locked > BRAKE_FILTER_COUNT) { + brake_sts_ = false; // 手动抱闸未解除 -> 正常走车 + cnt_released = 0; + } } else { + if (cnt_released < BRAKE_FILTER_COUNT + 1) cnt_released++; + if (cnt_released > BRAKE_FILTER_COUNT) { + brake_sts_ = true; // 手动抱闸已被人工解除 -> 禁止走车 + cnt_locked = 0; + } + } + + can_Message_t txmsg; + txmsg.id = MSG_TX_HEARTBEAT; + txmsg.isExt = true; + txmsg.len = 8; + txmsg.buf[0] = 0x20; // MM 模块编号 (20-驱动板) + txmsg.buf[1] = 0x02; // SS 子模块编号 (02-抱闸) + txmsg.buf[2] = 0x01; // EE 子模块错误编号 + txmsg.buf[3] = brake_sts_ ? 0x01 : 0x02; // 01-发生 02-解除 + txmsg.buf[4] = axes[0].encoder_.hall_state_; + txmsg.buf[5] = axes[1].encoder_.hall_state_; + txmsg.buf[6] = axis0_brake_sts ? 1 : 0; // 1-手动抱闸未解除 + txmsg.buf[7] = axis1_brake_sts ? 1 : 0; + + // 心跳里顺带做霍尔错误自恢复 +#ifdef ENCODER_USE_HALL + for (size_t i = 0; i < AXIS_COUNT; ++i) { + Axis& a = axes[i]; + if (a.encoder_.error_ == Encoder::ERROR_ILLEGAL_HALL_STATE) { + if ((a.encoder_.hall_state_ != 0) && (a.encoder_.hall_state_ != 7)) { + a.encoder_.error_ = Encoder::ERROR_NONE; + a.error_ = Axis::ERROR_NONE; + } + } + } +#endif + + // 参考工程: 内部 5 分频, 500ms 真正发一帧, 降低总线负载 + static uint8_t divider = 0; + divider++; + if (divider < 5) { return false; } -} + divider = 0; -void CANSimple::clear_errors_callback(Axis& axis, const can_Message_t& msg) { - odrv.clear_errors(); // TODO: might want to clear axis errors only + return canbus_->send_message(txmsg); } +/* ------------------------------------------------------------------------- */ +/* 周期发送调度 */ +/* ------------------------------------------------------------------------- */ + uint32_t CANSimple::service_stack() { - uint32_t nextServiceTime = UINT32_MAX; - uint32_t now = HAL_GetTick(); + const uint32_t now = HAL_GetTick(); - // TODO: remove this polling loop and replace with protocol hook - for (size_t i = 0; i < AXIS_COUNT; ++i) { - bool node_id_changed = (axes[i].config_.can.node_id != node_ids_[i]) || (axes[i].config_.can.is_extended != extended_node_ids_[i]); - if (node_id_changed) { - renew_subscription(i); + auto remaining = [now](uint32_t last, uint32_t period) -> uint32_t { + const uint32_t elapsed = now - last; + return (elapsed >= period) ? 0 : (period - elapsed); + }; + + // 每次 service 最多发 3 帧 (与参考工程一致, 避免总线突发); + // 发满就留到下个周期, 各帧仍由自己的周期定时器限速。 + int budget = 3; + + // 0x21C 速度反馈 20ms + if ((budget > 0) && ((now - axes[0].can_.last_encoder) >= VEL_ESTIMATE_PERIOD_MS)) { + if (send_vel_estimate()) { + axes[0].can_.last_encoder = now; + budget--; } } - struct periodic { - const uint32_t& rate; - uint32_t& last_time; - bool (CANSimple::* callback)(const Axis& axis); - }; + // 0x270/0x271 轴错误 + if ((budget >= 2) && ((now - axes[0].can_.last_axis_error) >= AXIS_ERROR_PERIOD_MS)) { + send_axis_error(axes[0], MSG_TX_AXIS_ERROR); + send_axis_error(axes[1], MSG_TX_AXIS_ERROR); + axes[0].can_.last_axis_error = now; + budget -= 2; + } - for (auto& axis : axes) { - std::array periodics = {{ - {axis.config_.can.heartbeat_rate_ms, axis.can_.last_heartbeat, &CANSimple::send_heartbeat}, - {axis.config_.can.encoder_rate_ms, axis.can_.last_encoder, &CANSimple::get_encoder_estimates_callback}, - {axis.config_.can.motor_error_rate_ms, axis.can_.last_motor_error, &CANSimple::get_motor_error_callback}, - {axis.config_.can.encoder_error_rate_ms, axis.can_.last_encoder_error, &CANSimple::get_encoder_error_callback}, - {axis.config_.can.controller_error_rate_ms, axis.can_.last_controller_error, &CANSimple::get_controller_error_callback}, - {axis.config_.can.sensorless_error_rate_ms, axis.can_.last_sensorless_error, &CANSimple::get_sensorless_error_callback}, - {axis.config_.can.encoder_count_rate_ms, axis.can_.last_encoder_count, &CANSimple::get_encoder_count_callback}, - {axis.config_.can.iq_rate_ms, axis.can_.last_iq, &CANSimple::get_iq_callback}, - {axis.config_.can.sensorless_rate_ms, axis.can_.last_sensorless, &CANSimple::get_sensorless_estimates_callback}, - {axis.config_.can.bus_vi_rate_ms, axis.can_.last_bus_vi, &CANSimple::get_bus_voltage_current_callback}, - }}; - - MEASURE_TIME(axis.task_times_.can_heartbeat) { - for (auto& msg : periodics) { - if (msg.rate > 0) { - if ((now - msg.last_time) >= msg.rate) { - if (std::invoke(msg.callback, this, axis)) { - msg.last_time = now; - } - } + // 0x272/0x273 电机错误 + if ((budget >= 2) && ((now - axes[0].can_.last_motor_error) >= MOTOR_ERROR_PERIOD_MS)) { + send_axis_motor_error(axes[0], MSG_TX_AXIS_MOTOR_ERROR); + send_axis_motor_error(axes[1], MSG_TX_AXIS_MOTOR_ERROR); + axes[0].can_.last_motor_error = now; + budget -= 2; + } - int nextAxisService = msg.last_time + msg.rate - now; - nextServiceTime = std::min(nextServiceTime, static_cast(std::max(0, nextAxisService))); - } - } - } + // 0x275/0x276 编码器错误 + if ((budget >= 2) && ((now - axes[0].can_.last_encoder_error) >= ENCODER_ERROR_PERIOD_MS)) { + send_encoder_error(axes[0], MSG_TX_ENCODER_ERROR); + send_encoder_error(axes[1], MSG_TX_ENCODER_ERROR); + axes[0].can_.last_encoder_error = now; + budget -= 2; } - return nextServiceTime; -} + // 0x21A/0x21E 累积编码器计数 + if ((budget >= 2) && ((now - axes[0].can_.last_encoder_count) >= ENCODER_COUNT_PERIOD_MS)) { + send_encoder_count(axes[0], MSG_TX_ENCODER_COUNT_AXIS0); + send_encoder_count(axes[1], MSG_TX_ENCODER_COUNT_AXIS1); + axes[0].can_.last_encoder_count = now; + budget -= 2; + } -bool CANSimple::send_heartbeat(const Axis& axis) { - can_Message_t txmsg; - txmsg.id = axis.config_.can.node_id << NUM_CMD_ID_BITS; - txmsg.id += MSG_ODRIVE_HEARTBEAT; // heartbeat ID - txmsg.isExt = axis.config_.can.is_extended; - txmsg.len = 8; + // 0x274 心跳/抱闸状态 + if ((budget > 0) && ((now - axes[0].can_.last_heartbeat) >= HEARTBEAT_PERIOD_MS)) { + //send_heartbeat(); + axes[0].can_.last_heartbeat = now; + budget--; + } - can_setSignal(txmsg, axis.error_, 0, 32, true); - can_setSignal(txmsg, uint8_t(axis.current_state_), 32, 8, true); + uint32_t next_service = remaining(axes[0].can_.last_encoder, VEL_ESTIMATE_PERIOD_MS); + next_service = std::min(next_service, remaining(axes[0].can_.last_axis_error, AXIS_ERROR_PERIOD_MS)); + next_service = std::min(next_service, remaining(axes[0].can_.last_motor_error, MOTOR_ERROR_PERIOD_MS)); + next_service = std::min(next_service, remaining(axes[0].can_.last_encoder_error, ENCODER_ERROR_PERIOD_MS)); + next_service = std::min(next_service, remaining(axes[0].can_.last_encoder_count, ENCODER_COUNT_PERIOD_MS)); + next_service = std::min(next_service, remaining(axes[0].can_.last_heartbeat, HEARTBEAT_PERIOD_MS)); - // Motor flags - uint8_t motorFlags = axis.motor_.error_ != 0; + return std::max(next_service, 1UL); +} - // Encoder flags - uint8_t encoderFlags = axis.encoder_.error_ != 0; +/* ------------------------------------------------------------------------- */ +/* 版本信息 (强制放到指定 Flash 地址) */ +/* ------------------------------------------------------------------------- */ - // Controller flags - uint8_t controllerFlags =axis.controller_.error_ != 0; - uint8_t trajDone = uint8_t(axis.controller_.trajectory_done_) << 7; - controllerFlags |= trajDone; +typedef struct { + uint32_t software_version; // 软件版本 + uint32_t publish_data; // 发布日期 +} VersionInfo; - can_setSignal(txmsg, motorFlags, 40, 8, true); - can_setSignal(txmsg, encoderFlags, 48, 8, true); - can_setSignal(txmsg, controllerFlags, 56, 8, true); +__attribute__((section(".version_info"), used)) +const VersionInfo version_info = { + .software_version = 0x0107, + .publish_data = 0x20261013, +}; - return canbus_->send_message(txmsg); +uint16_t get_hardware_version(void) { + return *(volatile uint16_t*)(0x08000188); +} + +uint32_t get_software_version(void) { + return *(volatile uint32_t*)0x0800C188; } + +uint32_t get_publish_data(void) { + return *(volatile uint32_t*)(0x0800C188 + 4); +} + +/* ------------------------------------------------------------------------- */ +/* 已停用: 标准 ODrive "nodeID|cmdID" 命令解析 */ +/* ------------------------------------------------------------------------- */ + +// do_command() 已停用: 原来按 (node_id << 5 | cmd_id) 解析节点的命令, +// 现在改为固定 CAN ID, 直接在 handle_can_message() 的 switch (msg.id) 里分发。 +// +// 原流程 (保留备查): +// for (auto& axis : axes) { +// if ((axis.config_.can.node_id == node_id) && +// (axis.config_.can.is_extended == msg.isExt)) { +// do_command(axis, msg); +// return; +// } +// } +// do_command() 内部: case MSG_SET_AXIS_REQUESTED_STATE / MSG_SET_INPUT_VEL / +// MSG_GET_ENCODER_ESTIMATES / ... 等 cmd_id 分支, 均已由上面的 switch 取代。 diff --git a/Firmware/communication/can/can_simple.hpp b/Firmware/communication/can/can_simple.hpp index 58b6982a5..3ef1690dd 100644 --- a/Firmware/communication/can/can_simple.hpp +++ b/Firmware/communication/can/can_simple.hpp @@ -4,40 +4,56 @@ #include "canbus.hpp" #include "axis.hpp" +/** + * @brief 本工程自定义的 CAN 协议层 (移植自 odrive-vlago 工程) + * + * 接收 (全部为扩展帧, 由 init() 里的两个硬件过滤 bank 完成筛选): + * 0x208 设置 PID (并回读 PID) + * 0x217 获取母线电压/电流 (RTR) + * 0x219 上控速度指令 (左右轮, 两个 float) + * 0x220 上控速度指令 (与 0x219 同格式) + * 0x221 运行模式 (模式 7 切换 PID) + * 0x222 档位 1~4 + * 0x229 驾驶模式 1-舒适 2-运动 + * 0x21B 抱闸控制 (并回读抱闸状态) + * 0x21D 获取版本信息 + * 0x277 识别/校准命令 + * 0x279 摇杆 ADC 值 (本地摇杆控车, 差速/原地转圈模型) + * 0x701 OTA 复位 (bootloader 接管) + * + * 发送 (扩展帧): + * 0x217, 0x21A, 0x21C, 0x21D, 0x21E, 0x270~0x276 + * + * 说明: 原来 ODrive 标准的 "nodeID<<5 | cmdID" 命令解析 (do_command) 已停用, + * 现在完全按上面的固定 CAN ID 分发, 见 can_simple.cpp 的 switch (msg.id)。 + */ class CANSimple { public: - enum { - MSG_CO_NMT_CTRL = 0x000, // CANOpen NMT Message REC - MSG_ODRIVE_HEARTBEAT, - MSG_ODRIVE_ESTOP, - MSG_GET_MOTOR_ERROR, // Errors - MSG_GET_ENCODER_ERROR, - MSG_GET_SENSORLESS_ERROR, - MSG_SET_AXIS_NODE_ID, - MSG_SET_AXIS_REQUESTED_STATE, - MSG_SET_AXIS_STARTUP_CONFIG, - MSG_GET_ENCODER_ESTIMATES, - MSG_GET_ENCODER_COUNT, - MSG_SET_CONTROLLER_MODES, - MSG_SET_INPUT_POS, - MSG_SET_INPUT_VEL, - MSG_SET_INPUT_TORQUE, - MSG_SET_LIMITS, - MSG_START_ANTICOGGING, - MSG_SET_TRAJ_VEL_LIMIT, - MSG_SET_TRAJ_ACCEL_LIMITS, - MSG_SET_TRAJ_INERTIA, - MSG_GET_IQ, - MSG_GET_SENSORLESS_ESTIMATES, - MSG_RESET_ODRIVE, - MSG_GET_BUS_VOLTAGE_CURRENT, - MSG_CLEAR_ERRORS, - MSG_SET_LINEAR_COUNT, - MSG_SET_POS_GAIN, - MSG_SET_VEL_GAINS, - MSG_GET_ADC_VOLTAGE, - MSG_GET_CONTROLLER_ERROR, - MSG_CO_HEARTBEAT_CMD = 0x700, // CANOpen NMT Heartbeat SEND + enum : uint32_t { + // ---- 接收: 扩展帧 0x2xx 组 (bank0: id=0x200, mask=0x700) ---- + MSG_SET_AXIS_CONFIG_PID = 0x208, // 设置 PID + MSG_GET_VBUS_VOLTAGE = 0x217, // 获取母线电压 (RTR) + MSG_SET_VLAGO_INPUT_VEL = 0x219, // 上控速度指令 + MSG_SET_VLAGO_BRAKE_VEL = 0x21B, // 抱闸控制 + MSG_GET_VLAGO_VERSION = 0x21D, // 获取版本信息 + MSG_SET_UP_CONTROLL_VEL = 0x220, // 上控速度指令 (与 0x219 同格式) + MSG_SET_OPERATE_MODE = 0x221, // 运行模式 (切换 PID) + MSG_SET_LEVEL = 0x222, // 档位 + MSG_SET_DRIVE_MODE = 0x229, // 驾驶模式 1-舒适 2-运动 + MSG_IDENTIFY = 0x277, // 识别/校准命令 + MSG_SET_VLAGO_ADC = 0x279, // 摇杆 ADC 值 (本地摇杆控车) + + // ---- 接收: 扩展帧精确匹配 (bank1) ---- + MSG_OTA_RESET = 0x701, // OTA 复位 + + // ---- 发送: 扩展帧 ---- + MSG_TX_ENCODER_COUNT_AXIS0 = 0x21A, // 轴0 累积编码器计数 + MSG_TX_VEL_ESTIMATE = 0x21C, // 左右轮速度反馈 + MSG_TX_ENCODER_COUNT_AXIS1 = 0x21E, // 轴1 累积编码器计数 + MSG_TX_AXIS_ERROR = 0x270, // 轴错误 (0x270/0x271) + MSG_TX_AXIS_MOTOR_ERROR = 0x272, // 电机错误 (0x272/0x273) + MSG_TX_HEARTBEAT = 0x274, // 心跳/抱闸状态 + MSG_TX_ENCODER_ERROR = 0x275, // 编码器错误 (0x275/0x276) }; CANSimple(CanBusBase* canbus) : canbus_(canbus) {} @@ -46,68 +62,45 @@ class CANSimple { uint32_t service_stack(); private: - - bool renew_subscription(size_t i); - bool send_heartbeat(const Axis& axis); - + // 统一的接收回调: 一个 switch (msg.id) 分发所有命令 + static void can_message_callback(void* ctx, const can_Message_t& msg); void handle_can_message(const can_Message_t& msg); - void do_command(Axis& axis, const can_Message_t& cmd); - - // Get functions (msg.rtr bit must be set) - bool get_motor_error_callback(const Axis& axis); - bool get_encoder_error_callback(const Axis& axis); - bool get_controller_error_callback(const Axis& axis); - bool get_sensorless_error_callback(const Axis& axis); - bool get_encoder_estimates_callback(const Axis& axis); - bool get_encoder_count_callback(const Axis& axis); - bool get_iq_callback(const Axis& axis); - bool get_sensorless_estimates_callback(const Axis& axis); - bool get_bus_voltage_current_callback(const Axis& axis); - // msg.rtr bit must NOT be set - bool get_adc_voltage_callback(const Axis& axis, const can_Message_t& msg); - - // Set functions - static void set_axis_nodeid_callback(Axis& axis, const can_Message_t& msg); - static void set_axis_requested_state_callback(Axis& axis, const can_Message_t& msg); - static void set_axis_startup_config_callback(Axis& axis, const can_Message_t& msg); - static void set_input_pos_callback(Axis& axis, const can_Message_t& msg); - static void set_input_vel_callback(Axis& axis, const can_Message_t& msg); - static void set_input_torque_callback(Axis& axis, const can_Message_t& msg); - static void set_controller_modes_callback(Axis& axis, const can_Message_t& msg); - static void set_limits_callback(Axis& axis, const can_Message_t& msg); - static void set_traj_vel_limit_callback(Axis& axis, const can_Message_t& msg); - static void set_traj_accel_limits_callback(Axis& axis, const can_Message_t& msg); - static void set_traj_inertia_callback(Axis& axis, const can_Message_t& msg); - static void set_linear_count_callback(Axis& axis, const can_Message_t& msg); - static void set_pos_gain_callback(Axis& axis, const can_Message_t& msg); - static void set_vel_gains_callback(Axis& axis, const can_Message_t& msg); - - // Other functions - static void nmt_callback(const Axis& axis, const can_Message_t& msg); - static void estop_callback(Axis& axis, const can_Message_t& msg); - static void clear_errors_callback(Axis& axis, const can_Message_t& msg); - static void start_anticogging_callback(const Axis& axis, const can_Message_t& msg); - - static constexpr uint8_t NUM_NODE_ID_BITS = 6; - static constexpr uint8_t NUM_CMD_ID_BITS = 11 - NUM_NODE_ID_BITS; - - // Utility functions - static constexpr uint32_t get_node_id(uint32_t msgID) { - return (msgID >> NUM_CMD_ID_BITS); // Upper 6 or more bits - }; - - static constexpr uint8_t get_cmd_id(uint32_t msgID) { - return (msgID & 0x01F); // Bottom 5 bits - } + // ---- 接收命令处理 ---- + void set_axis_pid_config_callback(const can_Message_t& msg); // 0x208 + void get_vbus_voltage_callback(const can_Message_t& msg); // 0x217 + void set_vlago_input_vel_callback(const can_Message_t& msg); // 0x219 / 0x220 / 0x279 + void set_vlago_brake_vel_callback(const can_Message_t& msg); // 0x21B + void set_operate_mode_callback(const can_Message_t& msg); // 0x221 + void set_level_callback(const can_Message_t& msg); // 0x222 + void set_drive_mode_callback(const can_Message_t& msg); // 0x229 + void get_vlago_version_callback(const can_Message_t& msg); // 0x21D + void identify_callback(const can_Message_t& msg); // 0x277 + void handle_ota_message(const can_Message_t& msg); // 0x701 + + // ---- 发送 ---- + bool send_axis_error(Axis& axis, uint32_t msg_id); // 0x270/0x271 + bool send_axis_motor_error(Axis& axis, uint32_t msg_id); // 0x272/0x273 + bool send_encoder_error(Axis& axis, uint32_t msg_id); // 0x275/0x276 + bool send_encoder_count(Axis& axis, uint32_t msg_id); // 0x21A/0x21E + bool send_vel_estimate(); // 0x21C + bool send_heartbeat(); // 0x274 + + // ---- 辅助 ---- + void set_brake(size_t i, bool release); + void stop_axes(); + + // 已停用: 标准 ODrive "nodeID|cmdID" 命令解析 do_command(), + // 不再使用也不再声明, 原来的实现以注释形式保留在 can_simple.cpp 末尾。 CanBusBase* canbus_; - CanBusBase::CanSubscription* subscription_handles_[AXIS_COUNT]; + CanBusBase::CanSubscription* subscription_handle_ = nullptr; // bank0: 0x2xx + CanBusBase::CanSubscription* ota_subscription_handle_ = nullptr; // bank1: 0x701 - // TODO: we this is a hack but actually we should use protocol hooks to - // renew our filter when the node ID changes - uint32_t node_ids_[AXIS_COUNT]; - bool extended_node_ids_[AXIS_COUNT]; + static uint32_t msg_lost_count_; + static bool brake_sts_; // 0-手动抱闸不解除, 1-手动抱闸解除 + static bool back_push_sts_; // 推行状态 + static bool brake_released_[AXIS_COUNT]; // 抱闸输出逻辑状态, true=已解除 }; #endif diff --git a/Firmware/communication/communication.cpp b/Firmware/communication/communication.cpp index d83255ae3..9a76aad47 100644 --- a/Firmware/communication/communication.cpp +++ b/Firmware/communication/communication.cpp @@ -43,11 +43,13 @@ void init_communication(void) { odrv.misconfigured_ = true; } +#ifdef USE_UART_ANY if (odrv.config_.enable_uart_a && uart_a) { start_uart_server(uart_a); } else if (odrv.config_.enable_uart_b && uart_b) { start_uart_server(uart_b); } +#endif start_usb_server(); @@ -71,6 +73,7 @@ int _write(int file, const char* data, int len) __attribute__((used)); int _write(int file, const char* data, int len) { fibre::cbufptr_t buf{(const uint8_t*)data, (const uint8_t*)data + len}; +#ifdef USE_UART_ANY if (odrv.config_.uart0_protocol == ODrive::STREAM_PROTOCOL_TYPE_STDOUT || odrv.config_.uart0_protocol == ODrive::STREAM_PROTOCOL_TYPE_ASCII_AND_STDOUT) { uart0_stdout_sink.write(buf); @@ -79,6 +82,7 @@ int _write(int file, const char* data, int len) { osMessagePut(uart_event_queue, 3, 0); } } +#endif if (odrv.config_.usb_cdc_protocol == ODrive::STREAM_PROTOCOL_TYPE_STDOUT || odrv.config_.usb_cdc_protocol == ODrive::STREAM_PROTOCOL_TYPE_ASCII_AND_STDOUT) { diff --git a/Firmware/communication/interface_uart.cpp b/Firmware/communication/interface_uart.cpp index 9cfc6eda2..994d75837 100644 --- a/Firmware/communication/interface_uart.cpp +++ b/Firmware/communication/interface_uart.cpp @@ -12,6 +12,9 @@ #include #include +//qiaoxu add: 未启用任何 UART 时, 整个 UART 通信服务不参与编译 +#ifdef USE_UART_ANY + #define UART_TX_BUFFER_SIZE 64 #define UART_RX_BUFFER_SIZE 64 @@ -112,7 +115,9 @@ LegacyProtocolStreamBased fibre_over_uart(&uart_rx_stream, &uart_tx_stream); fibre::AsyncStreamSinkMultiplexer<2> uart_tx_multiplexer(uart_tx_stream); fibre::BufferedStreamSink<64> uart0_stdout_sink(uart_tx_multiplexer); // Used in communication.cpp +#ifdef USE_ASCII_PROTOCOL AsciiProtocol ascii_over_uart(&uart_rx_stream, &uart_tx_multiplexer); +#endif bool uart0_stdout_pending = false; @@ -121,10 +126,13 @@ static void uart_server_thread(void * ctx) { if (odrv.config_.uart0_protocol == ODrive::STREAM_PROTOCOL_TYPE_FIBRE) { fibre_over_uart.start({}); - } else if (odrv.config_.uart0_protocol == ODrive::STREAM_PROTOCOL_TYPE_ASCII + } +#ifdef USE_ASCII_PROTOCOL + else if (odrv.config_.uart0_protocol == ODrive::STREAM_PROTOCOL_TYPE_ASCII || odrv.config_.uart0_protocol == ODrive::STREAM_PROTOCOL_TYPE_ASCII_AND_STDOUT) { ascii_over_uart.start(); } +#endif for (;;) { osEvent event = osMessageGet(uart_event_queue, osWaitForever); @@ -204,3 +212,5 @@ void HAL_UART_TxCpltCallback(UART_HandleTypeDef* huart) { osMessagePut(uart_event_queue, 2, 0); } } + +#endif // USE_UART_ANY diff --git a/Firmware/communication/interface_usb.cpp b/Firmware/communication/interface_usb.cpp index 68625ef5c..ca501fdec 100644 --- a/Firmware/communication/interface_usb.cpp +++ b/Firmware/communication/interface_usb.cpp @@ -1,6 +1,6 @@ #include "interface_usb.h" -#include "ascii_protocol.hpp" +#include "ascii_protocol.hpp" //qiaoxu: 纯头文件, 不产生代码; 是否启用由 USE_ASCII_PROTOCOL 控制 #include @@ -150,7 +150,9 @@ LegacyProtocolPacketBased fibre_over_usb(&usb_native_rx_stream, &usb_native_tx_s fibre::AsyncStreamSinkMultiplexer<2> usb_cdc_tx_multiplexer(usb_cdc_tx_stream); fibre::BufferedStreamSink<64> usb_cdc_stdout_sink(usb_cdc_tx_multiplexer); // Used in communication.cpp +#ifdef USE_ASCII_PROTOCOL AsciiProtocol ascii_over_cdc(&usb_cdc_rx_stream, &usb_cdc_tx_multiplexer); +#endif bool usb_cdc_stdout_pending = false; @@ -177,10 +179,13 @@ static void usb_server_thread(void * ctx) { if (odrv.config_.usb_cdc_protocol == ODrive::STREAM_PROTOCOL_TYPE_FIBRE) { fibre_over_cdc.start({}); - } else if (odrv.config_.usb_cdc_protocol == ODrive::STREAM_PROTOCOL_TYPE_ASCII + } +#ifdef USE_ASCII_PROTOCOL + else if (odrv.config_.usb_cdc_protocol == ODrive::STREAM_PROTOCOL_TYPE_ASCII || odrv.config_.usb_cdc_protocol == ODrive::STREAM_PROTOCOL_TYPE_ASCII_AND_STDOUT) { ascii_over_cdc.start(); } +#endif } break; case 2: { // USB disconnected event From 38bca1b4b629173f25b7ac0823055c2b19dc2ce0 Mon Sep 17 00:00:00 2001 From: qiaoxu Date: Tue, 20 Oct 2026 20:32:05 +0800 Subject: [PATCH 3/4] =?UTF-8?q?=E5=B7=B2=E8=A7=A3=E5=86=B3SPI=E6=8A=A5?= =?UTF-8?q?=E9=94=99=E5=92=8C=E9=9C=8D=E5=B0=94PID=E5=8F=82=E6=95=B0?= =?UTF-8?q?=E5=81=8F=E5=A4=A7=E7=A9=BA=E8=BD=BD=E9=9C=87=E8=8D=A1=E6=97=B6?= =?UTF-8?q?=E6=8A=A5=E9=94=99?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Firmware/Board/v3/Inc/board.h | 6 ++++++ Firmware/MotorControl/controller.hpp | 4 ++-- Firmware/MotorControl/encoder.hpp | 6 +++--- Firmware/MotorControl/main.cpp | 16 ++++++++++++++++ Firmware/communication/can/can_simple.cpp | 14 +++++++++++++- 5 files changed, 40 insertions(+), 6 deletions(-) diff --git a/Firmware/Board/v3/Inc/board.h b/Firmware/Board/v3/Inc/board.h index af4d18b15..7c205adba 100644 --- a/Firmware/Board/v3/Inc/board.h +++ b/Firmware/Board/v3/Inc/board.h @@ -33,6 +33,12 @@ //#define ENCODER_USE_SPI_ABS_AMS #define ENCODER_USE_HALL +//qiaoxu add: 驻坡增益调度开关 +// 定义 : 零速附近检测到较大 Iq 时临时提高 vel_gain / vel_integrator_gain (原行为) +// 注释掉 : 关闭调度, 增益始终等于上位机通过 0x208 / 0x221 下发的基准 PID +// 用途 : A/B 对比, 判断振荡是否由该调度引入 +#define ENABLE_ZHUPO_GAIN_SCHED + //qiaoxu add: 通信外设裁剪开关 (不用的外设注释掉, 对应代码不参与编译, 可减小固件体积) // USE_UART_A : UART4 (GPIO1/GPIO2, ODrive 的 uart0) // USE_UART_B : USART2 (GPIO3/GPIO4, ODrive 的 uart1) diff --git a/Firmware/MotorControl/controller.hpp b/Firmware/MotorControl/controller.hpp index b7ca90703..41a8199ce 100644 --- a/Firmware/MotorControl/controller.hpp +++ b/Firmware/MotorControl/controller.hpp @@ -51,8 +51,8 @@ class Controller : public ODriveIntf::ControllerIntf { uint8_t load_encoder_axis = -1; // default depends on Axis number and is set in load_configuration(). Set to -1 to select sensorless estimator. float mechanical_power_bandwidth = 20.0f; // [rad/s] filter cutoff for mechanical power for spinout detction float electrical_power_bandwidth = 20.0f; // [rad/s] filter cutoff for electrical power for spinout detection - float spinout_electrical_power_threshold = 10.0f; // [W] electrical power threshold for spinout detection - float spinout_mechanical_power_threshold = -10.0f; // [W] mechanical power threshold for spinout detection + float spinout_electrical_power_threshold = 1e9;//10.0f; //qiaoxu modify 如果不改小的话当速度环PID不合适抖动时易报错controller.cpp:422-440:ERROR_SPINOUT_DETECTED,ERROR_UNKNOWN_TORQUE // [W] electrical power threshold for spinout detection + float spinout_mechanical_power_threshold = -1e9;//-10.0f; //qiaoxu modify 同上 [W] mechanical power threshold for spinout detection // custom setters Controller* parent; diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp index b605e3fde..16cbcd18d 100644 --- a/Firmware/MotorControl/encoder.hpp +++ b/Firmware/MotorControl/encoder.hpp @@ -22,9 +22,9 @@ class Encoder : public ODriveIntf::EncoderIntf { float calib_range = 0.02f; // Accuracy required to pass encoder cpr check float calib_scan_distance = 16.0f * M_PI; // rad electrical float calib_scan_omega = 4.0f * M_PI; // rad/s electrical - float bandwidth = 100.0f;//1000.0f; - int32_t phase_offset = 46; // Offset between encoder count and rotor electrical phase - float phase_offset_float = 0.5068125128746033f; // Sub-count phase alignment offset + float bandwidth = 60.0f;//1000.0f; + int32_t phase_offset = 45; // Offset between encoder count and rotor electrical phase + float phase_offset_float = 1.4791250228881836; // Sub-count phase alignment offset int32_t cpr = 24;//(2048 * 4); // Default resolution of CUI-AMT102 encoder, float index_offset = 0.0f; bool use_index = false; diff --git a/Firmware/MotorControl/main.cpp b/Firmware/MotorControl/main.cpp index fa842f35f..0246a1e90 100644 --- a/Firmware/MotorControl/main.cpp +++ b/Firmware/MotorControl/main.cpp @@ -876,16 +876,32 @@ extern "C" int main(void) { get_gpio(3).config(GPIO_MODE_INPUT, GPIO_PULLUP); get_gpio(4).config(GPIO_MODE_INPUT, GPIO_PULLUP); + // 不论霍尔还是磁编: 磁编的 CS 必须一直保持高电平(未选中), + // 否则它会在 SPI3 上抢 MISO, 导致 DRV8301 寄存器回读失败 -> 双轴 DRV 故障 + get_gpio(7).config(GPIO_MODE_OUTPUT_PP, GPIO_NOPULL); // M0 AS5047 CS + get_gpio(7).write(true); + get_gpio(6).config(GPIO_MODE_OUTPUT_PP, GPIO_NOPULL); // M1 AS5047 CS + get_gpio(6).write(true); //qiaoxu #ifdef ENCODER_USE_SPI_ABS_AMS // 磁编电机,每次进闭环都会被覆盖。 axes[0].encoder_.config_.abs_spi_cs_gpio_pin = 7; axes[0].encoder_.abs_spi_cs_pin_init(); axes[0].encoder_.config_.direction = -1; + axes[0].motor_.config_.set_current_control_bandwidth(300.0f); axes[1].encoder_.config_.abs_spi_cs_gpio_pin = 6; axes[1].encoder_.abs_spi_cs_pin_init(); axes[1].encoder_.config_.direction = 1; + axes[1].motor_.config_.set_current_control_bandwidth(300.0f); #endif + +#ifdef ENCODER_USE_HALL + axes[0].encoder_.config_.direction = 1; + axes[0].motor_.config_.set_current_control_bandwidth(64.0f); + axes[1].encoder_.config_.direction = 1; + axes[1].motor_.config_.set_current_control_bandwidth(64.0f); +#endif + //axes[1].encoder_.config_.mode = Encoder::MODE_HALL; /////////////////////////////////////////// // Init usb irq binary semaphore, and start with no tokens by removing the starting one. diff --git a/Firmware/communication/can/can_simple.cpp b/Firmware/communication/can/can_simple.cpp index 28aaf5b45..91bd6865c 100644 --- a/Firmware/communication/can/can_simple.cpp +++ b/Firmware/communication/can/can_simple.cpp @@ -570,6 +570,9 @@ void CANSimple::set_vlago_input_vel_callback(const can_Message_t& msg) { #endif // 3---驻坡: 零速附近且电流较大时提高增益, 稳定后恢复 + // 编译开关见 board.h: 注释掉 ENABLE_ZHUPO_GAIN_SCHED 即整体关闭该调度, + // 关闭后增益始终跟随上位机 (0x208 / 0x221) 下发的基准 PID, 便于 A/B 对比。 +#ifdef ENABLE_ZHUPO_GAIN_SCHED static uint8_t zhupo_cnt[AXIS_COUNT] = {}; static uint8_t zhupo_flag[AXIS_COUNT] = {}; if ((std::fabs(input_vel[i]) < 1.0f) && (std::fabs(vel_est) <= 5.0f)) { @@ -608,6 +611,15 @@ void CANSimple::set_vlago_input_vel_callback(const can_Message_t& msg) { axis.controller_.config_.vel_integrator_gain = PID_I; #endif } +#else // !ENABLE_ZHUPO_GAIN_SCHED: 不做驻坡增益调度, 始终用上位机下发值 +#ifdef ENCODER_USE_SPI_ABS_AMS + axis.controller_.config_.vel_gain = PID_P_ENCODER_ABS; + axis.controller_.config_.vel_integrator_gain = PID_I_ENCODER_ABS; +#else + axis.controller_.config_.vel_gain = PID_P; + axis.controller_.config_.vel_integrator_gain = PID_I; +#endif +#endif // ENABLE_ZHUPO_GAIN_SCHED } } @@ -919,7 +931,7 @@ bool CANSimple::send_vel_estimate() { bool CANSimple::send_heartbeat() { // 未完成识别前打开抱闸, 避免校准被机械闸卡住 if (!(axes[0].encoder_.config_.pre_calibrated && axes[1].encoder_.config_.pre_calibrated)) { - set_brake(0, true); + set_brake(0, true);//开闸 set_brake(1, true); } else { msg_lost_count_++; From e4bc0b4dda7679c7a4e0b4b63d627a91055d85ba Mon Sep 17 00:00:00 2001 From: qiaoxu Date: Fri, 23 Oct 2026 11:55:54 +0800 Subject: [PATCH 4/4] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E7=A3=81=E7=BC=96AS5047?= =?UTF-8?q?=E9=9B=B6=E4=BD=8D=E5=81=8F=E7=A7=BB=E5=86=99=E5=85=A5=E6=88=90?= =?UTF-8?q?=E5=8A=9F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- "Firmware/AS5047\346\240\241\345\207\206.md" | 398 ++++++++++++++++ Firmware/Board/v3/Inc/board.h | 10 +- Firmware/MotorControl/axis.cpp | 67 +++ Firmware/MotorControl/axis.hpp | 14 + Firmware/MotorControl/encoder.cpp | 474 ++++++++++++++++++- Firmware/MotorControl/encoder.hpp | 97 ++++ Firmware/MotorControl/main.cpp | 13 +- Firmware/communication/can/can_simple.cpp | 337 ++++++++++++- Firmware/communication/can/can_simple.hpp | 23 +- Firmware/communication/can/odrive_can.cpp | 10 +- 10 files changed, 1417 insertions(+), 26 deletions(-) create mode 100644 "Firmware/AS5047\346\240\241\345\207\206.md" diff --git "a/Firmware/AS5047\346\240\241\345\207\206.md" "b/Firmware/AS5047\346\240\241\345\207\206.md" new file mode 100644 index 000000000..f59e22f97 --- /dev/null +++ "b/Firmware/AS5047\346\240\241\345\207\206.md" @@ -0,0 +1,398 @@ +# AS5047P 磁编码器零位校准 + +本文档说明本工程中 AS5047P 磁编码器零位校准的实现原理、CAN 协议、操作流程与排错。 +移植自 `MC_V0.5.6` 工程,代码位置与命令 ID 已适配本工程(`ODrive_V0.5.6_modify`)。 + +相关手册见源工程的 `磁编AS5047P-ATSM.pdf`(重点:第 13~18 页寄存器与 SPI 时序、第 27~29 页 OTP 烧写流程)。 + +--- + +## 1. 目标 + +让电机上电后**不需要跑 `AXIS_STATE_ENCODER_OFFSET_CALIBRATION`** 就能直接进入闭环控制,做法是用 +"开环直流对齐 + 硬件零位写入"替代 ODrive 原本的开环扫掠标定。 + +两条可选路线: + +| 路线 | 做法 | 掉电后 | 适用 | +|---|---|---|---| +| **A:软件偏移** | 只写 `phase_offset = X`(存 flash) | 保持有效 | 单台调试、少量样机 | +| **B:硬件零位** | 写 ZPOSM/ZPOSL(易失)或烧 OTP(永久) | 易失版会丢;OTP 永久有效 | 量产:烧一次 OTP 后退 `phase_offset = 0`,所有机器用同一份固件 | + +--- + +## 2. 原理 + +### 2.1 AS5047P 是逐帧流水线 SPI + +手册原文: + +> An SPI transaction consists of a 16-bit command frame followed by a 16-bit data frame +> The SPI read is **sampled on the rising edge of CSn** and the data is transmitted on MISO with the next read command + +即:**命令帧和数据帧各占一个 CS 低电平周期**,不是拼在同一个 CS 段里。所以 + +- 读操作:本周期发命令,下周期发 NOP 收回数据; +- 写操作:本周期发写命令,下周期发数据帧; +- **每个采样周期只发一帧**,所有 SPI 事务 `length = 1`。 + +(早期实现曾把写命令和数据拼成一个 `length = 2` 的 CS 段,实测写不进去,已废弃。) + +### 2.2 寄存器 + +| 地址 | 名称 | 说明 | +|---|---|---| +| 0x3FFE | ANGLEUNC | 未做动态误差补偿的角度(原始角) | +| 0x3FFF | ANGLECOM | 做了动态误差补偿的角度 | +| 0x0016 | ZPOSM | 零位偏移高 8 位 = ANGLE[13:6] | +| 0x0017 | ZPOSL | 零位偏移低 6 位 = ANGLE[5:0] | +| 0x0018 | SETTINGS1 | 客户设置 1(bit0 是出厂位,烧写时必须为 0) | +| 0x0019 | SETTINGS2 | 客户设置 2 | +| 0x0003 | PROG | 编程寄存器:PROGEN=bit0,OTPREF=bit2,PROGOTP=bit3,PROGVER=bit6 | + +两个要点: + +1. **ODrive 每周期发的 `0xFFFF` 就是"读 ANGLECOM(0x3FFF)"**: + `bit14 = R/W = 1`、`ADDR = 0x3FFF`、`bit15 = 偶校验 = 1`,完全合法。所以实际使用的角度寄存器是 ANGLECOM。 +2. **ZPOS 是整体平移刻度**:`ANGLECOM = (原始角 − ZPOS) mod 16384`。只要 X 是在**电气 0 位置**测到的原始角, + 把它写进 ZPOS 后,无论转子后来停在哪里,计数都会**恰好在电气 0 处归零**。 + +### 2.3 相位计算与两种偏移的换算 + +```cpp +corrected_enc = count_in_cpr_ - phase_offset; // encoder.cpp +interpolated_enc = corrected_enc + interpolation_; // 静止时 interpolation_ = 0.5 +ph = elec_rad_per_enc * (interpolated_enc - phase_offset_float); +phase_ = wrap_pm_pi(ph) * direction; // 仅当 is_ready_ 为 true 才更新 +``` + +静止时 `interpolation_ = 0.5`,代入得: + +``` +ph = elec_rad_per_enc * (count_in_cpr_ - phase_offset + 0.5 - phase_offset_float) +``` + +**真正生效的等效零位偏移(计数)= `phase_offset + phase_offset_float - 0.5`** + +对绝对式编码器(`mode_ & MODE_FLAG_ABS`),`count_in_cpr_` 每周期直接被绝对读数覆盖, +**不受 `phase_offset` 影响**,所以硬件零位和软件偏移是两条独立路径,可以互相换算: + +| 状态 | 硬件 ZPOS | 电气 0 处计数 | `phase_offset` | `phase_offset_float` | +|---|---|---|---|---| +| 只做 A | 0 | X | **X** | **0.5** | +| 做 B | X | 0 | **0** | **0.5** | + +**绝对不要两个都做**,否则重复补偿 X 个计数。误差换算成电角度是 `X × pole_pairs / 16384` 圈, +按 X=780、pole_pairs=4 算就是 68° 电角度,电机直接失步。 + +### 2.4 X 的多值性(重要) + +电气零点每个电周期出现一次,即每 `16384 / pole_pairs` 个计数重复。直流对齐只会把**离得最近的那个磁极** +拉到定子磁场方向,所以转子起始位置不同,测到的 X 会相差整数倍 `16384 / pole_pairs`。 + +例:实测两次得到 X = 2987 和 7040,差 4053,而 `16384 / 4 = 4096` —— 当 `pole_pairs = 4` 时两者 +指的是同一个电气位置(剩余 43 个计数是直流对齐的重复性误差)。**两个值都是合法的。** + +--- + +## 3. 固件实现要点 + +### 3.1 搭周期读的便车,绝不往 SPI 仲裁器插新事务 + +所有标定/烧写帧都是**临时改写 `encoder_.spi_task_` 本周期要发的那一帧**,仲裁器链表长度恒为 1, +与正常周期读完全一致。 + +原因(踩过的坑):从线程往 `Stm32SpiArbiter` 里额外插一笔阻塞事务时,如果某次 `start()` 返回 false, +该任务会**永久卡在链表表头**——因为链表非空时 `transfer_async()` 不再调用 `start()`,只能靠 +`on_complete()` 推它,而 `on_complete()` 也不会再被触发。结果周期读连续 51 ms 收不到样本, +累计触发 `ENCODER_ERROR_ABS_SPI_COM_FAIL`,并且调用线程会在等待回调的 `while` 里**永久挂死** +(表现为 CAN 无应答)。 + +### 3.2 CAN 线程绝不长时间阻塞 + +`can_server_thread` 的正常收发只在 `HAL_CAN_GetError() == HAL_CAN_ERROR_NONE` 时才执行, +原实现只额外处理了 `HAL_CAN_ERROR_TIMEOUT`: + +```cpp +if (status == HAL_CAN_ERROR_NONE) { ...收发... } +else if (status == HAL_CAN_ERROR_TIMEOUT) { ...复位... } +// 其它错误码落不到任何分支 → for(;;) 空转死循环,永远出不来 +``` + +任何其它错误码(ACK 错、位错、error-passive、bus-off…)都会让 CAN **彻底卡死**,而且 +`HAL_CAN_ErrorCallback` 是空的,错误码没人清。已改为无条件复位 + 重启 + `osDelay(1)` 让出 CPU。 + +**因此 OTP 烧写被拆成 `start` / `poll` 两半**,CAN 线程只发起、不等待,结果由 +`service_stack()` 每轮循环查询后主动上报。 + +### 3.3 `is_ready_` / `pre_calibrated` 的死锁 + +`phase_` 只在 `is_ready_` 为 true 时更新;而 `MODE_SPI_ABS_AMS` 下 `is_ready_` 只在 `abs_spi_cb` +里、且 `config_.pre_calibrated` **已经**为 true 时才置起。 + +同时 `check_pre_calibrated()`(只由 `set_pre_calibrated()` 调用)会在 `pre_calibrated && !is_ready_` +时把 `pre_calibrated` 改回 false。 + +所以**通过接口去设 `pre_calibrated = true` 会互相等待、永远置不上**。正确做法是直接给 +`config_.pre_calibrated` 赋值,绕过 setter(本工程 0x277 处理里就是这么做的)。 + +### 3.4 参数 + +| 项 | 值 | +|---|---| +| 采样率 / 帧间隔 | 8 kHz / 125 µs | +| 标定序列(读+可选写) | 12 帧 ≈ 1.5 ms | +| OTP 序列 | ~40 帧 + 轮询,轮询上限 4000 次(≈1 s) | +| `encoder.config.cpr` | 16384 | +| `phase_offset_float` | 0.5 | +| 对齐电流 / 爬升 | `axis.config.phase_lock_current`(默认 10 A)/ `phase_lock_ramp_time`(默认 0.4 s) | + +--- + +## 4. CAN 协议 + +命令帧:**扩展帧 ID `0x277`,DLC = 8**,`byte1 ~ byte7 = 01 02 03 04 05 06 07` 固定校验字, +任一位不符则整帧忽略。 + +> 本工程的 `0x277` 原本用于"识别/校准命令"(尾缀 `11 22 33 44 55 66 77`)。两者**共用同一 ID, +> 靠固定尾缀区分**,互不重叠:`handle_can_message()` 里先判 `01..07` 尾缀走磁编流程,否则走识别流程。 + +应答帧:**扩展帧 ID `0x278`,DLC = 8** + +``` +byte0 = 命令回显(0x00 / 0x11 / 0x33 / 0x44) +byte1 = 数据高 8 位 +byte2 = 数据低 8 位 +byte3 = 结果码 / 状态位 +byte4 = 附加数据(仅 0x00 使用) +byte5-7 = 0 +``` + +`byte1 = byte2 = 0xFF` 表示失败(14 位数据不可能是 0xFFFF,不会误判)。 + +### 4.1 `byte0 = 0x00` 读磁场强度与诊断信息 + +``` +0x277 00 01 02 03 04 05 06 07 +``` + +流程:读 `MAG(0x3FFD)` → 读 `DIAAGC(0x3FFC)`,**纯读、无副作用**,只占 4 个采样周期(约 0.5 ms), +闭环运行中调用也安全。 + +应答 `0x278`: + +| 字节 | 含义 | +|---|---| +| 0 | `0x00` | +| 1-2 | 磁场强度(MAG,14 bit,byte1 = 高 8 位) | +| 3 | 诊断状态位 | +| 4 | AGC 值 | + +byte3 状态位:bit0 = LF(内部偏移补偿环路已完成),bit1 = COF(CORDIC 溢出), +bit2 = MAGH(磁场过强),bit3 = MAGL(磁场过弱)。 + +DIAAGC 原始位定义(手册 Figure 21):`bit7:0 = AGC`,`bit8 = LF`,`bit9 = COF`,`bit10 = MAGH`,`bit11 = MAGL`。 + +读取失败时:`byte1/2 = 0xFFFF`、`byte3 = 0xFF`、`byte4 = 0xFF`。 + +> 正常安装时 LF 应为 1,MAGH/MAGL/COF 都为 0。用 MAG 和 AGC 判断磁铁与芯片的间隙是否合适。 + +### 4.2 `byte0 = 0x11` 锁定到电角度 0 并读回 X + +``` +0x277 11 01 02 03 04 05 06 07 +``` + +流程: + +1. 把 axis0 切到 `AXIS_STATE_PHASE_LOCK_ZERO`(自定义状态 14,**开环直流对齐**:定子磁场固定在 + 定子空间的电角度 0,相位源被切到 `open_loop_controller_.phase_`,不经过编码器,所以是真对齐, + 与当前 `phase_offset` 对不对无关); +2. 等状态机进入该状态 → 等电流按 `phase_lock_ramp_time` 爬到设定值并稳定 200 ms → 等转子静止 + (连续 2 ms 内计数变化 ≤ 4); +3. 读回 `ZPOSM`、`ZPOSL`、`ANGLECOM`,**不做任何写操作**; +4. 反解 `X = (count + ZPOS) mod 16384`; +5. 设 `phase_offset = count`、`phase_offset_float = 0.5`、`pre_calibrated = true`(直接赋值); +6. 应答 `0x278: 11 00 ...`。 + +**无副作用**是关键:不写 ZPOS,所以反复执行不会把偏移一轮轮叠加;上电后读到的 ZPOS 始终等于 +OTP 里的内容,`0x44` 才能判断是否已烧过。 + +锁定期间 CAN 线程最长阻塞约 1.6 s(500 ms 等状态 + ramp + 200 ms 等稳定 + 500 ms 等静止)。 +等待循环里反复喂狗;想缩短可调小 `axis.config.phase_lock_ramp_time`。 + +### 4.3 `byte0 = 0x22` 退出锁定 + +``` +0x277 22 01 02 03 04 05 06 07 +``` + +axis0 回 `AXIS_STATE_IDLE` 并 disarm。**无应答。** + +### 4.4 `byte0 = 0x33` 把 X 软写进 ZPOSM/ZPOSL(易失) + +``` +0x277 33 01 02 03 04 05 06 07 +``` + +1. 校验轴静止; +2. 用 `0x11` 缓存的 X:写 `ZPOSM = X >> 6`、`ZPOSL = X & 0x3F`,读回 ZPOSM 校验写入生效; +3. 设 `phase_offset = 0`、`phase_offset_float = 0.5`、`pre_calibrated = true`; +4. 应答 `0x278: 33 00 ...`,C 为写入后读回的计数,**应在 0 附近**。 + +必须用 `0x11` 缓存的 X,**不能现场重读**:写 ZPOS 是整体平移,只要 X 是在电气 0 处测到的, +之后转子漂到哪都对;现场重读会把漂移后的错误位置固化成零位。 + +> ZPOSM/ZPOSL 是**易失寄存器**,掉电丢失。软写后再掉电,计数会变回 X,而 `phase_offset = 0` +> 还留在 flash 里 —— 两者错配 X 个计数,**上电即失步**。走这条路每次上电都要重发 `0x11 + 0x33`, +> 否则请用路线 A(只 `0x11`)或烧 OTP。 + +### 4.5 `byte0 = 0x44` 把 X 烧进 OTP(**不可逆**) + +``` +0x277 44 01 02 03 04 05 06 07 +``` + +**前置条件(任一不满足立即拒绝,结果码 6)**: + +- 已发过 `0x11` 拿到 X; +- **上电后没往 ZPOS 软写过**(即没发过 `0x33`); +- 当前处于 `AXIS_STATE_IDLE`。 + +**应答分两条**: + +| 帧 | 时机 | byte1/2 | byte3 | +|---|---|---|---| +| 第 1 条 | 立即 | `FFFF` | `0` = 已开始;`6` = 前置条件不满足;`7` = 上次还在跑 | +| 第 2 条 | 序列跑完后(几十 ms ~ 1 s)由 `service_stack()` 主动上报 | 从 OTP 刷新后读回重建的零位 | 最终结果码 | + +**结果码**: + +| 码 | 含义 | 处置 | +|---|---|---| +| 0 | 成功(仅出现在第 1 条) | — | +| 1 | SPI 超时 | 可重试,OTP 未动 | +| 2 | 烧前比对不符 | **OTP 未动**,可重试 | +| 3 | PROG 轮询超时 | **PROGOTP 已置位但没等到完成,芯片状态不明,不要再烧** | +| 4 | 烧后比对不符 | 芯片可能已烧坏,不要再烧 | +| 5 | OTP 已烧过 | 预检在**任何写操作之前**中止,什么都没动,安全 | +| 6 | 前置条件不满足 | 按提示补条件 | +| 7 | 上一次还在进行中 | 等第 2 条应答 | + +**电气前提(手册 Figure 37,动手前务必确认)**: + +| 参数 | 要求 | +|---|---| +| VDD(3.3 V 模式) | **3.3 ~ 3.5 V** | +| VDD(5 V 模式) | 4.5 ~ 5.5 V | +| IProg | 烧写瞬时最大 **100 mA** | +| 温度 | 25 °C ± 20 °C | +| 重烧 | **不允许**("A reprogramming is not allowed") | + +标准 3.3 V LDO 很可能贴着下限甚至越界。**烧写前先量编码器供电脚的实际电压**。 + +**固件执行的步骤(手册 step 2~15)**: + +1. 预检:读 0x0016/0x0017,非 0 说明已烧过 → 中止(结果码 5); +2. 写 `ZPOSM = X>>6`、`ZPOSL = X&0x3F`、`SETTINGS1 = 0x0000`、`SETTINGS2 = 0x0000`; +3. 读回 0x0016~0x0019 比对(SETTINGS1 的 bit0 读回恒为 1,比对时屏蔽)→ 不符则中止(结果码 2); +4. `PROG = 0x0001`(PROGEN=1),隔 2 帧(250 µs,满足手册 ≥100 µs)后 `PROG = 0x0008`(PROGOTP=1); +5. 轮询读 `PROG` 直到 `0x0001`; +6. 把 0x0016~0x0019 全部写 0(清空易失内容); +7. `PROG = 0x0040`(PROGVER=1);`PROG = 0x0004`(OTPREF=1,用 OTP 刷新易失内容); +8. 读回 0x0016~0x0019 比对 → 这才是"真烧进去了"的凭据(结果码 4 表示失败); +9. 成功后固件自动设 `phase_offset = 0`、`phase_offset_float = 0.5`、`pre_calibrated = true`。 + +**手册 step 1 / 16~19(上电与复验)必须由人工完成**: + +> **烧完一定要断电重新上电**,再发一次 `0x11` 核对读数。第 2 条应答的 `byte1/2` 应等于烧进去的 X。 + +--- + +## 5. 操作流程 + +### 5.1 调试 / 验证(不烧 OTP) + +``` +上电 → 0x11 → 0x22 → 0x33 → 启动闭环,小幅点动确认出力方向 +``` + +### 5.2 写入 flash 的软件偏移(路线 A,掉电安全) + +``` +1. 0x11 → 记下应答 byte1/2 组成的 X +2. 0x22 +3. 把 phase_offset = X、phase_offset_float = 0.5 写进代码 / 用 save_configuration 存 flash +4. 直接进闭环 +``` + +### 5.3 烧 OTP(路线 B,量产) + +``` +1. 确认供电 3.3~3.5 V、温度 25±20 °C +2. 冷启动(务必是重新上电后的状态) +3. 0x11 → 记下 X;连发 5 次看 X 的重复性 +4. 0x22 +5. 0x44 → 收第 1 条(byte3=0),等第 2 条 +6. 第 2 条 byte3 = 0 且 byte1/2 == X → 成功 +7. 断电重新上电,再发 0x11 复验(手册 step 16~19) +8. 代码里改为 phase_offset = 0、phase_offset_float = 0.5、pre_calibrated = true +``` + +> **全程不要发 `0x33`**,否则预检失效,必须重新上电再来。 + +--- + +## 6. 注意事项 + +### 6.1 `direction` 不受本流程覆盖 + +`phase_ = wrap_pm_pi(ph) * direction`。`run_phase_lock_zero()` 是开环直流对齐,天然测不出方向符号, +既不检查也不设置 `direction`。**方向错了就是正反馈,直接飞车。** + +沿用之前标定好的值,或手转轴确认:读数增大的方向对应 `direction = 1`。 +也可先跑一次 `AXIS_STATE_ENCODER_DIR_FIND`。 + +### 6.2 直流对齐的重复性 + +实测两次 X 相差 43 个计数(`pole_pairs = 4` 时约 3.8° 电角度)。烧 OTP 前建议连发几次 `0x11` +观察抖动;抖动偏大时先把 `phase_lock_current` 调大让磁场更"硬",对齐更稳定。 +电压模式(电机未校准)下把 `phase_lock_voltage` 调到能让转子明显被拉住的电压。 + +### 6.3 对齐精度与分辨率 + +整数计数的分辨率是 `pole_pairs × 360 / 16384` 电角度(`pole_pairs = 4` 时约 0.088°), +远高于直流对齐本身的误差,所以 `phase_offset_float` 固定取 0.5 即可,不需要做分数补偿。 + +--- + +## 7. 排错 + +| 现象 | 原因 | 处理 | +|---|---|---| +| `ENCODER_ERROR_ABS_SPI_COM_FAIL` | 早期版本从线程往仲裁器插阻塞事务,任务卡死在链表表头,周期读断供 51 ms | 已改为搭周期读便车;不要恢复旧写法 | +| CAN 无任何应答、且永久不恢复 | `can_server_thread` 遇到 NONE/TIMEOUT 之外的错误码时空转死循环 | 已修复;早期版本需断电重启 | +| `0x11` 应答 `byte1/2 = FFFF` | 轴未静止 / SPI 读取失败 / 状态机超时 / 编码器模式不是 `MODE_SPI_ABS_AMS` | 确认 0x22 后轴确实不动;确认 `board.h` 打开的是 `ENCODER_USE_SPI_ABS_AMS` | +| `0x11` 后电机不动或有异响 | `phase_lock_current` 太小 / 电机未标定走了电压模式 | 加大电流或先做电机标定 | +| `0x33` 后计数不在 0 附近 | ZPOS 未写进去(读回校验失败) | 检查 SPI;结果码会给出失败原因 | +| `0x44` 立即应答 `byte3 = 6` | 没先发 0x11 / 已发过 0x33 / 不在 IDLE | 重新上电后按 5.3 重来 | +| `0x44` 结果码 `5` | 这颗芯片的 OTP 已经烧过 | 预检未动任何东西;直接使用即可 | +| 上电后电机不转 / 失步 | OTP 烧成功但代码里 `phase_offset` 还是 X(应改 0);或 `direction` 错 | 按 5.3 第 8 步修正 | + +--- + +## 8. 改动文件清单(本工程) + +| 文件 | 内容 | +|---|---| +| `Firmware/MotorControl/encoder.hpp` | AS5047 序列状态机、OTP 状态机、`as5047_*` 接口声明 | +| `Firmware/MotorControl/encoder.cpp` | 逐帧流水线帧构造与序列推进、ZPOS 读写、OTP 烧写流程、`abs_spi_cb` / `abs_spi_start_transaction` 的"搭便车"分流 | +| `Firmware/MotorControl/axis.hpp` | `AXIS_STATE_PHASE_LOCK_ZERO`(自定义状态 14)、`phase_lock_current/voltage/ramp_time` | +| `Firmware/MotorControl/axis.cpp` | `run_phase_lock_zero()` 开环直流对齐 + 状态机分支 | +| `Firmware/communication/can/can_simple.hpp` / `.cpp` | `0x277` 命令分发与 `0x278` 应答、`0x00/0x11/0x22/0x33/0x44` 处理、`as5047_otp_report()` | +| `Firmware/communication/can/odrive_can.cpp` | 修复 CAN 线程错误码分支缺失导致的空转死循环 | +| `Firmware/Board/v3/Inc/board.h` | 编码器类型开关(`ENCODER_USE_SPI_ABS_AMS` / `ENCODER_USE_HALL` 二选一) | + +> 使用本功能必须把 `board.h` 里的编码器类型切到 `ENCODER_USE_SPI_ABS_AMS`(`cpr = 16384`、 +> `phase_offset = 0`、`phase_offset_float = 0.5`、`direction = -1` 由该分支的默认值给出)。 +> 霍尔模式下 `0x277` 仍会响应,但会被编码器层直接判为不支持(应答 `FFFF`)。 diff --git a/Firmware/Board/v3/Inc/board.h b/Firmware/Board/v3/Inc/board.h index 7c205adba..8d736ba67 100644 --- a/Firmware/Board/v3/Inc/board.h +++ b/Firmware/Board/v3/Inc/board.h @@ -29,10 +29,12 @@ #define SHUNT_RESISTANCE (2e-3f) //产品板 #endif -//qiaoxu add -//#define ENCODER_USE_SPI_ABS_AMS -#define ENCODER_USE_HALL - +//qiaoxu add: 编码器类型 (二选一, 必须且只能定义一个) +// ENCODER_USE_HALL : 霍尔, cpr = 24 +// ENCODER_USE_SPI_ABS_AMS : AS5047P 磁编, cpr = 16384, 支持 CAN 0x277 零位标定/OTP 烧写 +#define ENCODER_USE_SPI_ABS_AMS +//#define ENCODER_USE_HALL +#define DEBUG_USB //是否用USB调试,打开的话不发CAN ID = 0x274 //qiaoxu add: 驻坡增益调度开关 // 定义 : 零速附近检测到较大 Iq 时临时提高 vel_gain / vel_integrator_gain (原行为) // 注释掉 : 关闭调度, 增益始终等于上位机通过 0x208 / 0x221 下发的基准 PID diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 03735ce0d..b220daf86 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -254,6 +254,68 @@ bool Axis::run_lockin_spin(const LockinConfig_t &lockin_config, bool remain_arme } +/** + * @brief 把转子锁定在电角度 0(开环直流对齐 / DC injection)。 + * + * 与 run_lockin_spin() 的区别: + * - 不要求 motor_.is_calibrated_,也不要求 encoder_.config_.direction 已知; + * - phase_ 恒为 0、phase_vel_ 恒为 0,即定子磁场矢量固定在电角度 0,转子被拉到电角度 0; + * - 只有收到新的 requested_state_(如 IDLE / CLOSED_LOOP_CONTROL)才会退出。 + * + * 已校准(相电阻、相电感有效)时走电流环:Id = phase_lock_current,Iq = 0; + * 未校准时 PI 增益为 0/NaN,电流环不可用,改走电压模式:Vd = phase_lock_voltage,Vq = 0, + * 由 motor.config.current_lim 触发 CURRENT_LIMIT_VIOLATION 做保护。 + */ +bool Axis::run_phase_lock_zero() { + const bool use_current_mode = motor_.is_calibrated_ + && (motor_.config_.phase_inductance > 0.0f) + && (motor_.config_.phase_resistance > 0.0f); + + const float magnitude = use_current_mode ? config_.phase_lock_current + : config_.phase_lock_voltage; + const float ramp_time = config_.phase_lock_ramp_time > 0.01f ? config_.phase_lock_ramp_time : 0.01f; + + CRITICAL_SECTION() { + // Reset state variables + open_loop_controller_.Idq_setpoint_ = {0.0f, 0.0f}; + open_loop_controller_.Vdq_setpoint_ = {0.0f, 0.0f}; + open_loop_controller_.phase_ = 0.0f; // ★ 电角度锁定在 0 + open_loop_controller_.initial_phase_ = 0.0f; + open_loop_controller_.phase_vel_ = 0.0f; // ★ 相位不再走动 + + open_loop_controller_.max_current_ramp_ = use_current_mode ? magnitude / ramp_time : INFINITY; + open_loop_controller_.max_voltage_ramp_ = use_current_mode ? INFINITY : magnitude / ramp_time; + open_loop_controller_.max_phase_vel_ramp_ = INFINITY; + open_loop_controller_.target_current_ = use_current_mode ? magnitude : 0.0f; + open_loop_controller_.target_voltage_ = use_current_mode ? 0.0f : magnitude; + open_loop_controller_.target_vel_ = 0.0f; + open_loop_controller_.total_distance_ = 0.0f; + + motor_.current_control_.enable_current_control_src_ = use_current_mode; + motor_.current_control_.Idq_setpoint_src_.connect_to(&open_loop_controller_.Idq_setpoint_); + motor_.current_control_.Vdq_setpoint_src_.connect_to(&open_loop_controller_.Vdq_setpoint_); + + motor_.current_control_.phase_src_.connect_to(&open_loop_controller_.phase_); + acim_estimator_.rotor_phase_src_.connect_to(&open_loop_controller_.phase_); + + motor_.phase_vel_src_.connect_to(&open_loop_controller_.phase_vel_); + motor_.current_control_.phase_vel_src_.connect_to(&open_loop_controller_.phase_vel_); + acim_estimator_.rotor_phase_vel_src_.connect_to(&open_loop_controller_.phase_vel_); + } + wait_for_control_iteration(); + + motor_.arm(&motor_.current_control_); + + // 一直保持锁定,直到上位机改写 requested_state_(CAN 0x277 / byte0 = 0x22 → IDLE) + while ((requested_state_ == AXIS_STATE_UNDEFINED) && motor_.is_armed_) { + osDelay(1); + } + + motor_.disarm(); + + return true; +} + bool Axis::start_closed_loop_control() { bool sensorless_mode = config_.enable_sensorless_mode; @@ -543,6 +605,11 @@ void Axis::run_state_machine_loop() { status = encoder_.run_hall_phase_calibration(); } break; + case AXIS_STATE_PHASE_LOCK_ZERO: { + // 电角度 0 锁定:不检查 motor_.is_calibrated_ / encoder_.config_.direction + status = run_phase_lock_zero(); + } break; + case AXIS_STATE_HOMING: { Controller::ControlMode stored_control_mode = controller_.config_.control_mode; Controller::InputMode stored_input_mode = controller_.config_.input_mode; diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index 2288adadc..35135a4d5 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -31,6 +31,12 @@ class Axis : public ODriveIntf::AxisIntf { bool finish_on_enc_idx = false; }; + // 自定义轴状态:把转子锁定在电角度 0(开环直流对齐)。 + // 取值 14,排在 ENCODER_HALL_PHASE_CALIBRATION(13) 之后,避开 ODrive 官方已占用的值。 + // 注意:这里刻意不改 odrive-interface.yaml / autogen,避免每次重新生成时被覆盖。 + static constexpr ODriveIntf::AxisIntf::AxisState AXIS_STATE_PHASE_LOCK_ZERO = + static_cast(14); + struct TaskTimes { TaskTimer thermistor_update; TaskTimer encoder_update; @@ -94,6 +100,13 @@ class Axis : public ODriveIntf::AxisIntf { LockinConfig_t sensorless_ramp = default_sensorless(); LockinConfig_t general_lockin; + // 电角度 0 锁定(由 CAN 0x277 指令触发)的幅值与爬升时间。 + // 电机已校准(相电阻/电感有效)时用电流环,取 phase_lock_current [A]; + // 电机未校准时用电压模式,取 phase_lock_voltage [V],靠 motor.config.current_lim 做过流保护。 + float phase_lock_current = 10.0f; // [A] + float phase_lock_voltage = 1.0f; // [V] + float phase_lock_ramp_time = 0.4f; // [s] + CANConfig_t can; // custom setters @@ -157,6 +170,7 @@ class Axis : public ODriveIntf::AxisIntf { bool stop_closed_loop_control(); bool run_lockin_spin(const LockinConfig_t &lockin_config, bool remain_armed, std::function loop_cb = {} ); + bool run_phase_lock_zero(); bool run_closed_loop_control_loop(); bool run_homing(); bool run_idle_loop(); diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index 183506064..74fac7eea 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -530,13 +530,28 @@ bool Encoder::abs_spi_start_transaction() { if (mode_ & MODE_FLAG_ABS){ if (Stm32SpiArbiter::acquire_task(&spi_task_)) { spi_task_.ncs_gpio = abs_spi_cs_gpio_; - spi_task_.tx_buf = (uint8_t*)abs_spi_dma_tx_; - spi_task_.rx_buf = (uint8_t*)abs_spi_dma_rx_; - spi_task_.length = 1; + spi_task_.length = 1; // AS5047P 逐帧流水线:所有事务都只有一帧 spi_task_.on_complete = [](void* ctx, bool success) { ((Encoder*)ctx)->abs_spi_cb(success); }; spi_task_.on_complete_ctx = this; spi_task_.next = nullptr; - + + // ★ 零位标定 / OTP 烧写期间改写本周期要发的那一帧。注意:仍然是同一个 + // spi_task_、同一笔事务,仲裁器链表长度完全不变 —— 这一点是关键。若从线程 + // 额外插入阻塞事务,一旦某次 start() 失败该任务就会永久卡在表头,链表再也 + // 不推进,周期读连续 51ms 收不到样本 → ENCODER_ERROR_ABS_SPI_COM_FAIL。 + bool hijack = false; + if (mode_ == MODE_SPI_ABS_AMS) { + hijack = (as5047_otp_step_ != AS5047_OTP_OFF) ? as5047_otp_prepare() + : as5047_seq_prepare(); + } + if (hijack) { + spi_task_.tx_buf = (uint8_t*)&as5047_seq_tx_; + spi_task_.rx_buf = (uint8_t*)&as5047_seq_rx_; + } else { + spi_task_.tx_buf = (uint8_t*)abs_spi_dma_tx_; + spi_task_.rx_buf = (uint8_t*)abs_spi_dma_rx_; + } + spi_arbiter_->transfer_async(&spi_task_); } else { return false; @@ -560,6 +575,443 @@ uint8_t cui_parity(uint16_t v) { return ~v & 3; } +/////////////////////////////////////////////////////////////////////////// +// AS5047P 零位标定 —— 见 磁编AS5047P-ATSM.pdf +// +// 命令帧 (Figure 13):bit15 = PARC(bit14:0 偶校验),bit14 = R/W(0 写 / 1 读),bit13:0 = 地址 +// 读数据帧 (Figure 14):bit15 = PARD(偶校验),bit14 = EF(帧错误),bit13:0 = 数据 +// 写数据帧 (Figure 16):bit15 = PARD(偶校验),bit14 = 恒 0,bit13:0 = 数据 +// +// ★ 关键:AS5047P 是"逐帧流水线"协议,命令帧和数据帧各占一个 CS 低电平周期,而不是拼在 +// 同一个 CS 段里发。手册原话: +// "The SPI read is sampled on the rising edge of CSn and the data is transmitted +// on MISO with the next read command." +// 本工程的周期读就是这么工作的(本周期发命令、下周期收数据),所以这里复用同样的 +// 机制即可 —— 每个采样周期只发一帧,全部事务的 length 都是 1。 +/////////////////////////////////////////////////////////////////////////// +static uint16_t as5047_build_frame(uint16_t payload14, bool is_read) { + uint16_t frame = payload14 & 0x3fffu; + if (is_read) { + frame |= (1u << 14); + } + // ams_parity() 返回 16bit 整体的偶校验;此时 bit15 = 0,等价于对 bit14:0 取偶校验 + return frame | ((uint16_t)ams_parity(frame) << 15); +} + +// 按当前步骤填好本周期要发的帧;返回 false 表示无任务,走正常的周期读(0xFFFF) +bool Encoder::as5047_seq_prepare() { + switch (as5047_step_) { + // --- 可选:写 ZPOSM / ZPOSL = as5047_zpos_target_ --- + case AS5047_WR_M_CMD: as5047_seq_tx_ = as5047_build_frame(0x0016u, false); return true; + case AS5047_WR_M_DAT: as5047_seq_tx_ = as5047_build_frame((uint16_t)((as5047_zpos_target_ >> 6) & 0x00ffu), false); return true; + case AS5047_WR_L_CMD: as5047_seq_tx_ = as5047_build_frame(0x0017u, false); return true; + case AS5047_WR_L_DAT: as5047_seq_tx_ = as5047_build_frame((uint16_t)(as5047_zpos_target_ & 0x003fu), false); return true; + + // --- 读回 ZPOSM / ZPOSL / 计数 --- + case AS5047_RD_ZM_CMD: as5047_seq_tx_ = as5047_build_frame(0x0016u, true); return true; + case AS5047_RD_ZL_CMD: as5047_seq_tx_ = as5047_build_frame(0x0017u, true); return true; + case AS5047_RD_CMD: as5047_seq_tx_ = as5047_build_frame(0x3fffu, true); return true; // ANGLECOM + + // --- 读磁场强度 / 诊断信息 --- + case AS5047_RD_MAG_CMD: as5047_seq_tx_ = as5047_build_frame(0x3ffdu, true); return true; // MAG + case AS5047_RD_DIAG_CMD: as5047_seq_tx_ = as5047_build_frame(0x3ffcu, true); return true; // DIAAGC + + case AS5047_RD_ZM_DAT: + case AS5047_RD_ZL_DAT: + case AS5047_RD_DAT: + case AS5047_RD_MAG_DAT: + case AS5047_RD_DIAG_DAT: as5047_seq_tx_ = as5047_build_frame(0x0000u, true); return true; // NOP + + default: + return false; // AS5047_IDLE / AS5047_DONE + } +} + +// 标定帧不是角度,所以这里不设置 abs_spi_pos_updated_(update() 里会继续用上一笔 pos_abs_, +// 只跳过十几个采样周期,对 spi_error_rate_ 的贡献远低于 0.05 阈值) +void Encoder::as5047_seq_advance() { + const uint16_t rx = as5047_seq_rx_; + const bool rx_ok = !ams_parity(rx) && !(rx & (1u << 14)); // 偶校验正确且 EF 未置位 + + switch (as5047_step_) { + case AS5047_WR_M_CMD: as5047_step_ = AS5047_WR_M_DAT; break; + case AS5047_WR_M_DAT: as5047_step_ = AS5047_WR_L_CMD; break; + case AS5047_WR_L_CMD: as5047_step_ = AS5047_WR_L_DAT; break; + case AS5047_WR_L_DAT: as5047_step_ = AS5047_RD_ZM_CMD; break; + + case AS5047_RD_ZM_CMD: as5047_step_ = AS5047_RD_ZM_DAT; break; + case AS5047_RD_ZM_DAT: + if (rx_ok) as5047_zposm_rb_ = rx & 0x3fffu; + as5047_step_ = AS5047_RD_ZL_CMD; + break; + case AS5047_RD_ZL_CMD: as5047_step_ = AS5047_RD_ZL_DAT; break; + case AS5047_RD_ZL_DAT: + if (rx_ok) as5047_zposl_rb_ = rx & 0x3fffu; + as5047_step_ = AS5047_RD_CMD; + break; + + case AS5047_RD_CMD: as5047_step_ = AS5047_RD_DAT; break; // 本帧是上一次访问的输出,丢弃 + case AS5047_RD_DAT: + if (rx_ok) as5047_rd_count_ = rx & 0x3fffu; + as5047_step_ = AS5047_DONE; + break; + + // --- 读 MAG → 读 DIAAGC(入口由 as5047_read_diagnostics() 设为 RD_MAG_CMD)--- + case AS5047_RD_MAG_CMD: as5047_step_ = AS5047_RD_MAG_DAT; break; + case AS5047_RD_MAG_DAT: + if (rx_ok) as5047_mag_ = rx & 0x3fffu; + as5047_step_ = AS5047_RD_DIAG_CMD; + break; + case AS5047_RD_DIAG_CMD: as5047_step_ = AS5047_RD_DIAG_DAT; break; + case AS5047_RD_DIAG_DAT: + if (rx_ok) as5047_diag_ = rx & 0x3fffu; + as5047_step_ = AS5047_DONE; + break; + + default: break; + } +} + +// 等待轴静止。零位是一个位置量,而且序列里前后两次采样相隔若干采样周期,轴一转就会在 +// 两次采样之间引入额外偏差,让结果无法判读 —— 所以标定前必须先确认轴没在动。 +bool Encoder::as5047_wait_still(uint32_t timeout_ms, uint32_t tol_counts) { + constexpr int32_t CPR = 16384; // AS5047P 14bit + int32_t prev = pos_abs_; + uint32_t waited = 0; + + while (waited < timeout_ms) { + osDelay(2); + waited += 2; + int32_t cur = pos_abs_; + int32_t d = cur - prev; + if (d > CPR / 2) d -= CPR; // 处理 0/16383 回绕 + if (d < -CPR / 2) d += CPR; + if (d < 0) d = -d; + if ((uint32_t)d <= tol_counts) { + return true; + } + prev = cur; + } + return false; +} + +// 读回 ZPOSM/ZPOSL 和计数,不动硬件任何状态。上电后第一次调用读到的 ZPOS 就是 OTP 内容。 +bool Encoder::as5047_read_zero_and_count(uint16_t* zpos, uint16_t* count, uint32_t timeout_ms) { + if (mode_ != MODE_SPI_ABS_AMS) { + return false; + } + + as5047_rd_count_ = 0xffffu; + as5047_zposm_rb_ = 0xffffu; + as5047_zposl_rb_ = 0xffffu; + as5047_step_ = AS5047_RD_ZM_CMD; + + uint32_t waited = 0; + while (as5047_step_ != AS5047_DONE) { + osDelay(1); + if (++waited >= timeout_ms) { + as5047_step_ = AS5047_IDLE; + return false; + } + } + as5047_step_ = AS5047_IDLE; + + // ZPOSM 是 8bit、ZPOSL 是 6bit,读回 0xFFFF 只可能是没读到 + if (as5047_zposm_rb_ == 0xffffu || as5047_zposl_rb_ == 0xffffu || as5047_rd_count_ == 0xffffu) { + return false; + } + + if (zpos) *zpos = (uint16_t)(((as5047_zposm_rb_ & 0x00ffu) << 6) | (as5047_zposl_rb_ & 0x003fu)); + if (count) *count = as5047_rd_count_; + return true; +} + +// 软写 ZPOSM/ZPOSL = zpos,然后读回 ZPOSM 和计数。 +// 只轮询状态机,**不做任何阻塞式 SPI**,因此无论底层发生什么都能在 timeout_ms 内返回。 +bool Encoder::as5047_write_zpos(uint16_t zpos, uint16_t* count, uint16_t* zposm_rb, + uint32_t timeout_ms) { + if (mode_ != MODE_SPI_ABS_AMS) { + return false; + } + + as5047_zpos_target_ = (uint16_t)(zpos & 0x3fffu); + as5047_rd_count_ = 0xffffu; + as5047_zposm_rb_ = 0xffffu; + as5047_zposl_rb_ = 0xffffu; + as5047_step_ = AS5047_WR_M_CMD; + + uint32_t waited = 0; + while (as5047_step_ != AS5047_DONE) { + osDelay(1); + if (++waited >= timeout_ms) { + as5047_step_ = AS5047_IDLE; + return false; + } + } + as5047_step_ = AS5047_IDLE; + + if (count) *count = as5047_rd_count_; + if (zposm_rb) *zposm_rb = as5047_zposm_rb_; + return true; +} + +// 读磁场强度 MAG(0x3FFD) 和诊断信息 DIAAGC(0x3FFC)。纯读,无副作用。 +bool Encoder::as5047_read_diagnostics(uint16_t* magnitude, uint16_t* diaagc, uint32_t timeout_ms) { + if (mode_ != MODE_SPI_ABS_AMS) { + return false; + } + + as5047_mag_ = 0xffffu; + as5047_diag_ = 0xffffu; + as5047_step_ = AS5047_RD_MAG_CMD; + + uint32_t waited = 0; + while (as5047_step_ != AS5047_DONE) { + osDelay(1); + if (++waited >= timeout_ms) { + as5047_step_ = AS5047_IDLE; + return false; + } + } + as5047_step_ = AS5047_IDLE; + + // MAG 是 14bit,读回 0xFFFF 只可能是没读到 + if (as5047_mag_ == 0xffffu || as5047_diag_ == 0xffffu) { + return false; + } + + if (magnitude) *magnitude = as5047_mag_; + if (diaagc) *diaagc = as5047_diag_; + return true; +} + +/////////////////////////////////////////////////////////////////////////// +// AS5047P OTP 烧写(不可逆,只能烧一次) +// 严格按手册第 27-28 页的 19 步流程(Figure 35-38)展开;本段负责 step 2~15, +// step 1/16 的"Power ON cycle"与最终 step 17~19 的复验必须由人工断电上电完成。 +// 电气前提:烧写期间 VDD 必须 3.3~3.5V(5V 模式 4.5~5.5V),瞬时电流可达 100mA, +// 温度 25±20℃,且手册明确写了"A reprogramming is not allowed"。 +/////////////////////////////////////////////////////////////////////////// + +// PROG 轮询上限:每次轮询占 2 个采样周期,4000 次约 1s +static constexpr uint32_t AS5047_OTP_POLL_LIMIT = 4000; + +// 待写入 0x0016+idx 的值(idx 顺序与寄存器地址顺序一致,方便与读回值直接比对) +uint16_t Encoder::as5047_otp_write_value(uint8_t idx) const { + return (idx < 4) ? as5047_otp_expected_[idx] : 0u; +} + +bool Encoder::as5047_otp_prepare() { + switch (as5047_otp_step_) { + // 读 0x0016+idx(预检用 idx 0/1 读 ZPOSM/ZPOSL;step1/step2 用 idx 0..3) + case AS5047_OTP_PRE_CMD: + case AS5047_OTP_RD1_CMD: + case AS5047_OTP_RD2_CMD: + as5047_seq_tx_ = as5047_build_frame((uint16_t)(0x0016u + as5047_otp_idx_), true); + return true; + case AS5047_OTP_PRE_DAT: + case AS5047_OTP_RD1_DAT: + case AS5047_OTP_RD2_DAT: + as5047_seq_tx_ = as5047_build_frame(0x0000u, true); + return true; + + // 写 0x0016+idx + case AS5047_OTP_WR_CMD: + case AS5047_OTP_CLR_CMD: + as5047_seq_tx_ = as5047_build_frame((uint16_t)(0x0016u + as5047_otp_idx_), false); + return true; + case AS5047_OTP_WR_DAT: + as5047_seq_tx_ = as5047_build_frame(as5047_otp_write_value(as5047_otp_idx_), false); + return true; + case AS5047_OTP_CLR_DAT: // step 11:把整个非易失内容写 0 + as5047_seq_tx_ = as5047_build_frame(0x0000u, false); + return true; + + // 写 PROG 寄存器 + case AS5047_OTP_PROGEN_CMD: + case AS5047_OTP_BURN_CMD: + case AS5047_OTP_PROGVER_CMD: + case AS5047_OTP_OTPREF_CMD: + as5047_seq_tx_ = as5047_build_frame(0x0003u, false); + return true; + case AS5047_OTP_PROGEN_DAT: as5047_seq_tx_ = as5047_build_frame(0x0001u, false); return true; // PROGEN + case AS5047_OTP_BURN_DAT: as5047_seq_tx_ = as5047_build_frame(0x0008u, false); return true; // PROGOTP + case AS5047_OTP_PROGVER_DAT: as5047_seq_tx_ = as5047_build_frame(0x0040u, false); return true; // PROGVER + case AS5047_OTP_OTPREF_DAT: as5047_seq_tx_ = as5047_build_frame(0x0004u, false); return true; // OTPREF + + // 轮询 PROG + case AS5047_OTP_POLL_CMD: as5047_seq_tx_ = as5047_build_frame(0x0003u, true); return true; + case AS5047_OTP_POLL_DAT: as5047_seq_tx_ = as5047_build_frame(0x0000u, true); return true; + + default: + return false; + } +} + +// 比对 rd[0..3] = ZPOSM/ZPOSL/SETTINGS1/SETTINGS2 与期望值。 +// SETTINGS1 的 bit0 是出厂预编程位,读回恒为 1,比对时必须屏蔽(手册 step 7/15/18 强制要求) +bool Encoder::as5047_otp_verify(const uint16_t* rd) const { + if ((rd[0] & 0x00ffu) != as5047_otp_expected_[0]) return false; + if ((rd[1] & 0x003fu) != as5047_otp_expected_[1]) return false; + if ((rd[2] & 0xfffeu) != (as5047_otp_expected_[2] & 0xfffeu)) return false; + if ((rd[3] & 0x3fffu) != as5047_otp_expected_[3]) return false; + return true; +} + +void Encoder::as5047_otp_advance() { + const uint16_t rx = as5047_seq_rx_; + const bool rx_ok = !ams_parity(rx) && !(rx & (1u << 14)); + + switch (as5047_otp_step_) { + // ---- 预检:ZPOSM/ZPOSL 都应读回 0。非 0 说明 OTP 已经烧过了 ---- + case AS5047_OTP_PRE_CMD: as5047_otp_step_ = AS5047_OTP_PRE_DAT; break; + case AS5047_OTP_PRE_DAT: + if (!rx_ok || (rx & 0x3fffu) != 0u) { + as5047_otp_fail_ = 5; + as5047_otp_step_ = AS5047_OTP_FAILED; + } else if (++as5047_otp_idx_ < 2) { + as5047_otp_step_ = AS5047_OTP_PRE_CMD; + } else { + as5047_otp_idx_ = 0; + as5047_otp_step_ = AS5047_OTP_WR_CMD; + } + break; + + // ---- step 2/5:写 SETTINGS1/2 和 ZPOSM/ZPOSL ---- + case AS5047_OTP_WR_CMD: as5047_otp_step_ = AS5047_OTP_WR_DAT; break; + case AS5047_OTP_WR_DAT: + if (++as5047_otp_idx_ < 4) { + as5047_otp_step_ = AS5047_OTP_WR_CMD; + } else { + as5047_otp_idx_ = 0; + as5047_otp_step_ = AS5047_OTP_RD1_CMD; + } + break; + + // ---- step 6/7:读回 step1 并比对,不对就绝不往下烧 ---- + case AS5047_OTP_RD1_CMD: as5047_otp_step_ = AS5047_OTP_RD1_DAT; break; + case AS5047_OTP_RD1_DAT: + if (rx_ok) as5047_otp_rd1_[as5047_otp_idx_] = rx & 0x3fffu; + if (++as5047_otp_idx_ < 4) { + as5047_otp_step_ = AS5047_OTP_RD1_CMD; + } else if (as5047_otp_verify(as5047_otp_rd1_)) { + as5047_otp_step_ = AS5047_OTP_PROGEN_CMD; + } else { + as5047_otp_fail_ = 2; + as5047_otp_step_ = AS5047_OTP_FAILED; + } + break; + + // ---- step 8/9:PROGEN=1 后过 2 帧(250us)再 PROGOTP=1,满足手册">=100us" ---- + case AS5047_OTP_PROGEN_CMD: as5047_otp_step_ = AS5047_OTP_PROGEN_DAT; break; + case AS5047_OTP_PROGEN_DAT: as5047_otp_step_ = AS5047_OTP_BURN_CMD; break; + case AS5047_OTP_BURN_CMD: as5047_otp_step_ = AS5047_OTP_BURN_DAT; break; + case AS5047_OTP_BURN_DAT: + as5047_otp_polls_ = 0; + as5047_otp_step_ = AS5047_OTP_POLL_CMD; + break; + + // ---- step 10:轮询 PROG 直到读回 0x0001 ---- + case AS5047_OTP_POLL_CMD: as5047_otp_step_ = AS5047_OTP_POLL_DAT; break; + case AS5047_OTP_POLL_DAT: + if (rx_ok && (rx & 0x3fffu) == 0x0001u) { + as5047_otp_idx_ = 0; + as5047_otp_step_ = AS5047_OTP_CLR_CMD; + } else if (++as5047_otp_polls_ > AS5047_OTP_POLL_LIMIT) { + as5047_otp_fail_ = 3; + as5047_otp_step_ = AS5047_OTP_FAILED; + } else { + as5047_otp_step_ = AS5047_OTP_POLL_CMD; + } + break; + + // ---- step 11:把非易失内容全部写 0(这样 step 13 刷新后读到的才是真 OTP 内容)---- + case AS5047_OTP_CLR_CMD: as5047_otp_step_ = AS5047_OTP_CLR_DAT; break; + case AS5047_OTP_CLR_DAT: + if (++as5047_otp_idx_ < 4) { + as5047_otp_step_ = AS5047_OTP_CLR_CMD; + } else { + as5047_otp_step_ = AS5047_OTP_PROGVER_CMD; + } + break; + + // ---- step 12/13:PROGVER=1(guard band)→ OTPREF=1(用 OTP 刷新易失内容)---- + case AS5047_OTP_PROGVER_CMD: as5047_otp_step_ = AS5047_OTP_PROGVER_DAT; break; + case AS5047_OTP_PROGVER_DAT: as5047_otp_step_ = AS5047_OTP_OTPREF_CMD; break; + case AS5047_OTP_OTPREF_CMD: as5047_otp_step_ = AS5047_OTP_OTPREF_DAT; break; + case AS5047_OTP_OTPREF_DAT: + as5047_otp_idx_ = 0; + as5047_otp_step_ = AS5047_OTP_RD2_CMD; + break; + + // ---- step 14/15:读回 step2 并比对,这才是"烧进去了"的真凭据 ---- + case AS5047_OTP_RD2_CMD: as5047_otp_step_ = AS5047_OTP_RD2_DAT; break; + case AS5047_OTP_RD2_DAT: + if (rx_ok) as5047_otp_rd2_[as5047_otp_idx_] = rx & 0x3fffu; + if (++as5047_otp_idx_ < 4) { + as5047_otp_step_ = AS5047_OTP_RD2_CMD; + } else if (as5047_otp_verify(as5047_otp_rd2_)) { + as5047_otp_step_ = AS5047_OTP_DONE; + } else { + as5047_otp_fail_ = 4; + as5047_otp_step_ = AS5047_OTP_FAILED; + } + break; + + default: break; + } +} + +// 发起 OTP 烧写。立刻返回,不代表任何步骤已完成 —— 结果用 as5047_burn_otp_poll() 查。 +bool Encoder::as5047_burn_otp_start(uint16_t zpos, uint32_t timeout_ms) { + if (mode_ != MODE_SPI_ABS_AMS || as5047_otp_step_ != AS5047_OTP_OFF) { + return false; + } + + // 期望烧进 OTP 的内容,顺序与 0x0016..0x0019 一致 + as5047_otp_expected_[0] = (uint16_t)((zpos >> 6) & 0x00ffu); // ZPOSM + as5047_otp_expected_[1] = (uint16_t)(zpos & 0x003fu); // ZPOSL + as5047_otp_expected_[2] = 0x0000u; // SETTINGS1:bit0 出厂位必须清 0,其余保持默认(ABZ/UVW 未使用) + as5047_otp_expected_[3] = 0x0000u; // SETTINGS2 + + for (uint8_t i = 0; i < 4; ++i) { + as5047_otp_rd1_[i] = 0; + as5047_otp_rd2_[i] = 0; + } + as5047_otp_fail_ = 0; + as5047_otp_idx_ = 0; + as5047_otp_polls_ = 0; + as5047_step_ = AS5047_IDLE; // 确保没有残留的半截序列会和 OTP 序列抢帧 + // 整条序列期间周期读被占用,abs_spi_pos_updated_ 不置位,把 COM_FAIL 计数清零避免误报 + spi_error_rate_ = 0.0f; + as5047_otp_deadline_ = HAL_GetTick() + timeout_ms; + as5047_otp_step_ = AS5047_OTP_PRE_CMD; + return true; +} + +bool Encoder::as5047_burn_otp_poll(uint16_t* out_zpos, uint8_t* fail_code) { + if (as5047_otp_step_ != AS5047_OTP_DONE && as5047_otp_step_ != AS5047_OTP_FAILED) { + // 还没跑完。序列本身靠采样中断推进,这里只做墙上时钟超时保护 + if ((int32_t)(HAL_GetTick() - as5047_otp_deadline_) < 0) { + return false; + } + as5047_otp_step_ = AS5047_OTP_OFF; + if (fail_code) *fail_code = 1; + if (out_zpos) *out_zpos = 0xffffu; + return true; + } + + const bool ok = (as5047_otp_step_ == AS5047_OTP_DONE); + as5047_otp_step_ = AS5047_OTP_OFF; + if (fail_code) *fail_code = ok ? 0u : as5047_otp_fail_; + if (out_zpos) { + // 从 step2 读回值重建实际烧进去的零位 + *out_zpos = ok ? (uint16_t)(((as5047_otp_rd2_[0] & 0x00ffu) << 6) | (as5047_otp_rd2_[1] & 0x003fu)) + : 0xffffu; + } + return true; +} + void Encoder::abs_spi_cb(bool success) { uint16_t pos; @@ -567,6 +1019,20 @@ void Encoder::abs_spi_cb(bool success) { goto done; } + // OTP 烧写序列期间周期读被整段占用,位置本来就不更新;每周期把 COM_FAIL 计数器压住, + // 免得长序列把 spi_error_rate_ 推过 0.05 阈值误报 ENCODER_ERROR_ABS_SPI_COM_FAIL + if (as5047_otp_step_ != AS5047_OTP_OFF) { + spi_error_rate_ = 0.0f; + as5047_otp_advance(); + goto done; + } + + // AS5047P 零位标定期间收回来的帧不是角度,交给序列机处理,不当作位置数据 + if (as5047_step_ != AS5047_IDLE) { + as5047_seq_advance(); + goto done; + } + switch (mode_) { case MODE_SPI_ABS_AMS: { uint16_t rawVal = abs_spi_dma_rx_[0]; diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp index 16cbcd18d..e2e518996 100644 --- a/Firmware/MotorControl/encoder.hpp +++ b/Firmware/MotorControl/encoder.hpp @@ -174,10 +174,107 @@ class Encoder : public ODriveIntf::EncoderIntf { uint16_t abs_spi_dma_rx_[1]; Stm32SpiArbiter::SpiTask spi_task_; + // ---- AS5047P 零位标定 ---- + // 实现要点:**不往 SPI 仲裁器里插任何新事务**,而是每个采样周期临时改写 spi_task_ 的 + // 命令字,搭本来就在跑的周期读的便车。这样周期读一步不落,abs_spi_pos_updated_ 不会 + // 出现长窗口为 false,也就不可能触发 ENCODER_ERROR_ABS_SPI_COM_FAIL。 + // A) 单帧流水线序列:可选地软写 ZPOSM/ZPOSL,然后读回 ZPOSM/ZPOSL/计数 + enum As5047Step : uint8_t { + AS5047_IDLE = 0, + AS5047_WR_M_CMD, // 命令帧:写 ZPOSM + AS5047_WR_M_DAT, // 数据帧:ZPOSM = as5047_zpos_target_ >> 6 + AS5047_WR_L_CMD, // 命令帧:写 ZPOSL + AS5047_WR_L_DAT, // 数据帧:ZPOSL = as5047_zpos_target_ & 0x3F + AS5047_RD_ZM_CMD, // 命令帧:读 ZPOSM(0x0016) + AS5047_RD_ZM_DAT, + AS5047_RD_ZL_CMD, // 命令帧:读 ZPOSL(0x0017) + AS5047_RD_ZL_DAT, + AS5047_RD_CMD, // 命令帧:读 ANGLECOM(0x3FFF) —— 周期读用的就是这个寄存器 + AS5047_RD_DAT, + AS5047_RD_MAG_CMD, // 命令帧:读 MAG(0x3FFD) 磁场强度(CORDIC 幅值) + AS5047_RD_MAG_DAT, + AS5047_RD_DIAG_CMD,// 命令帧:读 DIAAGC(0x3FFC) 诊断与 AGC + AS5047_RD_DIAG_DAT, + AS5047_DONE, + }; + volatile uint8_t as5047_step_ = AS5047_IDLE; + volatile uint16_t as5047_zpos_target_ = 0; // 要写入的 ZPOS + volatile uint16_t as5047_zposm_rb_ = 0xffffu; // ZPOSM 读回值(应 = zpos_target >> 6) + volatile uint16_t as5047_zposl_rb_ = 0xffffu; // ZPOSL 读回值(应 = zpos_target & 0x3F) + volatile uint16_t as5047_rd_count_ = 0xffffu; // 读回的 14bit 绝对计数 + volatile uint16_t as5047_mag_ = 0xffffu; // MAG(0x3FFD) 读回值 + volatile uint16_t as5047_diag_ = 0xffffu; // DIAAGC(0x3FFC) 读回值 + uint16_t as5047_seq_tx_ = 0; + uint16_t as5047_seq_rx_ = 0; + bool as5047_seq_prepare(); + void as5047_seq_advance(); + + // B) OTP 烧写序列(不可逆!)严格按手册 Figure 35-38 的 19 步流程展开。 + // 每个采样周期一帧,整条序列约 40 帧 + 轮询,期间 abs_spi_pos_updated_ 不置位, + // 所以必须 disarm 后执行。 + enum As5047OtpStep : uint8_t { + AS5047_OTP_OFF = 0, + AS5047_OTP_PRE_CMD, // 烧前预检:读 0x0016/0x0017,非 0 说明 OTP 已经烧过 + AS5047_OTP_PRE_DAT, + AS5047_OTP_WR_CMD, // idx 0..3:写 0x0016+idx + AS5047_OTP_WR_DAT, + AS5047_OTP_RD1_CMD, // idx 0..3:读回 0x0016+idx(step 6) + AS5047_OTP_RD1_DAT, + AS5047_OTP_PROGEN_CMD, // PROG = 0x0001(step 8) + AS5047_OTP_PROGEN_DAT, + AS5047_OTP_BURN_CMD, // PROG = 0x0008(step 9) + AS5047_OTP_BURN_DAT, + AS5047_OTP_POLL_CMD, // 读 PROG 直到 0x0001(step 10) + AS5047_OTP_POLL_DAT, + AS5047_OTP_CLR_CMD, // idx 0..3:清零易失内容(step 11) + AS5047_OTP_CLR_DAT, + AS5047_OTP_PROGVER_CMD, // PROG = 0x0040(step 12) + AS5047_OTP_PROGVER_DAT, + AS5047_OTP_OTPREF_CMD, // PROG = 0x0004(step 13) + AS5047_OTP_OTPREF_DAT, + AS5047_OTP_RD2_CMD, // idx 0..3:读回 0x0016+idx(step 14) + AS5047_OTP_RD2_DAT, + AS5047_OTP_DONE, + AS5047_OTP_FAILED, + }; + volatile uint8_t as5047_otp_step_ = AS5047_OTP_OFF; + volatile uint8_t as5047_otp_idx_ = 0; + volatile uint32_t as5047_otp_polls_ = 0; + volatile uint8_t as5047_otp_fail_ = 0; + uint32_t as5047_otp_deadline_ = 0; // 墙上时钟超时(毫秒) + uint16_t as5047_otp_expected_[4] = {0, 0, 0, 0}; // 期望值:ZPOSM / ZPOSL / SETTINGS1 / SETTINGS2 + uint16_t as5047_otp_rd1_[4] = {0, 0, 0, 0}; + uint16_t as5047_otp_rd2_[4] = {0, 0, 0, 0}; + bool as5047_otp_prepare(); + void as5047_otp_advance(); + uint16_t as5047_otp_write_value(uint8_t idx) const; + bool as5047_otp_verify(const uint16_t* rd) const; + constexpr float getCoggingRatio(){ return 1.0f / 3600.0f; } + // AS5047P 零位标定:只能在 CMSIS 线程上下文调用;内部只轮询状态机(带超时), + // 不做阻塞式 SPI。 + // 读回当前的 ZPOSM/ZPOSL(组合成 14bit zpos)和 14bit 绝对计数。**不动硬件任何状态**, + // 上电后第一次调用读到的 ZPOS 就是 OTP 里的内容(未烧过则为 0)。 + bool as5047_read_zero_and_count(uint16_t* zpos, uint16_t* count, uint32_t timeout_ms = 50); + // 软写 AS5047P 的 ZPOSM/ZPOSL = zpos(**易失寄存器,掉电丢失**),随后读回 ZPOSM 和计数。 + bool as5047_write_zpos(uint16_t zpos, uint16_t* count, uint16_t* zposm_rb, + uint32_t timeout_ms = 50); + // ★ OTP 烧写,**不可逆,只能烧一次**。前置条件(调用方必须保证): + // 1) 轴已 disarm;2) 编码器供电 3.3~3.5V(5V 模式 4.5~5.5V),瞬时 100mA;3) 未软写过 ZPOS。 + // 拆成 start/poll 两半是刻意的:整条序列几十毫秒到 1 秒,不能阻塞 CAN 线程。 + bool as5047_burn_otp_start(uint16_t zpos, uint32_t timeout_ms = 3000); + // 返回 true 表示已经结束(成功/失败/超时),out_zpos/fail_code 有效;false = 仍在进行 + bool as5047_burn_otp_poll(uint16_t* out_zpos, uint8_t* fail_code); + // 当前 14bit 绝对计数(MODE_SPI_ABS_AMS 下就是 ANGLECOM 的原始值,已含 ZPOS) + uint16_t as5047_current_count() const { return (uint16_t)(pos_abs_ & 0x3fffu); } + // 读磁场强度 MAG(0x3FFD, 14bit) 和诊断信息 DIAAGC(0x3FFC)。纯读,无副作用。 + bool as5047_read_diagnostics(uint16_t* magnitude, uint16_t* diaagc, uint32_t timeout_ms = 50); + // 等待轴静止(用周期读得到的 pos_abs_ 判定)。零位是位置量,轴在转时标定没有意义。 + bool as5047_wait_still(uint32_t timeout_ms = 200, uint32_t tol_counts = 4); + }; #endif // __ENCODER_HPP diff --git a/Firmware/MotorControl/main.cpp b/Firmware/MotorControl/main.cpp index 0246a1e90..a685fd14d 100644 --- a/Firmware/MotorControl/main.cpp +++ b/Firmware/MotorControl/main.cpp @@ -888,20 +888,25 @@ extern "C" int main(void) { axes[0].encoder_.config_.abs_spi_cs_gpio_pin = 7; axes[0].encoder_.abs_spi_cs_pin_init(); axes[0].encoder_.config_.direction = -1; - axes[0].motor_.config_.set_current_control_bandwidth(300.0f); + axes[0].motor_.config_.set_current_control_bandwidth(1000.0f); axes[1].encoder_.config_.abs_spi_cs_gpio_pin = 6; axes[1].encoder_.abs_spi_cs_pin_init(); axes[1].encoder_.config_.direction = 1; - axes[1].motor_.config_.set_current_control_bandwidth(300.0f); + axes[1].motor_.config_.set_current_control_bandwidth(1000.0f); #endif #ifdef ENCODER_USE_HALL + axes[0].encoder_.config_.mode = Encoder::MODE_HALL; axes[0].encoder_.config_.direction = 1; - axes[0].motor_.config_.set_current_control_bandwidth(64.0f); + axes[0].motor_.config_.set_current_control_bandwidth(96.0f); axes[1].encoder_.config_.direction = 1; - axes[1].motor_.config_.set_current_control_bandwidth(64.0f); + axes[1].motor_.config_.set_current_control_bandwidth(96.0f); #endif + axes[0].motor_.config_.current_lim = 46.0f; //增大电流,用于越障 + axes[1].motor_.config_.current_lim = 46.0f; + odrv.config_.dc_bus_undervoltage_trip_level = 17.5f;//思商电池:规格书放电截止电压21V,BMS实际是2.8*7=19.6V停止供电,3*7=21V恢复供电。控制器欠压保护值由21V改为2.5*7=17.5V + //axes[1].encoder_.config_.mode = Encoder::MODE_HALL; /////////////////////////////////////////// // Init usb irq binary semaphore, and start with no tokens by removing the starting one. diff --git a/Firmware/communication/can/can_simple.cpp b/Firmware/communication/can/can_simple.cpp index 91bd6865c..49f93d2c0 100644 --- a/Firmware/communication/can/can_simple.cpp +++ b/Firmware/communication/can/can_simple.cpp @@ -341,6 +341,9 @@ void CANSimple::can_message_callback(void* ctx, const can_Message_t& msg) { static_cast(ctx)->handle_can_message(msg); } +// 前向声明: 定义在下方 "磁编 AS5047P 零位标定" 一节, 这里分发时要用 +static bool as5047_has_cmd_tail(const can_Message_t& msg); + void CANSimple::handle_can_message(const can_Message_t& msg) { if (!msg.isExt) { return; @@ -361,9 +364,10 @@ void CANSimple::handle_can_message(const can_Message_t& msg) { get_vbus_voltage_callback(msg); break; - case MSG_SET_VLAGO_INPUT_VEL: // 0x219 上控速度指令 - case MSG_SET_UP_CONTROLL_VEL: // 0x220 上控速度指令 - case MSG_SET_VLAGO_ADC: // 0x279 摇杆 ADC (本地摇杆控车) + //case 0x290: + case MSG_SET_VLAGO_INPUT_VEL: // 0x219 域控速度指令 + //case MSG_SET_UP_CONTROLL_VEL: // 0x220 上控速度指令 + //case MSG_SET_VLAGO_ADC: // 0x279 摇杆 ADC (本地摇杆控车) set_vlago_input_vel_callback(msg); break; @@ -387,9 +391,23 @@ void CANSimple::handle_can_message(const can_Message_t& msg) { get_vlago_version_callback(msg); break; - case MSG_IDENTIFY: // 0x277 识别/校准 - identify_callback(msg); - break; + case MSG_IDENTIFY: { // 0x277: 磁编零位标定 (尾缀 01..07) 或 识别/校准 (尾缀 11..77) + // 两者共用 0x277, 靠 byte1~byte7 的固定尾缀区分, 互不重叠 + if (as5047_has_cmd_tail(msg)) { + const uint8_t sub = msg.buf[0]; + if (sub == 0x00) { + as5047_read_diag_callback(msg); // 读磁场强度/诊断 + } else if (sub == 0x33) { + as5047_zero_offset_callback(msg); // 软写 ZPOS (易失) + } else if (sub == 0x44) { + as5047_burn_otp_callback(msg); // 烧 OTP (不可逆) + } else { + phase_lock_zero_callback(msg); // 0x11 锁定 / 0x22 退出 + } + } else { + identify_callback(msg); + } + } break; case MSG_OTA_RESET: // 0x701 OTA 复位 handle_ota_message(msg); @@ -717,6 +735,297 @@ void CANSimple::get_vlago_version_callback(const can_Message_t& msg) { canbus_->send_message(txmsg); } +/* ------------------------------------------------------------------------- */ +/* 磁编 AS5047P 零位标定: CAN 0x277 (命令) / 0x278 (应答) */ +/* byte1~byte7 = 01 02 03 04 05 06 07 固定校验字, 与识别命令 (11..77) 区分 */ +/* ------------------------------------------------------------------------- */ + +// 校验 byte1~byte7 固定尾缀 01 02 03 04 05 06 07 +static bool as5047_has_cmd_tail(const can_Message_t& msg) { + static const uint8_t expected[7] = {0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07}; + if (msg.len < 8) { + return false; + } + for (size_t i = 0; i < sizeof(expected); ++i) { + if (msg.buf[i + 1] != expected[i]) { + return false; + } + } + return true; +} + +// 0x278 应答: byte0 = 命令回显, byte1 = 数据高 8 位, byte2 = 数据低 8 位, +// byte3 = 结果码 (0/未用 = 成功, 非 0 = 失败原因), byte4-7 = 0 +void CANSimple::send_count_reply(uint8_t cmd_echo, uint16_t count, uint8_t code) { + can_Message_t txmsg; + txmsg.id = PHASE_LOCK_REPLY_MSG_ID; + txmsg.isExt = true; + txmsg.len = 8; + can_setSignal(txmsg, cmd_echo, 0, 8, true); + can_setSignal(txmsg, (uint8_t)(count >> 8), 8, 8, true); // byte1 = 高 8 位 + can_setSignal(txmsg, (uint8_t)(count & 0xffu), 16, 8, true); // byte2 = 低 8 位 + can_setSignal(txmsg, code, 24, 8, true); // byte3 = 结果码 + // byte4-7 已经是 0 + + canbus_->send_message(txmsg); +} + +// 0x278 诊断应答: byte0 = 命令回显(0x00), byte1/2 = 磁场强度, byte3 = 诊断状态位, +// byte4 = AGC 值, byte5-7 = 0 +void CANSimple::send_diag_reply(uint8_t cmd_echo, uint16_t magnitude, uint8_t status, uint8_t agc) { + can_Message_t txmsg; + txmsg.id = PHASE_LOCK_REPLY_MSG_ID; + txmsg.isExt = true; + txmsg.len = 8; + can_setSignal(txmsg, cmd_echo, 0, 8, true); + can_setSignal(txmsg, (uint8_t)(magnitude >> 8), 8, 8, true); // byte1 = 高 8 位 + can_setSignal(txmsg, (uint8_t)(magnitude & 0xffu), 16, 8, true); // byte2 = 低 8 位 + can_setSignal(txmsg, status, 24, 8, true); // byte3 = 状态位 + can_setSignal(txmsg, agc, 32, 8, true); // byte4 = AGC + // byte5-7 已经是 0 + + canbus_->send_message(txmsg); +} + +// 0x00: 读 AS5047 的磁场强度和诊断信息 (仅 axis0) +// 流程: 读 MAG(0x3FFD) → 读 DIAAGC(0x3FFC), 纯读操作、无副作用 +// 应答 byte3 状态位: bit0 = LF 偏移补偿完成, bit1 = COF CORDIC 溢出, +// bit2 = MAGH 磁场过强, bit3 = MAGL 磁场过弱 +void CANSimple::as5047_read_diag_callback(const can_Message_t& msg) { + if (!as5047_has_cmd_tail(msg)) { + return; + } + + const uint16_t INVALID = 0xffffu; + uint16_t magnitude = INVALID; + uint16_t diaagc = INVALID; + + axes[0].watchdog_feed(); + const bool ok = axes[0].encoder_.as5047_read_diagnostics(&magnitude, &diaagc, 50); + axes[0].watchdog_feed(); + + if (!ok) { + send_diag_reply(0x00, INVALID, 0xffu, 0xffu); + return; + } + + // DIAAGC 位定义 (手册 Figure 21): bit8=LF, bit9=COF, bit10=MAGH, bit11=MAGL, bit7:0=AGC + const uint8_t status = (uint8_t)((((diaagc >> 8) & 0x1u) << 0) // bit0 = LF + | (((diaagc >> 9) & 0x1u) << 1) // bit1 = COF + | (((diaagc >> 10) & 0x1u) << 2) // bit2 = MAGH + | (((diaagc >> 11) & 0x1u) << 3)); // bit3 = MAGL + + send_diag_reply(0x00, magnitude, status, (uint8_t)(diaagc & 0xffu)); +} + +// 0x11 期间读到的"电气 0 处的真实原始计数", 供 0x33 / 0x44 使用。 +// 必须是 0x11 当时的值: 写 ZPOS 是**整体平移刻度**, 只要 X 是在电气 0 位置测到的, +// 之后无论转子停在哪里, 计数都会恰好在电气 0 处归零。若让 0x33 现场重读, +// 转子一旦漂移开 (0x22 解锁之后是常事) 就会把错误的零位固化下来。 +static uint16_t as5047_cached_zero = 0xffffu; + +// 上电后是否已经往 ZPOSM/ZPOSL 软写过东西。 +// OTP 烧写前必须为 false —— 只有"没被软写过"时, 读回的 ZPOS 才等于 OTP 里的内容, +// 才能用"ZPOS 是否为 0"判断这颗芯片的 OTP 是否已经烧过。 +static bool as5047_volatile_touched = false; + +// OTP 序列是否正在进行 (结果由 service_stack() 每轮循环上报, 故意不阻塞 CAN 线程) +static bool as5047_otp_pending = false; + +// 等 AXIS_STATE_PHASE_LOCK_ZERO 真正跑起来并且转子对齐到位 +static bool wait_for_phase_lock_zero(Axis& axis) { + // 1) 等状态机进入该状态 + bool entered = false; + for (uint32_t i = 0; i < 500; ++i) { + axis.watchdog_feed(); + if (axis.current_state_ == Axis::AXIS_STATE_PHASE_LOCK_ZERO) { + entered = true; + break; + } + osDelay(1); + } + if (!entered) { + return false; + } + + // 2) 电流按 phase_lock_ramp_time 爬到设定值, 再留一段固定稳定时间给转子对齐 + uint32_t settle_ms = (uint32_t)(axis.config_.phase_lock_ramp_time * 1000.0f) + 200u; + for (uint32_t i = 0; i < settle_ms; ++i) { + axis.watchdog_feed(); + osDelay(1); + } + return true; +} + +// 电角度 0 锁定 (0x11) / 退出锁定 (0x22) +// byte0 = 0x11 : 锁定到电角度 0 → 读计数 X → 设好软件侧对齐参数, 应答 0x278 +// byte0 = 0x22 : 退出锁定 (回到 IDLE, disarm), 无应答 +void CANSimple::phase_lock_zero_callback(const can_Message_t& msg) { + if (!as5047_has_cmd_tail(msg)) { + return; // 校验字不符, 忽略整帧 + } + + const uint8_t cmd = can_getSignal(msg, 0, 8, true); + + // ---- 0x22: 退出锁定 ---- + // 把 requested_state_ 从 UNDEFINED 改掉, run_phase_lock_zero() 的等待循环就会退出 + if (cmd == 0x22) { + axes[0].requested_state_ = Axis::AXIS_STATE_IDLE; + axes[0].watchdog_feed(); + return; // 无应答 + } + + if (cmd != 0x11) { + return; + } + + // ---- 0x11: 锁定到电角度 0 → 读计数 X → 设好软件侧对齐参数 ---- + // run_phase_lock_zero() 是开环直流对齐: 定子磁场固定在定子空间的电角度 0 + // (phase 源切到 open_loop_controller_.phase_, 不经过编码器), + // 所以转子确实被拉到真实电角度 0, 与当前 phase_offset 对不对无关。 + axes[0].watchdog_feed(); + axes[0].requested_state_ = Axis::AXIS_STATE_PHASE_LOCK_ZERO; + + uint16_t x = 0xffffu; + + if (wait_for_phase_lock_zero(axes[0]) && + axes[0].encoder_.as5047_wait_still(500, 4)) { + + uint16_t zpos_now = 0xffffu; + uint16_t count = 0xffffu; + + // 这里刻意**不写** ZPOS: 一是无副作用, 反复执行不会像早先那样把偏移一轮轮叠加; + // 二是保证上电后读到的 ZPOS 始终等于 OTP 里的内容, 0x44 才能判断是否已烧过 + if (axes[0].encoder_.as5047_read_zero_and_count(&zpos_now, &count)) { + // 硬件零位是 zpos_now, 所以 ANGLECOM = (原始角 - zpos_now) mod 16384, + // 反解出真实原始角 X —— 这才是要写进 ZPOS 的值 + x = (uint16_t)((count + zpos_now) & 0x3fffu); + as5047_cached_zero = x; + + // 当前硬件零位是 zpos_now, 电气 0 处的计数就是 count, 所以软件侧等效偏移 + // (phase_offset + phase_offset_float - 0.5) 必须等于 count + axes[0].encoder_.config_.phase_offset = (int32_t)count; + axes[0].encoder_.config_.phase_offset_float = 0.5f; + // 这里必须直接赋值而不是用 setter: setter 会触发 check_pre_calibrated(), + // 而 is_ready_ 恰恰要等 pre_calibrated 为 true 之后才由 abs_spi_cb 置起, + // 走 setter 会互相等待、永远置不上 (死锁) + axes[0].encoder_.config_.pre_calibrated = true; + axes[0].watchdog_feed(); + } + } + + send_count_reply(0x11, x); +} + +// 0x33: 把 0x11 读到的 X 写入 ZPOSM/ZPOSL (仅 axis0) +// 应答 0x278: byte0 = 0x33, byte1/2 = 写入后读回的计数 (应 ≈ 0) +// byte1 = byte2 = 0xFF 表示失败: 没先发 0x11 / 轴未静止 / ZPOS 写入校验没过 +// +// 注意这里用的是 0x11 缓存的 X 而不是现场重读: 写 ZPOS 是整体平移刻度, 只要 X 是 +// 在电气 0 位置测到的, 之后无论转子停在哪里, 计数都会恰好在电气 0 处归零。 +void CANSimple::as5047_zero_offset_callback(const can_Message_t& msg) { + if (!as5047_has_cmd_tail(msg)) { + return; + } + + if (as5047_cached_zero == 0xffffu) { + // 没先发过 0x11, 没有可用的 X + send_count_reply(0x33, 0xffffu); + return; + } + + const uint16_t x = as5047_cached_zero; + axes[0].watchdog_feed(); + + // 轴必须静止, 否则写 ZPOS 前后读数会漂 + if (!axes[0].encoder_.as5047_wait_still(500, 4)) { + send_count_reply(0x33, 0xffffu); + return; + } + + uint16_t count = 0xffffu; + uint16_t zposm_rb = 0xffffu; + if (!axes[0].encoder_.as5047_write_zpos(x, &count, &zposm_rb) || + zposm_rb != (uint16_t)((x >> 6) & 0x00ffu)) { + // ZPOSM 读回值和写入值不符, 说明没写进去, 此时不能动 phase_offset + send_count_reply(0x33, 0xffffu); + return; + } + + // 硬件零位现在是 x, 即计数在电气 0 处归零, 故软件侧等效偏移必须为 0 + // (phase_offset + phase_offset_float - 0.5 = 0) + axes[0].encoder_.config_.phase_offset = 0; + axes[0].encoder_.config_.phase_offset_float = 0.5f; + axes[0].encoder_.config_.pre_calibrated = true; + as5047_volatile_touched = true; // ZPOS 被软写过了, 之后再想烧 OTP 必须先重新上电 + axes[0].watchdog_feed(); + + send_count_reply(0x33, count); +} + +// 0x44: 把 0x11 读到的 X 烧进 OTP (**不可逆, 只能烧一次**) +// 应答分两条 (ID 都是 0x278, DLC = 8): byte0 = 0x44, byte1/2 = 相关值, byte3 = 结果码 +// 第 1 条 (立即): byte1/2 = 0xFFFF, byte3 = 0 表示"已开始", 非 0 为拒绝启动的原因码 +// 第 2 条 (序列跑完后由 service_stack 主动上报): byte3 = 最终结果码, byte1/2 = 从 OTP 重建的零位 +// 结果码: 0=成功(仅第1条) 1=SPI 超时 2=烧前比对不符 3=PROG 轮询超时 4=烧后比对不符 +// 5=OTP 已烧过 6=前置条件不满足 7=上一次烧写还在进行中 +// +// ★ 关键: 这里**绝不阻塞等待**。整条序列要跑几十到几百毫秒, 把 CAN 线程占住会让 +// HAL_CAN 的错误码被置起来, 而 can_server_thread 只处理 NONE/TIMEOUT, 其余会掉进 +// 死循环 —— 那正是 CAN 彻底卡死的根源 (已在 odrive_can.cpp 里修掉)。 +void CANSimple::as5047_burn_otp_callback(const can_Message_t& msg) { + if (!as5047_has_cmd_tail(msg)) { + return; + } + + if (as5047_otp_pending) { + send_count_reply(0x44, 0xffffu, 7); // 上一次还没跑完 + return; + } + + // 前置条件: 先发过 0x11 拿到 X; 上电后没往 ZPOS 软写过 (否则无法预检是否已烧过); + // 且不在闭环/锁定里 —— OTP 序列期间位置输出会失效, 带载运行会失控 + if (as5047_cached_zero == 0xffffu || as5047_volatile_touched || + axes[0].current_state_ != Axis::AXIS_STATE_IDLE) { + send_count_reply(0x44, 0xffffu, 6); + return; + } + + axes[0].watchdog_feed(); + if (!axes[0].encoder_.as5047_burn_otp_start(as5047_cached_zero, 3000)) { + send_count_reply(0x44, 0xffffu, 1); + return; + } + as5047_otp_pending = true; + + send_count_reply(0x44, 0xffffu, 0); // 立即确认"已开始", 最终结果稍后单独上报 +} + +// 由 service_stack() 每轮 CAN 循环调用: OTP 序列跑完就主动上报结果 +void CANSimple::as5047_otp_report() { + if (!as5047_otp_pending) { + return; + } + + uint16_t out_zpos = 0xffffu; + uint8_t fail = 0xff; + if (!axes[0].encoder_.as5047_burn_otp_poll(&out_zpos, &fail)) { + return; // 还在跑 + } + + as5047_otp_pending = false; + + if (fail == 0) { + // 硬件零位已经永久变成 X, 电气 0 处计数归零, 故软件侧等效偏移必须为 0 + axes[0].encoder_.config_.phase_offset = 0; + axes[0].encoder_.config_.phase_offset_float = 0.5f; + axes[0].encoder_.config_.pre_calibrated = true; + as5047_volatile_touched = true; + } + + send_count_reply(0x44, out_zpos, fail); +} + // 0x277: 识别/校准命令, buf[1..7] 必须为 0x11..0x77 void CANSimple::identify_callback(const can_Message_t& msg) { if (msg.len < 8) { @@ -755,6 +1064,9 @@ void CANSimple::identify_callback(const can_Message_t& msg) { odrv.reboot(); } break; + case 6: + odrv.erase_configuration();//第一步需先擦掉配置,防止数据混乱,擦除后会复位 + break; default: break; } @@ -1084,11 +1396,16 @@ uint32_t CANSimple::service_stack() { // 0x274 心跳/抱闸状态 if ((budget > 0) && ((now - axes[0].can_.last_heartbeat) >= HEARTBEAT_PERIOD_MS)) { - //send_heartbeat(); +#ifndef DEBUG_USB + send_heartbeat(); +#endif axes[0].can_.last_heartbeat = now; budget--; } + // AS5047 OTP 序列跑完则主动上报结果 (非阻塞设计的另一半: 发起在 0x44 里, 结果在这里查) + as5047_otp_report(); + uint32_t next_service = remaining(axes[0].can_.last_encoder, VEL_ESTIMATE_PERIOD_MS); next_service = std::min(next_service, remaining(axes[0].can_.last_axis_error, AXIS_ERROR_PERIOD_MS)); next_service = std::min(next_service, remaining(axes[0].can_.last_motor_error, MOTOR_ERROR_PERIOD_MS)); @@ -1110,14 +1427,14 @@ typedef struct { __attribute__((section(".version_info"), used)) const VersionInfo version_info = { - .software_version = 0x0107, - .publish_data = 0x20261013, + .software_version = 0x0108, + .publish_data = 0x20261020, }; uint16_t get_hardware_version(void) { return *(volatile uint16_t*)(0x08000188); } - + uint32_t get_software_version(void) { return *(volatile uint32_t*)0x0800C188; } diff --git a/Firmware/communication/can/can_simple.hpp b/Firmware/communication/can/can_simple.hpp index 3ef1690dd..b54e0c75e 100644 --- a/Firmware/communication/can/can_simple.hpp +++ b/Firmware/communication/can/can_simple.hpp @@ -17,12 +17,13 @@ * 0x229 驾驶模式 1-舒适 2-运动 * 0x21B 抱闸控制 (并回读抱闸状态) * 0x21D 获取版本信息 - * 0x277 识别/校准命令 + * 0x277 识别/校准命令 (尾缀 11 22 33 44 55 66 77) + * 或 磁编 AS5047P 零位标定 (尾缀 01 02 03 04 05 06 07, 见 PHASE_LOCK_REPLY_MSG_ID) * 0x279 摇杆 ADC 值 (本地摇杆控车, 差速/原地转圈模型) * 0x701 OTA 复位 (bootloader 接管) * * 发送 (扩展帧): - * 0x217, 0x21A, 0x21C, 0x21D, 0x21E, 0x270~0x276 + * 0x217, 0x21A, 0x21C, 0x21D, 0x21E, 0x270~0x276, 0x278 (磁编零位标定应答) * * 说明: 原来 ODrive 标准的 "nodeID<<5 | cmdID" 命令解析 (do_command) 已停用, * 现在完全按上面的固定 CAN ID 分发, 见 can_simple.cpp 的 switch (msg.id)。 @@ -78,6 +79,24 @@ class CANSimple { void identify_callback(const can_Message_t& msg); // 0x277 void handle_ota_message(const can_Message_t& msg); // 0x701 + // ---- 磁编 AS5047P 零位标定 / 电角度 0 锁定 / OTP 烧写: ID 0x277 (扩展帧), DLC = 8 ---- + // byte0 = 0x00 : 读 AS5047 磁场强度与诊断信息, 应答 0x278 + // byte0 = 0x11 : 锁定到电角度 0 → 读计数 X → 设好软件侧对齐参数, 应答 0x278 + // byte0 = 0x22 : 退出锁定 (回到 IDLE, disarm), 无应答 + // byte0 = 0x33 : 把 0x11 读到的 X 软写进 ZPOSM/ZPOSL (易失), 再读回计数, 应答 0x278 + // byte0 = 0x44 : 把 0x11 读到的 X 烧进 OTP (不可逆, 只能烧一次), 应答 0x278 + // byte1~byte7 = 01 02 03 04 05 06 07 固定校验字 (与 identify_callback 的尾缀区分) + static constexpr uint32_t PHASE_LOCK_REPLY_MSG_ID = 0x278; + void phase_lock_zero_callback(const can_Message_t& msg); // 0x11 / 0x22 + void as5047_read_diag_callback(const can_Message_t& msg); // 0x00 + void as5047_zero_offset_callback(const can_Message_t& msg); // 0x33 + void as5047_burn_otp_callback(const can_Message_t& msg); // 0x44 + void as5047_otp_report(); // OTP 结果上报 (service_stack 调用) + // 0x278 应答: byte0 = 命令回显, byte1/2 = 14bit 数据 (byte1 高 8 位), byte3 = 结果码 + void send_count_reply(uint8_t cmd_echo, uint16_t count, uint8_t code = 0); + // 0x278 诊断应答: byte0 = 0x00, byte1/2 = 磁场强度, byte3 = 诊断状态位, byte4 = AGC + void send_diag_reply(uint8_t cmd_echo, uint16_t magnitude, uint8_t status, uint8_t agc); + // ---- 发送 ---- bool send_axis_error(Axis& axis, uint32_t msg_id); // 0x270/0x271 bool send_axis_motor_error(Axis& axis, uint32_t msg_id); // 0x272/0x273 diff --git a/Firmware/communication/can/odrive_can.cpp b/Firmware/communication/can/odrive_can.cpp index db6def57a..0474012c3 100644 --- a/Firmware/communication/can/odrive_can.cpp +++ b/Firmware/communication/can/odrive_can.cpp @@ -64,11 +64,17 @@ void ODriveCAN::can_server_thread() { // wait at least 1ms to prevent busy-spin on failed sends osSemaphoreWait(sem_can, std::max(next_service_time, 1UL)); - } else if (status == HAL_CAN_ERROR_TIMEOUT) { + } else { + // ★ 原来这里只判 HAL_CAN_ERROR_TIMEOUT,其它任何错误码(ACK 错/位错/格式错/ + // error-passive/bus-off 等)都落不到任何分支里 —— 于是这个 for(;;) 既不收也不发 + // 地空转死循环,而且永远出不来,外部表现就是 "CAN 彻底卡死收不到数据"。 + // HAL_CAN_ErrorCallback 是空的(复位那行被注释掉了),错误码一旦置上就没人清, + // 所以这里必须无条件复位并重新拉起 CAN,再让出 CPU 避免空转。 HAL_CAN_ResetError(handle_); status = HAL_CAN_Start(handle_); if (status == HAL_OK) - status = HAL_CAN_ActivateNotification(handle_, CAN_IT_RX_FIFO0_MSG_PENDING | CAN_IT_TX_MAILBOX_EMPTY); + status = HAL_CAN_ActivateNotification(handle_, CAN_IT_RX_FIFO0_MSG_PENDING | CAN_IT_RX_FIFO1_MSG_PENDING | CAN_IT_TX_MAILBOX_EMPTY); + osDelay(1); } } }