diff --git a/LICENSE b/LICENSE index 53dcc14..b9e37f9 100644 --- a/LICENSE +++ b/LICENSE @@ -1,6 +1,6 @@ MIT License -Copyright (c) 2021 Richard Unger +Copyright (c) 2025 Richard Unger Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal diff --git a/README.md b/README.md index d3ad0a6..e007990 100644 --- a/README.md +++ b/README.md @@ -10,20 +10,15 @@ The intent is to keep the core of SimpleFOC clean, and thus easy to maintain, un ## New Release -v1.0.9 - Released July 2025, for Simple FOC 2.3.5 or later +v1.1.0 - Released xxx 2026, for Simple FOC 2.3.x or later -What's changed since 1.0.8? -- FluxObserverSensor for sensorless FOC thanks to [@Candas1](https://github.com/Candas1) -- AS5600 fast mode support -- Improvements to SmoothingSensor and LinearHall thanks to [@dekutree64](https://github.com/dekutree64) -- Improvements to CalibratedSensor and HybridStepper thanks to [@askuric](https://github.com/askuric) -- AS5600 driver bugfix thanks to [@zbas](https://github.com/zbas) -- Calibrated sensor improvements thanks to [@Schnilz](https://github.com/Schnilz) -- ESP32HWEncoder bugfix thanks to [@AndBindStyle](https://github.com/AndBondStyle) -- STM32HWEncoder fix thanks to [@AntonEvmenenko](https://github.com/AntonEvmenenko) -- Bugfixes [included](https://github.com/simplefoc/Arduino-FOC-drivers/issues?q=milestone%3A1.0.9) - +What's changed since 1.0.9? +- Bugfixes [included](https://github.com/simplefoc/Arduino-FOC-drivers/issues?q=milestone%3A1.1.0) +- new SPI driver for MT6826S magnetic sensors +- new SPI driver for KTH78xx magnetic sensors +- new SPI driver for the DRV8376 integrated BLDC driver from TI +- new SPI driver for the DRV8320 / DRV8323 thanks to [@Moddingear](https://github.com/Moddingear) ## What is included @@ -35,7 +30,10 @@ Software to control gate driver ICs or integrated driver ICs which have advanced - [TMC6200 driver](src/drivers/tmc6200/) - SPI driver for Trinamics TMC6200 motor driver IC. - [DRV8316 driver](src/drivers/drv8316/) - SPI driver for TI's DRV8316 motor driver IC. + - [DRV8376 driver](src/drivers/drv8376/) - SPI driver for TI's DRV8376 motor driver IC. + - [DRV8320/DRV8323 driver](src/drivers/drv832x/) - SPI driver for TI's DRV832x motor driver ICs. - [STSPIN32G4 driver](src/drivers/stspin32g4/) - I2C and BLDCDriver for the STSPIN32G4 integrated gate driver MCU. + - [SimpleFOCNano driver](src/drivers/simplefocnano/) - BLDCDriver subclass preconfigured for SimpleFOCNano ### Encoders @@ -55,12 +53,20 @@ Drivers for various position sensor ICs. In many cases these hardware-specific d - [ESP32 Hardware Encoder](src/encoders/esp32hwencoder/) - ESP32 Hardware timer based encoder driver for ABI type quadrature encoders. - [SC60228 SPI driver](src/encoders/sc60228/) - SPI driver for SemiMent SC60288 magnetic encoder IC. - [MA330 SPI driver](src/encoders/ma330/) - SPI driver for the MPS MagAlpha MA330 absolute position magnetic rotary encoder IC. - - [MT6816 SPI driver](src/encoders/mt6816/) - SPI driver for the MagnTek MT6816 absolute position magnetic rotary encoder IC. - [MT6701 SSI driver](src/encoders/mt6701/) - SSI driver for the MagnTek MT6701 absolute position magnetic rotary encoder IC. + - [MT6816 SPI driver](src/encoders/mt6816/) - SPI driver for the MagnTek MT6816 absolute position magnetic rotary encoder IC. + - [MT6826S SPI driver](src/encoders/mt6826s/) - SPI driver for the MagnTek MT6826S absolute position magnetic rotary encoder IC. - [MT6835 SPI driver](src/encoders/mt6835/) - SPI driver for the MagnTek MT6835 21 bit magnetic rotary encoder IC. - [STM32 PWM sensor driver](src/encoders/stm32pwmsensor/) - STM32 native timer-based driver for PWM angle sensors. + - [KTH78xx SPI driver](src/encoders/kth78xx/) - SPI driver for the Conntek KTH78xx series absolute position magnetic rotary encoder ICs. + +### Special Encoders + - [CalibratedSensor](src/encoders/calibrated/) - A sensor which can calibrate for eccentricity on the magnet placement. - [SmoothingSensor](src/encoders/smoothing/) - A SimpleFOC Sensor wrapper implementation which adds angle extrapolation. + - [LinearHall](src/encoders/linearhall/) - A HallSensor with linear interpolation between ticks + - [FluxObserver](src/encoders/MXLEMMING_observer/) - A "sensorless sensor" implementing flux observer (requires current sensing) + - ### Communications @@ -85,7 +91,7 @@ Load and store SimpleFOC motor settings, based on register abstraction. Storing Drive different kinds of motors, or use alternate algorithms to SimpleFOC's default BLDCMotor and StepperMotor classes. - - [HybridStepperMotor](motors/HybridStepperMotor/) - Drive stepper motors with 3 phases. + - [HybridStepperMotor](https://github.com/simplefoc/Arduino-FOC/blob/master/src/HybridStepperMotor.h) - HybridStepper has moved to the main repostory. ### Utilities diff --git a/examples/comms/can/can_custom_regs/can_custom_regs.ino b/examples/comms/can/can_custom_regs/can_custom_regs.ino new file mode 100644 index 0000000..0ccb9eb --- /dev/null +++ b/examples/comms/can/can_custom_regs/can_custom_regs.ino @@ -0,0 +1,73 @@ +#include "Arduino.h" +#include "SimpleFOC.h" +#include "SimpleFOCDrivers.h" +#include "SimpleCANio.h" +#include "comms/can/CANCommander.h" + +// can pins +#define CAN_RX NC // define the RX pin +#define CAN_TX NC // define the TX pin +#define CAN_SHDN NC // define the Shhutdown pin (inverse of enable) if needed +#define CAN_ENABLE NC // define the ENABLE pin if needed +#define CAN_ID 1 + +// 3pwm pins +#define PHA PA6 +#define PHB PA7 +#define PHC PA8 + +BLDCMotor motor = BLDCMotor(7); // 7 pole pairs +BLDCDriver3PWM driver = BLDCDriver3PWM(PHA, PHB, PHC); + +//CANio can(PIN_CAN0_RX, PIN_CAN0_TX); // Create CAN object +CANio can(CAN_RX, CAN_TX, NC, CAN_ENABLE); // Create CAN object + +CANCommander commander(can, CAN_ID); +void setup() +{ + Serial.begin(115200); + SimpleFOCDebug::enable(&Serial); + + commander.init(); + commander.addMotor(&motor); + + // add custom register to control built-in LED + pinMode(LED_BUILTIN, OUTPUT); + commander.addCustomRegister( + 0xF0, // register id (should be > 0xE0) + 1, // size in bytes + [](RegisterIO& comms, FOCMotor* motor) -> bool { + // Read handler for custom register 0xF0 + uint8_t customValue = digitalRead(LED_BUILTIN); + comms << customValue; + return true; + }, + [](RegisterIO& comms, FOCMotor* motor) -> bool { + // Write handler for custom register 0xF0 + uint8_t receivedValue; + comms >> receivedValue; + digitalWrite(LED_BUILTIN, receivedValue ? HIGH : LOW); + return true; + } + ); + + delay(5000); + + motor.linkDriver(&driver); + driver.voltage_power_supply = 12; + driver.init(); + + motor.init(); + motor.initFOC(); + + Serial.println("Setup complete!"); + delay(10); +} + +void loop() +{ + motor.loopFOC(); + motor.move(); + + commander.run(); +} \ No newline at end of file diff --git a/examples/comms/can/can_example/can_example.ino b/examples/comms/can/can_example/can_example.ino new file mode 100644 index 0000000..8c5e581 --- /dev/null +++ b/examples/comms/can/can_example/can_example.ino @@ -0,0 +1,52 @@ +#include "Arduino.h" +#include "SimpleFOC.h" +#include "SimpleFOCDrivers.h" +#include "SimpleCANio.h" +#include "comms/can/CANCommander.h" + +// can pins +#define CAN_RX NC // define the RX pin +#define CAN_TX NC // define the TX pin +#define CAN_SHDN NC // define the Shhutdown pin (inverse of enable) if needed +#define CAN_ENABLE NC // define the ENABLE pin if needed +#define CAN_ID 1 + +// 3pwm pins +#define PHA PA6 +#define PHB PA7 +#define PHC PA8 + +BLDCMotor motor = BLDCMotor(7); // 7 pole pairs +BLDCDriver3PWM driver = BLDCDriver3PWM(PHA, PHB, PHC); + +//CANio can(PIN_CAN0_RX, PIN_CAN0_TX); // Create CAN object +CANio can(CAN_RX, CAN_TX, NC, CAN_ENABLE); // Create CAN object + +CANCommander commander(can, CAN_ID); +void setup() +{ + Serial.begin(115200); + SimpleFOCDebug::enable(&Serial); + + commander.init(); + commander.addMotor(&motor); + delay(5000); + + motor.linkDriver(&driver); + driver.voltage_power_supply = 12; + driver.init(); + + motor.init(); + motor.initFOC(); + + Serial.println("Setup complete!"); + delay(10); +} + +void loop() +{ + motor.loopFOC(); + motor.move(); + + commander.run(); +} \ No newline at end of file diff --git a/examples/encoders/calibrated_sensor/calibrated/calibrated.ino b/examples/encoders/calibrated_sensor/calibrated/calibrated.ino index 6e4e303..16f6668 100644 --- a/examples/encoders/calibrated_sensor/calibrated/calibrated.ino +++ b/examples/encoders/calibrated_sensor/calibrated/calibrated.ino @@ -13,6 +13,7 @@ BLDCDriver3PWM driver = BLDCDriver3PWM(PB4,PC7,PB10,PA9); // instantiate the calibrated sensor object // argument 1 - sensor object // argument 2 - number of samples in the LUT (default 200) +// argument 3 - pointer to LUT array (if null, LUT will be filled only during calibration) CalibratedSensor sensor_calibrated = CalibratedSensor(sensor); // voltage set point variable @@ -57,6 +58,8 @@ void setup() { // Running calibration // it will ouptut the LUT and the zero electrical angle to the serial monitor !!!! sensor_calibrated.calibrate(motor); + // print the LUT to serial monitor + sensor_calibrated.printLUT(motor, Serial); //Serial.println("Calibrating Sensor Done."); // Linking sensor to motor object diff --git a/examples/encoders/calibrated_sensor/calibration_save/calibration_save.ino b/examples/encoders/calibrated_sensor/calibration_save/calibration_save.ino index da49b73..1798cc4 100644 --- a/examples/encoders/calibrated_sensor/calibration_save/calibration_save.ino +++ b/examples/encoders/calibrated_sensor/calibration_save/calibration_save.ino @@ -4,19 +4,18 @@ * * 1. Run this Sketch as is and wait for the calibration data to be generated and printed over Serial. * 2. Then copy the output from Serial to calibrationLut, zero_electric_angle and sensor_direction - * 3. Change values_provided to true and run the Sketch again to see the motor skipping the calibration. + * 3. Run the same example again and the code will use the provided calibration data and skip the calibration procedure. */ #include #include #include "encoders/calibrated/CalibratedSensor.h" -// fill this array with the calibration values outputed by the calibration procedure -float calibrationLut[50] = {0}; +// Replace these with the calibration data obtained from the first run +uint16_t calibrationLut[50] = {0}; float zero_electric_angle = 0; Direction sensor_direction = Direction::UNKNOWN; -const bool values_provided = false; // magnetic sensor instance - SPI MagneticSensorSPI sensor = MagneticSensorSPI(AS5147_SPI, 14); @@ -29,7 +28,10 @@ StepperDriver4PWM driver = StepperDriver4PWM(10, 9, 5, 6,8); // argument 2 - number of samples in the LUT (default 200) // argument 3 - pointer to the LUT array (defualt nullptr) CalibratedSensor sensor_calibrated = CalibratedSensor( - sensor, sizeof(calibrationLut) / sizeof(calibrationLut[0])); + sensor, + sizeof(calibrationLut)/sizeof(calibrationLut[0]), // number of samples + sensor_direction==Direction::UNKNOWN ? nullptr : calibrationLut // pointer to LUT array +); // voltage set point variable float target_voltage = 2; @@ -64,12 +66,17 @@ void setup() { // initialize motor motor.init(); - if(values_provided) { + if(sensor_direction != Direction::UNKNOWN){ + Serial.println(F("Using saved calibration data.")); + // provide the saved zero angle and direction motor.zero_electric_angle = zero_electric_angle; motor.sensor_direction = sensor_direction; } else { // Running calibration + Serial.println(F("Running calibration...")); sensor_calibrated.calibrate(motor); + // print the LUT to serial monitor + sensor_calibrated.printLUT(motor, Serial); } // Linking sensor to motor object diff --git a/library.properties b/library.properties index f3234cb..38e9695 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=SimpleFOCDrivers -version=1.0.9 +version=1.1.0 author=Simplefoc maintainer=Simplefoc sentence=A library of supporting drivers for SimpleFOC. Motor drivers chips, encoder chips, current sensing and supporting code. diff --git a/src/comms/README.md b/src/comms/README.md index 30fb84d..ce7df38 100644 --- a/src/comms/README.md +++ b/src/comms/README.md @@ -29,6 +29,9 @@ Implementations are available for either ASCII based protocol (TextIO) or binary [SimpleFOCRegisters.h](./SimpleFOCRegisters.h) contains a list of registers known to SimpleFOC. These registers can be read and/or written and code is provided to serialize/deserialize them. +**📋 Complete register reference**: See [REGISTERS.md](./REGISTERS.md) for the full table with access types, data types, and sizes. + The SimpleFOC packet based IO (PacketCommander, Telemetry), I2CCommander and SettingsStorage as well as our Python API [PySimpleFOC](https://github.com/simplefoc/pysimplefoc) are all based on this register abstraction, and therefore share the same register names/ids. -If implementing your own communications protocol, we encourage you to base it on the Register abstraction if appropriate. This will provide you with ready to use code to access/write the register values, and will make your solution easier to use due to the shared common conventions. \ No newline at end of file +If implementing your own communications protocol, we encourage you to base it on the Register abstraction if appropriate. This will provide you with ready to use code to access/write the register values, and will make your solution easier to use due to the shared common conventions. + diff --git a/src/comms/REGISTERS.md b/src/comms/REGISTERS.md new file mode 100644 index 0000000..03f0aed --- /dev/null +++ b/src/comms/REGISTERS.md @@ -0,0 +1,119 @@ +# SimpleFOC CAN Registers Reference + +This document summarizes the registers defined in `SimpleFOCRegisters.h`, their access type (RO/RW/WO), data type, and size in bytes. Sizes reflect payload bytes in a single CAN frame. + +Note: Multi-value registers fit in one frame (≤ 8 bytes) only where indicated; larger compound values are still sent as single frames using compact encoding as implemented by `SimpleFOCRegisters`. + +## Legend +- RO: Read-only +- RW: Read/Write +- WO: Write-only +- u8: uint8_t (1 byte) +- u32: uint32_t (4 bytes) +- f32: float (4 bytes) +- 3x f32: three floats (12 bytes, may be sent across multiple frames by higher-level tooling) + +## Registers + +| ID | Name | Access | Type | Size | +|----|------|--------|------|------| +| 0x00 | REG_STATUS | RO | u8×4 (motor, last_reg, last_err, status) | 4 | +| 0x01 | REG_TARGET | RW | f32 | 4 | +| 0x03 | REG_ENABLE_ALL | WO | u8 | 1 | +| 0x04 | REG_ENABLE | RW | u8 | 1 | +| 0x05 | REG_CONTROL_MODE | RW | u8 | 1 | +| 0x06 | REG_TORQUE_MODE | RW | u8 | 1 | +| 0x07 | REG_MODULATION_MODE | RW | u8 | 1 | +| 0x09 | REG_ANGLE | RO | f32 | 4 | +| 0x10 | REG_POSITION | RO | u32 + f32 (turns + rad) | 8 | +| 0x11 | REG_VELOCITY | RO | f32 | 4 | +| 0x12 | REG_SENSOR_ANGLE | RO | f32 | 4 | +| 0x13 | REG_SENSOR_MECHANICAL_ANGLE | RO | f32 | 4 | +| 0x14 | REG_SENSOR_VELOCITY | RO | f32 | 4 | +| 0x15 | REG_SENSOR_TIMESTAMP | RO | u32 | 4 | +| 0x16 | REG_PHASE_VOLTAGE | RW | 3x f32 | 12 | +| 0x17 | REG_PHASE_STATE | RW | u8×3 | 3 | +| 0x18 | REG_DRIVER_ENABLE | RW | u8 | 1 | +| 0x1A | REG_TELEMETRY_REG | RW | u8 + n×(u8+u8) | variable | +| 0x1B | REG_TELEMETRY_CTRL | RW | u8 | 1 | +| 0x1C | REG_TELEMETRY_DOWNSAMPLE | RW | u32 | 4 | +| 0x1D | REG_ITERATIONS_SEC | RO | u32 | 4 | +| 0x20 | REG_VOLTAGE_Q | RO | f32 | 4 | +| 0x21 | REG_VOLTAGE_D | RO | f32 | 4 | +| 0x22 | REG_CURRENT_Q | RO | f32 | 4 | +| 0x23 | REG_CURRENT_D | RO | f32 | 4 | +| 0x24 | REG_CURRENT_A | RO | f32 | 4 | +| 0x25 | REG_CURRENT_B | RO | f32 | 4 | +| 0x26 | REG_CURRENT_C | RO | f32 | 4 | +| 0x27 | REG_CURRENT_ABC | RO | 3x f32 | 12 | +| 0x28 | REG_CURRENT_DC | RO | f32 | 4 | +| 0x30 | REG_VEL_PID_P | RW | f32 | 4 | +| 0x31 | REG_VEL_PID_I | RW | f32 | 4 | +| 0x32 | REG_VEL_PID_D | RW | f32 | 4 | +| 0x33 | REG_VEL_PID_LIM | RW | f32 | 4 | +| 0x34 | REG_VEL_PID_RAMP | RW | f32 | 4 | +| 0x35 | REG_VEL_LPF_T | RW | f32 | 4 | +| 0x36 | REG_ANG_PID_P | RW | f32 | 4 | +| 0x37 | REG_ANG_PID_I | RW | f32 | 4 | +| 0x38 | REG_ANG_PID_D | RW | f32 | 4 | +| 0x39 | REG_ANG_PID_LIM | RW | f32 | 4 | +| 0x3A | REG_ANG_PID_RAMP | RW | f32 | 4 | +| 0x3B | REG_ANG_LPF_T | RW | f32 | 4 | +| 0x40 | REG_CURQ_PID_P | RW | f32 | 4 | +| 0x41 | REG_CURQ_PID_I | RW | f32 | 4 | +| 0x42 | REG_CURQ_PID_D | RW | f32 | 4 | +| 0x43 | REG_CURQ_PID_LIM | RW | f32 | 4 | +| 0x44 | REG_CURQ_PID_RAMP | RW | f32 | 4 | +| 0x45 | REG_CURQ_LPF_T | RW | f32 | 4 | +| 0x46 | REG_CURD_PID_P | RW | f32 | 4 | +| 0x47 | REG_CURD_PID_I | RW | f32 | 4 | +| 0x48 | REG_CURD_PID_D | RW | f32 | 4 | +| 0x49 | REG_CURD_PID_LIM | RW | f32 | 4 | +| 0x4A | REG_CURD_PID_RAMP | RW | f32 | 4 | +| 0x4B | REG_CURD_LPF_T | RW | f32 | 4 | +| 0x50 | REG_VOLTAGE_LIMIT | RW | f32 | 4 | +| 0x51 | REG_CURRENT_LIMIT | RW | f32 | 4 | +| 0x52 | REG_VELOCITY_LIMIT | RW | f32 | 4 | +| 0x53 | REG_DRIVER_VOLTAGE_LIMIT | RW | f32 | 4 | +| 0x54 | REG_PWM_FREQUENCY | RW | u32 | 4 | +| 0x55 | REG_DRIVER_VOLTAGE_PSU | RW | f32 | 4 | +| 0x56 | REG_VOLTAGE_SENSOR_ALIGN | RW | f32 | 4 | +| 0x5F | REG_MOTION_DOWNSAMPLE | RW | u32 | 4 | +| 0x60 | REG_ZERO_ELECTRIC_ANGLE | RO | f32 | 4 | +| 0x61 | REG_SENSOR_DIRECTION | RO | u8 | 1 | +| 0x62 | REG_ZERO_OFFSET | RW | f32 | 4 | +| 0x63 | REG_POLE_PAIRS | RO | u32 | 4 | +| 0x64 | REG_PHASE_RESISTANCE | RW | f32 | 4 | +| 0x65 | REG_KV | RW | f32 | 4 | +| 0x66 | REG_INDUCTANCE | RW | f32 | 4 | +| 0x67 | REG_CURA_GAIN | RW | f32 | 4 | +| 0x68 | REG_CURB_GAIN | RW | f32 | 4 | +| 0x69 | REG_CURC_GAIN | RW | f32 | 4 | +| 0x6A | REG_CURA_OFFSET | RW | f32 | 4 | +| 0x6B | REG_CURB_OFFSET | RW | f32 | 4 | +| 0x6C | REG_CURC_OFFSET | RW | f32 | 4 | +| 0x70 | REG_NUM_MOTORS | RO | u8 | 1 | +| 0x71 | REG_SYS_TIME | RO | u32 | 4 | +| 0x7F | REG_MOTOR_ADDRESS | RW | f32 | 4 | + +## Custom Registers +- Range starts at `REG_CUSTOM_START (0xE0)` and supports up to `MAX_CUSTOM_REGISTERS` (32). +- Register handlers provide size and read/write callbacks: + +```cpp +bool addCustomRegister(uint8_t reg, uint8_t size, + RegisterReadHandler readHandler, + RegisterWriteHandler writeHandler); +``` + +Example: +```cpp +bool myRead(RegisterIO& io, FOCMotor* m) { io << (float)m->shaft_velocity; return true; } +bool myWrite(RegisterIO& io, FOCMotor* m) { float v; io >> v; m->target = v; return true; } +commander.addCustomRegister(0xE0, 4, myRead, myWrite); +``` + +## Notes +- Floats and integers are transmitted as raw little-endian binary. +- Multi-component values (e.g., 3x floats) may exceed 8 bytes; higher layers should split or compress if needed. +- The version constant `SIMPLEFOC_REGISTERS_VERSION` changes only on incompatible schema updates. diff --git a/src/comms/SimpleFOCRegisters.cpp b/src/comms/SimpleFOCRegisters.cpp index c91feeb..be9866b 100644 --- a/src/comms/SimpleFOCRegisters.cpp +++ b/src/comms/SimpleFOCRegisters.cpp @@ -1,6 +1,7 @@ #include "./SimpleFOCRegisters.h" #include "BLDCMotor.h" +#include "communication/SimpleFOCDebug.h" #include "./telemetry/Telemetry.h" @@ -322,8 +323,14 @@ bool SimpleFOCRegisters::registerToComms(RegisterIO& comms, uint8_t reg, FOCMoto case SimpleFOCRegister::REG_NUM_MOTORS: case SimpleFOCRegister::REG_MOTOR_ADDRESS: case SimpleFOCRegister::REG_ENABLE_ALL: - default: return false; + default: + // check the custom registers + for (uint8_t i=0; ireg_id) { + return customRegisters[i]->readHandler(comms, motor); + } + } } return true; }; @@ -591,9 +598,17 @@ bool SimpleFOCRegisters::commsToRegister(RegisterIO& comms, uint8_t reg, FOCMoto case SimpleFOCRegister::REG_NUM_MOTORS: case SimpleFOCRegister::REG_MOTOR_ADDRESS: case SimpleFOCRegister::REG_ENABLE_ALL: - default: return false; + default: + // check the custom registers + for (uint8_t i=0; ireg_id) { + return customRegisters[i]->writeHandler(comms, motor); + } + } } + // if we reach here, the register is not found + SIMPLEFOC_DEBUG("Write to unknown register: ", reg); return false; }; @@ -691,10 +706,43 @@ uint8_t SimpleFOCRegisters::sizeOfRegister(uint8_t reg){ return 1; case SimpleFOCRegister::REG_DRIVER_ENABLE: case SimpleFOCRegister::REG_ENABLE_ALL: // write-only - default: // unknown register or write only register (no output) or can't handle in superclass + return 0; + default: + // register not in the predefined regs, check the custom ones + for (uint8_t i=0; ireg_id == reg) { + return customRegisters[i]->size; + } + } + // if the code commes here the register is either + // unknown register or write only register (no output) or can't handle in superclass + SIMPLEFOC_DEBUG("Size of unknown register requested: ", reg); return 0; } }; +bool SimpleFOCRegisters::addCustomRegister(uint8_t reg, uint8_t size, RegisterReadHandler readHandler, RegisterWriteHandler writeHandler) { + // check if we have space + if (customRegisterCount >= MAX_CUSTOM_REGISTERS) { + SIMPLEFOC_DEBUG("Custom register count exceeded maximum (32): ", reg); + return false; + } + // check if the register has valid id + if (reg < REG_CUSTOM_START) { + SIMPLEFOC_DEBUG("Custom register ID invalid (should be >= 0xE0): ", reg); + return false; + } + // check if the register is already registered + for (uint8_t i=0; ireg_id == reg) { + SIMPLEFOC_DEBUG("Custom register ID already registered: ", reg); + return false; + } + } + // add the custom register + customRegisters[customRegisterCount++] = new CustomRegisterHandler{reg, size, readHandler, writeHandler}; + return true; +} + SimpleFOCRegisters* SimpleFOCRegisters::regs = new SimpleFOCRegisters(); diff --git a/src/comms/SimpleFOCRegisters.h b/src/comms/SimpleFOCRegisters.h index 7def8e1..3932059 100644 --- a/src/comms/SimpleFOCRegisters.h +++ b/src/comms/SimpleFOCRegisters.h @@ -10,7 +10,8 @@ // does not change the version, but removing or changing the meaning of existing registers does, or changing the number of an existing register. #define SIMPLEFOC_REGISTERS_VERSION 0x01 - +#define MAX_CUSTOM_REGISTERS 32 +#define REG_CUSTOM_START 0xE0 typedef enum : uint8_t { REG_STATUS = 0x00, // RO - 1 byte (motor status) @@ -104,6 +105,18 @@ typedef enum : uint8_t { } SimpleFOCRegister; +// Custom register handlers +typedef bool (*RegisterReadHandler)(RegisterIO& comms, FOCMotor* motor); +typedef bool (*RegisterWriteHandler)(RegisterIO& comms, FOCMotor* motor); +// Custom register handler structure +struct CustomRegisterHandler { + uint8_t reg_id; + uint8_t size; + RegisterReadHandler readHandler; + RegisterWriteHandler writeHandler; +}; + + class SimpleFOCRegisters { public: SimpleFOCRegisters(); @@ -113,4 +126,10 @@ class SimpleFOCRegisters { virtual bool commsToRegister(RegisterIO& comms, uint8_t reg, FOCMotor* motor); static SimpleFOCRegisters* regs; + + // Method to add custom register handlers + bool addCustomRegister(uint8_t reg, uint8_t size, RegisterReadHandler readHandler, RegisterWriteHandler writeHandler); + protected: + CustomRegisterHandler* customRegisters[MAX_CUSTOM_REGISTERS] = {nullptr}; + uint8_t customRegisterCount = 0; }; diff --git a/src/comms/can/CANCommander.cpp b/src/comms/can/CANCommander.cpp new file mode 100644 index 0000000..3bec371 --- /dev/null +++ b/src/comms/can/CANCommander.cpp @@ -0,0 +1,196 @@ +#if has_SimpleCANio +#include "CANCommander.h" + +CANCommander::CANCommander(HardwareCAN& can, uint8_t addr, bool echo_enabled, int baudrate, bool no_filter) + : _can(&can), address(addr), echo(echo_enabled), baudrate(baudrate), no_filter(no_filter) { +} + +CANCommander::~CANCommander() { +} + +void CANCommander::init() { + + if(no_filter) { + // Accept all messages (promiscuous mode) + _can->filter(CanFilter(MASK_ACCEPT_ALL)); + }else{ + // Filter to accept only our address (broadcast filtering handled in software) + // Mask the address bits [28:21] - the most significant 8 bits + uint32_t address_mask = 0xFF << CAN_ADDRESS_SHIFT; // Mask bits [28:21] (8 bits for address) + uint32_t our_address_filter = (uint32_t)address << CAN_ADDRESS_SHIFT; + + // Use MASK_EXTENDED for 29-bit extended CAN IDs + // This will only accept messages where the address bits match + _can->filter(CanFilter(MASK_EXTENDED, our_address_filter, address_mask, FILTER_ANY_FRAME)); + } + _can->begin(baudrate); // 1 Mbps CAN speed +} + +void CANCommander::addMotor(FOCMotor* motor) { + if (numMotors < CANCOMMANDER_MAX_MOTORS) + motors[numMotors++] = motor; +} + +void CANCommander::run() { + // Process incoming CAN messages + if (_can->available() > 0) { + CanMsg msg = _can->read(); + if (!msg.isEmpty()) { + handleCANMessage(msg); + } + } +} + +void CANCommander::handleCANMessage(CanMsg& msg) { + // Parse CAN ID to extract fields (address is now in MSBs) + uint8_t msg_addr = (msg.id >> CAN_ADDRESS_SHIFT) & 0xFF; + uint8_t packet_type = (msg.id >> CAN_PACKET_TYPE_SHIFT) & 0xF; + uint8_t reg = (msg.id >> CAN_REGISTER_SHIFT) & 0xFF; + uint8_t motor_idx = (msg.id >> CAN_MOTOR_INDEX_SHIFT) & 0xF; + + // Check if message is for us (hardware filter should handle this, but double-check) + if (msg_addr != address && msg_addr != 0xFF) // 0xFF = broadcast + return; + + lastcommanderror = commanderror; + lastcommandregister = curRegister; + commanderror = false; + curRegister = reg; + + // Use motor index from CAN ID if provided, otherwise keep current + if (motor_idx < numMotors) { + curMotor = motor_idx; + } + + // Copy data to rx_buffer + rx_available = msg.data_length; + rx_pos = 0; + memcpy(rx_buffer, msg.data, msg.data_length); + + switch (packet_type) { + case CAN_READ_REQUEST: + // Read register and send response + sendRegisterResponse(reg, msg_addr); + break; + + case CAN_WRITE_REQUEST: + // Write to register + if (commsToRegister(reg)) { + if (echo) { + // Echo back the value + sendRegisterResponse(reg, msg_addr); + } + } else { + commanderror = true; + } + break; + + case CAN_SYNC: + // Sync request - respond with sync ack + tx_pos = 0; + tx_buffer[tx_pos++] = 0x01; // Sync ack + uint32_t sync_id = (address << CAN_ADDRESS_SHIFT) | + (CAN_SYNC << CAN_PACKET_TYPE_SHIFT); + CanMsg sync_msg = CanMsg(CanExtendedId(sync_id), tx_pos, tx_buffer); + _can->write(sync_msg); + break; + } +} + +void CANCommander::sendRegisterResponse(uint8_t reg, uint8_t dest_addr) { + // Prepare transmit buffer + tx_pos = 0; + + // Write register data to buffer + if (registerToComms(reg)) { + // Build CAN ID for response (address in MSBs) + uint32_t can_id = (address << CAN_ADDRESS_SHIFT) | + (CAN_READ_RESPONSE << CAN_PACKET_TYPE_SHIFT) | + (reg << CAN_REGISTER_SHIFT) | + (curMotor << CAN_MOTOR_INDEX_SHIFT); + + // Send response + CanMsg msg = CanMsg(CanExtendedId(can_id), tx_pos, tx_buffer); + _can->write(msg); + } +} + +bool CANCommander::commsToRegister(uint8_t reg) { + return SimpleFOCRegisters::regs->commsToRegister(*this, reg, motors[curMotor]); +} + +bool CANCommander::registerToComms(uint8_t reg) { + switch (reg) { + case SimpleFOCRegister::REG_STATUS: + *this << (uint8_t)curMotor; + *this << (uint8_t)lastcommandregister; + *this << (uint8_t)(lastcommanderror ? 1 : 0); + if (curMotor < numMotors) + *this << (uint8_t)motors[curMotor]->motor_status; + return true; + case SimpleFOCRegister::REG_NUM_MOTORS: + *this << numMotors; + return true; + default: + if (curMotor >= numMotors) { + commanderror = true; + return false; + } + return SimpleFOCRegisters::regs->registerToComms(*this, reg, motors[curMotor]); + } +} + +// RegisterIO interface implementation +RegisterIO& CANCommander::operator<<(uint8_t value) { + if (tx_pos < 8) { + tx_buffer[tx_pos++] = value; + } + return *this; +} + +RegisterIO& CANCommander::operator<<(uint32_t value) { + if (tx_pos + 4 <= 8) { + memcpy(&tx_buffer[tx_pos], &value, 4); + tx_pos += 4; + } + return *this; +} + +RegisterIO& CANCommander::operator<<(float value) { + if (tx_pos + 4 <= 8) { + memcpy(&tx_buffer[tx_pos], &value, 4); + tx_pos += 4; + } + return *this; +} + +RegisterIO& CANCommander::operator>>(uint8_t& value) { + if (rx_pos < rx_available) { + value = rx_buffer[rx_pos++]; + } else { + commanderror = true; + } + return *this; +} + +RegisterIO& CANCommander::operator>>(uint32_t& value) { + if (rx_pos + 4 <= rx_available) { + memcpy(&value, &rx_buffer[rx_pos], 4); + rx_pos += 4; + } else { + commanderror = true; + } + return *this; +} + +RegisterIO& CANCommander::operator>>(float& value) { + if (rx_pos + 4 <= rx_available) { + memcpy(&value, &rx_buffer[rx_pos], 4); + rx_pos += 4; + } else { + commanderror = true; + } + return *this; +} + +#endif \ No newline at end of file diff --git a/src/comms/can/CANCommander.h b/src/comms/can/CANCommander.h new file mode 100644 index 0000000..e634731 --- /dev/null +++ b/src/comms/can/CANCommander.h @@ -0,0 +1,92 @@ +#ifndef CANCOMMANDER_H +#define CANCOMMANDER_H +#if __cplusplus >= 201703L //Check for C++17 or later + #if __has_include("SimpleCANio.h") //C++17 required for __has_include + #define has_SimpleCANio 1 + #endif +#endif + +#ifdef has_SimpleCANio +#include "SimpleFOC.h" +#include "../SimpleFOCRegisters.h" +#include "../RegisterIO.h" +#include "SimpleCANio.h" + +#if !defined(CANCOMMANDER_MAX_MOTORS) +#define CANCOMMANDER_MAX_MOTORS 16 +#endif + +// CAN packet structure for register-based protocol +// CAN ID bits allocation (Optimized for hardware filtering): +// [27:20] - Node Address (8 bits) - MOST SIGNIFICANT for efficient filtering +// [19:16] - Packet Type (4 bits) +// [15:8] - Register number (8 bits) - see SimpleFOCRegisters for the register numbers +// [7:0] - Motor Index (8 bits) - Selects motor 0-256 on the node + +#define CAN_ADDRESS_SHIFT 20 // Address in MSBs for efficient filtering +#define CAN_PACKET_TYPE_SHIFT 16 // Packet type after address +#define CAN_REGISTER_SHIFT 8 // Register number +#define CAN_MOTOR_INDEX_SHIFT 0 // Motor selection (0-256) + +enum CANPacketType : uint8_t { + CAN_READ_REQUEST = 0x1, + CAN_WRITE_REQUEST = 0x2, + CAN_READ_RESPONSE = 0x3, + CAN_SYNC = 0xF +}; + +class CANCommander : public RegisterIO +{ +public: + CANCommander(HardwareCAN& can, uint8_t address = 0, bool echo = false, int baudrate = 1000000, bool no_filter = false); + virtual ~CANCommander(); + + void addMotor(FOCMotor* motor); + virtual void init(); + virtual void run(); + + // RegisterIO interface implementation + RegisterIO& operator<<(float value) override; + RegisterIO& operator<<(uint32_t value) override; + RegisterIO& operator<<(uint8_t value) override; + RegisterIO& operator>>(float& value) override; + RegisterIO& operator>>(uint32_t& value) override; + RegisterIO& operator>>(uint8_t& value) override; + + bool echo = false; + uint8_t address = 0; + int baudrate = 1000000; + bool no_filter = false; + + bool addCustomRegister(uint8_t reg, uint8_t size, + RegisterReadHandler readHandler, + RegisterWriteHandler writeHandler) { + return SimpleFOCRegisters::regs->addCustomRegister(reg, size, readHandler, writeHandler); + } + +protected: + virtual bool commsToRegister(uint8_t reg); + virtual bool registerToComms(uint8_t reg); + virtual void handleCANMessage(CanMsg& msg); + virtual void sendRegisterResponse(uint8_t reg, uint8_t dest_addr); + + HardwareCAN* _can; + FOCMotor* motors[CANCOMMANDER_MAX_MOTORS]; + uint8_t numMotors = 0; + uint8_t curMotor = 0; + uint8_t curRegister = REG_STATUS; + + bool commanderror = false; + bool lastcommanderror = false; + uint8_t lastcommandregister = REG_STATUS; + + // Transmit/receive buffers + uint8_t tx_buffer[8]; + uint8_t tx_pos = 0; + uint8_t rx_buffer[8]; + uint8_t rx_pos = 0; + uint8_t rx_available = 0; +}; + +#endif +#endif \ No newline at end of file diff --git a/src/comms/can/README.md b/src/comms/can/README.md new file mode 100644 index 0000000..418ade0 --- /dev/null +++ b/src/comms/can/README.md @@ -0,0 +1,164 @@ +# CAN Communication Protocol + +This document describes the CAN-based communication protocol for SimpleFOC motor control using the CANCommander class. + +## Overview + +The protocol uses **29-bit Extended CAN IDs** with a register-based architecture for efficient motor control and monitoring. The CAN ID structure is optimized for hardware filtering by placing the most significant bits (address) at the top. + +## CAN ID Structure + +The 29-bit CAN Extended ID is divided into the following fields: + +``` +Bit [28] - Not used +Bits [27:20] - Node Address (8 bits) - MSBs for efficient hardware filtering +Bits [19:16] - Packet Type (4 bits) - Request/Response type +Bits [15:8] - Register Number (8 bits) - Target register (see register table) +Bits [7:0] - Motor Index (8 bits) - Motor selection (0-255) +``` + +### Bit Shifts (Defined in CANCommander.h) + +```cpp +#define CAN_ADDRESS_SHIFT 20 // Address in MSBs for efficient filtering +#define CAN_PACKET_TYPE_SHIFT 16 // Packet type after address +#define CAN_REGISTER_SHIFT 8 // Register number +#define CAN_MOTOR_INDEX_SHIFT 0 // Motor selection (0-256) +``` + +## Packet Types + +The protocol defines the following packet types: + +| Value | Name | Description | +|-------|-------------------|--------------------------------------------------| +| 0x1 | CAN_READ_REQUEST | Request to read a register value | +| 0x2 | CAN_WRITE_REQUEST | Request to write a value to a register | +| 0x3 | CAN_READ_RESPONSE | Response containing register data | +| 0xF | CAN_SYNC | Synchronization message | + +## Protocol Operation + +### Reading a Register + +1. **Master** sends a `CAN_READ_REQUEST` with the target register number +2. **Node** responds with a `CAN_READ_RESPONSE` containing the register data + +### Writing to a Register + +1. **Master** sends a `CAN_WRITE_REQUEST` with register number and data payload +2. **Node** processes the write request +3. If echo is enabled, **Node** sends a `CAN_READ_RESPONSE` with the updated value + +### Addressing + +- **Unicast**: Direct message to a specific node address (0x00 - 0xFE) +- **Broadcast**: Address 0xFF sends to all nodes on the bus +- Hardware filtering is configured to accept only messages for the node's address or broadcast + +## Register Map + +The registers are defined in `SimpleFOCRegisters.h`. + +## Frame Diagram (Extended ID + Data) + +``` +CAN Extended ID (29 bits) +┌────────┬──────────────┬──────────────┬──────────────┬─────────────────┐ +│ Bit 28 │ 27 ........20│ 19 ....... 16│ 15 ........ 8│ 7 ........ 0 | +│ (Rsv) │ Address (8) │ Type (4) │ Register (8) │ Motor index (8) | +└────────┴──────────────┴──────────────┴──────────────┴─────────────────┘ +Data (0..8 bytes) +┌─────────────────────────────────────────────────────┐ +│ Payload (register-specific encoding: byte/float/u32)│ +└─────────────────────────────────────────────────────┘ +``` + +Small payloads (status, single byte flags) use only first bytes. Multi-byte values (float, uint32_t) are little-endian raw copies. + +## Implemented Registers (Summary) + +Selected register groups (see header for full list): + +| Group | Examples | +|-------|----------| +| Status & Control | `REG_STATUS`, `REG_ENABLE`, `REG_CONTROL_MODE`, `REG_TORQUE_MODE` | +| Motion Targets | `REG_TARGET` (position/velocity/torque depending on mode) | +| Sensor Feedback | `REG_ANGLE`, `REG_VELOCITY`, `REG_SENSOR_ANGLE`, `REG_SENSOR_VELOCITY` | +| Phase / Driver | `REG_PHASE_VOLTAGE`, `REG_PHASE_STATE`, `REG_DRIVER_ENABLE` | +| FOC Internal | `REG_VOLTAGE_Q/D`, `REG_CURRENT_Q/D`, `REG_CURRENT_ABC`, `REG_CURRENT_DC` | +| PID Velocity | `REG_VEL_PID_P/I/D/LIM/RAMP`, `REG_VEL_LPF_T` | +| PID Position | `REG_ANG_PID_P/I/D/LIM/RAMP`, `REG_ANG_LPF_T` | +| PID Current Q/D | `REG_CURQ_PID_*`, `REG_CURD_PID_*`, `REG_CURQ_LPF_T`, `REG_CURD_LPF_T` | +| Limits | `REG_VOLTAGE_LIMIT`, `REG_CURRENT_LIMIT`, `REG_VELOCITY_LIMIT`, `REG_PWM_FREQUENCY` | +| Motor Params | `REG_PHASE_RESISTANCE`, `REG_KV`, `REG_INDUCTANCE`, current gains & offsets | +| System Info | `REG_NUM_MOTORS`, `REG_SYS_TIME` | +| Custom Range | `REG_CUSTOM_START (0xE0)` .. +32 via registration | + +Registers are read-only (RO), write-only (WO) or read/write (R/W) as indicated in the header. + +**📋 Complete register reference**: See [REGISTERS.md](../REGISTERS.md) for the full table with access types, data types, and sizes. + +## Adding a Custom Register + +1. Pick an ID >= `REG_CUSTOM_START` (0xE0) +2. Define handlers: +```cpp +bool myRead(RegisterIO& io, FOCMotor* m) { io << (float)m->shaft_velocity; return true; } +bool myWrite(RegisterIO& io, FOCMotor* m) { float v; io >> v; m->target = v; return true; } +``` +3. Register with the commander (after `init()`): +```cpp +commander.addCustomRegister(0xE0, 4, myRead, myWrite); // size in bytes +``` +4. Access via normal CAN read/write frames using that register number. + +## CANCommander Core Features + +| Feature | Description | +|---------|-------------| +| Multi-motor addressing | Up to 16 motors per node (`motor_idx` field) | +| Register abstraction | Uniform access to control, telemetry, tuning parameters | +| Broadcast & Unicast | Address 0xFF = broadcast; per-node filtering in hardware | +| Echo-on-write | Optional read-back response for writes (`echo` flag) | +| Sync packets | Lightweight synchronization (`CAN_SYNC`) | +| Compact frames | Up to 2 floats per frame (8 data bytes) | +| Custom extension | User-defined registers without modifying core tables | +| Error tracking | Status register includes last error & register accessed | + +## Typical Exchange + +| Action | ID Fields | Data | +|--------|-----------|------| +| Read register | addr, READ_REQUEST, reg, motor | (empty) | +| Response | addr, READ_RESPONSE, reg, motor | encoded value | +| Write register | addr, WRITE_REQUEST, reg, motor | new value bytes | +| Optional echo | addr, READ_RESPONSE, reg, motor | echoed value | +| Sync | addr/broadcast, SYNC | optional ack byte | + +## Filtering +Nodes may enable strict filtering (default) or promiscuous (`no_filter=true`). Filtering masks only the address bits; broadcast (0xFF) frames pass via software check. + +## Notes +- All numeric types are raw binary (IEEE754 float, uint32_t little-endian). +- Multi-frame transfers are not used; split larger conceptual sets into separate reads. +- Keep custom register sizes ≤8 bytes for single-frame responses. +- Version tag: `SIMPLEFOC_REGISTERS_VERSION` changes only on incompatible register layout updates. + +## Minimal Usage Snippet +```cpp +CANio can(rxPin, txPin); +CANCommander commander(can, 0x12); // address 0x12 +commander.init(); +commander.addMotor(&motor); +// in loop: +commander.run(); +``` + +Add custom: +```cpp +commander.addCustomRegister(0xE1, 4, myRead, myWrite); +``` + +Refer to `SimpleFOCRegisters.h` for the authoritative register list. diff --git a/src/comms/streams/BinaryIOUnbuffered.cpp b/src/comms/streams/BinaryIOUnbuffered.cpp new file mode 100644 index 0000000..0fd3547 --- /dev/null +++ b/src/comms/streams/BinaryIOUnbuffered.cpp @@ -0,0 +1,100 @@ +// #include "./BinaryIOUnbuffered.h" + +// BinaryIOUnbuffered::BinaryIOUnbuffered(Stream& io) : _io(io) { +// // nothing to do here +// }; + +// BinaryIOUnbuffered::~BinaryIOUnbuffered(){ +// // nothing to do here +// }; + + +// BinaryIOUnbuffered& BinaryIOUnbuffered::operator<<(float value) { +// _io.write((uint8_t*)&value, 4); +// return *this; +// }; + + +// BinaryIOUnbuffered& BinaryIOUnbuffered::operator<<(uint32_t value) { +// _io.write((uint8_t*)&value, 4); +// return *this; +// }; + + +// BinaryIOUnbuffered& BinaryIOUnbuffered::operator<<(uint8_t value) { +// _io.write(value); +// return *this; +// }; + + +// BinaryIOUnbuffered& BinaryIOUnbuffered::operator<<(char value) { +// _io.write((uint8_t)value); +// return *this; +// }; + + +// BinaryIOUnbuffered& BinaryIOUnbuffered::operator<<(Packet value) { +// if (value.type!=0x00) { +// _io.write(MARKER_BYTE); +// _io.write(value.payload_size+1); +// _io.write(value.type); +// } +// return *this; +// }; + + +// BinaryIOUnbuffered& BinaryIOUnbuffered::operator<<(Separator value) { +// // separator is ignored in binary mode +// return *this; +// }; + + +// // Immediate flush - no buffering, so nothing to do +// void BinaryIOUnbuffered::_flush() { +// // No internal buffer to flush +// }; + + +// BinaryIOUnbuffered& BinaryIOUnbuffered::operator>>(float &value) { +// remaining -= _io.readBytes((uint8_t*)&value, 4); +// return *this; +// }; + + +// BinaryIOUnbuffered& BinaryIOUnbuffered::operator>>(uint32_t &value) { +// remaining -= _io.readBytes((uint8_t*)&value, 4); +// return *this; +// }; + + +// BinaryIOUnbuffered& BinaryIOUnbuffered::operator>>(uint8_t &value) { +// value = (uint8_t)_io.read(); +// remaining--; +// return *this; +// }; + + +// PacketIO& BinaryIOUnbuffered::operator>>(Packet& value) { +// while (!in_sync && _io.available() > 0) { +// if (_io.peek() == MARKER_BYTE) +// in_sync = true; +// else +// _io.read(); +// } +// if (_io.peek() == MARKER_BYTE) { +// _io.read(); // discard the marker +// } +// if (!in_sync || _io.available() < 3) { // size, frame type, payload = 3 bytes minimum frame size +// value.type = 0x00; +// value.payload_size = 0; +// return *this; +// } +// value.payload_size = (uint8_t)_io.read() - 1; +// value.type = (uint8_t)_io.read(); +// remaining = value.payload_size; +// return *this; +// }; + +// bool BinaryIOUnbuffered::is_complete() { +// return (remaining <= 0); +// }; diff --git a/src/comms/streams/BinaryIOUnbuffered.h b/src/comms/streams/BinaryIOUnbuffered.h new file mode 100644 index 0000000..5ad55a4 --- /dev/null +++ b/src/comms/streams/BinaryIOUnbuffered.h @@ -0,0 +1,34 @@ +// #pragma once + +// #include "../RegisterIO.h" +// #include +// #include "Arduino.h" + + +// #define MARKER_BYTE 0xA5 + +// // Unbuffered Binary IO - writes data immediately without buffering +// // Useful for real-time applications where latency is critical +// // Trade-off: More serial writes (potentially slower overall) but lower latency + +// class BinaryIOUnbuffered : public PacketIO { +// public: +// BinaryIOUnbuffered(Stream& io); +// virtual ~BinaryIOUnbuffered(); +// BinaryIOUnbuffered& operator<<(float value) override; +// BinaryIOUnbuffered& operator<<(uint32_t value) override; +// BinaryIOUnbuffered& operator<<(uint8_t value) override; +// BinaryIOUnbuffered& operator<<(char value) override; +// BinaryIOUnbuffered& operator<<(Packet value) override; +// BinaryIOUnbuffered& operator<<(Separator value) override; +// BinaryIOUnbuffered& operator>>(float& value) override; +// BinaryIOUnbuffered& operator>>(uint32_t& value) override; +// BinaryIOUnbuffered& operator>>(uint8_t& value) override; +// PacketIO& operator>>(Packet& value) override; +// bool is_complete() override; +// virtual void _flush() override; + +// protected: +// Stream& _io; +// uint8_t remaining = 0; +// }; diff --git a/src/current_sense/phoque/Phoque_CurrentSense.cpp b/src/current_sense/phoque/Phoque_CurrentSense.cpp new file mode 100644 index 0000000..91d4fe0 --- /dev/null +++ b/src/current_sense/phoque/Phoque_CurrentSense.cpp @@ -0,0 +1,322 @@ +#if defined(ARDUINO_PHOQUE) + +#include "Phoque_CurrentSense.hpp" +#include +#include + +static constexpr float adc_voltage = 3.3f; +static constexpr float adc_resolution = 4095.f; + +static ADC_HandleTypeDef hadc1; +static ADC_HandleTypeDef hadc2; + +static DMA_HandleTypeDef hdma_adc1; +static DMA_HandleTypeDef hdma_adc2; + +Phoque_CurrentSense::Phoque_CurrentSense(float _shunt_resistor, float _gain, bool _read_bemf) + :read_bemf(_read_bemf) +{ + gain_c = gain_b = gain_a = 1.0f /_shunt_resistor / _gain; + skip_align = true; +} + +Phoque_CurrentSense::Phoque_CurrentSense(float _mVpA, bool _read_bemf) + :read_bemf(_read_bemf) +{ + gain_c = gain_b = gain_a = 1000.f/_mVpA; + skip_align = true; +} + +Phoque_CurrentSense::~Phoque_CurrentSense() +{ + if (initialized) + { + delete[] adc1_buffer; + delete[] adc2_buffer; + } + +} + +void Phoque_CurrentSense::GPIO_Init() +{ + __HAL_RCC_GPIOC_CLK_ENABLE(); + __HAL_RCC_GPIOF_CLK_ENABLE(); + __HAL_RCC_GPIOA_CLK_ENABLE(); + __HAL_RCC_GPIOB_CLK_ENABLE(); + + __HAL_RCC_ADC12_CLK_ENABLE(); +} + +void Phoque_CurrentSense::DMA_Init() +{ + /* DMA controller clock enable */ + __HAL_RCC_DMAMUX1_CLK_ENABLE(); + __HAL_RCC_DMA1_CLK_ENABLE(); + __HAL_RCC_DMA2_CLK_ENABLE(); + + /* DMA interrupt init */ + /* DMA1_Channel1_IRQn interrupt configuration */ + HAL_NVIC_SetPriority(DMA1_Channel1_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(DMA1_Channel1_IRQn); + /* DMA1_Channel2_IRQn interrupt configuration */ + HAL_NVIC_SetPriority(DMA1_Channel2_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(DMA1_Channel2_IRQn); + + // Enable external clock for ADC + RCC_PeriphCLKInitTypeDef PeriphClkInit; + PeriphClkInit.PeriphClockSelection = RCC_PERIPHCLK_ADC12; + PeriphClkInit.Adc12ClockSelection = RCC_ADC12CLKSOURCE_PLL; + HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit); +} + +void Phoque_CurrentSense::OPAMP_Init() +{ + //do nothing +} + +int Phoque_CurrentSense::ADC1_Init(ADC_HandleTypeDef* hadc1) +{ + ADC_MultiModeTypeDef multimode = {0}; + hadc1->Instance = ADC1; + hadc1->Init.ClockPrescaler = ADC_CLOCK_ASYNC_DIV8; + hadc1->Init.Resolution = ADC_RESOLUTION_12B; + hadc1->Init.DataAlign = ADC_DATAALIGN_RIGHT; + hadc1->Init.GainCompensation = 0; + hadc1->Init.ScanConvMode = ADC_SCAN_ENABLE; + hadc1->Init.EOCSelection = ADC_EOC_SINGLE_CONV; + hadc1->Init.LowPowerAutoWait = DISABLE; + hadc1->Init.ContinuousConvMode = DISABLE; + + hadc1->Init.DiscontinuousConvMode = DISABLE; + hadc1->Init.ExternalTrigConv = ADC_EXTERNALTRIG_T1_TRGO; + hadc1->Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_RISING; + hadc1->Init.DMAContinuousRequests = ENABLE; + hadc1->Init.Overrun = ADC_OVR_DATA_PRESERVED; + + if (HAL_ADC_Init(hadc1) != HAL_OK) + { + SIMPLEFOC_DEBUG("HAL_ADC_Init failed!"); + } + + /** Configure the ADC multi-mode + */ + multimode.Mode = ADC_MODE_INDEPENDENT; + if (HAL_ADCEx_MultiModeConfigChannel(hadc1, &multimode) != HAL_OK) + { + SIMPLEFOC_DEBUG("HAL_ADCEx_MultiModeConfigChannel 1 failed!"); + } + + return 0; +} + +int Phoque_CurrentSense::ADC2_Init(ADC_HandleTypeDef* hadc2) +{ + hadc2->Instance = ADC2; + hadc2->Init.ClockPrescaler = ADC_CLOCK_ASYNC_DIV8; + hadc2->Init.Resolution = ADC_RESOLUTION_12B; + hadc2->Init.DataAlign = ADC_DATAALIGN_RIGHT; + hadc2->Init.GainCompensation = 0; + hadc2->Init.ScanConvMode = ADC_SCAN_ENABLE; + hadc2->Init.EOCSelection = ADC_EOC_SINGLE_CONV; + hadc2->Init.LowPowerAutoWait = DISABLE; + hadc2->Init.ContinuousConvMode = DISABLE; + + hadc2->Init.DiscontinuousConvMode = DISABLE; + hadc2->Init.ExternalTrigConv = ADC_EXTERNALTRIG_T1_TRGO; + hadc2->Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_RISING; + hadc2->Init.DMAContinuousRequests = ENABLE; + hadc2->Init.Overrun = ADC_OVR_DATA_PRESERVED; + + if (HAL_ADC_Init(hadc2) != HAL_OK) + { + SIMPLEFOC_DEBUG("HAL_ADC_Init failed!"); + } + + return 0; +} + +void Phoque_CurrentSense::DMA1_Init(ADC_HandleTypeDef *hadc, DMA_HandleTypeDef *hdma_adc, DMA_Channel_TypeDef* channel, uint32_t request) +{ + hdma_adc->Instance = channel; + hdma_adc->Init.Request = request; + hdma_adc->Init.Direction = DMA_PERIPH_TO_MEMORY; + hdma_adc->Init.PeriphInc = DMA_PINC_DISABLE; + hdma_adc->Init.MemInc = DMA_MINC_ENABLE; + hdma_adc->Init.PeriphDataAlignment = DMA_PDATAALIGN_HALFWORD; + hdma_adc->Init.MemDataAlignment = DMA_MDATAALIGN_HALFWORD; + hdma_adc->Init.Mode = DMA_CIRCULAR; + hdma_adc->Init.Priority = DMA_PRIORITY_LOW; + HAL_DMA_DeInit(hdma_adc); + if (HAL_DMA_Init(hdma_adc) != HAL_OK) + { + SIMPLEFOC_DEBUG("HAL_DMA_Init failed!"); + } + __HAL_LINKDMA(hadc, DMA_Handle, *hdma_adc); +} + +void* Phoque_CurrentSense::SyncLowSide(void* _driver_params, void* _cs_params) +{ + STM32DriverParams* driver_params = (STM32DriverParams*)_driver_params; + Stm32CurrentSenseParams* cs_params = (Stm32CurrentSenseParams*)_cs_params; + + // stop all the timers for the driver + stm32_pause(driver_params); + + //Timer has repetition counter, so this argument is not needed, hence we can let it be garbage + Stm32AdcInterruptConfig adc_int_config; + + bool tim_interrupt = _initTimerInterruptDownsampling(cs_params, driver_params, adc_int_config); + if(tim_interrupt) { + // error in the timer interrupt initialization + SIMPLEFOC_DEBUG("STM32-CS: timer has no repetition counter, ADC interrupt not supported for this Phoque"); + return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; + } + + // set the trigger output event + LL_TIM_SetTriggerOutput(cs_params->timer_handle->Instance, LL_TIM_TRGO_UPDATE); + + // restart all the timers of the driver + stm32_resume(driver_params); + + // return the cs parameters + // successfully initialized + // TODO verify if success in future + return _cs_params; +} + +void* Phoque_CurrentSense::ConfigureADC(const void* driver_params, const int pinA,const int pinB,const int pinC) +{ + _UNUSED(driver_params); + + HAL_Init(); + GPIO_Init(); + DMA_Init(); + hadc1.Init.NbrOfConversion = 0; + hadc2.Init.NbrOfConversion = 0; + int adc1_len = ADC1_Init(&hadc1); + int adc2_len = ADC2_Init(&hadc2); + adc1_buffer = new uint16_t[adc1_len]; + adc2_buffer = new uint16_t[adc2_len]; + + HAL_ADCEx_Calibration_Start(&hadc1,ADC_SINGLE_ENDED); + HAL_ADCEx_Calibration_Start(&hadc2,ADC_SINGLE_ENDED); + + DMA1_Init(&hadc1, &hdma_adc1, DMA1_Channel1, DMA_REQUEST_ADC1); + DMA1_Init(&hadc2, &hdma_adc2, DMA1_Channel2, DMA_REQUEST_ADC2); + + if (HAL_ADC_Start_DMA(&hadc1, (uint32_t*)adc1_buffer, adc1_len) != HAL_OK) + { + SIMPLEFOC_DEBUG("ADC1 DMA read init failed"); + } + if (HAL_ADC_Start_DMA(&hadc2, (uint32_t*)adc2_buffer, adc2_len) != HAL_OK) + { + SIMPLEFOC_DEBUG("ADC2 DMA read init failed"); + } + + Stm32CurrentSenseParams* params = new Stm32CurrentSenseParams { + .pins = { pinA, pinB, pinC }, + .adc_voltage_conv = (adc_voltage) / (adc_resolution), + .timer_handle = ((STM32DriverParams*)driver_params)->timers_handle[0], + }; + + return params; +} + +float Phoque_CurrentSense::readMillivolt(const int pin) +{ + return readRaw(pin) * ((Stm32CurrentSenseParams*)params)->adc_voltage_conv; +} + +PhaseCurrent_s Phoque_CurrentSense::getPhaseCurrents(){ + PhaseCurrent_s current; + current.a = (readMillivolt(pinA) - offset_ia)*gain_a; // amps + current.b = (readMillivolt(pinB) - offset_ib)*gain_b; // amps + current.c = (readMillivolt(pinC) - offset_ic)*gain_c; // amps + return current; +} + +int Phoque_CurrentSense::init() +{ + if (driver==nullptr) { + SIMPLEFOC_DEBUG("CUR: Driver not linked!"); + return 0; + } + if (initialized) + { + return 1; + } + + // configure ADC variables + params = ConfigureADC(driver->params,pinA,pinB,pinC); + // if init failed return fail + if (params == SIMPLEFOC_CURRENT_SENSE_INIT_FAILED) return 0; + // sync the driver + _delay(10); + void* r = SyncLowSide(driver->params, params); + if(r == SIMPLEFOC_CURRENT_SENSE_INIT_FAILED) return 0; + // set the center pwm (0 voltage vector) + if(driver_type==DriverType::BLDC) + { + BLDCDriver* driver_bldc = static_cast(driver); + driver_bldc->setPwm(driver->voltage_limit/2, driver->voltage_limit/2, driver->voltage_limit/2); + } + + // calibrate zero offsets + _delay(10); + calibrateOffsets(); + // set zero voltage to all phases + if(driver_type==DriverType::BLDC) + static_cast(driver)->setPwm(0,0,0); + // set the initialized flag + initialized = (params!=SIMPLEFOC_CURRENT_SENSE_INIT_FAILED); + // return success + return 1; +} + +void Phoque_CurrentSense::calibrateOffsets(){ + const int calibration_rounds = 2000; + + // find adc offset = zero current voltage + uint32_t accA = 0, accB = 0, accC = 0; + // read the adc voltage 1000 times ( arbitrary number ) + for (int i = 0; i < calibration_rounds; i++) { + const uint16_t curru(readRaw(pinA)), currv(readRaw(pinB)), currw(readRaw(pinC)); + //check if new data + if (curru >= 0x1000 || currv >= 0x1000 || currw >= 0x1000) + { + //no new data: this iteration didn't happen + i--; + //wait for interrupt (dma irq should do it) + __WFI(); + continue; + } + //invalidate current data + adc2_buffer[0] = adc2_buffer[1] = adc1_buffer[0] = 0xffff; + accA += curru; + accB += currv; + accC += currw; + } + // calculate the mean offsets + offset_ia = accA * ((Stm32CurrentSenseParams*)params)->adc_voltage_conv / calibration_rounds; + offset_ib = accB * ((Stm32CurrentSenseParams*)params)->adc_voltage_conv / calibration_rounds; + offset_ic = accC * ((Stm32CurrentSenseParams*)params)->adc_voltage_conv / calibration_rounds; +} + +int Phoque_CurrentSense::driverAlign(float align_voltage, bool modulation_centered) +{ + _UNUSED(align_voltage); + _UNUSED(modulation_centered); + return 0; +} + +extern "C" { +void DMA1_Channel1_IRQHandler(void) { + HAL_DMA_IRQHandler(&hdma_adc1); +} + +void DMA1_Channel2_IRQHandler(void) { + HAL_DMA_IRQHandler(&hdma_adc2); +} +} + +#endif \ No newline at end of file diff --git a/src/current_sense/phoque/Phoque_CurrentSense.hpp b/src/current_sense/phoque/Phoque_CurrentSense.hpp new file mode 100644 index 0000000..504d296 --- /dev/null +++ b/src/current_sense/phoque/Phoque_CurrentSense.hpp @@ -0,0 +1,41 @@ +#pragma once + +#if defined(ARDUINO_PHOQUE) + +#include "common/base_classes/CurrentSense.h" + +class Phoque_CurrentSense : public CurrentSense +{ +protected: + volatile uint16_t *adc1_buffer = nullptr; + volatile uint16_t *adc2_buffer = nullptr; + bool read_bemf; +public: + Phoque_CurrentSense(float shunt_resistor, float gain, bool read_bemf=false); + Phoque_CurrentSense(float mVpA, bool read_bemf=false); + virtual ~Phoque_CurrentSense(); + + virtual int init() override; + virtual PhaseCurrent_s getPhaseCurrents() override; + virtual int driverAlign(float align_voltage, bool modulation_centered = false) override; + + virtual float readMillivolt(const int pin); + virtual uint16_t readRaw(const int pin) = 0; + +protected: + void GPIO_Init(); + void DMA_Init(); + virtual void OPAMP_Init(); + //Configure ADC1, child classes should configure each rank to sample, return number of samples + virtual int ADC1_Init(ADC_HandleTypeDef* hadc1); + //Configure ADC2, child classes should configure each rank to sample, return number of samples + virtual int ADC2_Init(ADC_HandleTypeDef* hadc2); + virtual void DMA1_Init(ADC_HandleTypeDef *hadc, DMA_HandleTypeDef *hdma_adc, DMA_Channel_TypeDef* channel, uint32_t request); + virtual void* SyncLowSide(void* driver_params, void* cs_params); + virtual void* ConfigureADC(const void* driver_params, const int pinA, const int pinB, const int pinC); + + virtual void calibrateOffsets(); +}; + +#endif + diff --git a/src/current_sense/phoque2a/Phoque2a_CurrentSense.cpp b/src/current_sense/phoque2a/Phoque2a_CurrentSense.cpp new file mode 100644 index 0000000..b7fbe80 --- /dev/null +++ b/src/current_sense/phoque2a/Phoque2a_CurrentSense.cpp @@ -0,0 +1,205 @@ +#if defined(ARDUINO_PHOQUE2a) + +#include "Phoque2a_CurrentSense.hpp" +#include "communication/SimpleFOCDebug.h" +#include "current_sense/hardware_specific/stm32/stm32_adc_utils.h" + + + +Phoque2a_CurrentSense::Phoque2a_CurrentSense(float _shunt_resistor, float _gain, bool _read_bemf) + :Phoque_CurrentSense(_shunt_resistor, _gain, _read_bemf) +{ + pinA = A_CURRU; + pinB = A_CURRV; + pinC = A_CURRW; +} + +Phoque2a_CurrentSense::Phoque2a_CurrentSense(float mVpA, bool _read_bemf) + :Phoque_CurrentSense(mVpA, _read_bemf) +{ + pinA = A_CURRU; + pinB = A_CURRV; + pinC = A_CURRW; +} + +Phoque2a_CurrentSense::~Phoque2a_CurrentSense() +{ +} + +int Phoque2a_CurrentSense::ADC1_Init(ADC_HandleTypeDef* hadc1) +{ + ADC_ChannelConfTypeDef sConfig = {0}; + + hadc1->Init.NbrOfConversion += 3 + read_bemf * 2; + + Phoque_CurrentSense::ADC1_Init(hadc1); + + /** Configure Regular Channel (PA1 / phase U current) + */ + sConfig.Channel = _getADCChannel(analogInputToPinName(A_CURRU), ADC1); //ADC_CHANNEL_2; + sConfig.Rank = ADC_REGULAR_RANK_1; + sConfig.SamplingTime = ADC_SAMPLETIME_6CYCLES_5; + sConfig.SingleDiff = ADC_SINGLE_ENDED; + sConfig.OffsetNumber = ADC_OFFSET_NONE; + sConfig.Offset = 0; + if (HAL_ADC_ConfigChannel(hadc1, &sConfig) != HAL_OK) + { + SIMPLEFOC_DEBUG("HAL_ADC_ConfigChannel failed!"); + } + + if (read_bemf) + { + /* Configure Regular Channel (PB0 / BEMFV / Phase V) + */ + sConfig.Channel = _getADCChannel(analogInputToPinName(A_BEMFV), ADC1); //ADC_CHANNEL_15; + sConfig.Rank = ADC_REGULAR_RANK_2; + sConfig.SamplingTime = ADC_SAMPLETIME_6CYCLES_5; + sConfig.SingleDiff = ADC_SINGLE_ENDED; + sConfig.OffsetNumber = ADC_OFFSET_NONE; + sConfig.Offset = 0; + if (HAL_ADC_ConfigChannel(hadc1, &sConfig) != HAL_OK) + { + SIMPLEFOC_DEBUG("HAL_ADC_ConfigChannel failed!"); + } + + /* Configure Regular Channel (PB1 / BEMFW / Phase W) + */ + sConfig.Channel = _getADCChannel(analogInputToPinName(A_BEMFW), ADC1); //ADC_CHANNEL_12; + sConfig.Rank = ADC_REGULAR_RANK_3; + sConfig.SamplingTime = ADC_SAMPLETIME_6CYCLES_5; + sConfig.SingleDiff = ADC_SINGLE_ENDED; + sConfig.OffsetNumber = ADC_OFFSET_NONE; + sConfig.Offset = 0; + if (HAL_ADC_ConfigChannel(hadc1, &sConfig) != HAL_OK) + { + SIMPLEFOC_DEBUG("HAL_ADC_ConfigChannel failed!"); + } + } + + //****************************************************************** + // Aux analog readings + /* Configure Regular Channel (PB12, mosfet temperature) + */ + sConfig.Channel = _getADCChannel(analogInputToPinName(A_TEMPERATURE), ADC1); //ADC_CHANNEL_11; + sConfig.Rank = read_bemf ? ADC_REGULAR_RANK_4 : ADC_REGULAR_RANK_2; + sConfig.SamplingTime = ADC_SAMPLETIME_47CYCLES_5; + sConfig.SingleDiff = ADC_SINGLE_ENDED; + sConfig.OffsetNumber = ADC_OFFSET_NONE; + sConfig.Offset = 0; + if (HAL_ADC_ConfigChannel(hadc1, &sConfig) != HAL_OK) + { + SIMPLEFOC_DEBUG("HAL_ADC_ConfigChannel failed!"); + } + + /** Configure Regular Channel (PA3 / Bus voltage monitor) + */ + sConfig.Channel = _getADCChannel(analogInputToPinName(A_VBUS), ADC1); //ADC_CHANNEL_4; + sConfig.Rank = read_bemf ? ADC_REGULAR_RANK_5 : ADC_REGULAR_RANK_3; + sConfig.SamplingTime = ADC_SAMPLETIME_47CYCLES_5; + sConfig.SingleDiff = ADC_SINGLE_ENDED; + sConfig.OffsetNumber = ADC_OFFSET_NONE; + sConfig.Offset = 0; + if (HAL_ADC_ConfigChannel(hadc1, &sConfig) != HAL_OK) + { + SIMPLEFOC_DEBUG("HAL_ADC_ConfigChannel failed!"); + } + return hadc1->Init.NbrOfConversion; +} + +int Phoque2a_CurrentSense::ADC2_Init(ADC_HandleTypeDef* hadc2) +{ + ADC_ChannelConfTypeDef sConfig = {0}; + + hadc2->Init.NbrOfConversion += 3 + read_bemf; + + Phoque_CurrentSense::ADC2_Init(hadc2); + + /** Configure Regular Channel (PC4 / phase V current) + */ + sConfig.Channel = _getADCChannel(analogInputToPinName(A_CURRV), ADC2); //ADC_CHANNEL_5; + sConfig.Rank = ADC_REGULAR_RANK_1; + sConfig.SamplingTime = ADC_SAMPLETIME_6CYCLES_5; + sConfig.SingleDiff = ADC_SINGLE_ENDED; + sConfig.OffsetNumber = ADC_OFFSET_NONE; + sConfig.Offset = 0; + if (HAL_ADC_ConfigChannel(hadc2, &sConfig) != HAL_OK) + { + SIMPLEFOC_DEBUG("HAL_ADC_ConfigChannel failed!"); + } + /** Configure Regular Channel (PB2 / phase W current) + */ + sConfig.Channel = _getADCChannel(analogInputToPinName(A_CURRW), ADC2); //ADC_CHANNEL_12; + sConfig.Rank = ADC_REGULAR_RANK_2; + sConfig.SamplingTime = ADC_SAMPLETIME_6CYCLES_5; + sConfig.SingleDiff = ADC_SINGLE_ENDED; + sConfig.OffsetNumber = ADC_OFFSET_NONE; + sConfig.Offset = 0; + if (HAL_ADC_ConfigChannel(hadc2, &sConfig) != HAL_OK) + { + SIMPLEFOC_DEBUG("HAL_ADC_ConfigChannel failed!"); + } + + if(read_bemf) + { + /** Configure Regular Channel (PA0 / BEMFU / Phase U) + */ + sConfig.Channel = _getADCChannel(analogInputToPinName(A_BEMFU), ADC2); //ADC_CHANNEL_1; + sConfig.Rank = ADC_REGULAR_RANK_3; + sConfig.SamplingTime = ADC_SAMPLETIME_6CYCLES_5; + sConfig.SingleDiff = ADC_SINGLE_ENDED; + sConfig.OffsetNumber = ADC_OFFSET_NONE; + sConfig.Offset = 0; + if (HAL_ADC_ConfigChannel(hadc2, &sConfig) != HAL_OK) + { + SIMPLEFOC_DEBUG("HAL_ADC_ConfigChannel failed!"); + } + } + + /** Configure Regular Channel (PA4 / Potentiometer) + */ + sConfig.Channel = _getADCChannel(analogInputToPinName(A_POTENTIOMETER), ADC2); //ADC_CHANNEL_17; + sConfig.Rank = read_bemf ? ADC_REGULAR_RANK_4 : ADC_REGULAR_RANK_3; + sConfig.SamplingTime = ADC_SAMPLETIME_47CYCLES_5; + sConfig.SingleDiff = ADC_SINGLE_ENDED; + sConfig.OffsetNumber = ADC_OFFSET_NONE; + sConfig.Offset = 0; + if (HAL_ADC_ConfigChannel(hadc2, &sConfig) != HAL_OK) + { + SIMPLEFOC_DEBUG("HAL_ADC_ConfigChannel failed!"); + } + return hadc2->Init.NbrOfConversion; +} + +uint16_t Phoque2a_CurrentSense::readRaw(const int pin) +{ + switch (pin) + { + case A_CURRU: + case -1: + return adc1_buffer[0]; + case A_CURRV: + case -2: + return adc2_buffer[0]; + case A_CURRW: + case -3: + return adc2_buffer[1]; + + case A_BEMFU: + return adc2_buffer[2]; + case A_BEMFV: + return adc1_buffer[1]; + case A_BEMFW: + return adc1_buffer[2]; + + case A_POTENTIOMETER: + return adc2_buffer[2+read_bemf]; + case A_TEMPERATURE: + return adc1_buffer[1+read_bemf*2]; + case A_VBUS: + return adc1_buffer[2+read_bemf*2]; + default: + return 0; + } +} + +#endif \ No newline at end of file diff --git a/src/current_sense/phoque2a/Phoque2a_CurrentSense.hpp b/src/current_sense/phoque2a/Phoque2a_CurrentSense.hpp new file mode 100644 index 0000000..99d538e --- /dev/null +++ b/src/current_sense/phoque2a/Phoque2a_CurrentSense.hpp @@ -0,0 +1,27 @@ +#pragma once + +#if defined(ARDUINO_PHOQUE2a) + +#include + +class Phoque2a_CurrentSense : public Phoque_CurrentSense +{ + //Current sense for the Phoque2 board + //for best performance, center modulation should be off +private: + volatile uint16_t *adc1_buffer = nullptr; + volatile uint16_t *adc2_buffer = nullptr; + bool read_bemf; +public: + Phoque2a_CurrentSense(float shunt_resistor, float gain, bool read_bemf=false); + Phoque2a_CurrentSense(float mVpA, bool read_bemf=false); + virtual ~Phoque2a_CurrentSense(); + + virtual uint16_t readRaw(const int pin) override; + +private: + virtual int ADC1_Init(ADC_HandleTypeDef* hadc1) override; + virtual int ADC2_Init(ADC_HandleTypeDef* hadc2) override; +}; + +#endif \ No newline at end of file diff --git a/src/drivers/drv832x/drv832x.cpp b/src/drivers/drv832x/drv832x.cpp new file mode 100644 index 0000000..ac77555 --- /dev/null +++ b/src/drivers/drv832x/drv832x.cpp @@ -0,0 +1,194 @@ + + +#include "./drv832x.h" + +#define BIT_READ_MASK 0x07FF + + +void DRV832xDriver3PWM::init(SPIClass* _spi) { + DRV832xDriver::init(_spi); + setRegistersLocked(false); + delayMicroseconds(1); + DRV832xDriver::setPWMMode(DRV832x_PWMMode::PWM3_Mode); + BLDCDriver3PWM::init(); +}; + + +void DRV832xDriver6PWM::init(SPIClass* _spi) { + DRV832xDriver::init(_spi); + setRegistersLocked(false); + delayMicroseconds(1); + DRV832xDriver::setPWMMode(DRV832x_PWMMode::PWM6_Mode); // default mode is 6-PWM + BLDCDriver6PWM::init(); +}; + +void DRV832xDriver::init(SPIClass* _spi) { + spi = _spi; + + //setup pins + pinMode(cs, OUTPUT); + digitalWrite(cs, HIGH); // switch off + + //SPI has an internal SPI-device counter, it is possible to call "begin()" from different devices + spi->begin(); + + if (_isset(nFault)) { + pinMode(nFault, INPUT); + // TODO add interrupt handler on the nFault pin if configured + // add configuration for how to handle faults... idea: interrupt handler calls a callback, depending on the type of fault + // consider what would be a useful configuration in practice? What do we want to do on a fault, e.g. over-temperature for example? + + //attachInterrupt(digitalPinToInterrupt(nFault), handleInterrupt, PinStatus::FALLING); + } + /*for (size_t i = 0; i < 7; i++) + { + auto read = readSPI(i); + Serial.printf("DRV8323 Register %d = %#x\n", i, read); + }*/ +}; + + +uint16_t DRV832xDriver::readSPI(uint8_t addr) { + digitalWrite(cs, 0); + spi->beginTransaction(settings); + addr &= 0xf; + uint16_t data = (addr<<11)|0x8000; + uint16_t result = spi->transfer16(data); + spi->endTransaction(); + digitalWrite(cs, 1); + //Serial.printf("SPI read result: @%#x -> %#x\n", data, result); + return result; +} + + +uint16_t DRV832xDriver::writeSPI(uint8_t addr, uint16_t value) { + digitalWrite(cs, 0); + spi->beginTransaction(settings); + addr &= 0xf; + value &= BIT_READ_MASK; + uint16_t data = (addr<<11)|value; + uint16_t result = spi->transfer16(data); + spi->endTransaction(); + digitalWrite(cs, 1); + //Serial.printf("SPI write result: @%#x -> %#x\n", data, result); + return result; +} + +#define def_getset(name, type, address, register_type, register_name) \ +type DRV832xDriver::get##name() {\ + register_type data;\ + data.reg = readSPI(address) & BIT_READ_MASK;\ + return static_cast(data.register_name);\ +}\ +\ +void DRV832xDriver::set##name(type value) {\ + register_type data;\ + data.reg = readSPI(address) & BIT_READ_MASK;\ + data.register_name = value;\ + delayMicroseconds(1);\ + writeSPI(address, data.reg);\ +} + +#define def_isset(name, address, register_type, register_name) \ +bool DRV832xDriver::is##name() {\ + register_type data;\ + data.reg = readSPI(address) & BIT_READ_MASK;\ + return static_cast(data.register_name);\ +}\ +\ +void DRV832xDriver::set##name(bool value) {\ + register_type data;\ + data.reg = readSPI(address) & BIT_READ_MASK;\ + data.register_name = value;\ + delayMicroseconds(1);\ + writeSPI(address, data.reg);\ +} + +DRV832xStatus DRV832xDriver::getStatus() { + Fault data0; + VGS data1; + uint16_t result = readSPI(Fault_Status_ADDR); + data0.reg = (result & BIT_READ_MASK); + delayMicroseconds(1); // delay at least 400ns between operations + result = readSPI(VGS_Status_ADDR); + data1.reg = (result & BIT_READ_MASK); + return DRV832xStatus(data0, data1); +} + +void DRV832xDriver::clearFault() { + uint16_t result = readSPI(Driver_Control_ADDR); + Driver_Control data; + data.reg = (result & BIT_READ_MASK); + data.CLR_FLT |= 1; + delayMicroseconds(1); // delay at least 400ns between operations + result = writeSPI(Driver_Control_ADDR, data.reg); +} + + +bool DRV832xDriver::isRegistersLocked(){ + uint16_t result = readSPI(Gate_Drive_HS_ADDR); + Gate_Drive_HS data; + data.reg = (result & BIT_READ_MASK); + return data.LOCK==LOCK_LOCK; +} +void DRV832xDriver::setRegistersLocked(bool lock){ + uint16_t result = readSPI(Gate_Drive_HS_ADDR); + Gate_Drive_HS data; + data.reg = (result & BIT_READ_MASK); + data.LOCK = lock?LOCK_LOCK:LOCK_UNLOCK; + delayMicroseconds(1); // delay at least 400ns between operations + result = writeSPI(Gate_Drive_HS_ADDR, data.reg); +} + +def_isset(Brake, Driver_Control_ADDR, Driver_Control, BRAKE) +def_isset(Coast, Driver_Control_ADDR, Driver_Control, COAST) +def_isset(OnePWMInvertDirection, Driver_Control_ADDR, Driver_Control, _1PWM_DIR) +def_isset(OnePWMAsynchronousRectification, Driver_Control_ADDR, Driver_Control, _1PWM_COM) +def_getset(PWMMode, DRV832x_PWMMode, Driver_Control_ADDR, Driver_Control, PWM_MODE) +def_isset(OvertemperatureReporting, Driver_Control_ADDR, Driver_Control, OTW_REP) +def_isset(DisableGateDriveFault, Driver_Control_ADDR, Driver_Control, DIS_GDF) +def_isset(DisableChargePumpUndervoltage, Driver_Control_ADDR, Driver_Control, DIS_CPUV) + +def_getset(VDSLevel, DRV832x_VDS_LVL, OCP_Control_ADDR, OCP_Control, VDS_LVL); +def_getset(OCPMode, DRV832x_OCPMode, OCP_Control_ADDR, OCP_Control, OCP_MODE) +def_getset(OCPDeglitchTime, DRV832x_OCPDeglitch, OCP_Control_ADDR, OCP_Control, OCP_DEG) +def_getset(DeadTime, DRV832x_DEADTIME, OCP_Control_ADDR, OCP_Control, DEAD_TIME); + +def_getset(HighSideChargeCurrent, DRV832x_IDRIVEP, Gate_Drive_HS_ADDR, Gate_Drive_HS, IDRIVEP_HS); +def_getset(HighSideDischargeCurrent, DRV832x_IDRIVEN, Gate_Drive_HS_ADDR, Gate_Drive_HS, IDRIVEN_HS); +def_getset(LowSideChargeCurrent, DRV832x_IDRIVEP, Gate_Drive_LS_ADDR, Gate_Drive_LS, IDRIVEP_LS); +def_getset(LowSideDischargeCurrent, DRV832x_IDRIVEN, Gate_Drive_LS_ADDR, Gate_Drive_LS, IDRIVEN_LS); + +def_getset(DriveTime, DRV832x_TDRIVE, Gate_Drive_LS_ADDR, Gate_Drive_LS, TDRIVE); +def_isset(CycleByCycle, Gate_Drive_LS_ADDR, Gate_Drive_LS, CBC); +def_isset(RetryTime, OCP_Control_ADDR, OCP_Control, TRETRY); + +def_getset(CurrentSenseOvercurrentSensitivity, DRV832x_CS_VSEN_LVL, CSA_Control_ADDR, CSA_Control, SEN_LVL); +def_isset(CurrentSenseCalibrateA, CSA_Control_ADDR, CSA_Control, CSA_CAL_A); +def_isset(CurrentSenseCalibrateB, CSA_Control_ADDR, CSA_Control, CSA_CAL_B); +def_isset(CurrentSenseCalibrateC, CSA_Control_ADDR, CSA_Control, CSA_CAL_C); + + +void DRV832xDriver::calibrate_current_sense() +{ + CSA_Control csa_control_before, csa_control_cal; + csa_control_before.reg = readSPI(CSA_Control_ADDR); + csa_control_cal.reg = csa_control_before.reg; + csa_control_cal.CSA_CAL_A = 1; + csa_control_cal.CSA_CAL_B = 1; + csa_control_cal.CSA_CAL_C = 1; + writeSPI(CSA_Control_ADDR, csa_control_cal.reg); + delay(1); //Calibration routine requires 100us, we're waiting for 1ms to be safe + csa_control_before.CSA_CAL_A = 0; + csa_control_before.CSA_CAL_B = 0; + csa_control_before.CSA_CAL_C = 0; + writeSPI(CSA_Control_ADDR, csa_control_before.reg); //restore settings, as the calibration may have changed the sensitivity to 40V/V +} + +def_isset(CurrentSenseOvercurrentDisable, CSA_Control_ADDR, CSA_Control, DIS_SEN); +def_getset(CurrentSenseGain, DRV832x_CSAGain, CSA_Control_ADDR, CSA_Control, CSA_GAIN) +def_isset(CurrentSenseOvercurrentResistor, CSA_Control_ADDR, CSA_Control, LS_REF); +def_isset(CurrentSenseBidirectionnal, CSA_Control_ADDR, CSA_Control, VREF_DIV); +def_isset(CurrentSenseFET, CSA_Control_ADDR, CSA_Control, CSA_FET); + + diff --git a/src/drivers/drv832x/drv832x.h b/src/drivers/drv832x/drv832x.h new file mode 100644 index 0000000..e287f94 --- /dev/null +++ b/src/drivers/drv832x/drv832x.h @@ -0,0 +1,279 @@ + + +#ifndef SIMPLEFOC_DRV832x +#define SIMPLEFOC_DRV832x + + +#include "Arduino.h" +#include +#include +#include + +#include "./drv832x_registers.h" + + +enum DRV832x_PWMMode { + PWM6_Mode = 0b00, //default + PWM3_Mode = 0b01, + PWM1_Mode = 0b10, + Independant_Mode = 0b11 +}; + +enum DRV832x_IDRIVEP { + IDRIVEP_10mA, + IDRIVEP_30mA, + IDRIVEP_60mA, + IDRIVEP_80mA, + IDRIVEP_120mA, + IDRIVEP_140mA, + IDRIVEP_170mA, + IDRIVEP_190mA, + IDRIVEP_260mA, + IDRIVEP_330mA, + IDRIVEP_370mA, + IDRIVEP_440mA, + IDRIVEP_570mA, + IDRIVEP_680mA, + IDRIVEP_820mA, + IDRIVEP_1000mA, //default +}; + +enum DRV832x_IDRIVEN { + IDRIVEN_20mA, + IDRIVEN_60mA, + IDRIVEN_120mA, + IDRIVEN_160mA, + IDRIVEN_240mA, + IDRIVEN_280mA, + IDRIVEN_340mA, + IDRIVEN_380mA, + IDRIVEN_520mA, + IDRIVEN_660mA, + IDRIVEN_740mA, + IDRIVEN_880mA, + IDRIVEN_1140mA, + IDRIVEN_1360mA, + IDRIVEN_1640mA, + IDRIVEN_2000mA, //default +}; + +enum DRV832x_LOCK +{ + LOCK_LOCK=0b110, + LOCK_UNLOCK=0b011 //default +}; + +enum DRV832x_TDRIVE +{ + TDRIVE_500ns, + TDRIVE_1000ns, + TDRIVE_2000ns, + TDRIVE_4000ns //default +}; + +enum DRV832x_DEADTIME +{ + DEADTIME_50ns, + DEADTIME_100ns, //default + DEADTIME_200ns, + DEADTIME_400ns +}; + +enum DRV832x_OCPMode +{ + OCPMode_LATCH_FAULT, + OCPMode_RETRY_FAULT, //default + OCPMode_REPORT, + OCPMode_IGNORE +}; + +enum DRV832x_OCPDeglitch +{ + OCPDeglitch_2us, + OCPDeglitch_4us,//default + OCPDeglitch_6us, + OCPDeglitch_8us +}; + +enum DRV832x_VDS_LVL +{ + VDS_LVL_60mV, + VDS_LVL_130mV, + VDS_LVL_200mV, + VDS_LVL_260mV, + VDS_LVL_310mV, + VDS_LVL_450mV, + VDS_LVL_530mV, + VDS_LVL_600mV, + VDS_LVL_680mV, + VDS_LVL_750mV, //default + VDS_LVL_940mV, + VDS_LVL_1130mV, + VDS_LVL_1300mV, + VDS_LVL_1500mV, + VDS_LVL_1700mV, + VDS_LVL_1880mV, +}; + +enum DRV832x_CSAGain +{ + CSAGain_5, + CSAGain_10, + CSAGain_20,//default + CSAGain_40 +}; + +enum DRV832x_CS_VSEN_LVL +{ + CSAGain_250mV, + CSAGain_500mV, + CSAGain_750mV, + CSAGain_1000mV//default +}; + +class DRV832xFault { +public: + DRV832xFault(Fault status) : fault_reg(status) {}; + ~DRV832xFault() {}; + + bool isFault() { return fault_reg.FAULT; }; + bool isOverCurrent() { return fault_reg.VDS_OCP; }; + bool isGateDriveFault() { return fault_reg.GDF; }; + bool isUndervoltageLockout() { return fault_reg.UVLO; }; + bool isOverTemperature() { return fault_reg.OTSD; }; + bool isOverCurrent_Ah() {return fault_reg.VDS_HA; }; + bool isOverCurrent_Al() {return fault_reg.VDS_LA; }; + bool isOverCurrent_Bh() {return fault_reg.VDS_HB; }; + bool isOverCurrent_Bl() {return fault_reg.VDS_LB; }; + bool isOverCurrent_Ch() {return fault_reg.VDS_HC; }; + bool isOverCurrent_Cl() {return fault_reg.VDS_LC; }; + + Fault fault_reg; +}; + + +class DRV832xVGS { +public: + DRV832xVGS(VGS status) : vgs_reg(status) {}; + ~DRV832xVGS() {}; + + + bool isOverCurrent_As() { return vgs_reg.SA_OC==0b1; }; + bool isOverCurrent_Bs() { return vgs_reg.SB_OC==0b1; }; + bool isOverCurrent_Cs() { return vgs_reg.SC_OC==0b1; }; + bool isOverTemperatureWarning() { return vgs_reg.OTW==0b1; }; + bool isChargePumpUndervoltage() { return vgs_reg.CPUV==0b1; }; + bool isGateDriveFault_Ah() { return vgs_reg.VGS_HA==0b1; }; + bool isGateDriveFault_Al() { return vgs_reg.VGS_LA==0b1; }; + bool isGateDriveFault_Bh() { return vgs_reg.VGS_HB==0b1; }; + bool isGateDriveFault_Bl() { return vgs_reg.VGS_LB==0b1; }; + bool isGateDriveFault_Ch() { return vgs_reg.VGS_HC==0b1; }; + bool isGateDriveFault_Cl() { return vgs_reg.VGS_LC==0b1; }; + + VGS vgs_reg; +}; + + + +class DRV832xStatus : public DRV832xFault, public DRV832xVGS { + public: + DRV832xStatus(Fault faults, VGS vgs) : DRV832xFault(faults), DRV832xVGS(vgs) {}; + ~DRV832xStatus() {}; +}; + +#define decl_getset(name, type) type get##name(); void set##name(type); +#define decl_isset(name) bool is##name(); void set##name(bool); + +class DRV832xDriver { + + public: + DRV832xDriver(int cs, int nFault = NOT_SET, uint32_t spi_clock = 1000000) : cs(cs), nFault(nFault), spi(&SPI), settings(spi_clock, MSBFIRST, SPI_MODE1) {}; + virtual ~DRV832xDriver() {}; + + virtual void init(SPIClass* _spi = &SPI); + + void clearFault(); // TODO check for fault condition methods + + DRV832xStatus getStatus(); + + decl_isset(RegistersLocked); + + //Driver control + decl_isset(Brake); + decl_isset(Coast); + decl_isset(OnePWMInvertDirection); + decl_isset(OnePWMAsynchronousRectification); + decl_getset(PWMMode, DRV832x_PWMMode); + decl_isset(OvertemperatureReporting); + decl_isset(DisableGateDriveFault); + decl_isset(DisableChargePumpUndervoltage); + + //OCP Control + decl_getset(VDSLevel, DRV832x_VDS_LVL); + decl_getset(OCPMode, DRV832x_OCPMode); + decl_getset(OCPDeglitchTime, DRV832x_OCPDeglitch); + decl_getset(DeadTime, DRV832x_DEADTIME); + + //Drive current + decl_getset(HighSideChargeCurrent, DRV832x_IDRIVEP); + decl_getset(HighSideDischargeCurrent, DRV832x_IDRIVEN); + decl_getset(LowSideChargeCurrent, DRV832x_IDRIVEP); + decl_getset(LowSideDischargeCurrent, DRV832x_IDRIVEN); + + //Misc drive + decl_getset(DriveTime, DRV832x_TDRIVE); + decl_isset(CycleByCycle); + decl_isset(RetryTime); + + //Current sense (DRV8323 only) + decl_getset(CurrentSenseOvercurrentSensitivity, DRV832x_CS_VSEN_LVL); + decl_isset(CurrentSenseCalibrateA); + decl_isset(CurrentSenseCalibrateB); + decl_isset(CurrentSenseCalibrateC); + void calibrate_current_sense(); + + decl_isset(CurrentSenseOvercurrentDisable); + decl_getset(CurrentSenseGain, DRV832x_CSAGain); + decl_isset(CurrentSenseOvercurrentResistor); + decl_isset(CurrentSenseBidirectionnal); + decl_isset(CurrentSenseFET); + + private: + uint16_t readSPI(uint8_t addr); + uint16_t writeSPI(uint8_t addr, uint16_t value); + + int cs; + int nFault; + SPIClass* spi; + SPISettings settings; +}; + +#undef decl_getset +#undef decl_isset + +class DRV832xDriver3PWM : public DRV832xDriver, public BLDCDriver3PWM { + + public: + DRV832xDriver3PWM(int phA,int phB,int phC, int cs, int en = NOT_SET, int nFault = NOT_SET, uint32_t spi_clock = 1000000) : + DRV832xDriver(cs, nFault, spi_clock), BLDCDriver3PWM(phA, phB, phC, en) { enable_active_high=false; }; + virtual ~DRV832xDriver3PWM() {}; + + virtual void init(SPIClass* _spi = &SPI) override; + +}; + + + +class DRV832xDriver6PWM : public DRV832xDriver, public BLDCDriver6PWM { + + public: + DRV832xDriver6PWM(int phA_h,int phA_l,int phB_h,int phB_l,int phC_h,int phC_l, int cs, int en = NOT_SET, int nFault = NOT_SET, uint32_t spi_clock = 1000000) : + DRV832xDriver(cs, nFault, spi_clock), BLDCDriver6PWM(phA_h, phA_l, phB_h, phB_l, phC_h, phC_l, en) { enable_active_high=false; }; + virtual ~DRV832xDriver6PWM() {}; + + virtual void init(SPIClass* _spi = &SPI) override; + +}; + + +#endif diff --git a/src/drivers/drv832x/drv832x_registers.h b/src/drivers/drv832x/drv832x_registers.h new file mode 100644 index 0000000..b6af386 --- /dev/null +++ b/src/drivers/drv832x/drv832x_registers.h @@ -0,0 +1,128 @@ + + +#pragma once + + +#define Fault_Status_ADDR 0x0 +#define VGS_Status_ADDR 0x1 +#define Driver_Control_ADDR 0x2 +#define Gate_Drive_HS_ADDR 0x3 +#define Gate_Drive_LS_ADDR 0x4 +#define OCP_Control_ADDR 0x5 +#define CSA_Control_ADDR 0x6 + + +//Read-only +typedef union { + struct + { + + bool VDS_LC:1, //Overcurrent on C low-side mosfet + VDS_HC:1, //Overcurrent on C high-side mosfet + VDS_LB:1, //Overcurrent on B low-side mosfet + VDS_HB:1, //Overcurrent on B high-side mosfet + VDS_LA:1, //Overcurrent on A low-side mosfet + VDS_HA:1, //Overcurrent on A high-side mosfet + OTSD:1, //Overtemperature shutdown fault + UVLO:1, //undervoltage lockout fault + GDF:1, //Gate drive fault + VDS_OCP:1, //VDS monitor overcurrent fault + FAULT:1; //Mirrors nFAULT pin + //uint8_t :5; + }; + uint16_t reg : 11; +} Fault; + +//Read-only +typedef union { + struct + { + + bool VGS_LC:1, //Gate drive fault on C low-side mosfet + VGS_HC:1, //Gate drive fault on C high-side mosfet + VGS_LB:1, //Gate drive fault on B low-side mosfet + VGS_HB:1, //Gate drive fault on B high-side mosfet + VGS_LA:1, //Gate drive fault on A low-side mosfet + VGS_HA:1, //Gate drive fault on A high-side mosfet + CPUV:1, //Charge-pump undervoltage fault + OTW:1, //Overtemp warning + SC_OC:1, //Overcurrent on phase C sense amplifier + SB_OC:1, //Overcurrent on phase B sense amplifier + SA_OC:1; //Overcurrent on phase A sense amplifier + //uint8_t :5; + }; + uint16_t reg : 11; +} VGS; + +typedef union { + struct + { + + bool CLR_FLT:1, //Clear faults, automatically clears itself + BRAKE:1, //Turn on all low-side mosfets in 1PWM + COAST:1, //Put mosfet in Hi-Z + _1PWM_DIR:1, //Invert 1PWM dir pin + _1PWM_COM:1; //1PWM asynchronous rectification + uint8_t PWM_MODE:2; //00 = 6PWM, 01=3PWM, 10=1PWM, 11=Independant + bool OTW_REP:1, //Overtemp warning report on nFAULT + DIS_GDF:1, //Disable gate drive fault + DIS_CPUV:1, //Disable charge pump undervoltage lockout fault + :1; + //uint8_t :5; + }; + uint16_t reg : 11; +} Driver_Control; + +typedef union { + struct + { + uint8_t IDRIVEN_HS:4, //high side discharge current + IDRIVEP_HS:4, //high side charge current + LOCK:3; //110 = lock registers, 011b = unlock registers + //uint8_t :5; + }; + uint16_t reg : 11; +} Gate_Drive_HS; + +typedef union { + struct + { + uint8_t IDRIVEN_LS:4, //low side discharge current + IDRIVEP_LS:4, //low side charge current + TDRIVE:2; //gate current peak duration + bool CBC:1; //cycle-by-cycle operation + //uint8_t :5; + }; + uint16_t reg : 11; +} Gate_Drive_LS; + +typedef union { + struct + { + uint8_t VDS_LVL:4, //vds protection voltage + OCP_DEG:2, //overcurrent deglitch time + OCP_MODE:2, //overcurrent fault registering + DEAD_TIME:2; //dead-time + bool TRETRY:1; //VDS_OCP and SEN_OCP retry time + //uint8_t :5; + }; + uint16_t reg : 11; +} OCP_Control; + +//Only for DRV8323 +typedef union { + struct + { + uint8_t SEN_LVL:2; //Sense OCP voltage (value*0.25) V + bool CSA_CAL_C:1, //Short inputs of current sense for offset cal + CSA_CAL_B:1, //Short inputs of current sense for offset cal + CSA_CAL_A:1, //Short inputs of current sense for offset cal + DIS_SEN:1; //Disable sense overcurrent fault + uint8_t CSA_GAIN:2; //(2^value)*5 V/V sensitivity + bool LS_REF:1, //VDS OCP measurement source + VREF_DIV:1, //Unidirectional current sense + CSA_FET:1; //Change current sense amp positive input + //uint8_t :5; + }; + uint16_t reg : 11; +} CSA_Control; \ No newline at end of file diff --git a/src/drivers/drv8376/README.md b/src/drivers/drv8376/README.md new file mode 100644 index 0000000..8e28e6a --- /dev/null +++ b/src/drivers/drv8376/README.md @@ -0,0 +1,43 @@ + +# DRV8376 SPI and SimpleFOC driver + +DRV8376 is an integrated FET motor driver from TI, capable of 4.5A peak output currents, and operating over the wide range of 4.5V-70V supply. It features 5V and 3.3V LDOs and up to 100kHz PWM frequency. It has internal current sensing and analog current sense outputs for 3 phases. + +It is available in SPI and hardware-configured versions. This is a driver for the SPI version. + +## Setup + +Connect your MCU to the driver as per the datasheets. + +You will need at least: + +- 3x PWM and 3x EN +- or 6x PWM + +And optionally, but recommended: + +- DRVOFF +- nSLEEP +- nFAULT +- SOA, SOB, SOC for current sensing +- VREF to set current sensing reference voltage +- ILIM to set current limit + +## Software configuration + +TODO finish documentation and add an example + +```c++ +DRV8376Driver6PWM driver = DRV8376Driver6PWM(PIN_AH, PIN_AL, PIN_BH, PIN_BL, PIN_CH, PIN_CL, PIN_nCS, PIN_DRVOFF); + + +void setup() { + ... + + driver.init(); + + ... +} + + +``` \ No newline at end of file diff --git a/src/drivers/drv8376/drv8376.cpp b/src/drivers/drv8376/drv8376.cpp new file mode 100644 index 0000000..c1b6f9a --- /dev/null +++ b/src/drivers/drv8376/drv8376.cpp @@ -0,0 +1,232 @@ + +#include "./drv8376.h" + + +DRV8376::DRV8376(int nCS, SPISettings settings) : nCS(nCS), settings(settings) { +} + + +DRV8376::~DRV8376() { +} + + +void DRV8376::init(SPIClass* _spi) { + spi = _spi; + if (nCS >= 0) { + pinMode(nCS, OUTPUT); + digitalWrite(nCS, HIGH); + } +} + + +void DRV8376::clearFault() { + DRV8376_FaultClear_Reg reg; + reg.FLT_CLR = 1; + writeFaultClear(reg); +} + + +void DRV8376::setRegistersLocked(bool locked) { + DRV8376_SystemCtrl_Reg reg = readSystemCtrl(); + reg.REG_LOCK = locked ? 1 : 0; + reg.WRITE_KEY = DRV8376_SYSTEM_CTRL_WRITE_KEY; + writeSystemCtrl(reg); +} + + +DRV8376_Status_Reg DRV8376::readStatus() { + DRV8376_Status_Reg reg; + reg.reg = readRegister(DRV8376_REG_STATUS); + return reg; +} + + +DRV8376_RawStatus_Reg DRV8376::readRawStatus() { + DRV8376_RawStatus_Reg reg; + reg.reg = readRegister(DRV8376_REG_RAWSTATUS); + return reg; +} + + +DRV8376_TempStatus_Reg DRV8376::readTempStatus() { + DRV8376_TempStatus_Reg reg; + reg.reg = readRegister(DRV8376_REG_TEMPSTATUS); + return reg; +} + + +DRV8376_SupplyStatus_Reg DRV8376::readSupplyStatus() { + DRV8376_SupplyStatus_Reg reg; + reg.reg = readRegister(DRV8376_REG_SUPPLYSTATUS); + return reg; +} + + +DRV8376_DriverStatus_Reg DRV8376::readDriverStatus() { + DRV8376_DriverStatus_Reg reg; + reg.reg = readRegister(DRV8376_REG_DRIVERSTATUS); + return reg; +} + + +DRV8376_IntfStatus_Reg DRV8376::readIntfStatus() { + DRV8376_IntfStatus_Reg reg; + reg.reg = readRegister(DRV8376_REG_INTFSTATUS); + return reg; +} + + +DRV8376_FaultMode_Reg DRV8376::readFaultMode() { + DRV8376_FaultMode_Reg reg; + reg.reg = readRegister(DRV8376_REG_FAULT_MODE); + return reg; +} + + +void DRV8376::writeFaultMode(const DRV8376_FaultMode_Reg& reg) { + writeRegister(DRV8376_REG_FAULT_MODE, reg.reg); +} + + +DRV8376_DriverFaultCtrl_Reg DRV8376::readDriverFaultCtrl() { + DRV8376_DriverFaultCtrl_Reg reg; + reg.reg = readRegister(DRV8376_REG_DRIVER_FAULT_CTRL); + return reg; +} + + +void DRV8376::writeDriverFaultCtrl(const DRV8376_DriverFaultCtrl_Reg& reg) { + writeRegister(DRV8376_REG_DRIVER_FAULT_CTRL, reg.reg); +} + + +DRV8376_FaultClear_Reg DRV8376::readFaultClear() { + DRV8376_FaultClear_Reg reg; + reg.reg = readRegister(DRV8376_REG_FAULT_CLEAR); + return reg; +} + + +void DRV8376::writeFaultClear(const DRV8376_FaultClear_Reg& reg) { + writeRegister(DRV8376_REG_FAULT_CLEAR, reg.reg); +} + + +DRV8376_PWMCtrl1_Reg DRV8376::readPWMCtrl1() { + DRV8376_PWMCtrl1_Reg reg; + reg.reg = readRegister(DRV8376_REG_PWM_CTRL_1); + return reg; +} + + +void DRV8376::writePWMCtrl1(const DRV8376_PWMCtrl1_Reg& reg) { + writeRegister(DRV8376_REG_PWM_CTRL_1, reg.reg); +} + + +DRV8376_PreDriverCtrl_Reg DRV8376::readPreDriverCtrl() { + DRV8376_PreDriverCtrl_Reg reg; + reg.reg = readRegister(DRV8376_REG_PREDRIVER_CTRL); + return reg; +} + + +void DRV8376::writePreDriverCtrl(const DRV8376_PreDriverCtrl_Reg& reg) { + writeRegister(DRV8376_REG_PREDRIVER_CTRL, reg.reg); +} + + +DRV8376_CSAControl_Reg DRV8376::readCSAControl() { + DRV8376_CSAControl_Reg reg; + reg.reg = readRegister(DRV8376_REG_CSA_CTRL); + return reg; +} + + +void DRV8376::writeCSAControl(const DRV8376_CSAControl_Reg& reg) { + writeRegister(DRV8376_REG_CSA_CTRL, reg.reg); +} + + +DRV8376_SystemCtrl_Reg DRV8376::readSystemCtrl() { + DRV8376_SystemCtrl_Reg reg; + reg.reg = readRegister(DRV8376_REG_SYSTEM_CTRL); + return reg; +} + + +void DRV8376::writeSystemCtrl(const DRV8376_SystemCtrl_Reg& reg) { + writeRegister(DRV8376_REG_SYSTEM_CTRL, reg.reg); +} + + +uint16_t DRV8376::readRegister(uint8_t reg) { + uint8_t head = (reg & 0x3F)<<1 | 0x01; // Read command + // TODO parity + uint8_t data[3] = {head, 0, 0}; + transact24(data); + return (data[1] << 8) | data[2]; +} + + +void DRV8376::writeRegister(uint8_t reg, uint16_t value) { + uint8_t head = (reg & 0x3F)<<1 | 0x00; // Write command + // TODO parity + uint8_t data[3] = {head , (uint8_t)((value >> 8) & 0xFF), (uint8_t)(value & 0xFF)}; + transact24(data); + // TODO check response +} + + +void DRV8376::transact24(uint8_t* data) { + if (nCS >= 0) { + digitalWrite(nCS, LOW); + } + spi->beginTransaction(settings); + spi->transfer(data, 3); + spi->endTransaction(); + if (nCS >= 0) { + digitalWrite(nCS, HIGH); + } +} + + + +bool DRV8376::getParity(uint16_t data) { + bool parity = false; // TODO check me + for (int i = 0; i < 16; i++) { + parity ^= (data >> i) & 0x01; + } + return parity; +} + + + + +void DRV8376Driver3PWM::init(SPIClass* _spi) { + DRV8376::init(_spi); + setRegistersLocked(false); + delayMicroseconds(1); + DRV8376_PWMCtrl1_Reg pwmReg = readPWMCtrl1(); + pwmReg.PWM_MODE = 0x02; // set 3-PWM mode TODO difference between modes 0x02 and 0x03? + writePWMCtrl1(pwmReg); + BLDCDriver3PWM::init(); +}; + + + + +void DRV8376Driver6PWM::init(SPIClass* _spi) { + DRV8376::init(_spi); + setRegistersLocked(false); + delayMicroseconds(1); + DRV8376_PWMCtrl1_Reg pwmReg = readPWMCtrl1(); + pwmReg.PWM_MODE = 0x00; // set 6-PWM mode TODO difference between modes 0x00 and 0x01? + writePWMCtrl1(pwmReg); + BLDCDriver6PWM::init(); +}; + + + + + diff --git a/src/drivers/drv8376/drv8376.h b/src/drivers/drv8376/drv8376.h new file mode 100644 index 0000000..9074057 --- /dev/null +++ b/src/drivers/drv8376/drv8376.h @@ -0,0 +1,93 @@ + +#pragma once + +#include +#include +#include + +#include "./drv8376_registers.h" + + +/* +Data is captured on the falling edge of the SCLK pin and data is propagated on the rising edge of the SCLK +pin. The most significant bit (MSB) is shifted in and out first. +*/ +#define DRV8376SPISettings SPISettings(1000000, MSBFIRST, SPI_MODE3) + + +class DRV8376 { +public: + DRV8376(int nCS = -1, SPISettings settings = DRV8376SPISettings); + virtual ~DRV8376(); + + virtual void init(SPIClass* _spi = &SPI); + + void clearFault(); + + void setRegistersLocked(bool locked); + + DRV8376_Status_Reg readStatus(); + DRV8376_RawStatus_Reg readRawStatus(); + DRV8376_TempStatus_Reg readTempStatus(); + DRV8376_SupplyStatus_Reg readSupplyStatus(); + DRV8376_DriverStatus_Reg readDriverStatus(); + DRV8376_IntfStatus_Reg readIntfStatus(); + + DRV8376_FaultMode_Reg readFaultMode(); + void writeFaultMode(const DRV8376_FaultMode_Reg& reg); + DRV8376_DriverFaultCtrl_Reg readDriverFaultCtrl(); + void writeDriverFaultCtrl(const DRV8376_DriverFaultCtrl_Reg& reg); + DRV8376_FaultClear_Reg readFaultClear(); + void writeFaultClear(const DRV8376_FaultClear_Reg& reg); + DRV8376_PWMCtrl1_Reg readPWMCtrl1(); + void writePWMCtrl1(const DRV8376_PWMCtrl1_Reg& reg); + DRV8376_PreDriverCtrl_Reg readPreDriverCtrl(); + void writePreDriverCtrl(const DRV8376_PreDriverCtrl_Reg& reg); + DRV8376_CSAControl_Reg readCSAControl(); + void writeCSAControl(const DRV8376_CSAControl_Reg& reg); + DRV8376_SystemCtrl_Reg readSystemCtrl(); + void writeSystemCtrl(const DRV8376_SystemCtrl_Reg& reg); + +protected: + SPIClass* spi; + SPISettings settings; + int nCS; + + uint16_t readRegister(uint8_t reg); + void writeRegister(uint8_t reg, uint16_t value); + void transact24(uint8_t* data); + bool getParity(uint16_t data); +}; + + + + + + +class DRV8376Driver3PWM : public DRV8376, public BLDCDriver3PWM { + + public: + DRV8376Driver3PWM(int phA,int phB,int phC, int cs = -1, int en = NOT_SET, SPISettings settings = DRV8376SPISettings) : + DRV8376(cs, settings), BLDCDriver3PWM(phA, phB, phC, en) { enable_active_high=false; }; + virtual ~DRV8376Driver3PWM() {}; + + virtual void init(SPIClass* _spi = &SPI) override; + +}; + + + +class DRV8376Driver6PWM : public DRV8376, public BLDCDriver6PWM { + + public: + DRV8376Driver6PWM(int phA_h,int phA_l,int phB_h,int phB_l,int phC_h,int phC_l, int cs = -1, int en = NOT_SET, SPISettings settings = DRV8376SPISettings) : + DRV8376(cs, settings), BLDCDriver6PWM(phA_h, phA_l, phB_h, phB_l, phC_h, phC_l, en) { enable_active_high=false; }; + virtual ~DRV8376Driver6PWM() {}; + + virtual void init(SPIClass* _spi = &SPI) override; + +}; + + + + diff --git a/src/drivers/drv8376/drv8376_registers.h b/src/drivers/drv8376/drv8376_registers.h new file mode 100644 index 0000000..a1bd29c --- /dev/null +++ b/src/drivers/drv8376/drv8376_registers.h @@ -0,0 +1,339 @@ +#pragma once + +#include + + +#define DRV8376_REG_STATUS 0x00 +#define DRV8376_REG_RAWSTATUS 0x02 +#define DRV8376_REG_TEMPSTATUS 0x04 +#define DRV8376_REG_SUPPLYSTATUS 0x05 +#define DRV8376_REG_DRIVERSTATUS 0x06 +#define DRV8376_REG_INTFSTATUS 0x07 + + +#define DRV8376_REG_FAULT_MODE 0x10 +#define DRV8376_REG_DRIVER_FAULT_CTRL 0x13 +#define DRV8376_REG_FAULT_CLEAR 0x17 +#define DRV8376_REG_PWM_CTRL_1 0x20 +#define DRV8376_REG_PREDRIVER_CTRL 0x22 +#define DRV8376_REG_CSA_CTRL 0x23 +#define DRV8376_REG_SYSTEM_CTRL 0x3F + + + +#define DRV8376_SYSTEM_CTRL_WRITE_KEY 0x05 + + +/** + * STATUS REGISTERS + */ + + +typedef union { + struct { + uint16_t FAULT : 1; // 0h = No fault condition is detected + // 1h = Fault condition is detected + uint16_t OTF : 1; // 0h = No overtemperature warning / shutdown is detected + // 1h = Overtemperature warning / shutdown is detected + uint16_t UVP : 1; // 0h = No undervoltage voltage condition is detected on CP + // 1h = Undervoltage voltage condition is detected on CP + uint16_t OVP : 1; // 0h = No over voltage condition is detected + // 1h = Over voltage condition is detected + uint16_t : 1; + uint16_t OCP : 1; // 0h = No overcurrent condition is detected + // 1h = Overcurrent condition is detected + uint16_t SPIFLT : 1; // 0h = No SPI fault is detected + // 1h = SPI fault is detected + uint16_t RESET : 1; // 0h = Cleared Value + // 1h = Device has undergone power on reset + uint16_t SYSFLT : 1; // 0h = No OTP read fault is detected + // 1h = OTP read fault detected + uint16_t DNRDY_STS : 1; // 0h = Device is Ready + // 1h = Device is NOT Ready + uint16_t : 5; + uint16_t PARITY : 1; // 15 Parity Bit if SPI_PEN is set to '1' otherwise reserved + }; + uint16_t reg; +} DRV8376_Status_Reg; + + + + + +typedef union { + struct { + uint16_t : 1; + uint16_t OTF_RSTS : 1; // Overtemperature Shutdown Raw Fault Status + uint16_t UVP_RSTS : 1; // CP Undervoltage Raw Fault Status. + uint16_t OVP_RSTS : 1; // Overvoltage Raw Fault Status + uint16_t : 1; + uint16_t OCP_RSTS : 1; // Overcurrent Fault Raw Status + uint16_t SPIFLT_RSTS : 1; // SPI Fault status. Status remains latched until cleared by write to FLT_CLR or reset pulse on nSLEEP + uint16_t RESET : 1; + uint16_t SYSFLT_RSTS : 1; // OTP Read fault occurred. Status remains latched until cleared by write to FLT_CLR + uint16_t DNRDY_RSTS : 1; // Device Not Ready Status + uint16_t : 1; + uint16_t OTW_RSTS : 1; // OT Warning Raw Status + uint16_t DRVOFF_RSTS : 1; // Status of DRV_OFF pin + uint16_t : 2; + uint16_t PARITY : 1; // 15 Parity Bit if SPI_PEN is set to '1' otherwise reserved + }; + uint16_t reg; +} DRV8376_RawStatus_Reg; + + + + + + +typedef union { + struct { + uint16_t OTSD : 1; // Overtemperature Shutdown Fault status. Can be cleared by write to + // FLT_CLR or reset pulse on nSLEEP + // 0h = No overtemperature shutdown is detected + // 1h = Overtemperature shutdown is detected + uint16_t OTW : 1; // Overtemperature Warning Fault status. Can be cleared by write to + // FLT_CLR or reset pulse on nSLEEP + // 0h = No overtemperature warning is detected + // 1h = Overtemperature warning is detected + uint16_t : 13; + uint16_t PARITY : 1; // 15 Parity Bit if SPI_PEN is set to '1' otherwise reserved + }; + uint16_t reg; +} DRV8376_TempStatus_Reg; + + + + + + +typedef union { + struct { + uint16_t : 4; + uint16_t CP_UV : 1; // Charge Pump Undervoltage fault status + // 0h = No charge pump undervoltage is detected + // 1h = Charge pump undervoltage is detected + uint16_t : 1; + uint16_t VM_OV : 1; // Vm Over Voltage Fault Status + // 0h = No Vm over voltage is detected + // 1h = Vm over voltage is detected + uint16_t : 8; + uint16_t PARITY : 1; // 15 Parity Bit if SPI_PEN is set to '1' otherwise reserved + }; + uint16_t reg; +} DRV8376_SupplyStatus_Reg; + + + + + +typedef union { + struct { + uint16_t OCPA_LS : 1; // 0h = No overcurrent detected on low-side MOSFET of OUTA + // 1h = Overcurrent detected on low-side MOSFET of OUTA + uint16_t OCPB_LS : 1; // 0h = No overcurrent detected on low-side MOSFET of OUTB + // 1h = Overcurrent detected on low-side MOSFET of OUTB + uint16_t OCPC_LS : 1; // 0h = No overcurrent detected on low-side MOSFET of OUTC + // 1h = Overcurrent detected on low-side MOSFET of OUTC + uint16_t : 1; + uint16_t OCPA_HS : 1; // 0h = No overcurrent detected on high-side MOSFET of OUTA + // 1h = Overcurrent detected on high-side MOSFET of OUTA + uint16_t OCPB_HS : 1; // 0h = No overcurrent detected on high-side MOSFET of OUTB + // 1h = Overcurrent detected on high-side MOSFET of OUTB + uint16_t OCPC_HS : 1; // 0h = No overcurrent detected on high-side MOSFET of OUTC + // 1h = Overcurrent detected on high-side MOSFET of OUTC + uint16_t : 8; + uint16_t PARITY : 1; // 15 Parity Bit if SPI_PEN is set to '1' otherwise reserved + }; + uint16_t reg; +} DRV8376_DriverStatus_Reg; + + + + + +typedef union { + struct { + uint16_t FRM_ERR : 1; // 0h = No SPI Frame Error is detected + // 1h = SPI Frame Error is detected + uint16_t : 1; + uint16_t SPI_PARITY : 1; // 0h = No SPI Parity Error is detected + // 1h = SPI Parity Error is detected + uint16_t : 1; + uint16_t OTPLD_ERR : 1; // 0h = No OTP read error is detected + // 1h = OTP read error is detected + uint16_t : 10; + uint16_t PARITY : 1; // 15 Parity Bit if SPI_PEN is set to '1' otherwise reserved + }; + uint16_t reg; +} DRV8376_IntfStatus_Reg; + + + + + + + +/** + * CONTROL REGISTERS + */ + + +typedef union { + struct { + uint16_t OTW_MODE : 1; // 0h = Over temperature reporting on nFAULT is disabled + // 1h = Over temperature reporting on nFAULT is enabled + uint16_t : 3; + uint16_t OCP_MODE : 2; // 0h = Over Current causes a latched fault + // 1h = Over Current causes an automatic retrying fault + // 2h = Over Current is report only but no action is taken + // 3h = Over Current is not reported and no action is taken + uint16_t : 1; + uint16_t SPIFLT_MODE : 1; // 0h = SPI fault reporting on nFAULT pin is disabled + // 1h = SPI fault reporting on nFAULT pin is enabled + uint16_t : 1; + uint16_t OVP_MODE : 1; // 0h = Over Voltage protection is disabled + // 1h = Over Voltage protection is enabled + uint16_t : 3; + uint16_t ILIMFLT_MODE : 1; // 0h = ILIMIT reporting on nFAULT pin is disabled + // 1h = ILIMIT reporting on nFAULT pin is enabled + uint16_t : 1; + uint16_t PARITY : 1; // 15 Parity Bit if SPI_PEN is set to '1' otherwise reserved + }; + uint16_t reg; +} DRV8376_FaultMode_Reg; + + + + + +typedef union { + struct { + uint16_t OCP_LVL : 1; // 0h = 4.5A + // 1h = 2.5A + uint16_t : 1; + uint16_t OCP_TRETRY : 1; // 0h = 5ms + // 1h = 500ms + uint16_t : 1; + uint16_t OCP_DEG : 2; // 0h = OCP Deglitch time is 0.6 µs + // 1h = OCP Deglitch time is 1.25 µs + // 2h = OCP Deglitch time is 1.6 µs + // 3h = OCP Deglitch time is 2 µs + uint16_t : 2; + uint16_t OVP_SEL : 1; // 0h = VM overvoltage level is 65V + // 1h = VM overvoltage level is 35V + uint16_t : 6; + uint16_t PARITY : 1; // 15 Parity Bit if SPI_PEN is set to '1' otherwise reserved + }; + uint16_t reg; +} DRV8376_DriverFaultCtrl_Reg; + + + + + +typedef union { + struct { + uint16_t FLT_CLR : 1; // 0h = No clear fault command is issued + // 1h = To clear the latched faults, write '1' to this bit + uint16_t : 14; + uint16_t PARITY : 1; // 15 Parity Bit if SPI_PEN is set to '1' otherwise reserved + }; + uint16_t reg; +} DRV8376_FaultClear_Reg; + + +typedef union { + struct { + uint16_t PWM_MODE : 2; // 0h = 6x mode + // 1h = 6x mode + // 2h = 3x mode + // 3h = 3x mode + uint16_t EN_ASR : 1; // 0h = Active Demagnetization is Disabled + // 1h = Active Demagnetization is Enabled + uint16_t EN_AAR : 1; // 0h = Active Demagnetization AAR is Disabled + // 1h = Active Demagnetization AAR is Enabled + uint16_t : 1; + uint16_t ILIM_MODE : 1; // 0h = Current recirculation through FETs (Brake mode) + // 1h = Current recirculation through diodes (coast mode) + uint16_t PWM_100_FREQ_SEL : 2; // 0h = 20KHz + // 1h = 40KHz + // 2h = 10KHz + // 3h = None + uint16_t : 7; + uint16_t PARITY : 1; // 15 Parity Bit if SPI_PEN is set to '1' otherwise reserved + }; + uint16_t reg; +} DRV8376_PWMCtrl1_Reg; + + + + + +typedef union { + struct { + uint16_t SLEW_RATE : 2; // Slew rate settings + // 0h = Slew rate is 1100 V/µs + // 1h = Slew rate is 500 V/µs + // 2h = Slew rate is 250 V/µs + // 3h = Slew rate is 50 V/µs + uint16_t AD_COMP_TH_LS : 1; // Active demag low side comparator threshold + // 0h = active demag comparator threshold is 100mA + // 1h = active demag comparator threshold is 150mA + uint16_t AD_COMP_TH_HS : 1; // Active demag high side comparator threshold + // 0h = active demag comparator threshold is 100mA + // 1h = active demag comparator threshold is 150mA + uint16_t ADMAG_TMARGIN : 4; // Wait time before determining HiZ. N*4*100ns + uint16_t ILIM_BLANK_SEL : 3; // Current Limit Blanking Time Selection + // 0h = 5.5us for slew rate of 50 and 1.8us for all other slew rates. + // 1h = 6.0us for slew rate of 50 and 2.3us for all other slew rates. + // 2h = 6.5us for slew rate of 50 and 2.8us for all other slew rates. + // 3h = 7.5us for slew rate of 50 and 3.8us for all other slew rates. + uint16_t : 4; + uint16_t PARITY : 1; // 15 Parity Bit if SPI_PEN is set to '1' otherwise reserved + }; + uint16_t reg; +} DRV8376_PreDriverCtrl_Reg; + + + + + +typedef union { + struct { + uint16_t CSA_GAIN : 2; // 0h = CSA gain is 0.4 V/A + // 1h = CSA gain is 1.0 V/A + // 2h = CSA gain is 2.5 V/A + // 3h = CSA gain is 5.0 V/A + uint16_t : 13; + uint16_t PARITY : 1; // 15 Parity Bit if SPI_PEN is set to '1' otherwise reserved + }; + uint16_t reg; +} DRV8376_CSAControl_Reg; + + + + + +typedef union { + struct { + uint16_t : 6; + uint16_t SPI_PEN : 1; // Parity Enable for SPI + // 0h = Parity Disabled + // 1h = Parity Enabled + uint16_t REG_LOCK : 1; // Register Lock Bit + // 0h = Registers Unlocked + // 1h = Registers Locked + uint16_t : 2; + uint16_t SDO_ODEN : 1; // SDO in Open Drain Mode + // 0h = SDO in Push Pull Mode + // 1h = SDO in Open Drain Mode + uint16_t SDO_VSEL : 1; // SDO Output Voltage Select + // 0h = AVDD + // 1h = GVDD + uint16_t WRITE_KEY : 3; // 0x5 Write Key Specific to this register. + uint16_t PARITY : 1; // 15 Parity Bit if SPI_PEN is set to '1' otherwise reserved + }; + uint16_t reg; +} DRV8376_SystemCtrl_Reg; + diff --git a/src/drivers/simplefocnano/README.md b/src/drivers/simplefocnano/README.md index 596a5ad..ff2bdba 100644 --- a/src/drivers/simplefocnano/README.md +++ b/src/drivers/simplefocnano/README.md @@ -18,11 +18,11 @@ SimpleFOCNanoDriver driver = SimpleFOCNanoDriver(); BLDCMotor motor = BLDCMotor(7); void setup() { - driver.voltage_power_supply = driver.getBusVoltage(); + driver.voltage_power_supply = driver.getBusVoltage(3.3f, 1024); driver.init(); motor.linkDriver(driver); motor.voltage_limit = driver.voltage_limit / 2.0f; - motor.controller = MotionControlMode::velocity_openloop; + motor.controller = MotionControlType::velocity_openloop; motor.init(); } @@ -59,7 +59,7 @@ As shown in the example you can read the bus voltage: :warning: *this is a slow function. Do not call it while motor is running!* ```c++ -float val = driver.getBusVoltage(); // get the bus voltage, in Volts +float val = driver.getBusVoltage(3.3f, 1024); // get the bus voltage, in Volts ``` ### SPI port diff --git a/src/drivers/simplefocnano/SimpleFOCNanoDriver.cpp b/src/drivers/simplefocnano/SimpleFOCNanoDriver.cpp index 5394ed5..ec36da6 100644 --- a/src/drivers/simplefocnano/SimpleFOCNanoDriver.cpp +++ b/src/drivers/simplefocnano/SimpleFOCNanoDriver.cpp @@ -1,7 +1,7 @@ #include "./SimpleFOCNanoDriver.h" -#if defined(ARDUINO_NANO_ESP32) || defined(ARDUINO_NANO_RP2040_CONNECT) || defined(ARDUINO_SAMD_NANO_33_IOT) || defined(ARDUINO_ARDUINO_NANO33BLE) || defined(ARDUINO_AVR_NANO) || defined(ARDUINO_NANO_MATTER) +#if defined(ARDUINO_NANO_ESP32) || defined(ARDUINO_NANO_RP2040_CONNECT) || defined(ARDUINO_SAMD_NANO_33_IOT) || defined(ARDUINO_ARDUINO_NANO33BLE) || defined(ARDUINO_AVR_NANO) || defined(ARDUINO_NANO_MATTER) || defined(ARDUINO_NANO_R4) SimpleFOCNanoDriver::SimpleFOCNanoDriver() : BLDCDriver3PWM(PIN_INU, PIN_INV, PIN_INW, PIN_ENU, PIN_ENV, PIN_ENW) { // nothing to do here diff --git a/src/drivers/simplefocnano/SimpleFOCNanoDriver.h b/src/drivers/simplefocnano/SimpleFOCNanoDriver.h index 7c63632..f778554 100644 --- a/src/drivers/simplefocnano/SimpleFOCNanoDriver.h +++ b/src/drivers/simplefocnano/SimpleFOCNanoDriver.h @@ -4,7 +4,7 @@ #include -#if defined(ARDUINO_NANO_ESP32) || defined(ARDUINO_NANO_RP2040_CONNECT) || defined(ARDUINO_SAMD_NANO_33_IOT) || defined(ARDUINO_ARDUINO_NANO33BLE) || defined(ARDUINO_AVR_NANO) || defined(ARDUINO_NANO_MATTER) +#if defined(ARDUINO_NANO_ESP32) || defined(ARDUINO_NANO_RP2040_CONNECT) || defined(ARDUINO_SAMD_NANO_33_IOT) || defined(ARDUINO_ARDUINO_NANO33BLE) || defined(ARDUINO_AVR_NANO) || defined(ARDUINO_NANO_MATTER) || defined(ARDUINO_NANO_R4) /* * Default pins for the SimpleFOC Nano board @@ -15,13 +15,35 @@ * */ +#ifndef D3 +#define D3 3 +#endif +#ifndef D4 +#define D4 4 +#endif +#ifndef D6 +#define D6 6 +#endif +#ifndef D7 +#define D7 7 +#endif +#ifndef D8 +#define D8 8 +#endif +#ifndef D9 +#define D9 9 +#endif +#ifndef D10 +#define D10 10 +#endif + -#define PIN_INU 3 -#define PIN_INV 6 -#define PIN_INW 9 -#define PIN_ENU 4 -#define PIN_ENV 7 -#define PIN_ENW 8 +#define PIN_INU D3 +#define PIN_INV D6 +#define PIN_INW D9 +#define PIN_ENU D4 +#define PIN_ENV D7 +#define PIN_ENW D8 #ifdef ARDUINO_NANO_RP2040_CONNECT #define PIN_nSLEEP 17 #define PIN_nFAULT 20 @@ -32,7 +54,7 @@ #define PIN_nRST A7 #endif #define PIN_VBUS A0 -#define PIN_nCS 10 +#define PIN_nCS D10 #define VBUS_CONV_FACTOR (22.0f/2.2f) diff --git a/src/drivers/stspin32g4/STSPIN32G4.cpp b/src/drivers/stspin32g4/STSPIN32G4.cpp index 2917791..33dc475 100644 --- a/src/drivers/stspin32g4/STSPIN32G4.cpp +++ b/src/drivers/stspin32g4/STSPIN32G4.cpp @@ -37,10 +37,45 @@ int STSPIN32G4::init(){ // TODO init fault monitor + bootstrap_charge(); + return isReady() ? 1 : 0; }; - +void STSPIN32G4::enable() +{ + bootstrap_charge(); + BLDCDriver6PWM::enable(); +} + + +void STSPIN32G4::bootstrap_charge() +{ + //store phase states, enable low side drive + static const int num_phases = sizeof(phase_state)/sizeof(phase_state[0]); + PhaseState phase_state_before[num_phases]; + bool none_disabled = true; + for (size_t i = 0; i < num_phases; i++) + { + phase_state_before[i] = phase_state[i]; + if (phase_state[i] == PHASE_HI || phase_state[i] == PHASE_OFF) + { + none_disabled = false; + } + } + if (none_disabled) + { + return; + } + setPhaseState(PHASE_LO, PHASE_LO, PHASE_LO); + setPwm(0,0,0); + _delay(bootstrap_capacitor_charge_time); + //restore phase states + for (size_t i = 0; i < num_phases; i++) + { + phase_state[i] = phase_state_before[i]; + } +} void STSPIN32G4::wake() { digitalWrite(STSPIN32G4_PIN_WAKE, HIGH); diff --git a/src/drivers/stspin32g4/STSPIN32G4.h b/src/drivers/stspin32g4/STSPIN32G4.h index 3df8137..ed12acf 100644 --- a/src/drivers/stspin32g4/STSPIN32G4.h +++ b/src/drivers/stspin32g4/STSPIN32G4.h @@ -116,8 +116,13 @@ class STSPIN32G4 : public BLDCDriver6PWM { STSPIN32G4(); ~STSPIN32G4(); + unsigned int bootstrap_capacitor_charge_time = 10; //time to let the bootstrap capacitors charge for, in ms. During his time, brakes are applied. This is not necessary if the motor is rotating, as the BEMF allows the caps to charge. + int init() override; + void enable() override; + + void bootstrap_charge(); void wake(); void sleep(); bool isReady(); diff --git a/src/encoders/calibrated/CalibratedSensor.cpp b/src/encoders/calibrated/CalibratedSensor.cpp index 089b17a..cfc6ee1 100644 --- a/src/encoders/calibrated/CalibratedSensor.cpp +++ b/src/encoders/calibrated/CalibratedSensor.cpp @@ -3,8 +3,10 @@ // CalibratedSensor() // sensor - instance of original sensor object // n_lut - number of samples in the LUT -CalibratedSensor::CalibratedSensor(Sensor &wrapped, int n_lut, float *lut) +CalibratedSensor::CalibratedSensor(Sensor &wrapped, int n_lut, uint16_t *lut) : _wrapped(wrapped), n_lut(n_lut), allocated(false), calibrationLut(lut) { + lut_resolution = _2PI / n_lut; + lut_resolution_inv = 1.0f / lut_resolution; }; CalibratedSensor::~CalibratedSensor() { @@ -34,52 +36,55 @@ float CalibratedSensor::getSensorAngle() if(!calibrationLut) { return _wrapped.getMechanicalAngle(); } + // raw encoder position e.g. 0-2PI - float raw_angle = fmodf(_wrapped.getMechanicalAngle(), _2PI); - raw_angle += raw_angle < 0 ? _2PI:0; + float raw_angle = _wrapped.getMechanicalAngle(); + // wrap to 0-2PI only if needed (for Encoder sensors that can go beyond 2PI) + if (raw_angle < 0 || raw_angle >= _2PI) raw_angle = _normalizeAngle(raw_angle); - // Calculate the resolution of the LUT in radians - float lut_resolution = _2PI / n_lut; // Calculate LUT index - int lut_index = raw_angle / lut_resolution; - - // Get calibration values from the LUT - float y0 = calibrationLut[lut_index]; - float y1 = calibrationLut[(lut_index + 1) % n_lut]; - - // Linearly interpolate between the y0 and y1 values - // Calculate the relative distance from the y0 (raw_angle has to be between y0 and y1) - // If distance = 0, interpolated offset = y0 - // If distance = 1, interpolated offset = y1 - float distance = (raw_angle - lut_index * lut_resolution) / lut_resolution; - float offset = (1 - distance) * y0 + distance * y1; - - // Calculate the calibrated angle - return raw_angle - offset; + int lut_index = raw_angle * lut_resolution_inv; + + // Get calibration values from the LUT and decode them + float lut_entry_lower = decodeOffsetU16(calibrationLut[lut_index]); + float lut_entry_higher = decodeOffsetU16(calibrationLut[ lut_index >= n_lut-1 ? 0 : (lut_index + 1)]); + + // Linearly interpolate between the two closest LUT entries (one lower and one higher than the raw angle) + // Calculate the distance between the raw angle and the lower LUT entry + // Distance is normalized to [0,1] + float lut_lower_angle = lut_index * lut_resolution; + float distance_lower = (raw_angle - lut_lower_angle) * lut_resolution_inv; + // Linearly interpolate between lower and higher LUT entries + float correction_offset = (1.0f - distance_lower) * lut_entry_lower + distance_lower * lut_entry_higher; + + // Calculate and normalize the calibrated angle to keep it in [0, 2PI) + float calibrated_angle = raw_angle - correction_offset; + if (calibrated_angle < 0 || calibrated_angle >= _2PI) calibrated_angle = _normalizeAngle(calibrated_angle); + return calibrated_angle; } // Perform filtering to linearize position sensor eccentricity // FIR n-sample average, where n = number of samples in the window // This filter has zero gain at electrical frequency and all integer multiples // So cogging effects should be completely filtered out -void CalibratedSensor::filter_error(float* error, float &error_mean, int n_ticks, int window){ +void CalibratedSensor::filter_error(float* error, float &error_mean, int samples_per_full_rotation, int window){ float window_buffer[window]; memset(window_buffer, 0, window*sizeof(float)); float window_sum = 0; int buffer_index = 0; // fill the inital window buffer for (int i = 0; i < window; i++) { - int ind = n_ticks - window/2 -1 + i; - window_buffer[i] = error[ind % n_ticks]; + int ind = samples_per_full_rotation - window/2 -1 + i; + window_buffer[i] = error[ind % samples_per_full_rotation]; window_sum += window_buffer[i]; } // calculate the moving average error_mean = 0; - for (int i = 0; i < n_ticks; i++) + for (int i = 0; i < samples_per_full_rotation; i++) { // Update buffer window_sum -= window_buffer[buffer_index]; - window_buffer[buffer_index] = error[( i + window/2 ) %n_ticks]; + window_buffer[buffer_index] = error[( i + window/2 ) %samples_per_full_rotation]; window_sum += window_buffer[buffer_index]; // update the buffer index buffer_index = (buffer_index + 1) % window; @@ -87,7 +92,7 @@ void CalibratedSensor::filter_error(float* error, float &error_mean, int n_ticks // Update filtered error error[i] = window_sum / (float)window; // update the mean value - error_mean += error[i] / n_ticks; + error_mean += error[i] / samples_per_full_rotation; } } @@ -96,11 +101,13 @@ void CalibratedSensor::calibrate(FOCMotor &motor, int settle_time_ms) { // if the LUT is already defined, skip the calibration - if(calibrationLut == NULL) { + if (calibrationLut == NULL) { allocated = true; - calibrationLut = new float[n_lut]; + calibrationLut = new uint16_t[n_lut]; + } else { + SIMPLEFOC_DEBUG("SEN_CAL: Overwriting pre-defined LUT for calibration."); } - motor.monitor_port->println("Starting Sensor Calibration."); + SIMPLEFOC_DEBUG("SEN_CAL: Starting Sensor Calibration."); // Calibration variables @@ -110,21 +117,30 @@ void CalibratedSensor::calibrate(FOCMotor &motor, int settle_time_ms) // set the inital electric angle to 0 float elec_angle = 0.0; - // Calibration parameters - // The motor will take a n_pos samples per electrical cycle - // which amounts to n_ticks (n_pos * motor.pole_pairs) samples per mechanical rotation - // Additionally, the motor will take n2_ticks steps to reach any of the n_ticks posiitons - // incrementing the electrical angle by deltaElectricalAngle each time - int n_pos = 5; - int _NPP = motor.pole_pairs; // number of pole pairs which is user input - const int n_ticks = n_pos * _NPP; // number of positions to be sampled per mechanical rotation. Multiple of NPP for filtering reasons (see later) - const int n2_ticks = 5; // increments between saved samples (for smoothing motion) - float deltaElectricalAngle = _2PI * _NPP / (n_ticks * n2_ticks); // Electrical Angle increments for calibration steps - float error[n_ticks]; // pointer to error array (average of forward & backward) - memset(error, 0, n_ticks*sizeof(float)); - // The fileter window size is set to n_pos - one electrical cycle - // important for cogging filtering !!! - const int window = n_pos; // window size for moving average filter of raw error + /* + * Calibration procedure + * The calibration will rotate the motor shaft in small steps in total n_lut positions (in both directions) + * It will stop at each of these positions and read the sensor angle + * The error between the expected angle (from the motor electrical angle) and the actual angle + * (from the sensor) is stored in an array + */ + + // number of pole pairs which is user input + const int pole_pairs = motor.pole_pairs; + // number of positions per electrical cycle (If LUT size is not multiple of NPP, it is rounded up) + const int samples_per_elec_rotation = ceil((float)n_lut / (float)pole_pairs); + // number of positions to be sampled per mechanical rotation. + // Ideally it would be equal to the LUT size, but if the LUT size is not multiple of NPP, it is rounded up + const int samples_per_full_rotation = samples_per_elec_rotation * pole_pairs; + // pointer to error array (average of forward & backward) + float error[samples_per_full_rotation]; + memset(error, 0, samples_per_full_rotation*sizeof(float)); + + // Additional parameters for calibration loop + // total number of position to iterate through between each sampling position (to reduce jumps if LUT size is small) + const int intermeditate_positions = 5; + // Electrical Angle increments for calibration steps + float el_angle_increment = _2PI * pole_pairs / (samples_per_full_rotation * intermeditate_positions); // find the first guess of the motor.zero_electric_angle @@ -136,7 +152,7 @@ void CalibratedSensor::calibrate(FOCMotor &motor, int settle_time_ms) motor.current_sense = nullptr; motor.linkSensor(&this->_wrapped); if(!motor.initFOC()){ - motor.monitor_port->println("Failed to align the sensor."); + SIMPLEFOC_DEBUG("SEN_CAL: Failed to align the sensor."); return; } // link back the sensor and current sense @@ -153,22 +169,21 @@ void CalibratedSensor::calibrate(FOCMotor &motor, int settle_time_ms) /* Start Calibration - Loop over electrical angles from 0 to NPP*2PI, once forward, once backward + Loop over electrical angles from 0 to pole_pairs*2PI, once forward, once backward store actual position and error as compared to electrical angle */ /* forwards rotation */ - motor.monitor_port->print("Rotating: "); - motor.monitor_port->println( motor.sensor_direction == Direction::CCW ? "CCW" : "CW" ); + SIMPLEFOC_DEBUG(motor.sensor_direction == Direction::CCW ? "SEN_CAL: Rotating: CCW" : "SEN_CAL: Rotating: CW" ); float zero_angle_prev = 0.0; - for (int i = 0; i < n_ticks; i++) + for (int i = 0; i < samples_per_full_rotation; i++) { - for (int j = 0; j < n2_ticks; j++) // move to the next location + for (int j = 0; j < intermeditate_positions; j++) // move to the next location { _wrapped.update(); - elec_angle += deltaElectricalAngle; + elec_angle += el_angle_increment; motor.setPhaseVoltage(voltage_calibration, 0, elec_angle); } @@ -177,10 +192,10 @@ void CalibratedSensor::calibrate(FOCMotor &motor, int settle_time_ms) _wrapped.update(); // calculate the error theta_actual = (int)motor.sensor_direction * (_wrapped.getAngle() - theta_init); - error[i] = 0.5 * (theta_actual - elec_angle / _NPP); + error[i] = 0.5 * (theta_actual - elec_angle / pole_pairs); // calculate the current electrical zero angle - float zero_angle = ((int)motor.sensor_direction * _wrapped.getMechanicalAngle() * _NPP ) - (elec_angle + _PI_2); + float zero_angle = ((int)motor.sensor_direction * _wrapped.getMechanicalAngle() * pole_pairs ) - (elec_angle + _PI_2); zero_angle = _normalizeAngle(zero_angle); // remove the 2PI jumps if(zero_angle - zero_angle_prev > _PI){ @@ -189,26 +204,25 @@ void CalibratedSensor::calibrate(FOCMotor &motor, int settle_time_ms) zero_angle = zero_angle + _2PI; } zero_angle_prev = zero_angle; - avg_elec_angle += zero_angle/n_ticks; + avg_elec_angle += zero_angle/samples_per_full_rotation; - // motor.monitor_port->print(">zero:"); - // motor.monitor_port->println(zero_angle); - // motor.monitor_port->print(">zero_average:"); - // motor.monitor_port->println(avg_elec_angle/2.0); +#ifdef SIMPLEFOC_CALIBRATEDSENSOR_DEBUG + SIMPLEFOC_DEBUG(">zero:",zero_angle); + SIMPLEFOC_DEBUG(">zero_average:", (float)(avg_elec_angle)); +#endif } _delay(2000); /* backwards rotation */ - motor.monitor_port->print("Rotating: "); - motor.monitor_port->println( motor.sensor_direction == Direction::CCW ? "CW" : "CCW" ); - for (int i = n_ticks - 1; i >= 0; i--) + SIMPLEFOC_DEBUG(motor.sensor_direction == Direction::CCW ? "SEN_CAL: Rotating: CW" : "SEN_CAL: Rotating: CCW" ); + for (int i = samples_per_full_rotation - 1; i >= 0; i--) { - for (int j = 0; j < n2_ticks; j++) // move to the next location + for (int j = 0; j < intermeditate_positions; j++) // move to the next location { _wrapped.update(); - elec_angle -= deltaElectricalAngle; + elec_angle -= el_angle_increment; motor.setPhaseVoltage(voltage_calibration, 0, elec_angle); } @@ -217,9 +231,9 @@ void CalibratedSensor::calibrate(FOCMotor &motor, int settle_time_ms) _wrapped.update(); // calculate the error theta_actual = (int)motor.sensor_direction * (_wrapped.getAngle() - theta_init); - error[i] += 0.5 * (theta_actual - elec_angle / _NPP); + error[i] += 0.5 * (theta_actual - elec_angle / pole_pairs); // calculate the current electrical zero angle - float zero_angle = ((int)motor.sensor_direction * _wrapped.getMechanicalAngle() * _NPP ) - (elec_angle + _PI_2); + float zero_angle = ((int)motor.sensor_direction * _wrapped.getMechanicalAngle() * pole_pairs ) - (elec_angle + _PI_2); zero_angle = _normalizeAngle(zero_angle); // remove the 2PI jumps if(zero_angle - zero_angle_prev > _PI){ @@ -228,12 +242,11 @@ void CalibratedSensor::calibrate(FOCMotor &motor, int settle_time_ms) zero_angle = zero_angle + _2PI; } zero_angle_prev = zero_angle; - avg_elec_angle += zero_angle/n_ticks; - - // motor.monitor_port->print(">zero:"); - // motor.monitor_port->println(zero_angle); - // motor.monitor_port->print(">zero_average:"); - // motor.monitor_port->println(avg_elec_angle/2.0); + avg_elec_angle += zero_angle/samples_per_full_rotation; +#ifdef SIMPLEFOC_CALIBRATEDSENSOR_DEBUG + SIMPLEFOC_DEBUG(">zero:", zero_angle); + SIMPLEFOC_DEBUG(">zero_average:", (float)(avg_elec_angle/2.0)); +#endif } // get post calibration mechanical angle. @@ -248,23 +261,27 @@ void CalibratedSensor::calibrate(FOCMotor &motor, int settle_time_ms) // calculating the average zero electrical angle from the forward calibration. motor.zero_electric_angle = _normalizeAngle(avg_elec_angle / (2.0)); - motor.monitor_port->print("Average Zero Electrical Angle: "); - motor.monitor_port->println(motor.zero_electric_angle); + SIMPLEFOC_DEBUG("SEN_CAL: Average Zero Electrical Angle: ", motor.zero_electric_angle); _delay(1000); // Perform filtering to linearize position sensor eccentricity // FIR n-sample average, where n = number of samples in one electrical cycle // This filter has zero gain at electrical frequency and all integer multiples // So cogging effects should be completely filtered out + + // The fileter window size is set to samples_per_elec_rotation - one electrical cycle + // important for cogging filtering !!! + // window size for moving average filter of raw error + const int window_size = samples_per_elec_rotation; float error_mean = 0.0; - this->filter_error(error, error_mean, n_ticks, window); + this->filter_error(error, error_mean, samples_per_full_rotation, window_size); _delay(1000); // calculate offset index int index_offset = floor((float)n_lut * raw_offset / _2PI); - float dn = n_ticks / (float)n_lut; + float dn = samples_per_full_rotation / (float)n_lut; - motor.monitor_port->println("Constructing LUT."); + SIMPLEFOC_DEBUG("SEN_CAL: Constructing LUT."); _delay(1000); // Build Look Up Table for (int i = 0; i < n_lut; i++) @@ -272,36 +289,48 @@ void CalibratedSensor::calibrate(FOCMotor &motor, int settle_time_ms) int ind = index_offset + i*motor.sensor_direction; if (ind > (n_lut - 1)) ind -= n_lut; if (ind < 0) ind += n_lut; - calibrationLut[ind] = (float)(error[(int)(i * dn)] - error_mean); + float offset_value = (float)(error[(int)(i * dn)] - error_mean); // negate the error if the sensor is in the opposite direction - calibrationLut[ind] = (int)motor.sensor_direction * calibrationLut[ind]; + offset_value = (int)motor.sensor_direction * offset_value; + // encode to uint16_t + calibrationLut[ind] = encodeOffsetU16(offset_value); } - motor.monitor_port->println(""); _delay(1000); + SIMPLEFOC_DEBUG("SEN_CAL: Sensor Calibration Done."); + + // pre-compute inverse LUT resolution for faster lookups + lut_resolution = _2PI / n_lut; + lut_resolution_inv = 1.0 / lut_resolution; +} + +// print the LUT for debugging +void CalibratedSensor::printLUT(FOCMotor &motor, Print &printer) +{ // Display the LUT - motor.monitor_port->print("float calibrationLut["); - motor.monitor_port->print(n_lut); - motor.monitor_port->println("] = {"); + printer.println(F("")); + printer.println(F("// Calibrated Sensor LUT")); + printer.print(F("uint16_t calibrationLut[")); + printer.print(n_lut); + printer.println(F("] = {")); _delay(100); for (int i=0;i < n_lut; i++){ - motor.monitor_port->print(calibrationLut[i],6); - if(i < n_lut - 1) motor.monitor_port->print(", "); + printer.print(calibrationLut[i]); + if(i < n_lut - 1) printer.print(F(", ")); _delay(1); } - motor.monitor_port->println("};"); + printer.println(F("")); + printer.println(F("};")); _delay(1000); // Display the zero electrical angle - motor.monitor_port->print("float zero_electric_angle = "); - motor.monitor_port->print(motor.zero_electric_angle,6); - motor.monitor_port->println(";"); + printer.print(F("float zero_electric_angle = ")); + printer.print(motor.zero_electric_angle); + printer.println(F(";")); // Display the sensor direction - motor.monitor_port->print("Direction sensor_direction = "); - motor.monitor_port->println(motor.sensor_direction == Direction::CCW ? "Direction::CCW;" : "Direction::CW;"); + printer.print(F("Direction sensor_direction = ")); + printer.println(motor.sensor_direction == Direction::CCW ? "Direction::CCW;" : "Direction::CW;"); + printer.println(F("")); _delay(1000); - - motor.monitor_port->println("Sensor Calibration Done."); -} - +} \ No newline at end of file diff --git a/src/encoders/calibrated/CalibratedSensor.h b/src/encoders/calibrated/CalibratedSensor.h index 29a0a8d..7fc588a 100644 --- a/src/encoders/calibrated/CalibratedSensor.h +++ b/src/encoders/calibrated/CalibratedSensor.h @@ -5,7 +5,15 @@ #include "BLDCMotor.h" #include "common/base_classes/FOCMotor.h" #include "common/foc_utils.h" +#include "communication/SimpleFOCDebug.h" +// LUT quantization constants for uint16_t encoding +// Maps radians in range [-PI, PI] to uint16_t [0, 65535] +// The real error is probably going to be much smaller than that +// range so in some cases it might make sense to +// raise the LUT_SCALE constants to improve resolution. +#define LUT_SCALE 10430.2191955f // 65535 / (2*PI) ~ +#define LUT_OFFSET 32767.0f // center value for symmetric range class CalibratedSensor: public Sensor{ @@ -16,7 +24,7 @@ class CalibratedSensor: public Sensor{ * @param n_lut the number of entries in the lut * @param lut the look up table (if null, the lut will be allocated on the heap) */ - CalibratedSensor(Sensor& wrapped, int n_lut = 200, float* lut = nullptr); + CalibratedSensor(Sensor& wrapped, int n_lut = 200, uint16_t* lut = nullptr); ~CalibratedSensor(); @@ -30,6 +38,15 @@ class CalibratedSensor: public Sensor{ */ virtual void calibrate(FOCMotor& motor, int settle_time_ms = 30); + /** + * Print the LUT to the provided Print instance + * @param motor the FOCMotor instance (for direction info) + * @param printer the Print instance to print to + * + * Print in the form of C++ code array for easy copy-pasting + */ + void printLUT(FOCMotor& motor, Print &printer); + // voltage to run the calibration: user input float voltage_calibration = 1; protected: @@ -58,7 +75,14 @@ class CalibratedSensor: public Sensor{ // depending on the size of the lut // will be allocated in the calibrate function if not given. bool allocated; - float* calibrationLut; + uint16_t* calibrationLut; + // pre-computed inverse LUT resolution for faster lookups: n_lut / (2*PI) + float lut_resolution_inv { 0.0f }; + float lut_resolution { 0.0f }; + + // Helper functions for quantization + inline uint16_t encodeOffsetU16(float offset) { return (uint16_t)(offset * LUT_SCALE + LUT_OFFSET); } + inline float decodeOffsetU16(uint16_t encoded) { return (encoded - LUT_OFFSET) / LUT_SCALE; } }; #endif diff --git a/src/encoders/calibrated/README.md b/src/encoders/calibrated/README.md index 9a1c823..f8cfdb5 100644 --- a/src/encoders/calibrated/README.md +++ b/src/encoders/calibrated/README.md @@ -1,33 +1,43 @@ # Calibrated Sensor -by [@MarethyuPrefect](https://github.com/MarethyuPrefect) +Originally implemented by [@MarethyuPrefect](https://github.com/MarethyuPrefect) -A SimpleFOC Sensor wrapper implementation which adds sensor eccentricity calibration. +A SimpleFOC Sensor wrapper that corrects for **eccentricity errors** in magnetic sensor measurements. It builds a lookup table (LUT) during calibration to linearize sensor output and improve torque control accuracy. -Please also see our [forum thread](https://community.simplefoc.com/t/simplefoc-sensor-eccentricity-calibration/2212) on this topic. +**More info**: See the [SimpleFOC forum thread](https://community.simplefoc.com/t/simplefoc-sensor-eccentricity-calibration/2212) for detailed discussion. +## The Problem: Sensor Eccentricity -When you mount your (magnetic) sensor on your frame or motor, there will always be a slight misalignment between magnet and sensor (measurement system). This misalignment between center of rotation and the center of the sensor is called the eccentricity error. +When mounting a magnetic sensor on a motor, the magnet and sensor centers are rarely perfectly aligned. This **eccentricity error** causes: +- Non-linear sensor output across the rotor's range +- Variable measurement errors as the motor rotates +- Distortion in I_q (torque) control, reducing control performance -As a result your measurement system output is non-linear with respect to the rotor of the motor. This will cause an error with respect to the ideal torque you attempt to create with the I_q vector as function of the position. You could interpret this as a disturbance on your control loop which you want to minimize for optimal performance. +Example: A 0.5mm offset on a 3mm-diameter magnet can introduce ~10% error in angle measurement. -This calibration compensates the sensor reading in a feed forward fashion such that your performance improves. +## The Solution: Calibration LUT + +CalibratedSensor runs a calibration routine that: +1. Rotates the motor through several full turns +2. Records raw sensor readings at fixed intervals (creates lookup table) +3. Outputs corrective offset values (or stores them on disk) +4. Applies the LUT to sensor readings in real-time, linearizing output + +Result: Non-linearity is greatly reduced, improving torque control loop stability and accuracy. ## Hardware setup -Connect your sensor as usual. Make sure the sensor is working 'normally' i.e. without calibration first. Once things are working and tuned without sensor calibration, you can add the CalibratedSensor to see if you get an improvement. +Connect your sensor as usual. Make sure the sensor is working 'normally' i.e. without calibration first. Once things are working and tuned without sensor calibration, you can add the `CalibratedSensor` to see if you get an improvement. Note that during calibration, the motor is turned through several turns, and should be in an unloaded condition. Please ensure your hardware setup can support the motor rotating through full turns. ## Softwate setup -The CalibratedSensor acts as a wrapper to the actual sensor class. When creating the CalibratedSensor object, provide the real -sensor to the constructor of CalibratedSensor. +The `CalibratedSensor` acts as a wrapper to the actual `Sensor` class. When creating the `CalibratedSensor` object, provide the real sensor to the constructor of `CalibratedSensor`. -First, initialize the real sensor instance as normal. Then, call calibrate() on the CalibratedSensor instance. Then link the -CalibratedSensor to the motor and call motor.initFOC(). +First, initialize the real sensor instance as normal. Then, call `calibrate()` on the `CalibratedSensor` instance. Then link the `CalibratedSensor` to the motor and call `motor.initFOC()`. The motor will then use the calibrated sensor instance. @@ -42,6 +52,11 @@ BLDCDriver3PWM driver = BLDCDriver3PWM(PB4,PC7,PB10,PA9); CalibratedSensor sensor_calibrated = CalibratedSensor(sensor); void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable debug output + SimpleFOCDebug::enable(&Serial); + sensor.init(); // Link motor to sensor motor.linkSensor(&sensor); @@ -49,18 +64,8 @@ void setup() { driver.voltage_power_supply = 20; driver.init(); motor.linkDriver(&driver); - // aligning voltage - motor.voltage_sensor_align = 8; - motor.voltage_limit = 20; - // set motion control loop to be used - motor.controller = MotionControlType::torque; - - // use monitoring with serial - Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); - motor.monitor_variables = _MON_VEL; - motor.monitor_downsample = 10; // default 10 // initialize motor motor.init(); @@ -69,8 +74,6 @@ void setup() { sensor_calibrated.voltage_calibration = 6; // Running calibration sensor_calibrated.calibrate(motor); - - //Serial.println("Calibrating Sensor Done."); // Linking sensor to motor object motor.linkSensor(&sensor_calibrated); @@ -81,59 +84,75 @@ void setup() { Please see the more complete [example](https://github.com/simplefoc/Arduino-FOC-drivers/blob/master/examples/encoders/calibrated/sensor_calibration.ino) in our examples directory. -## EDIT March 2025 +## Saving the LUT persistently in the code +After running the calibration once, you can save the generated LUT and load it on startup to avoid recalibration on each startup. -The code has been rewritten to reduce its memory footprint and allow more flexible Lookup table (LUT) sizing. -Additionally, the calibrated sensor class now supports providing the saved LUT as a paramer for calibration. This allows you to save the LUT and load it on startup to avoid recalibration on each startup. +Make sure to call `calibrate()` once to generate the LUT, then use `printLUT()` to output the LUT to Serial. + +```cpp +sensor_calibrated.calibrate(motor); // run the calibration +sensor_calibrated.printLUT(motor, Serial); // print the LUT to serial monitor +``` -Once you do the calibration once, it will output something like this: +Copy the output and paste it into your code as shown below. ``` ... - -Starting Sensor Calibration. -MOT: Align sensor. +SEN_CAL: Starting Sensor Calibration. MOT: sensor_direction==CCW MOT: PP check: OK! -MOT: Zero elec. angle: 3.17 +MOT: Zero elec. angle: 2.83 MOT: No current sense. -MOT: Ready.Rotating: CCW -Rotating: CW -Average Zero Electrical Angle: 4.01 -Constructing LUT. - -float calibrationLut[50] = {0.003486, 0.005795, 0.007298, 0.008303, 0.008771, 0.007551, 0.005986, 0.004115, 0.001361, -0.001392, -0.004069, -0.007474, -0.010420, -0.013135, -0.014891, -0.017415, -0.018328, -0.019125, -0.018849, -0.017193, -0.015152, -0.012422, -0.008579, -0.003970, 0.000678, 0.005211, 0.009821, 0.013280, 0.016470, 0.018127, 0.018376, 0.016969, 0.016716, 0.015466, 0.013602, 0.011431, 0.008646, 0.006092, 0.003116, 0.000409, -0.002342, -0.004367, -0.005932, -0.006998, -0.007182, -0.007175, -0.006017, -0.003746, -0.001783, 0.000948}; -float zero_electric_angle = 4.007072; +MOT: Ready. +SEN_CAL: Rotating: CCW +SEN_CAL: Rotating: CW +SEN_CAL: Average Zero Electrical Angle: 2.74 +SEN_CAL: Constructing LUT. +SEN_CAL: Sensor Calibration Done. + +// Calibrated Sensor LUT +uint16_t calibrationLut[200] = {32701, 32694, 32689, 32684, 32678, 32671, 32665, 32661, 32656, 32649, 32644, 32641, 32638, 32634, 32629, 32624, 32619, 32612, 32609, 32607, 32604, 32600, 32596, 32594, 32594, 32591, 32588, 32587, 32588, 32592, 32593, 32594, 32594, 32594, 32597, 32602, 32605, 32609, 32614, 32620, 32627, 32632, 32636, 32642, 32649, 32658, 32666, 32673, 32681, 32687, 32692, 32702, 32711, 32719, 32725, 32732, 32740, 32748, 32755, 32760, 32766, 32773, 32781, 32787, 32792, 32796, 32797, 32800, 32804, 32807, 32809, 32809, 32810, 32810, 32812, 32810, 32808, 32808, 32808, 32808, 32807, 32804, 32804, 32803, 32802, 32803, 32805, 32805, 32804, 32803, 32803, 32805, 32804, 32802, 32803, 32805, 32820, 32824, 32826, 32823, 32817, 32810, 32807, 32807, 32807, 32805, 32803, 32803, 32806, 32809, 32808, 32807, 32806, 32804, 32802, 32799, 32796, 32791, 32791, 32791, 32789, 32787, 32786, 32786, 32785, 32783, 32781, 32783, 32784, 32786, 32788, 32790, 32790, 32791, 32791, 32795, 32800, 32803, 32806, 32810, 32815, 32820, 32824, 32827, 32832, 32838, 32845, 32850, 32856, 32860, 32863, 32868, 32874, 32879, 32883, 32886, 32890, 32894, 32899, 32902, 32903, 32906, 32911, 32912, 32911, 32909, 32905, 32903, 32902, 32899, 32895, 32891, 32886, 32883, 32879, 32872, 32866, 32860, 32856, 32850, 32843, 32835, 32827, 32818, 32810, 32804, 32798, 32790, 32781, 32774, 32768, 32761, 32753, 32745, 32739, 32735, 32729, 32722, 32716, 32708}; +float zero_electric_angle = 2.74; Direction sensor_direction = Direction::CCW; -Sensor Calibration Done ... ``` The LUT and sensor's zero angle and direction are outputed by the calibration process to the Serial terminal. So you can copy and paste them into your code. +## Workflow: Fast Recalibration Using Saved LUT + +If you calibrate once during setup and save the LUT to EEPROM or hardcode it, you can skip re-calibration on every startup: + +1. **First run**: Call `calibrate(motor)` → Serial outputs LUT +2. **Copy LUT**: Paste the generated values into your code +3. **Subsequent runs**: Pass LUT to constructor and DO NOT call `calibrate()` → instantaneous, no rotation needed + Your code will look something like this: ```c++ // number of LUT entries -const N_LUT = 50; -// Lookup table that has been ouptut from the calibration process -float calibrationLut[50] = {0.003486, 0.005795, 0.007298, 0.008303, 0.008771, 0.007551, 0.005986, 0.004115, 0.001361, -0.001392, -0.004069, -0.007474, -0.010420, -0.013135, -0.014891, -0.017415, -0.018328, -0.019125, -0.018849, -0.017193, -0.015152, -0.012422, -0.008579, -0.003970, 0.000678, 0.005211, 0.009821, 0.013280, 0.016470, 0.018127, 0.018376, 0.016969, 0.016716, 0.015466, 0.013602, 0.011431, 0.008646, 0.006092, 0.003116, 0.000409, -0.002342, -0.004367, -0.005932, -0.006998, -0.007182, -0.007175, -0.006017, -0.003746, -0.001783, 0.000948}; -float zero_electric_angle = 4.007072; +const N_LUT = 200; +// Lookup table that has been output from the calibration process +// The LUT is now stored as uint16_t for 50% memory savings (2 bytes vs 4 bytes per entry) +// Calibrated Sensor LUT +uint16_t calibrationLut[200] = {32701, 32694, 32689, 32684, 32678, 32671, 32665, 32661, 32656, 32649, 32644, 32641, 32638, 32634, 32629, 32624, 32619, 32612, 32609, 32607, 32604, 32600, 32596, 32594, 32594, 32591, 32588, 32587, 32588, 32592, 32593, 32594, 32594, 32594, 32597, 32602, 32605, 32609, 32614, 32620, 32627, 32632, 32636, 32642, 32649, 32658, 32666, 32673, 32681, 32687, 32692, 32702, 32711, 32719, 32725, 32732, 32740, 32748, 32755, 32760, 32766, 32773, 32781, 32787, 32792, 32796, 32797, 32800, 32804, 32807, 32809, 32809, 32810, 32810, 32812, 32810, 32808, 32808, 32808, 32808, 32807, 32804, 32804, 32803, 32802, 32803, 32805, 32805, 32804, 32803, 32803, 32805, 32804, 32802, 32803, 32805, 32820, 32824, 32826, 32823, 32817, 32810, 32807, 32807, 32807, 32805, 32803, 32803, 32806, 32809, 32808, 32807, 32806, 32804, 32802, 32799, 32796, 32791, 32791, 32791, 32789, 32787, 32786, 32786, 32785, 32783, 32781, 32783, 32784, 32786, 32788, 32790, 32790, 32791, 32791, 32795, 32800, 32803, 32806, 32810, 32815, 32820, 32824, 32827, 32832, 32838, 32845, 32850, 32856, 32860, 32863, 32868, 32874, 32879, 32883, 32886, 32890, 32894, 32899, 32902, 32903, 32906, 32911, 32912, 32911, 32909, 32905, 32903, 32902, 32899, 32895, 32891, 32886, 32883, 32879, 32872, 32866, 32860, 32856, 32850, 32843, 32835, 32827, 32818, 32810, 32804, 32798, 32790, 32781, 32774, 32768, 32761, 32753, 32745, 32739, 32735, 32729, 32722, 32716, 32708}; +float zero_electric_angle = 2.74; Direction sensor_direction = Direction::CCW; // provide the sensor class and the number of points in the LUT -CalibratedSensor sensor_calibrated = CalibratedSensor(sensor, N_LUT); +CalibratedSensor sensor_calibrated = CalibratedSensor(sensor, N_LUT, calibrationLut); ... void setup() { ... - // as LUT is provided to this function - sensor_calibrated.calibrate(motor, calibrationLut, zero_eletrical_angle, sensor_direction); - ... - + // No need to call calibrate() any more as the LUT is provided + // Only link the calibrated sensor to the motor motor.linkSensor(&sensor_calibrated); + // provide the saved zero angle and direction + motor.zero_electric_angle = zero_electric_angle; + motor.sensor_direction = sensor_direction; ... motor.initFOC(); @@ -143,7 +162,19 @@ void setup() { ``` +## Changelog + +#### **Dec 2024** +- A rewrite to reduce memory usage passed to `uint16_t` LUT type. + - Previous `float` LUT used 4 bytes per entry, `uint16_t` uses 2 bytes - a 50% reduction. + - The LUT values are scaled internally to maintain precision. + - Precision is 2PI/65536 radians (~0.005 degrees), which is sufficient for most applications. If more precision is needed, consider increasing the LUT_SCALE constant in `CalibratedSensor.h`. +- Fixed bug due to hardcoded number of sampled positions. + - Previously, the LUT size was variable but the number of sampled positions was hardcoded to `pole_pairs * 5`. + - Now the number of samples is derived from the LUT size provided to the constructor. + + ## Future work -- Reduce the LUT size by using a more efficient LUT type - maybe pass to uint16_t -- Use a more eficient LUT interpolation method - maybe a polynomial interpolation \ No newline at end of file +- Use a more efficient LUT interpolation method - maybe a polynomial interpolation +- Support for saving/loading LUT to/from persistent storage (EEPROM, Flash) diff --git a/src/encoders/esp32hwencoder/ESP32HWEncoder.cpp b/src/encoders/esp32hwencoder/ESP32HWEncoder.cpp index 8126043..da29c42 100644 --- a/src/encoders/esp32hwencoder/ESP32HWEncoder.cpp +++ b/src/encoders/esp32hwencoder/ESP32HWEncoder.cpp @@ -1,6 +1,6 @@ #include "ESP32HWEncoder.h" -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) +#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(CONFIG_SOC_PCNT_SUPPORTED) diff --git a/src/encoders/esp32hwencoder/ESP32HWEncoder.h b/src/encoders/esp32hwencoder/ESP32HWEncoder.h index ad3a42f..b90215e 100644 --- a/src/encoders/esp32hwencoder/ESP32HWEncoder.h +++ b/src/encoders/esp32hwencoder/ESP32HWEncoder.h @@ -5,7 +5,8 @@ #if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) - +#include "sdkconfig.h" +#if defined(CONFIG_SOC_PCNT_SUPPORTED) #include "driver/pcnt.h" #include "soc/pcnt_struct.h" #include "common/base_classes/Sensor.h" @@ -62,5 +63,5 @@ class ESP32HWEncoder : public Sensor{ int32_t cpr; // Counts per rotation = 4 * ppr for quadrature encoders float inv_cpr; }; - +#endif #endif \ No newline at end of file diff --git a/src/encoders/hall_any/HallSensorAny.cpp b/src/encoders/hall_any/HallSensorAny.cpp new file mode 100644 index 0000000..b4bf9e9 --- /dev/null +++ b/src/encoders/hall_any/HallSensorAny.cpp @@ -0,0 +1,259 @@ +#include "HallSensorAny.h" +#include "common/time_utils.h" +#include "communication/SimpleFOCDebug.h" + + +// seq 1 > 5 > 4 > 6 > 2 > 3 > 1 000 001 010 011 100 101 110 111 +const int8_t ELECTRIC_SECTORS[8] = { -1, 0, 4, 5, 2, 1, 3 , -1 }; + +/* + HallSensorAny(int hallA, int hallB , int cpr, int index) + - hallA, hallB, hallC - HallSensorAny A, B and C pins + - pp - pole pairs +*/ +HallSensorAny::HallSensorAny(int _hallA, int _hallB, int _hallC, int _pp, HallType _hall_type){ + + // hardware pins + pinA = _hallA; + pinB = _hallB; + pinC = _hallC; + hall_type = _hall_type; + last_print_type = hall_type; + for (size_t i = 0; i < sizeof(previous_states); i++) + { + previous_states[i] = -1; + } + + + // hall has 6 segments per electrical revolution + cpr = _pp * 6; + + // extern pullup as default + pullup = Pullup::USE_EXTERN; +} + +// HallSensorAny interrupt callback functions +// A channel +void HallSensorAny::handleA() { + A_active= digitalRead(pinA); + updateState(); +} +// B channel +void HallSensorAny::handleB() { + B_active = digitalRead(pinB); + updateState(); +} + +// C channel +void HallSensorAny::handleC() { + C_active = digitalRead(pinC); + updateState(); +} + +/** + * Updates the state and sector following an interrupt + */ +void HallSensorAny::updateState() { + int8_t new_hall_state = C_active + (B_active << 1) + (A_active << 2); + // glitch avoidance #1 - sometimes we get an interrupt but pins haven't changed + if (new_hall_state == hall_state_raw) return; + hall_state_raw = new_hall_state; + + static const int num_previous_states = sizeof(previous_states); + + //flip a line maybe + if (hall_type != HallType::UNKNOWN) + { + new_hall_state ^= static_cast(hall_type); + } + + long new_pulse_timestamp = _micros(); + if (hall_type == HallType::UNKNOWN) //Store previous steps for hall config detection + { + for (int i = num_previous_states - 2; i >= 0; i--) + { + previous_states[i+1] = previous_states[i]; + } + previous_states[0] = new_hall_state; + //7 and 0 are illegal in 120deg mode, so we're gonna try to see which line hel up during that time and flip it so it doesn't happen + if ((previous_states[1] == 0b111 || previous_states[1] == 0b000) && previous_states[2] != -1) + { + if (previous_states[2] == previous_states[0]) + { + //went back, can't do anything + } + else + { + hall_type = static_cast((0b111 - previous_states[0] ^ previous_states[2])%8); + previous_states[0] ^= static_cast(hall_type); + } + } + if (abs(electric_rotations) > 2) + { + hall_type = HallType::HALL_120; + } + } + + + + + int8_t new_electric_sector; + new_electric_sector = ELECTRIC_SECTORS[new_hall_state]; + int8_t electric_sector_dif = new_electric_sector - electric_sector; + if (electric_sector_dif > 3) { + //underflow + direction = Direction::CCW; + electric_rotations += direction; + } else if (electric_sector_dif < (-3)) { + //overflow + direction = Direction::CW; + electric_rotations += direction; + } else { + direction = (new_electric_sector > electric_sector)? Direction::CW : Direction::CCW; + } + electric_sector = new_electric_sector; + + // glitch avoidance #2 changes in direction can cause velocity spikes. Possible improvements needed in this area + if (direction == old_direction) { + // not oscilating or just changed direction + pulse_diff = new_pulse_timestamp - pulse_timestamp; + } else { + pulse_diff = 0; + } + + pulse_timestamp = new_pulse_timestamp; + total_interrupts++; + old_direction = direction; + if (onSectorChange != nullptr) onSectorChange(electric_sector); +} + +/** + * Optionally set a function callback to be fired when sector changes + * void onSectorChange(int sector) { + * ... // for debug or call driver directly? + * } + * sensor.attachSectorCallback(onSectorChange); + */ +void HallSensorAny::attachSectorCallback(void (*_onSectorChange)(int sector)) { + onSectorChange = _onSectorChange; +} + + + +// Sensor update function. Safely copy volatile interrupt variables into Sensor base class state variables. +void HallSensorAny::update() { + // Copy volatile variables in minimal-duration blocking section to ensure no interrupts are missed + if (use_interrupt){ + noInterrupts(); + }else{ + A_active = digitalRead(pinA); + B_active = digitalRead(pinB); + C_active = digitalRead(pinC); + updateState(); + } + + angle_prev_ts = pulse_timestamp; + long last_electric_rotations = electric_rotations; + int8_t last_electric_sector = electric_sector; + if (use_interrupt) interrupts(); + angle_prev = ((float)((last_electric_rotations * 6 + last_electric_sector) % cpr) / (float)cpr) * _2PI ; + full_rotations = (int32_t)((last_electric_rotations * 6 + last_electric_sector) / cpr); + if (last_print_type != hall_type) + { + last_print_type = hall_type; + switch (hall_type) + { + case HallType::HALL_120 : + SIMPLEFOC_DEBUG("HALL: Found type: HALL_120"); + break; + case HallType::HALL_60A : + SIMPLEFOC_DEBUG("HALL: Found type: HALL_60A"); + break; + case HallType::HALL_60B : + SIMPLEFOC_DEBUG("HALL: Found type: HALL_60B"); + break; + case HallType::HALL_60C : + SIMPLEFOC_DEBUG("HALL: Found type: HALL_60C"); + break; + + default: + SIMPLEFOC_DEBUG("HALL: Type unknown! Wtf!"); + break; + } + + } + +} + + + +/* + Shaft angle calculation + TODO: numerical precision issue here if the electrical rotation overflows the angle will be lost +*/ +float HallSensorAny::getSensorAngle() { + return ((float)(electric_rotations * 6 + electric_sector) / (float)cpr) * _2PI ; +} + +/* + Shaft velocity calculation + function using mixed time and frequency measurement technique +*/ +float HallSensorAny::getVelocity(){ + noInterrupts(); + long last_pulse_timestamp = pulse_timestamp; + long last_pulse_diff = pulse_diff; + interrupts(); + if (last_pulse_diff == 0 || ((long)(_micros() - last_pulse_timestamp) > last_pulse_diff*2) ) { // last velocity isn't accurate if too old + return 0; + } else { + return direction * (_2PI / (float)cpr) / (last_pulse_diff / 1000000.0f); + } + +} + +int HallSensorAny::needsSearch() +{ + return hall_type == HallType::UNKNOWN; +} + +// HallSensorAny initialisation of the hardware pins +// and calculation variables +void HallSensorAny::init(){ + // initialise the electrical rotations to 0 + electric_rotations = 0; + + // HallSensorAny - check if pullup needed for your HallSensorAny + if(pullup == Pullup::USE_INTERN){ + pinMode(pinA, INPUT_PULLUP); + pinMode(pinB, INPUT_PULLUP); + pinMode(pinC, INPUT_PULLUP); + }else{ + pinMode(pinA, INPUT); + pinMode(pinB, INPUT); + pinMode(pinC, INPUT); + } + + // init hall_state + A_active = digitalRead(pinA); + B_active = digitalRead(pinB); + C_active = digitalRead(pinC); + updateState(); + + pulse_timestamp = _micros(); + + // we don't call Sensor::init() here because init is handled in HallSensorAny class. +} + +// function enabling hardware interrupts for the callback provided +// if callback is not provided then the interrupt is not enabled +void HallSensorAny::enableInterrupts(void (*doA)(), void(*doB)(), void(*doC)()){ + // attach interrupt if functions provided + + // A, B and C callback + if(doA != nullptr) attachInterrupt(digitalPinToInterrupt(pinA), doA, CHANGE); + if(doB != nullptr) attachInterrupt(digitalPinToInterrupt(pinB), doB, CHANGE); + if(doC != nullptr) attachInterrupt(digitalPinToInterrupt(pinC), doC, CHANGE); + + use_interrupt = true; +} diff --git a/src/encoders/hall_any/HallSensorAny.h b/src/encoders/hall_any/HallSensorAny.h new file mode 100644 index 0000000..1d94554 --- /dev/null +++ b/src/encoders/hall_any/HallSensorAny.h @@ -0,0 +1,112 @@ +#pragma once + +#include "Arduino.h" +#include "common/base_classes/Sensor.h" + +class HallSensorAny: public Sensor{ + public: + + + enum class HallType : uint8_t + { + HALL_120 = 0, + HALL_60C = 0b001, + HALL_60B = 0b010, + HALL_60A = 0b100, + UNKNOWN = 0b111 + }; + + /** + HallSensorAny class constructor + @param encA HallSensor A pin + @param encB HallSensor B pin + @param encC HallSensor C pin + @param pp pole pairs (e.g hoverboard motor has 15pp and small gimbals often have 7pp) + @param hall_60deg Indicate if the hall sensors are 60 degrees apart electrically (means that they can all be one or off at the same time). In 60deg mode, B needs to lead, so you may need to swap the connections until you find one that works + */ + HallSensorAny(int encA, int encB, int encC, int pp, HallType hall_type = HallType::UNKNOWN); + + /** HallSensor initialise pins */ + void init(); + /** + * function enabling hardware interrupts for the HallSensor channels with provided callback functions + * if callback is not provided then the interrupt is not enabled + * + * @param doA pointer to the A channel interrupt handler function + * @param doB pointer to the B channel interrupt handler function + * @param doIndex pointer to the Index channel interrupt handler function + * + */ + void enableInterrupts(void (*doA)() = nullptr, void(*doB)() = nullptr, void(*doC)() = nullptr); + + // HallSensor interrupt callback functions + /** A channel callback function */ + void handleA(); + /** B channel callback function */ + void handleB(); + /** C channel callback function */ + void handleC(); + + + // pins A and B + int pinA; //!< HallSensor hardware pin A + int pinB; //!< HallSensor hardware pin B + int pinC; //!< HallSensor hardware pin C + bool use_interrupt; //!< True if interrupts have been attached + HallType hall_type, last_print_type; //!< Connectivity of hall sensor. The type indicates the pin to be swapped. Hall120 has no swapped pin + + // HallSensor configuration + Pullup pullup; //!< Configuration parameter internal or external pullups + int cpr;//!< HallSensor cpr number + + // Abstract functions of the Sensor class implementation + /** Interrupt-safe update */ + void update() override; + /** get current angle (rad) */ + float getSensorAngle() override; + /** get current angular velocity (rad/s) */ + float getVelocity() override; + + /** + * returns 0 if it does need search for absolute zero + * 0 - magnetic sensor (& encoder with index which is found) + * 1 - ecoder with index (with index not found yet) + */ + int needsSearch() override; + + + // whether last step was CW (+1) or CCW (-1). + Direction direction; + Direction old_direction; + + void attachSectorCallback(void (*onSectorChange)(int a) = nullptr); + + //last unique previous states, 0 = recent, used to detect the hall type + volatile uint8_t previous_states[3]; + // the current 3bit state of the hall sensors, without any line flipped + volatile int8_t hall_state_raw; + // the current sector of the sensor. Each sector is 60deg electrical + volatile int8_t electric_sector; + // the number of electric rotations + volatile long electric_rotations; + // this is sometimes useful to identify interrupt issues (e.g. weak or no pullup resulting in 1000s of interrupts) + volatile long total_interrupts; + + // variable used to filter outliers - rad/s + float velocity_max = 1000.0f; + + private: + + Direction decodeDirection(int oldState, int newState); + void updateState(); + + volatile unsigned long pulse_timestamp;//!< last impulse timestamp in us + volatile int A_active; //!< current active states of A channel + volatile int B_active; //!< current active states of B channel + volatile int C_active; //!< current active states of C channel + + // function pointer for on sector change call back + void (*onSectorChange)(int sector) = nullptr; + + volatile long pulse_diff; +}; diff --git a/src/encoders/kth78xx/KTH7812.cpp b/src/encoders/kth78xx/KTH7812.cpp new file mode 100644 index 0000000..f74bcd8 --- /dev/null +++ b/src/encoders/kth78xx/KTH7812.cpp @@ -0,0 +1,220 @@ + +#include "./KTH7812.h" + + +KTH7812::KTH7812(SPISettings settings, int nCS, bool fastmode, bool withcrc) : settings(settings), nCS(nCS), fastmode(fastmode), checkcrc(withcrc) { + // nix +} + +KTH7812::~KTH7812() { + // nix +} + +void KTH7812::init(SPIClass* _spi) { + spi = _spi; + if (nCS >= 0) { + pinMode(nCS, OUTPUT); + digitalWrite(nCS, HIGH); + } + spi->begin(); + transfer16(0x0000); // dummy transfer to wake up +}; + + + +float KTH7812::getCurrentAngle(){ + int32_t rawangle = -1; + if (checkcrc) + rawangle = readRawAngle12WithCRC(); + else + rawangle = readRawAngle16(); + if (rawangle < 0) // error + return -1.0f; + return rawangle / (float)KTH7812_CPR * _2PI; +}; + + + +uint16_t KTH7812::readRawAngle16(){ + uint16_t resp = transfer16(0x0000); + if (!fastmode) + resp = transfer16(0x0000); + return resp; +}; + + +int32_t KTH7812::readRawAngle12WithCRC(){ + uint16_t resp = transfer16(0x0000); + if (!fastmode) + resp = transfer16(0x0000); + uint8_t crc = resp & 0x000F; + // TODO check crc + return resp & 0xFFF0; +}; + + +uint8_t KTH7812::getGainTrim(){ + return readRegister(KTH7812_REG_GAIN); +}; + + +void KTH7812::setGainTrim(uint8_t gain){ + writeRegister(KTH7812_REG_GAIN, gain); +}; + + +KTH7812Trim KTH7812::getTrim(){ + KTH7812Trim trim; + trim.reg = readRegister(KTH7812_REG_TRIM); + return trim; +}; + + +void KTH7812::setTrim(KTH7812Trim trim){ + KTH7812Trim val = getTrim(); + val.xtrim = trim.xtrim; + val.ytrim = trim.ytrim; + writeRegister(KTH7812_REG_TRIM, val.reg); +}; + + +KTH7812_MGHL KTH7812::getMGHL(){ + KTH7812_MGHL mgh; + mgh.reg = readRegister(KTH7812_REG_MGHL); + return mgh; +}; + + +void KTH7812::setMGHL(KTH7812_MGHL mgh){ + KTH7812_MGHL val = getMGHL(); + val.mgh = mgh.mgh; + val.mgl = mgh.mgl; + writeRegister(KTH7812_REG_MGHL, val.reg); +}; + + +uint16_t KTH7812::getZero(){ + return readRegister(KTH7812_REG_ZLSB) | (readRegister(KTH7812_REG_ZMSB) << 8); +}; + + +void KTH7812::setZero(uint16_t zero){ + writeRegister(KTH7812_REG_ZLSB, zero & 0xFF); + writeRegister(KTH7812_REG_ZMSB, (zero >> 8) & 0xFF); +}; + + +bool KTH7812::getDirection(){ + return (readRegister(KTH7812_REG_RD) & KTH7812_RD_BIT) != 0; +}; + + +void KTH7812::setDirection(bool rd){ + uint8_t val = readRegister(KTH7812_REG_RD); + if (rd) + val |= KTH7812_RD_BIT; + else + val &= ~KTH7812_RD_BIT; + writeRegister(KTH7812_REG_RD, val); +}; + + +KTH7812_NPP KTH7812::getNPP(){ + KTH7812_NPP npp; + npp.reg = readRegister(KTH7812_REG_NPP); + return npp; +}; + + +void KTH7812::setNPP(KTH7812_NPP npp){ + KTH7812_NPP val = getNPP(); + val.npp = npp.npp; + writeRegister(KTH7812_REG_NPP, val.reg); +}; + + +uint16_t KTH7812::getPPT(){ + KTH7812_PPT0 ppt0; + ppt0.reg = readRegister(KTH7812_REG_PPT0); + uint16_t ppt = readRegister(KTH7812_REG_PPT1)<<2; + ppt |= (ppt0.pptl & 0b11); + return ppt; +}; + + +void KTH7812::setPPT(uint16_t ppt){ + KTH7812_PPT0 ppt0; + ppt0.reg = readRegister(KTH7812_REG_PPT0); + ppt0.pptl = (ppt & 0b11); + writeRegister(KTH7812_REG_PPT0, ppt0.reg); + writeRegister(KTH7812_REG_PPT1, (ppt >> 2) & 0xFF); +}; + + +uint8_t KTH7812::getABZLimit(){ + KTH7812_ABZL abz; + abz.reg = readRegister(KTH7812_REG_ABZL); + return abz.abzlimit; +}; + + +void KTH7812::setABZLimit(uint8_t limit){ + KTH7812_ABZL abz; + abz.reg = 0; + abz.abzlimit = limit; + writeRegister(KTH7812_REG_ABZL, abz.reg); +}; + + +KTH7812_PPT0 KTH7812::getZlZd(){ + KTH7812_PPT0 zlzd; + zlzd.reg = readRegister(KTH7812_REG_PPT0); + return zlzd; +}; + + +void KTH7812::setZlZd(KTH7812_PPT0 zlzd){ + KTH7812_PPT0 val = getZlZd(); + val.zl = zlzd.zl; + val.zd = zlzd.zd; + writeRegister(KTH7812_REG_PPT0, val.reg); +}; + + +uint16_t KTH7812::transfer16(uint16_t val){ + uint16_t buff = val; + spi->beginTransaction(settings); + if(nCS >= 0) + digitalWrite(nCS, LOW); + buff = spi->transfer16(buff); + if(nCS >= 0) + digitalWrite(nCS, HIGH); + spi->endTransaction(); + return buff; +}; + + +uint8_t KTH7812::readRegister(uint8_t reg){ + uint16_t cmd = 0x8000 | ((reg&0x3F) << 8); + uint16_t resp = transfer16(cmd); + resp = transfer16(0x0000); + // TODO status + return resp & 0xFF; +}; + + +uint8_t KTH7812::writeRegister(uint8_t reg, uint8_t value){ + uint16_t cmd = ((reg&0x3F) << 8) | (value & 0xFF); + uint16_t resp = transfer16(cmd); + delayMicroseconds(20100); // wait for write to complete + resp = transfer16(0x0000); + // TODO status + return (resp & 0xFF); // TODO response value +}; + + +uint8_t KTH7812::calcCrc(uint32_t angle, uint8_t status){ + // TODO write me + return 0; +}; + diff --git a/src/encoders/kth78xx/KTH7812.h b/src/encoders/kth78xx/KTH7812.h new file mode 100644 index 0000000..8e795d1 --- /dev/null +++ b/src/encoders/kth78xx/KTH7812.h @@ -0,0 +1,146 @@ + + +#pragma once + +#include "Arduino.h" +#include "SPI.h" + +#define KTH7812_ANGLE_MASK 0b1110000000000000 +#define KTH7812_CMD_MASK 0b1100000000000000 +#define KTH7812_ADDR_MASK 0b0011111100000000 +#define KTH7812_DATA_MASK 0b0000000011111111 + +#define KTH7812_CPR 65536 +#define KTH7812_BITORDER MSBFIRST + +#define KTH7812_REG_ZLSB 0x00 +#define KTH7812_REG_ZMSB 0x01 +#define KTH7812_REG_GAIN 0x02 +#define KTH7812_REG_TRIM 0x03 +#define KTH7812_REG_PPT0 0x04 +#define KTH7812_REG_PPT1 0x05 +#define KTH7812_REG_MGHL 0x06 +#define KTH7812_REG_NPP 0x07 +#define KTH7812_REG_ABZL 0x08 +#define KTH7812_REG_RD 0x09 + +#define KTH7812_REG_WDIS 0x40 + +#define KTH7812_WRDIS_BIT 0b00000010 +#define KTH7812_RD_BIT 0b10000000 + + + + +union KTH7812Trim { + struct { + uint8_t xtrim:1; + uint8_t ytrim:1; + uint8_t reserved:6; + }; + uint8_t reg; +}; + + +union KTH7812_MGHL { + struct { + uint8_t reserved:2; + uint8_t mgl:3; + uint8_t mgh:3; + }; + uint8_t reg; +}; + + +union KTH7812_NPP { + struct { + uint8_t reserved:5; + uint8_t npp:3; + }; + uint16_t reg; +}; + + +union KTH7812_PPT0 { + struct { + uint8_t reserved:2; + uint8_t zd:2; + uint8_t zl:2; + uint8_t pptl:2; + }; + uint16_t reg; +}; + + +union KTH7812_ABZL { + struct { + uint8_t abzlimit:3; + uint8_t reserved:5; + }; + uint16_t reg; +}; + + +#define KTH7812SPISettings SPISettings(1000000, KTH7812_BITORDER, SPI_MODE3) + +#define _2PI 6.28318530718f + + +class KTH7812 { +public: + KTH7812(SPISettings settings = KTH7812SPISettings, int nCS = -1, bool fastmode = false, bool withcrc = false); + virtual ~KTH7812(); + + virtual void init(SPIClass* _spi = &SPI); + + float getCurrentAngle(); // angle in radians, return current value + + uint16_t readRawAngle16(); // 16 bit angle, no CRC + int32_t readRawAngle12WithCRC(); // 12 bit angle with CRC + + uint8_t getGainTrim(); + void setGainTrim(uint8_t gain); + + KTH7812Trim getTrim(); + void setTrim(KTH7812Trim trim); + + KTH7812_MGHL getMGHL(); + void setMGHL(KTH7812_MGHL mgh); + + uint16_t getZero(); + void setZero(uint16_t zero); + + bool getDirection(); + void setDirection(bool rd); + + KTH7812_NPP getNPP(); + void setNPP(KTH7812_NPP npp); + + uint16_t getPPT(); + void setPPT(uint16_t ppt); + + uint8_t getABZLimit(); + void setABZLimit(uint8_t limit); + + KTH7812_PPT0 getZlZd(); + void setZlZd(KTH7812_PPT0 zlzd); + + + bool fastmode = false; + +private: + SPIClass* spi; + SPISettings settings; + int nCS = -1; + bool checkcrc = false; + + uint8_t laststatus = 0; + uint8_t lastcrc = 0; + + uint16_t transfer16(uint16_t); + uint8_t readRegister(uint8_t reg); + uint8_t writeRegister(uint8_t reg, uint8_t value); + uint8_t calcCrc(uint32_t angle, uint8_t status); + +}; + diff --git a/src/encoders/kth78xx/MagneticSensorKTH7812.cpp b/src/encoders/kth78xx/MagneticSensorKTH7812.cpp new file mode 100644 index 0000000..0b82382 --- /dev/null +++ b/src/encoders/kth78xx/MagneticSensorKTH7812.cpp @@ -0,0 +1,24 @@ + +#include "MagneticSensorKTH7812.h" + + +MagneticSensorKTH7812::MagneticSensorKTH7812(int nCS, bool withcrc, bool fastmode, SPISettings settings) : Sensor(), KTH7812(settings, nCS, fastmode, withcrc) { + // nix +}; + + +MagneticSensorKTH7812::~MagneticSensorKTH7812() { + // nix +}; + + +float MagneticSensorKTH7812::getSensorAngle() { + return getCurrentAngle(); +}; + + +void MagneticSensorKTH7812::init(SPIClass* _spi) { + this->KTH7812::init(_spi); + this->Sensor::init(); +}; + diff --git a/src/encoders/kth78xx/MagneticSensorKTH7812.h b/src/encoders/kth78xx/MagneticSensorKTH7812.h new file mode 100644 index 0000000..7a2d0dc --- /dev/null +++ b/src/encoders/kth78xx/MagneticSensorKTH7812.h @@ -0,0 +1,17 @@ + +#pragma once + +#include "common/base_classes/Sensor.h" +#include "./KTH7812.h" + +class MagneticSensorKTH7812 : public Sensor, public KTH7812 { +public: + MagneticSensorKTH7812(int nCS = -1, bool withcrc = false, bool fastmode = false, SPISettings settings = KTH7812SPISettings); + virtual ~MagneticSensorKTH7812(); + + virtual float getSensorAngle() override; + + virtual void init(SPIClass* _spi = &SPI); +}; + + diff --git a/src/encoders/kth78xx/README.md b/src/encoders/kth78xx/README.md new file mode 100644 index 0000000..59eb39f --- /dev/null +++ b/src/encoders/kth78xx/README.md @@ -0,0 +1,83 @@ +# KTH7812 SimpleFOC driver + +!warning not yet tested! + +Driver for the KTH7812 magnetic positon sensor. +This driver should also work with other models from the KTH78xx series. + +- access the registers of the KTH7812 to read/write settings +- read 16 bit angles +- TODO read 12 bit angles with 4 bit CRC +- supports "fast mode" (return angle with a single 16 bit read) + +## Hardware setup + +Connect as per normal for your SPI bus. No special hardware setup is needed to use this driver. + + +## Software setup + +Its actually easier to use than the standard SPI sensor class, because it is less generic: + +```c++ +#include "Arduino.h" +#include "Wire.h" +#include "SPI.h" +#include "SimpleFOC.h" +#include "SimpleFOCDrivers.h" +#include "encoders/kth78xx/MagneticSensorKTH7812.h" + +#define SENSOR1_CS 5 // some digital pin that you're using as the nCS pin +MagneticSensorKTH7812 sensor1(SENSOR1_CS); + + +void setup() { + sensor1.init(); +} + +unsigned long timestamp = 0; + +void loop() { + sensor.update(); + unsigned long now = millis(); + if (now - timestamp >= 1000) { // print angle every second + Serial.println(sensor.getAngle()); + timestamp = now; + } + delay(10); // update angle every 10ms +} +``` + + +Use another SPI bus: + +```c++ +void setup() { + sensor1.init(SPI2); +} +``` + +Here's how you can use it: + +```c++ + // update the sensor (only needed if using the sensor without a motor) + sensor1.update(); + + // get the angle, in radians, including full rotations + float a1 = sensor1.getAngle(); + + // get the velocity, in rad/s - note: you have to call getAngle() on a regular basis for it to work + float v1 = sensor1.getVelocity(); + + // get the angle, in radians, no full rotations + float a2 = sensor1.getCurrentAngle(); + + // get the raw 16 bit value + uint16_t raw = sensor1.readRawAngle16(); + + // set the ABZ resolution + sensor1.setABZLimit(1); // 8Mhz ABZ limit + sensor1.setPPT(1024-1); // set 1024 PPR = 4096 CPR for the ABZ output + +``` + diff --git a/src/encoders/linearhall/LinearHall.cpp b/src/encoders/linearhall/LinearHall.cpp index ad2efae..16f3f86 100644 --- a/src/encoders/linearhall/LinearHall.cpp +++ b/src/encoders/linearhall/LinearHall.cpp @@ -7,20 +7,29 @@ __attribute__((weak)) void ReadLinearHalls(int hallA, int hallB, int *a, int *b) *b = analogRead(hallB); } -LinearHall::LinearHall(int _hallA, int _hallB, int _pp){ +LinearHall::LinearHall(int _hallA, int _hallB, int _pp, SensorSpacing _sensor_spacing){ centerA = 512; centerB = 512; pinA = _hallA; pinB = _hallB; pp = _pp; electrical_rev = 0; + amplitude_ratio = 1.0f; + sensor_spacing = _sensor_spacing; prev_reading = 0; } -float LinearHall::getSensorAngle() { +float LinearHall::readSensors() { ReadLinearHalls(pinA, pinB, &lastA, &lastB); - //offset readings using center values, then compute angle - float reading = _atan2(lastA - centerA, lastB - centerB); + float a = lastA - centerA, b = (lastB - centerB) * amplitude_ratio; + if (sensor_spacing != SensorSpacing::_90) + b = (sensor_spacing==SensorSpacing::_60?-a:a) * _1_SQRT3 + b * _2_SQRT3; // Clarke transform, as in CurrentSense::getABCurrents + + return _atan2(a, b); +} + +float LinearHall::getSensorAngle() { + float reading = readSensors(); //handle rollover logic between each electrical revolution of the motor if (reading > prev_reading) { @@ -50,7 +59,7 @@ float LinearHall::getSensorAngle() { return result; } -void LinearHall::init(int _centerA, int _centerB) { +void LinearHall::init(int _centerA, int _centerB, float _amplitude_ratio) { // Skip configuring the pins here because they normally default to input anyway, and // this makes it possible to use ADC channel numbers instead of pin numbers when using // custom implementation of ReadLinearHalls, to avoid having to remap them every update. @@ -60,11 +69,11 @@ void LinearHall::init(int _centerA, int _centerB) { centerA = _centerA; centerB = _centerB; + amplitude_ratio = _amplitude_ratio; //establish initial reading for rollover handling electrical_rev = 0; - ReadLinearHalls(pinA, pinB, &lastA, &lastB); - prev_reading = _atan2(lastA - centerA, lastB - centerB); + prev_reading = readSensors(); } void LinearHall::init(FOCMotor *motor) { @@ -77,11 +86,21 @@ void LinearHall::init(FOCMotor *motor) { //pinMode(pinA, INPUT); //pinMode(pinB, INPUT); - int minA, maxA, minB, maxB; + // Get the initial reading, or time out if either of the sensors fails to give a nonzero value after 100ms + int minA = 0, maxA = 0, minB = 0, maxB = 0; + int32_t start = millis(); + while(minA == 0 || minB == 0) { + if ((int32_t)(millis() - start) >= 100) { + if(minA) SIMPLEFOC_DEBUG("LinearHall::init failed. Sensor B not responding."); + else if(minB) SIMPLEFOC_DEBUG("LinearHall::init failed. Sensor A not responding."); + else SIMPLEFOC_DEBUG("LinearHall::init failed. Sensors not responding."); + return; + } - ReadLinearHalls(pinA, pinB, &lastA, &lastB); - minA = maxA = centerA = lastA; - minB = maxB = centerB = lastB; + ReadLinearHalls(pinA, pinB, &lastA, &lastB); + minA = maxA = centerA = lastA; + minB = maxB = centerB = lastB; + } // move one mechanical revolution forward for (int i = 0; i <= 2000; i++) @@ -105,8 +124,15 @@ void LinearHall::init(FOCMotor *motor) { _delay(2); } + motor->setPhaseVoltage(0, 0, angle); + + amplitude_ratio = (float)(maxA - minA) / (float)(maxB - minB); + + SIMPLEFOC_DEBUG("LinearHall centerA: ", centerA); + SIMPLEFOC_DEBUG("LinearHall centerB: ", centerB); + SIMPLEFOC_DEBUG("LinearHall amplitude_ratio: ", amplitude_ratio); //establish initial reading for rollover handling electrical_rev = 0; - prev_reading = _atan2(lastA - centerA, lastB - centerB); + prev_reading = readSensors(); } diff --git a/src/encoders/linearhall/LinearHall.h b/src/encoders/linearhall/LinearHall.h index d17035e..7fc8598 100644 --- a/src/encoders/linearhall/LinearHall.h +++ b/src/encoders/linearhall/LinearHall.h @@ -6,23 +6,29 @@ // This function can be overridden with custom ADC code on platforms with poor analogRead performance. void ReadLinearHalls(int hallA, int hallB, int *a, int *b); -/** - * This sensor class is for two linear hall effect sensors such as 49E, which are - * positioned 90 electrical degrees apart (if one is centered on a rotor magnet, - * the other is half way between rotor magnets). - * It can also be used for a single magnet mounted to the motor shaft (set pp to 1). - * - * For more information, see this forum thread and PDF - * https://community.simplefoc.com/t/40-cent-magnetic-angle-sensing-technique/1959 - * https://gist.github.com/nanoparticle/00030ea27c59649edbed84f0a957ebe1 - */ class LinearHall: public Sensor{ public: - LinearHall(int hallA, int hallB, int pp); + enum SensorSpacing { + _60 = -1, // 60 degree spacing, equivalent to 120 degrees with one sensor flipped to opposite polarity + _90 = 0, // When one sensor is centered on a magnet, the other is half way between magnets + _120 = 1, // Sensors spaced 120 electrical degrees, same as digital halls use + }; - void init(int centerA, int centerB); // Initialize without moving motor + LinearHall(int hallA, int hallB, int pp, SensorSpacing sensor_spacing = SensorSpacing::_90); + + void init(int centerA, int centerB, float _amplitude_ratio = 1.0f); // Initialize without moving motor void init(class FOCMotor *motor); // Move motor to find center values + int centerA; + int centerB; + int lastA, lastB; + int electrical_rev; + float amplitude_ratio; // Correction factor if one sensor is slightly farther from the magnets + SensorSpacing sensor_spacing; + + protected: + float readSensors(); + // Get current shaft angle from the sensor hardware, and // return it as a float in radians, in the range 0 to 2PI. // - This method is pure virtual and must be implemented in subclasses. @@ -30,15 +36,9 @@ class LinearHall: public Sensor{ // Use update() when calling from outside code. float getSensorAngle() override; - int centerA; - int centerB; - int lastA, lastB; - - private: int pinA; int pinB; int pp; - int electrical_rev; float prev_reading; }; diff --git a/src/encoders/linearhall/README.md b/src/encoders/linearhall/README.md new file mode 100644 index 0000000..2845614 --- /dev/null +++ b/src/encoders/linearhall/README.md @@ -0,0 +1,47 @@ +# LinearHall + +by [@nanoparticle](https://github.com/nanoparticle), [@XieMaster](https://github.com/XieMaster) and [@dekutree64](https://github.com/dekutree64) + +This sensor class is for two linear hall effect sensors such as 49E to sense the rotor magnets of an outrunner or a diametric magnet mounted on the shaft (set pp to 1). The sensors can be spaced 60, 90 or 120 electrical degrees apart. 90 degrees will give a slightly more accurate reading (if one is centered on a rotor magnet, the other is half way between magnets), but sometimes it's convenient to position them 120 degrees like digital hall sensors, or 60 degrees which is equivalent to 120 with one sensor flipped to opposite polarity. + +Optimal distance of sensors to magnets is typically between 1mm and 3mm. Too close and the reading will saturate and become trapezoidal, too far and resolution diminishes. It can still work fairly well with as little as ±50 ADC units of resolution. + +The Arduino function analogRead has poor performance on some platforms such as STM32 and RP2350. The ReadLinearHalls function is declared "weaK" so you can override it with custom ADC code without having to modify the library code. + +Please also see our [forum thread](https://community.simplefoc.com/t/40-cent-magnetic-angle-sensing-technique/1959) on this topic, and this PDF for more detail on sensor placement https://gist.github.com/nanoparticle/00030ea27c59649edbed84f0a957ebe1 + + +## Example usage +```c++ +#include +#include +#include + +BLDCMotor motor = BLDCMotor(11); +BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); +LinearHall sensor = LinearHall(A0, A1, 11, LinearHall::SensorSpacing::_90); + +void setup() { + Serial.begin(115200); + motor.useMonitoring(Serial); // LinearHall uses this to print out the calibration results + + driver.voltage_power_supply = 12; + driver.init(); + motor.linkDriver(&driver); + motor.init(); + // initialize sensor hardware. This moves the motor to find the min/max sensor readings and + // averages them to get the center values. The motor can't move until motor.init is called, and + // motor.initFOC can't do its calibration until the sensor is intialized, so this must be done inbetween. + // You can then take the values printed to the serial monitor and pass them to sensor.init to + // avoid having to move the motor every time. In that case it doesn't matter whether sensor.init + // is called before or after motor.init. + sensor.init(&motor); + motor.linkSensor(&sensor); + motor.initFOC(); +} +``` + +## Future work + +- High-performance ADC configurations on platforms with poor analogRead performance. +- Support for redundancy using 3 sensors spaced 120 degrees. Probably best to make a subclass. \ No newline at end of file diff --git a/src/encoders/mt6816/MT6816.cpp b/src/encoders/mt6816/MT6816.cpp index fec02d0..27a0e70 100644 --- a/src/encoders/mt6816/MT6816.cpp +++ b/src/encoders/mt6816/MT6816.cpp @@ -18,8 +18,24 @@ void MT6816::init(SPIClass* _spi) { uint16_t MT6816::readRawAngle() { uint16_t angle_data = 0; - angle_data = spi_transfer16(MT6816_READ_REG_03) << 8; - angle_data |= spi_transfer16(MT6816_READ_REG_04); + uint8_t data[3] = {0}; + + data[0] = 0x83; // command to read MSB + data[1] = 0; + data[2] = 0; + + spi->beginTransaction(settings); + if (nCS>=0) + digitalWrite(nCS, 0); + + spi->transfer(data, 3); + + if (nCS>=0) + digitalWrite(nCS, 1); + spi->endTransaction(); + + angle_data = (data[1] & 0xFF) << 8; + angle_data |= (data[2] & 0xFF); if ((angle_data & MT6816_NO_MAGNET_WARNING_BIT) == MT6816_NO_MAGNET_WARNING_BIT) { this->no_magnetic_reading = true; @@ -31,7 +47,7 @@ uint16_t MT6816::readRawAngle() { return 0; } - return (angle_data >> 2); + return (angle_data >> 2) & 0x3FFF; // 14 bits of angle data } bool MT6816::parityCheck(uint16_t data) { @@ -42,15 +58,3 @@ bool MT6816::parityCheck(uint16_t data) { return (~data) & 1; } - -uint16_t MT6816::spi_transfer16(uint16_t outdata) { - spi->beginTransaction(settings); - if (nCS>=0) - digitalWrite(nCS, 0); - uint16_t result = spi->transfer16(outdata); - if (nCS>=0) - digitalWrite(nCS, 1); - spi->endTransaction(); - - return result; -} diff --git a/src/encoders/mt6816/MT6816.h b/src/encoders/mt6816/MT6816.h index 68b448a..7afaae2 100644 --- a/src/encoders/mt6816/MT6816.h +++ b/src/encoders/mt6816/MT6816.h @@ -30,7 +30,6 @@ class MT6816 { private: bool parityCheck(uint16_t data); - uint16_t spi_transfer16(uint16_t outdata); SPIClass* spi; SPISettings settings; bool no_magnetic_reading = false; diff --git a/src/encoders/mt6826s/MT6826S.cpp b/src/encoders/mt6826s/MT6826S.cpp new file mode 100644 index 0000000..bd5d415 --- /dev/null +++ b/src/encoders/mt6826s/MT6826S.cpp @@ -0,0 +1,325 @@ + +#include "./MT6826S.h" +#include "common/foc_utils.h" + + +MT6826S::MT6826S(SPISettings settings, int nCS) : settings(settings), nCS(nCS) { + // nix +}; + +MT6826S::~MT6826S() { + // nix +}; + + + +void MT6826S::init(SPIClass* _spi) { + spi = _spi; + if (nCS >= 0) { + pinMode(nCS, OUTPUT); + digitalWrite(nCS, HIGH); + } + spi->begin(); +}; + + + + +float MT6826S::getCurrentAngle(){ + uint32_t rawangle = readRawAngle15(); + if (checkcrc) { + if (lastcrc != calcCrc(rawangle, laststatus)) { + laststatus |= MT6826S_CRC_ERROR; + return -1; // return -1 to signal CRC error - the current angle has to be non-negative otherwise + } + } + return rawangle / (float)MT6826S_CPR * _2PI; +}; + + +// calculate crc8 of 21 angle bits and 3 status bits +// polynomial: x^8 + x^2 + x + 1 = 0x07, (0xE0 reflected) init is 0x00, no final xor +// TOOD table-based version +uint8_t MT6826S::calcCrc(uint32_t angle, uint8_t status) { + uint8_t crc = 0x00; + + uint8_t input = angle>>13; + crc ^= input; + for (int k = 8; k > 0; k--) + crc = (crc & (0x01<<7))?(crc<<1)^0x07:crc<<1; + + input = (angle>>5) & 0xFF; + crc ^= input; + for (int k = 8; k > 0; k--) + crc = (crc & (0x01<<7))?(crc<<1)^0x07:crc<<1; + + input = ((angle<<3) & 0xFF) | (status & 0x07); + crc ^= input; + for (int k = 8; k > 0; k--) + crc = (crc & (0x01<<7))?(crc<<1)^0x07:crc<<1; + + return crc; +}; + + + +uint16_t MT6826S::readRawAngle15(){ + uint8_t data[6]; // transact 48 bits + data[0] = (MT6826S_OP_ANGLE<<4); + data[1] = MT6826S_REG_ANGLE1; + data[2] = 0; + data[3] = 0; + data[4] = 0; + data[5] = 0; + spi->beginTransaction(settings); + if (nCS >= 0) + digitalWrite(nCS, LOW); + spi->transfer(data, 6); + if (nCS >= 0) + digitalWrite(nCS, HIGH); + spi->endTransaction(); + laststatus = data[4]&0x07; + lastcrc = data[5]; + return (data[2] << 7) | ((data[3]>>1)&0x7F); // 15 bits only +}; + + +uint8_t MT6826S::getStatus(){ + return laststatus; +}; + +uint8_t MT6826S::getCalibrationStatus(){ + uint8_t data[3] = {0}; + data[0] = MT6826S_OP_READ << 4 | MT6826S_REG_CAL_STATUS >> 8; + data[1] = MT6826S_REG_CAL_STATUS & 0xFF; + + spi->beginTransaction(settings); + if(nCS >= 0) + digitalWrite(nCS, LOW); + spi->transfer(data, 3); + if(nCS >= 0) + digitalWrite(nCS, HIGH); + spi->endTransaction(); + + return data[2] >> 6; +} + +/** + * Wait 6s after calling this method + */ +bool MT6826S::writeEEPROM(){ + delay(1); // wait at least 1ms + MT6826SCommand cmd; + cmd.cmd = MT6826S_OP_PROG; + cmd.addr = 0x000; + transfer24(&cmd); + return cmd.data == MT6826S_WRITE_ACK; +}; + + + + + +uint8_t MT6826S::getBandwidth(){ + MT6826SOptions5 opts = { .reg = readRegister(MT6826S_REG_OPTS5) }; + return opts.bw; +}; +void MT6826S::setBandwidth(uint8_t bw){ + MT6826SOptions5 opts = { .reg = readRegister(MT6826S_REG_OPTS5) }; + opts.bw = bw; + writeRegister(MT6826S_REG_OPTS5, opts.reg); +}; + +uint8_t MT6826S::getHysteresis(){ + MT6826SOptions3 opts = { .reg = getOptions3().reg }; + return opts.hyst; +}; +void MT6826S::setHysteresis(uint8_t hyst){ + MT6826SOptions3 opts = { .reg = getOptions3().reg }; + opts.hyst = hyst; + setOptions3(opts); +}; + +uint8_t MT6826S::getRotationDirection(){ + MT6826SOptions3 opts = { .reg = getOptions3().reg }; + return opts.rot_dir; +}; +void MT6826S::setRotationDirection(uint8_t dir){ + MT6826SOptions3 opts = { .reg = getOptions3().reg }; + opts.rot_dir = dir; + setOptions3(opts); +}; + + +uint16_t MT6826S::getABZResolution(){ + uint8_t hi = readRegister(MT6826S_REG_ABZ_RES1); + MT6826SABZRes lo = { + .reg = readRegister(MT6826S_REG_ABZ_RES2) + }; + return (hi << 6) | lo.abz_res_low; +}; +void MT6826S::setABZResolution(uint16_t res){ + uint8_t hi = (res >> 6); + MT6826SABZRes lo = { + .reg = readRegister(MT6826S_REG_ABZ_RES2) + }; + lo.abz_res_low = (res & 0x3F); + writeRegister(MT6826S_REG_ABZ_RES1, hi); + writeRegister(MT6826S_REG_ABZ_RES2, lo.reg); +}; + + + +bool MT6826S::isABZEnabled(){ + MT6826SABZRes lo = { + .reg = readRegister(MT6826S_REG_ABZ_RES2) + }; + return lo.abz_off==0; +}; +void MT6826S::setABZEnabled(bool enabled){ + MT6826SABZRes lo = { + .reg = readRegister(MT6826S_REG_ABZ_RES2) + }; + lo.abz_off = enabled?0:1; + writeRegister(MT6826S_REG_ABZ_RES2, lo.reg); +}; + + + +bool MT6826S::isABSwapped(){ + MT6826SABZRes lo = { + .reg = readRegister(MT6826S_REG_ABZ_RES2) + }; + return lo.ab_swap==1; +}; +void MT6826S::setABSwapped(bool swapped){ + MT6826SABZRes lo = { + .reg = readRegister(MT6826S_REG_ABZ_RES2) + }; + lo.ab_swap = swapped?1:0; + writeRegister(MT6826S_REG_ABZ_RES2, lo.reg); +}; + + + +uint16_t MT6826S::getZeroPosition(){ + uint8_t hi = readRegister(MT6826S_REG_ZERO1); + MT6826SOptions0 lo = { + .reg = readRegister(MT6826S_REG_ZERO2) + }; + return (hi << 4) | lo.zero_pos_low; +}; +void MT6826S::setZeroPosition(uint16_t pos){ + uint8_t hi = (pos >> 4); + MT6826SOptions0 lo = { + .reg = readRegister(MT6826S_REG_ZERO2) + }; + lo.zero_pos_low = pos & 0x0F; + writeRegister(MT6826S_REG_ZERO1, hi); + writeRegister(MT6826S_REG_ZERO2, lo.reg); +}; + + + +MT6826SOptions1 MT6826S::getOptions1(){ + MT6826SOptions1 result = { + .reg = readRegister(MT6826S_REG_OPTS1) + }; + return result; +}; +void MT6826S::setOptions1(MT6826SOptions1 opts){ + writeRegister(MT6826S_REG_OPTS1, opts.reg); +}; + + + +MT6826SOptions2 MT6826S::getOptions2(){ + MT6826SOptions2 result = { + .reg = readRegister(MT6826S_REG_OPTS2) + }; + return result; +}; +void MT6826S::setOptions2(MT6826SOptions2 opts){ + MT6826SOptions2 val = getOptions2(); + val.pwm_fq = opts.pwm_fq; + val.pwm_pol = opts.pwm_pol; + val.pwm_sel = opts.pwm_sel; + writeRegister(MT6826S_REG_OPTS2, val.reg); +}; + + + +MT6826SOptions3 MT6826S::getOptions3(){ + MT6826SOptions3 result = { + .reg = readRegister(MT6826S_REG_OPTS3) + }; + return result; +}; +void MT6826S::setOptions3(MT6826SOptions3 opts){ + MT6826SOptions3 val = getOptions3(); + val.rot_dir = opts.rot_dir; + val.hyst = opts.hyst; + writeRegister(MT6826S_REG_OPTS3, val.reg); +}; + + + +MT6826SOptions4 MT6826S::getOptions4(){ + MT6826SOptions4 result = { + .reg = readRegister(MT6826S_REG_OPTS4) + }; + return result; +}; +void MT6826S::setOptions4(MT6826SOptions4 opts){ + MT6826SOptions4 val = getOptions4(); + val.gpio_ds = opts.gpio_ds; + val.autocal_freq = opts.autocal_freq; + writeRegister(MT6826S_REG_OPTS4, val.reg); +}; + + + +MT6826SOptions5 MT6826S::getOptions5(){ + MT6826SOptions5 result = { + .reg = readRegister(MT6826S_REG_OPTS5) + }; + return result; +}; +void MT6826S::setOptions5(MT6826SOptions5 opts){ + MT6826SOptions5 val = getOptions5(); + val.bw = opts.bw; + val.z_edge = opts.z_edge; + writeRegister(MT6826S_REG_OPTS5, val.reg); +}; + + + + + + +void MT6826S::transfer24(MT6826SCommand* outValue) { + uint32_t buff = __builtin_bswap32(outValue->val); + spi->beginTransaction(settings); + if (nCS >= 0) + digitalWrite(nCS, LOW); + spi->transfer(&buff, 3); + if (nCS >= 0) + digitalWrite(nCS, HIGH); + spi->endTransaction(); + outValue->val = __builtin_bswap32(buff); +}; +uint8_t MT6826S::readRegister(uint16_t reg) { + MT6826SCommand cmd; + cmd.cmd = MT6826S_OP_READ; + cmd.addr = reg; + transfer24(&cmd); + return cmd.data; +}; +bool MT6826S::writeRegister(uint16_t reg, uint8_t value) { + MT6826SCommand cmd; + cmd.cmd = MT6826S_OP_WRITE; + cmd.addr = reg; + cmd.data = value; + transfer24(&cmd); + return cmd.data == MT6826S_WRITE_ACK; +}; \ No newline at end of file diff --git a/src/encoders/mt6826s/MT6826S.h b/src/encoders/mt6826s/MT6826S.h new file mode 100644 index 0000000..f24dc6c --- /dev/null +++ b/src/encoders/mt6826s/MT6826S.h @@ -0,0 +1,238 @@ + + +#pragma once + + + +#include "Arduino.h" +#include "SPI.h" + +#define MT6826S_OP_READ 0b0011 +#define MT6826S_OP_WRITE 0b0110 +#define MT6826S_OP_PROG 0b1100 +#define MT6826S_OP_ANGLE 0b1010 + +#define MT6826S_CMD_MASK 0b111100000000000000000000 +#define MT6826S_ADDR_MASK 0b000011111111111100000000 +#define MT6826S_DATA_MASK 0b000000000000000011111111 + +#define MT6826S_CPR 32768 + +#define MT6826S_STATUS_OVERSPEED 0x01 +#define MT6826S_STATUS_WEAKFIELD 0x02 +#define MT6826S_STATUS_UNDERVOLT 0x04 +#define MT6826S_CRC_ERROR 0x08 + +#define MT6826S_WRITE_ACK 0x55 + + + +#define MT6826S_REG_USERID 0x001 + +#define MT6826S_REG_ANGLE1 0x003 +#define MT6826S_REG_ANGLE2 0x004 +#define MT6826S_REG_ANGLE3 0x005 +#define MT6826S_REG_ANGLE4 0x006 + +#define MT6826S_REG_ABZ_RES1 0x007 +#define MT6826S_REG_ABZ_RES2 0x008 + +#define MT6826S_REG_ZERO1 0x009 +#define MT6826S_REG_ZERO2 0x00A + +#define MT6826S_REG_OPTS0 0x00A +#define MT6826S_REG_OPTS1 0x00B +#define MT6826S_REG_OPTS2 0x00C +#define MT6826S_REG_OPTS3 0x00D +#define MT6826S_REG_OPTS4 0x00E +#define MT6826S_REG_OPTS5 0x011 + +#define MT6826S_REG_POSINIT 0x0D8 +#define MT6826S_REG_CAL_STATUS 0x113 +#define MT6826S_REG_CALCTRL 0x155 + +#define MT6826S_CALCTRL_KEY_EN 0x5E + + +union MT6826SABZRes { + struct { + uint8_t ab_swap:1; + uint8_t abz_off:1; + uint8_t reserved:2; + uint8_t abz_res_low:4; + }; + uint8_t reg; +}; + + + +union MT6826SOptions0 { + struct { + uint8_t z_pul_wid:4; + uint8_t zero_pos_low:4; + }; + uint8_t reg; +}; + + + +union MT6826SOptions1 { + struct { + uint8_t uvw_res:4; + uint8_t uvw_off:1; + uint8_t uvw_mux:1; + uint8_t z_phase:2; + }; + uint8_t reg; +}; + + + +union MT6826SOptions2 { + struct { + uint8_t pwm_sel:3; + uint8_t pwm_pol:1; + uint8_t pwm_fq:1; + uint8_t reserved:3; + }; + uint8_t reg; +}; + + + +union MT6826SOptions3 { + struct { + uint8_t hyst:3; + uint8_t rot_dir:1; + uint8_t reserved:4; + }; + uint8_t reg; +}; + + + +union MT6826SOptions4 { + struct { + uint8_t reserved:4; + uint8_t autocal_freq:3; + uint8_t gpio_ds:1; + }; + uint8_t reg; +}; + + + +union MT6826SOptions5 { + struct { + uint8_t bw:3; + uint8_t reserved:4; + uint8_t z_edge:1; + }; + uint8_t reg; +}; + + + +union MT6826SPosInit { + struct { + uint8_t reserved:1; + uint8_t pos_init:1; + uint8_t reserved2:6; + }; + uint8_t reg; +}; + + + + +union MT6826SCommand { + struct { + uint32_t unused:8; + uint32_t data:8; + uint32_t addr:12; + uint32_t cmd:4; + }; + uint32_t val; +}; + + + + +#define MT6826S_BITORDER MSBFIRST + +#define MT6826SSPISettings SPISettings(1000000, MT6826S_BITORDER, SPI_MODE3) + + + + +class MT6826S { +public: + MT6826S(SPISettings settings = MT6826SSPISettings, int nCS = -1); + virtual ~MT6826S(); + + virtual void init(SPIClass* _spi = &SPI); + + + float getCurrentAngle(); // angle in radians, return current value + + uint16_t readRawAngle15(); // up to 15bit precision angle value + + + uint8_t getBandwidth(); + void setBandwidth(uint8_t bw); + + uint8_t getHysteresis(); + void setHysteresis(uint8_t hyst); + + uint8_t getRotationDirection(); + void setRotationDirection(uint8_t dir); + + uint16_t getABZResolution(); + void setABZResolution(uint16_t res); + + bool isABZEnabled(); + void setABZEnabled(bool enabled); + + bool isABSwapped(); + void setABSwapped(bool swapped); + + uint16_t getZeroPosition(); + void setZeroPosition(uint16_t pos); + + MT6826SOptions1 getOptions1(); + void setOptions1(MT6826SOptions1 opts); + + MT6826SOptions2 getOptions2(); + void setOptions2(MT6826SOptions2 opts); + + MT6826SOptions3 getOptions3(); + void setOptions3(MT6826SOptions3 opts); + + MT6826SOptions4 getOptions4(); + void setOptions4(MT6826SOptions4 opts); + + MT6826SOptions5 getOptions5(); + void setOptions5(MT6826SOptions5 opts); + + uint8_t getStatus(); + + uint8_t getCalibrationStatus(); + + bool setZeroFromCurrentPosition(); + bool writeEEPROM(); // wait 6s after calling this method + + bool checkcrc = false; + +private: + SPIClass* spi; + SPISettings settings; + int nCS = -1; + uint8_t laststatus = 0; + uint8_t lastcrc = 0; + + void transfer24(MT6826SCommand* outValue); + uint8_t readRegister(uint16_t reg); + bool writeRegister(uint16_t reg, uint8_t value); + uint8_t calcCrc(uint32_t angle, uint8_t status); + +}; \ No newline at end of file diff --git a/src/encoders/mt6826s/MagneticSensorMT6826S.cpp b/src/encoders/mt6826s/MagneticSensorMT6826S.cpp new file mode 100644 index 0000000..0b8131f --- /dev/null +++ b/src/encoders/mt6826s/MagneticSensorMT6826S.cpp @@ -0,0 +1,24 @@ + +#include "MagneticSensorMT6826S.h" + + +MagneticSensorMT6826S::MagneticSensorMT6826S(int nCS, SPISettings settings) : Sensor(), MT6826S(settings, nCS) { + // nix +}; + + +MagneticSensorMT6826S::~MagneticSensorMT6826S() { + // nix +}; + + +float MagneticSensorMT6826S::getSensorAngle() { + return getCurrentAngle(); +}; + + +void MagneticSensorMT6826S::init(SPIClass* _spi) { + this->MT6826S::init(_spi); + this->Sensor::init(); +}; + diff --git a/src/encoders/mt6826s/MagneticSensorMT6826S.h b/src/encoders/mt6826s/MagneticSensorMT6826S.h new file mode 100644 index 0000000..7aae8a9 --- /dev/null +++ b/src/encoders/mt6826s/MagneticSensorMT6826S.h @@ -0,0 +1,17 @@ + +#pragma once + +#include "common/base_classes/Sensor.h" +#include "./MT6826S.h" + +class MagneticSensorMT6826S : public Sensor, public MT6826S { +public: + MagneticSensorMT6826S(int nCS = -1, SPISettings settings = MT6826SSPISettings); + virtual ~MagneticSensorMT6826S(); + + virtual float getSensorAngle() override; + + virtual void init(SPIClass* _spi = &SPI); +}; + + diff --git a/src/encoders/mt6826s/README.md b/src/encoders/mt6826s/README.md new file mode 100644 index 0000000..ae2339d --- /dev/null +++ b/src/encoders/mt6826s/README.md @@ -0,0 +1,72 @@ + +# MT6826S SimpleFOC and SPI driver + +!warning not yet tested! +!warning work in progress! + +SPI Driver for the MT6826S magnetic position sensor. + + +## Hardware setup + +Connect as per normal for your SPI bus. No special hardware setup is needed to use this driver. + +## Software setup + +TODO improve documentation and add example to examples folder + +Its actually easier to use than the standard SPI sensor class, because it is less generic: + +```c++ +#include "Arduino.h" +#include "Wire.h" +#include "SPI.h" +#include "SimpleFOC.h" +#include "SimpleFOCDrivers.h" +#include "encoders/mt6826s/MagneticSensorMT6826S.h" + +#define SENSOR1_CS 5 // some digital pin that you're using as the nCS pin +MagneticSensorMT6826S sensor1(SENSOR1_CS); + + +void setup() { + sensor1.init(); +} + +unsigned long timestamp = 0; + +void loop() { + sensor.update(); + unsigned long now = millis(); + if (now - timestamp >= 1000) { // print angle every second + Serial.println(sensor.getAngle()); + timestamp = now; + } + delay(10); // update angle every 10ms +} +``` + + +Here's how you can use it: + +```c++ + // update the sensor (only needed if using the sensor without a motor) + sensor1.update(); + + // get the angle, in radians, including full rotations + float a1 = sensor1.getAngle(); + + // get the velocity, in rad/s - note: you have to call getAngle() on a regular basis for it to work + float v1 = sensor1.getVelocity(); + + // get the angle, in radians, no full rotations + float a2 = sensor1.getCurrentAngle(); + + // get the raw 15 bit value + uint16_t raw = sensor1.readRawAngle15(); + + // set the zero position + sensor1.setZeroPosition(sensor1.readRawAngle15()); // set to current position + +``` + diff --git a/src/encoders/stm32hwencoder/README.md b/src/encoders/stm32hwencoder/README.md index 1bdb1ca..91e806e 100644 --- a/src/encoders/stm32hwencoder/README.md +++ b/src/encoders/stm32hwencoder/README.md @@ -6,7 +6,7 @@ Big thank you to @conroy-cheers for originally contributing this code in pull re ## Warning -This code has not been tested much! Use at your own risk, your milage may vary. +This code has not been tested much! Use at your own risk, your milage may vary.\ I have tested on: STM32F401CC/TIM4-PB6,PB7 ## Hardware setup @@ -17,8 +17,7 @@ Not all of the timers support the encoder mode, so check the datasheet which tim An excellent option can be to use the STM32CubeIDE software's pin assignment view to quickly check which pins connect to which timer. -Note that the index (I) pin is currently not used. - +Right now the index pin can be any pin supporting interrupts. ## Software setup @@ -46,4 +45,22 @@ void setup() { encoder.init(); } +``` + +Set the encoder angle: + +```c++ +encoder.setCurrentAngle(1.28); +``` + +Set the encoder count: + +```c++ +encoder.setEncoderCount(693); +``` + +Get the encoder timer handle: + +```c++ +encoder.getEncoderTimerHandle(); ``` \ No newline at end of file diff --git a/src/encoders/stm32hwencoder/STM32HWEncoder.cpp b/src/encoders/stm32hwencoder/STM32HWEncoder.cpp index 1cea602..1973335 100644 --- a/src/encoders/stm32hwencoder/STM32HWEncoder.cpp +++ b/src/encoders/stm32hwencoder/STM32HWEncoder.cpp @@ -15,15 +15,30 @@ STM32HWEncoder::STM32HWEncoder(unsigned int _ppr, int pinA, int pinB, int pinI) index_found = false; } +// function returns encoder handle +TIM_HandleTypeDef STM32HWEncoder::getEncoderTimerHandle() { return encoder_handle; } + /* Shaft angle calculation */ float STM32HWEncoder::getSensorAngle() { return _2PI * encoder_handle.Instance->CNT / static_cast(cpr); } +/* + Set the current angle using CNT register +*/ +void STM32HWEncoder::setCurrentAngle(float set_angle) { + encoder_handle.Instance->CNT = set_angle * cpr / _2PI; +} +/* + Modify encoder count directly +*/ +void STM32HWEncoder::setEncoderCount(uint32_t ecount) { + encoder_handle.Instance->CNT = ecount; +} // getter for index pin -int STM32HWEncoder::needsSearch() { return false && !index_found; } +int STM32HWEncoder::needsSearch() { return hasIndex() && !index_found; } // private function used to determine if encoder has index int STM32HWEncoder::hasIndex() { return (_pinI!=NC); } @@ -34,11 +49,13 @@ void STM32HWEncoder::init() { TIM_TypeDef *InstanceA = (TIM_TypeDef *)pinmap_peripheral(_pinA, PinMap_TIM); if (!IS_TIM_ENCODER_INTERFACE_INSTANCE(InstanceA)) { initialized = false; + SIMPLEFOC_DEBUG("STM32HWEncoder pin A doesn't support encoder interface"); return; } TIM_TypeDef *InstanceB = (TIM_TypeDef *)pinmap_peripheral(_pinB, PinMap_TIM); if (InstanceA != InstanceB) { initialized = false; + SIMPLEFOC_DEBUG("STM32HWEncoder pin B is not in the same timer as A"); return; } pinmap_pinout(_pinA, PinMap_TIM); @@ -74,17 +91,37 @@ void STM32HWEncoder::init() { if (HAL_TIM_Encoder_Init(&encoder_handle, &encoder_config) != HAL_OK) { initialized = false; + SIMPLEFOC_DEBUG("Couldn't initialize timer in encoder mode!"); return; } - // TODO on STM32G4 MCUs we can use the TIMx_ETR pin for the index, and configure how it is handled automatically by the hardware - // on non-G4 MCUs we need to use an external interrupt to handle the index signal - // attachInterrupt(digitalPinToInterrupt(pinNametoDigitalPin(_pinI)), [this]() { - // encoder_handle.Instance->CNT = 0; // reset counter - // index_found = true; - // // detach interrupt - // detachInterrupt(digitalPinToInterrupt(pinNametoDigitalPin(_pinI))); - // }, index_polarity); + // TODO: figure out way to check if Index pin is specifically ETR line + //if(IS_TIM_ETR_INSTANCE()) + + // Encoder index configuration + //TIMEx_EncoderIndexConfigTypeDef encoder_indexconfig = {0}; + //encoder_indexconfig.Polarity = TIM_ENCODERINDEX_POLARITY_NONINVERTED; + //encoder_indexconfig.Prescaler = TIM_ENCODERINDEX_PRESCALER_DIV1; + //encoder_indexconfig.Filter = 0; + //encoder_indexconfig.FirstIndexEnable = ENABLE; + //encoder_indexconfig.Position = TIM_ENCODERINDEX_POSITION_00; + //encoder_indexconfig.Direction = TIM_ENCODERINDEX_DIRECTION_UP_DOWN; // Double check this is ok + //if (HAL_TIMEx_ConfigEncoderIndex(&encoder_handle, &encoder_indexconfig) != HAL_OK) + //{ + // SIMPLEFOC_DEBUG("Couldn't configure encoder index pin"); + // initialized = false; + // return; + //} + + // If index pin provided attach interrupt to reset the counter + // on index pulse + if(hasIndex()) + { + attachInterrupt(digitalPinToInterrupt(pinNametoDigitalPin(_pinI)), [this]() { + encoder_handle.Instance->CNT = 0; // reset counter + index_found = true; + }, index_polarity); + } if (HAL_TIM_Encoder_Start(&encoder_handle, TIM_CHANNEL_1) != HAL_OK) { initialized = false; diff --git a/src/encoders/stm32hwencoder/STM32HWEncoder.h b/src/encoders/stm32hwencoder/STM32HWEncoder.h index eb8ec5c..a55e3e7 100644 --- a/src/encoders/stm32hwencoder/STM32HWEncoder.h +++ b/src/encoders/stm32hwencoder/STM32HWEncoder.h @@ -21,6 +21,9 @@ class STM32HWEncoder : public Sensor { void init() override; int needsSearch() override; int hasIndex(); // !< function returning 1 if encoder has index pin and 0 if not. + void setCurrentAngle(float set_angle); // !< helper function for setting the angle by using the count register + void setEncoderCount(uint32_t ecount); // !< function for setting the count register + TIM_HandleTypeDef getEncoderTimerHandle(); // !< function for getting the encoder timer handle bool initialized = false; uint32_t cpr; //!< encoder cpr number