Candas1 / Split_Hoverboard_SimpleFOC

Split Hoverboards with C++ SimpleFOC (not yet)
MIT License
7 stars 5 forks source link

adding LowsideCurrentSense ... #6

Open RoboDurden opened 1 year ago

RoboDurden commented 1 year ago

In continuation of https://github.com/Candas1/Split_Hoverboard_SimpleFOC/issues/5

I have added the needed code to my cloned fork but can not push because Github_Desktop hangs and therefore can not make a pull request. But we might already talk about the best approach to add LowsideCurrentSense:

...
// BLDC motor & driver instance
BLDCMotor motor = BLDCMotor(BLDC_POLE_PAIRS);
BLDCDriver6PWM driver = BLDCDriver6PWM( BLDC_BH_PIN,BLDC_BL_PIN,  BLDC_GH_PIN,BLDC_GL_PIN,  BLDC_YH_PIN,BLDC_YL_PIN );

// shunt resistor value , gain value,  pins phase A,B,C
LowsideCurrentSense current_sense = LowsideCurrentSense(BLDC_CUR_Rds, BLDC_CUR_Gain, BLDC_CUR_G_PIN, BLDC_CUR_B_PIN, BLDC_CUR_Y_PIN);
setup()
{
...
/*  
  // succeeds but motor.initFOC(2.09,Direction::CCW)  hangs 
  // motor.initFOC() will turn motor forward and backward a bit and then also hangs
  // We probably need a SimpleFOC/src/current_sense/hardware_specific/gd32/gd32_mcu.cpp
  if (current_sense.init())
  {
    motor.linkCurrentSense(&current_sense);
    Blink(3,oLedOrange);
  }
  else
  {
    Blink(5);
    OUTN("current_sense.init() failed.")
    return;   // cancel simpleFOC setup
  }
*/

loop()
{
...
    if (current_sense.initialized)
    {
      PhaseCurrent_s currents = current_sense.getPhaseCurrents();
      float current_magnitude = current_sense.getDCCurrent();
      OUT2T("mA",current_magnitude*1000)  // milli Amps
      OUT2T("B mA",currents.b*1000)  // milli Amps
      OUT2T("C mA",currents.c*1000)  // milli Amps
    }
RoboDurden commented 1 year ago

Okay now i see that you indeed have dev branches for all three github repos. Sorry, Github is not intutive for me ;-)

Candas1 commented 1 year ago

No worries.

Candas1 commented 1 year ago

I have got the update event running at the right. (the small peaks are interferences) I also get value from inserted adc, but values are weird. DS1Z_QuickPrint1

But those 2 singles commands mess it up and introduce delays for the interrupt.

// user communication
command.run();
if (oOnOff.Get()) oKeepOn.Set(false);

DS1Z_QuickPrint2

But I used the interrupt only to check when the Update event is trigger by turning an output ON and OFF. I couldn't get the Channel 3 to work yet, need to find why it's not working. Once I have confirmed it's working I will remove the interrupt.

RoboDurden commented 1 year ago

Yes i think i also observed strange behavior with the SELF_HOLD_PIN / BUTTON_PIN as i could not reproduce the normal hoverboard behavior to turn off the board. Do not understand this.

To my understanding, the update event can trigger on overflow (in the middle) and underflow (beginning/ending). I think the beginning/ending is the time when the adc should take place as the gate-high-states are centered to the middle and you said that measurenments should take place when the low-side mosfets are conduction = hi=off & lo=on

As i wanted to call an update handler to trigger adc, i think i can check if it was an underflow and only then start adc. But you want to trigger adc directly i think. But i fear, the GD32 can not be configured to only fire the update event on underlow. Only overflow or overflow+underflow. Then i guess you indeed need the channel3. But with the channel3 you could also start the adc a few microseconds before the beginning/ending :-)

P.S. i had the idea to first do the time critical adc of gate currents then reconfigure the adc

    adc_channel_length_config(ADC_REGULAR_CHANNEL, aiPin.size());
    //adc_channel_length_config(ADC_REGULAR_CHANNEL, 1);

    int iSize = aiPin.size();
    for (int i=0; i<iSize; i++)
    {
        PinName pinname = DIGITAL_TO_PINNAME(aiPin[i]);
        uint32_t gd_pin =  gpio_pin[GD_PIN_GET(pinname)];
        uint32_t gd_port = gpio_port[GD_PORT_GET(pinname)];
        uint8_t gd_channel = get_adc_channel(pinname);

        gpio_mode_set(gd_port, GPIO_MODE_ANALOG, GPIO_PUPD_NONE, gd_pin);
        adc_regular_channel_config(i, gd_channel , ADC_SAMPLETIME_13POINT5);
    }

do the voltage and current_dc when we have all the time in the world and finally reconfigure adc back to the two time critical gate currencies.

Candas1 commented 1 year ago

This is what inserted adc is for. It runs critical sampling with highest priority, while regular adc samples non critical stuff another time. No need to reconfigure.

RoboDurden commented 1 year ago

So you can configure this "inserted adc" only once at the beginning and the update/channel3 event directly triggers putting normal adc on hold and "insert" its adc and wait the few us to finish to read the values or only these inserted adc conversion gets written via dma ? Everything without a handler function to be called ? Only (please) when these time critical adc have been finished to i can put a motor.loopFOC() in that final callback :-)

Candas1 commented 1 year ago

Yes regular adc is put on hold. We need to check how analog read is implemented. I think Arduino-FOC-drivers has a voltage reading 'driver' that uses it.

No 16khz is too fast for loopfoc.

RoboDurden commented 1 year ago

If you give me a callback when this inserted adc has finished, i can call loopFOC only every second call :-)

I overwrite the analog read function in my LowsideCurrentSenseTimer class

#define _ADC_VOLTAGE 3.3f
#define _ADC_RESOLUTION 4096.0f

float LowsideCurrentSenseTimer::ReadAdcVoltage(const int pin)
{
    if (_isset(pin))
        return ai_AdcBuffer[aiAdcMap[pin]] * _ADC_VOLTAGE / _ADC_RESOLUTION;
    return 0;
}
Candas1 commented 1 year ago

Got ch3 to work so I can define where I want to sample. It works without any interrupt. I get one sinusoidale phase current for one pin, but the other one is not reacting, just noise. Need to check if it's the right pin, if it's not defective, or if I did something wrong.

But I have guests from tomorrow to sunday so might be hard to find time to progress.

RoboDurden commented 1 year ago

@Candas1 , i downloaded via zip file your Split_Hoverboard_SimpleFOC-dev but i get compiler error

src\main.cpp:239:5: error: 'GPIO_BOP' was not declared in this scope; did you mean 'GPIOB'? I also do not understand the syntax of

  GPIO_BOP(GPIOB) = (uint32_t)GPIO_PIN_6; 
    motor.loopFOC();
    GPIO_BC(GPIOB) = (uint32_t)GPIO_PIN_6;

Are GPIO_BOP and GPIO_BC macro definitions ?

Also a lot warnings like

src\main.cpp:207:30: warning: ISO C++ forbids converting a string constant to 'char*' [-Wwrite-strings]
  207 |   command.add('T', doTarget, "target voltage");

Am i missing something ?

P.S. with command.add('T', doTarget, "target voltage"); i will be able to set the target speed to 10.2 Volt by entering T10.2 via the RTTStream serial communication ?

RoboDurden commented 1 year ago

@Candas1 , if i remove (comment) these strange GPIO_BOP macro lines and set target by

target = 1 * (motor.voltage_limit) * (ABS((float)(((millis()-iLoopStart)/10 + 250) % 1000) - 500) - 250)/250; The motor is not able to start spinning but only makes a heavy push every 2 seconds and the leds start blinking, i guess, the setup procedure. So board rebooting.

That is because i have replaced the on/off button with a switch and this happens when i keep the switch on. Only after i put the switch off than after the next "push", the motor begins to spin nicely backwards and forwards.

If i only put that switch quickly on and off like pushing a button, then after setup, the motor will also make this heavy "push" and then begins to rotate nicely.

This "push" will make the motor spin for about 720° degrees :-(

RoboDurden commented 1 year ago

Also, the if (current_sense.init()) is still not active and i do not see a gd32 hardware file in https://github.com/Candas1/Arduino-FOC/tree/dev-gd32/src/current_sense/hardware_specific to "catch" the channel3 interrupt. Nor a new LowsideCurrentSensor class to implement your inserted adc - which of course i am eager to see :-)

ideas welcome.

RoboDurden commented 1 year ago

So i continued with my own LowsideCurrentSenseTimer and successfully switched to the TIMER0 update handler

GD32F130C8\Simple FOC\src\drivers\hardware_specific\gd32\gd32_mcu.cpp needs

    nvic_irq_enable(TIMER0_BRK_UP_TRG_COM_IRQn, 0, 0);
    timer_interrupt_enable(TIMER_BLDC, TIMER_INT_UP);

Unfortunately the event handler is already defined and used in the GD32-Arduino-core::timer.c :

/* some devices have this. */
void TIMER0_BRK_UP_TRG_COM_IRQHandler(void)
{
    timerinterrupthandle(TIMER0);
}

So i kept my HardwareTimer in my class but set it to TIMER0

    pTimer = new HardwareTimer(instanceTimer);
    pTimer->attachInterrupt(&timer_cb);

Downside is that the current_sense.init(TIMER0,..) MUST be called before driver.init() because the constructor of HardwareTimer already initializes the TIMER0: timerHandle.init(timerDevice, &timerPeriod); But when the _configure6PWM() reconfigures the TIMER0, it works.

That way the update handler in timer.c forwards to my

void LowsideCurrentSenseTimer::timer_cb() 
{
    //digitalWrite(LED_RED, digitalRead(LED_RED) ^ 1);
    LowsideCurrentSenseTimer::iCount++;

    digitalWrite(LED_ORANGE, (LowsideCurrentSenseTimer::iCount % LowsideCurrentSenseTimer::iTimerHz) < (LowsideCurrentSenseTimer::iTimerHz/4) );

    // Start ADC conversion
    adc_software_trigger_enable(ADC_REGULAR_CHANNEL);
    LowsideCurrentSenseTimer::iMicrosTimerCb = getCurrentMicros();
}

It is called at 16 kHz, so i guess i have only the overflow event which is in the middle of the pwm cycle ? And not the begin/end that we want for adc sampling ? In the user manual i now read that overflow and underlow are always enabled or disabled:


GD32F1x0 User Manual
9.1.4. TIMER0 registers
Control register 0 (TIMERx_CTL0)
bit 2: UPS Update source
This bit is used to select the update event sources by software.
0: Any of the following events generate an update interrupt or DMA request:
– The UPG bit is set
– The counter generates an overflow or underflow event
– The slave mode controller generates an update event.
1: Only counter overflow/underflow generates an update interrupt or DMA request

Will try

void LowsideCurrentSenseTimer::timer_cb() 
{
    LowsideCurrentSenseTimer::iCountTimer0 = timer_counter_read(TIMER0);

and monitor with StmStudio..

Of course, using Channel3 instead of Update-Event will be better.

Ideas welcome :-)

update: the timer0 counter is unrelated to an overflow or underflow :-/ stmstudio iCountTimer0 The range from 0 to 1500 is exactly timerBldc_parameter_struct.period = SystemCoreClock / pwm_frequency;

Candas1 commented 1 year ago

Those macros are just the low level way to turn outputs on or off. I used this to measure duration from my oscilloscope. This code is compiling fine for me. I have the warnings but it's not important. Yes you can set up to T-15 or T15 but I usually increasing step by step otherwise it's too violent. E0 or E0 to disable or enable the smoothing.

I haven't uploaded the current sensing as it's still work in progress.

RoboDurden commented 1 year ago

Hi Candas :-) As you can read i would be happy if you share your (complete) code that "Got ch3 to work".

P.S. after removing some OUT(..) log output now my callback runs at 32 kHz. So two events called per period. But of course no use when the callback time is unrelated to the actual over/underflow :-/

RoboDurden commented 1 year ago

But the timer0_update_handler is called exactly every 31 us:

    void TIMER0_BRK_UP_TRG_COM_IRQHandler(void)
    {

        if (SET == timer_interrupt_flag_get(TIMER_BLDC,TIMER_INT_FLAG_UP))
        {
            uint32_t iNow = getCurrentMicros();
            LowsideCurrentSenseTimer::iMicrosAdcReady = iNow - LowsideCurrentSenseTimer::iMicrosTimerCb;
                LowsideCurrentSenseTimer::iMicrosTimerCb = iNow;
            LowsideCurrentSenseTimer::iCountTimer0 = timer_counter_read(TIMER0);

Which reflects the 32 kHz (overflow + underflow) of the 16 kHz pwm. But if the handler gets called exactly in sync, then the timer counter should always be the same (with some offset maybe). Yet i get

31: 80
31: 74
31: 1420
31: 79
31: 1475
31: 210
31: 430
31: 79 

(these log values are 100.000 microseconds appart.)

timer_counter_read(TIMER0); might be wrong but it resolves to TIMER_CNT(timer_periph) which leads to #define TIMER_CNT(timerx) REG32((timerx) + 0x00000024U) /*!< TIMER counter register */ and the user manual confirms: This bit-filed indicates the current counter value. Writing to this bit-filed can change the value of the counter

I do not understand this.

RoboDurden commented 1 year ago

As the log output via RTT is only every 100000 micro seconds i have stored the last 10 calls

LowsideCurrentSenseTimer::aiMillis[LowsideCurrentSenseTimer::iPosMillis] = timer_counter_read(TIMER0);
LowsideCurrentSenseTimer::iPosMillis = (LowsideCurrentSenseTimer::iPosMillis +1) % 10;

and that looks more like two different events within the 16 kHz period:

1413    1413    87      87      87      87      1412    1412    1412
87      87      1412    1412    1412    1412    88      88      88
87      87      1412    1412    1412    1412    1412    88      88
87      87      87      1412    1412    1412    1412    1412    88
87      87      87      1412    1412    1412    1412    88      88
1413    1413    87      87      87      87      1412    1412    1412
87      87      1412    1412    1412    1412    88      88      88
87      87      1412    1412    1412    1412    1412    88      88
87      87      87      1412    1412    1412    1412    88      88
87      87      87      1413    1413    1413    1413    88      88
1413    1413    87      87      87      87      1412    1412    1412

The range of the counter is 1500: timerBldc_parameter_struct.period = SystemCoreClock / pwm_frequency;

But still i do not understand why 87 and 1412, and why they do not toggle like 87 1412 87 1412 87

maybe you @robcazzaro have an idea :-)

RoboDurden commented 1 year ago

okay, i had to add thread safety:

      int iPos = LowsideCurrentSenseTimer::iPosMillis;
      LowsideCurrentSenseTimer::iPosMillis = -1;  // thread safty
      for (int i=9; i>0; i--)
        OUTT(LowsideCurrentSenseTimer::aiMillis[(iPos + i) % 10])
      LowsideCurrentSenseTimer::iPosMillis = iPos;  // thread safty

and

    void TIMER0_BRK_UP_TRG_COM_IRQHandler(void)
    {
        if (SET == timer_interrupt_flag_get(TIMER_BLDC,TIMER_INT_FLAG_UP))
        {
                uint32_t iCounter = timer_counter_read(TIMER0);
            if (LowsideCurrentSenseTimer::iPosMillis >= 0)  // thread safety
            {
                uint32_t iNow = getCurrentMicros();
                LowsideCurrentSenseTimer::iMicrosAdcReady = iNow - LowsideCurrentSenseTimer::iMicrosTimerCb;
                    LowsideCurrentSenseTimer::iMicrosTimerCb = iNow;
                LowsideCurrentSenseTimer::aiMillis[LowsideCurrentSenseTimer::iPosMillis] = iCounter;
                LowsideCurrentSenseTimer::iPosMillis = (LowsideCurrentSenseTimer::iPosMillis +1) % 10;
                //LowsideCurrentSenseTimer::iCountTimer0 = timer_counter_read(TIMER0);
                //LowsideCurrentSenseTimer::iCountTimer0 = timer_channel_capture_value_register_read(TIMER0,TIMER_CH_3);
            }

            //digitalWrite(LED_RED, digitalRead(LED_RED) ^ 1);
            LowsideCurrentSenseTimer::iCount++;
            digitalWrite(LED_ORANGE, (LowsideCurrentSenseTimer::iCount % LowsideCurrentSenseTimer::iTimerHz) < (LowsideCurrentSenseTimer::iTimerHz/4) );

            // Start ADC conversion only once in a peridod
            if (iCounter < 500)
                adc_software_trigger_enable(ADC_REGULAR_CHANNEL);

            timer_interrupt_flag_clear(TIMER_BLDC,TIMER_INT_FLAG_UP);
        }
    }

to get the nice two update events per pwm period:

46      1454    46      1454    46      1454    46      1454    45
46      1454    47      1454    45      1454    46      1454    46
45      1454    46      1454    46      1454    46      1454    46
46      1454    46      1454    46      1454    46      1454    46
46      1454    46      1454    46      1454    46      1454    46

Now i of course would like to understand how 46 and 1454 correspond to overflow and underflow. But as 1464 + 46 = 1500 = period, it seems that the interrupt handler has a 46 microseconds delay :-/

So i need the channel3 interrupt :-)

https://github.com/RoboDurden/Arduino-FOC/blob/master/src/current_sense/LowsideCurrentSenseTimer.cpp#L247 https://github.com/RoboDurden/Split_Hoverboard_SimpleFOC/blob/main/src/main.cpp#L327

Candas1 commented 1 year ago

You don't understand. It will happen with any interrupt. If another interrupt with higher or equal preemption is running, your interrupt will wait. That's why I haven't used interrupts.

The benefit of ch3 is that your trigger the event at a specific timer counter.

RoboDurden commented 1 year ago

Yes i understand that YOU do not need a timer interrupt handler. But as you do not share your progress, i have to progress from what i can do so far - and that is triggering adc from a timer0-interrupt handler :-)

i am a born loser !

Candas1 commented 1 year ago

I did all this now. This is waste of time. Anyway....

RoboDurden commented 1 year ago

Yes you already said that you all did this already. But i do not see your code online.

Candas1 commented 1 year ago

I will share it when it fully works. You don't believe me? 🤣

RoboDurden commented 1 year ago

Of course i believe you. The problem is that i do not want to wait :-) It is my time that i am wasting.

Candas1 commented 1 year ago

Your feedback about the smoothing would be welcome if you are bored. How is it comparing to EFeru's sinusoidal in terms of speed and consumption for example.

RoboDurden commented 1 year ago

Okay, i will do that tomorrow :-) But i had this nasty "push" before the motor starts spinning, so i continued with my repo to wait for your comment on this.

Candas1 commented 1 year ago

I don't have anything like this. The target is 0 at startup. I feed the sensor direction and zero angle so the calibration is skipped. Those are not parameters of initFOC function anymore in the dev branch of simplefoc.

RoboDurden commented 1 year ago

Yes and my sine one-liner also starts with 0. Okay, will inverstigate myself tomorrow.

Candas1 commented 1 year ago

I get one sinusoidale phase current for one pin, but the other one is not reacting, just noise. Need to check if it's the right pin, if it's not defective, or if I did something wrong.

Finally I had some time to work on it today. Actually the problem was my board, with another board I can sample the 2 phase currents. image

The driverAlign fails for some reason, I need to investigate why. But if I skip it I can run in FOC current mode. https://www.youtube.com/shorts/dns41fACYqU

Need to experiment more and check if there is anything to tweak.

Candas1 commented 1 year ago

Hi, some update. The gain was set as 50 but I don't think it's correct. Anyone knowledgeable about opamps ? This is for dual boards but maybe it's the same, so 2.5. Eferu was wrongly using the same gain of 11 for both phase current and dc current.

Also, I just remembered that with Eferu's firmware the current readings were inverted, so if it's the same I need negative gains for the phases.

When this is correct I need to tweak the current PID parameters.

RoboDurden commented 1 year ago

You could connect a battery with a resistor to one phase cable to have 10 A when the lower MOSFET is ON. With no inductivity the pwm current should be equal to a multimeter measurement. Then simply calculate the opamp gain from your adc readings.

The opamp gain depends on a little SMD resistor in the feedback line from its output back to it's input, I think.

As I read in your first link, you can also connect the minus pole of the battery to a phase cable and positive pole to hoverboard minus. Then the -10A should give an adc reading below the offset which should be 1.x volt. Something like 0.5V lower.

Candas1 commented 1 year ago

Some of this is actually supposed to be done in the function driveralign, It should power each phase separately and detect if pins are swapped or gains are inverted, I need to check why it fails.

That's one of the features I started to implement on Eferu but then I saw it's already available with SFOC.

RoboDurden commented 1 year ago

from my defines_2-0.h:

// BLDC low-side current sense pins (R_ds)
#define BLDC_CUR_Rds 0.008  // R_ds of the low side n-Chnannel mosfets
#define BLDC_CUR_Gain 50.0  // gain of the op-amp to amplify voltage_drain-source
#define BLDC_CUR_G_PIN _NC  // simpleFOC can handle 2 or 3 current sense pins
#define BLDC_CUR_B_PIN PB1
#define BLDC_CUR_Y_PIN PB0

Forgot where i got these numbers from :-/ Maybe i searched the net for what mosfets are found on the boards and did lookup the R_ds_on in a datasheet. Yes, the gain of 50 seems to be to high. With an offset at the middle of the 0 - 3.2 Volt adc range, that leaves 1.6 Volt to measure from 0 to +-35 A.

So 1.6V / (0.008 Ohm * 35 A) = 5.71 and a BLDC_CUR_Gain of 5.0 should fit better.

But attaching an externally defined 10A to a phase cable and simply measure the adc output should be the easiest way to verifiy the offset and gain.

I will be continuing my journey with my 10 km/h solar micro camper tomorrow, and will not have a test setup with me the next 2-3 weeks. So if you want me to test something, than i can do this only today.

RoboDurden commented 1 year ago

According to https://en.wikipedia.org/wiki/Operational_amplifier :: Closed-loop-amplifier closed loop amplifier the gain is 1 + Rf / Rg. With the hoverboard_schematics.pdf the gain for current_DC (shunt resistor) is 1 + 10/1 = 11 opamp current_dc The gain for phase currents should be 1 + 2.2/1 = 3.3 (at least for defines_1-0.h : opamp phase_current PH_AMP_VN is the source of the mosfet (which connects to the current_dc-shunt_resistor and PH_AMP_VP is the Drain of the mosfet which connects to one of the three phase cables. opamp phase_current pins

update: The phase current offset voltage is generated by the 220k : 40k voltage divider and should only be 40/260 3.3V = 0.5 Volt. So no -35A. But -10A with a gain of 3.3 would be `3.3 0.008 * -10 = -0.264.0.5 + -0.264 = +0.236 V` :-)

Candas1 commented 1 year ago

Found another schematic from another repository for our board, it's still missing the phase current sensing but it has more details.

Candas1 commented 1 year ago

I checked, I think layout 2.0 is same as 1.0: image

This is the op amp.

Candas1 commented 1 year ago

Ok the reason why driveralign was failing is because it can only swap 2 phase currents, not identify each phase current. With the following pinout driveralign doesn't fail anymore.

#define BLDC_CUR_G_PIN PB1  
#define BLDC_CUR_B_PIN PB0
#define BLDC_CUR_Y_PIN _NC  // simpleFOC can handle 2 or 3 current sense pins
RoboDurden commented 1 year ago

A bit strange that BLDC_CUR_B_PIN is not PB1 because the mapping green blue yellow to A B C like SFOC is using of course could be reversed.

Candas1 commented 1 year ago

Somes updates. On thursday I tweaked the gains and the PID parameters, it was running quite well. When I went back at it on saturday it was not working anymore. My power supply was going to CC mode and the mosfets were getting hot. I thought it was because of my changes and wasted a lot of time.

But it turned out the offsets were different and I had to tweak it to a different value each time for phase A. I need to investigate what is happening there.

RoboDurden commented 1 year ago

offset_ia etc. are not stored in some eeprom and are not even set to zero in the constructor. So when you uncomment the offset calculation and reset the board, the old values might still be in the ram. But when you turn off and on you get random numbers. I am not sure that floats always get initialized to zero.

Candas1 commented 1 year ago

I don't think it's a bug with SFOC, even if I force my own offset values after the calibration, I still need to tweak the values again. I believe the gains can change with temperature as we use mosfet RDSON instead of real shunt resistors, but the offsets should n't change.

Need to check if the adc readings are stable during the offset calibration. This used to be a problem even with Eferu in the past when I was playing with AT32 boards.

Those offsets are really important and should be done when the wheels are not moving. I believe this was also the reason why some boards would burn at start up with FOC torque mode. If your 0 is wrong you will apply a lot more current than expected, and your overcurrent protection won't pick it up.

Imagine if the board happens to reset while driving, and the offset calibration starts, this can go horribly wrong. It seems VESC is saving the offsets to eeprom instead of running it at each startup, we might have to do that as well.

RoboDurden commented 1 year ago

Great that you take this problem so seriously. board resets do happen with my little solar camper !!! The offset should be the middle of 3.2 V and defined by the 220k : 20k+20k voltage divider. The hot mosfet R_ds_on would only change the scaling around this offset. So not such a big problem ? If the 20k+20k resistors are close to the 220k on the board, they should heat equally and the offset would not change.

Candas1 commented 1 year ago

For offset calibration to happen, the pwms have to be set to 0,0,0 to turn all low side mosfets ON. Maybe this is generating a small movement and a delay between driver initialization and offset calibration can help.

I really want to get this right so I will take the time to investigate more.

Candas1 commented 1 year ago

I used another board and now the offsets are stable.

It's the 3rd board, 3 different behaviors. I need to figure out later what is wrong with those 2 boards.

RoboDurden commented 1 year ago

You could measure the input of the opamp to verify that the 40:220 voltage divider is stable. Then the adc in the gd32 must be the problem.

During offset calibration all high side MOSFET must be off and the low side MOSFETs on I guess. Maybe the code does not work. Or some MOSFETs are bad and have some leakage ? Then unplugging the motor phase cable might show a decrease in overall power consumption.

I still have this annoying problem with my gen1 board driving my solar camper but generating some heavy noise either over the battery cables or the UART bus and my i2c OLED collapses and the ads1115 i2c adc module sends wrong readings.

But something like that should affect all your adc inputs.

Maybe these boards had a heavy over current and the adc channel of that MOSFET is broken - a bit :-/

Just some ideas

Candas1 commented 1 year ago

After the offset calibration, Current is visible on one phase current even in voltage mode at zero target, which I think should not happen as the pwm duty cycle is 50% for all the phases so no current should flow. So it could be some leakage, need to check that phase with the oscilloscope.

I think the code is ok as it works fine with the last board, but I will try to find the root cause, more people will face this for sure in the future.

We could compile all those checks in a test firmware.

RoboDurden commented 1 year ago

The 2.0 layouts have analog gate drivers. Maybe check the gate voltages of the two corresponding MOSFET. If there is a leakage bit the MOSFET still okay, the gate voltage might not be pulled actively down to zero..

Greetings from my little Forrest bis zu 60 cm im steilen Hang

Candas1 commented 1 year ago

Some updates. All works well on the last 2 boards I tried. I will fix the other ones later. I experimented with the Foc current/torque mode, the way it works is counterintuitive but it was the same with Eferu. I need to try it with load.

I had bought this a few month ago but it's really overkill.... I thought I could use it as a load. image

I bought this recently for 8e, should be good enough for now: image It's just a metal disc spinning between magnets, I could probably adapt the same principle to a hoverboard wheel directly.

I experimented with pwm deadtime compensation, it makes the phase current waveform much more sinusoidal, and the motor stutter less at slow speed. It's highly experimental. I really like this new tool, it's not fully stable but better than stm studio already.

I will publish my recent code. I haven't touched the drivers recently.

Candas1 commented 1 year ago

I just published my code for current sensing. I added a parameter that lets you run an interrupt with a basic filtering of the phase current as I was experimenting.

This and this might need some tweaking to behave well at high duty cycle. I kept the default current PI parameters for now, it seemed to be ok.

SFOC doesn't seem to limit the current, it's not a problem during the tests without load, but it might be during a real drive. My gd32 implementation is using the overcurrent trigger pin that might kick in.

Now I need to check if analogread still works and doesn't break the current sensing, with the battery voltage for example. The SFOC team told me it's not working for them with STM32.

No news from the Arduino-gd32 about some fixes I need to close the PWM driver init.

RoboDurden commented 1 year ago

I will be back home in about a week.

I think I would like to write a little pin auto detect firmware to help beginners add the many different layouts.. Could it be possible that the six MOSFET outputs are always the same because of only timer0 having this extended pwm feature ?

With that nice stlink serial interface it should be easy to test the hall pins and led pins. Only the onoff button and the hold pin are a bit tricky.

Any chance your SFOC firmware will stay below 32k ? Layout 2.6 again seems to be a x6 MCU :-/